v4l2uvc.c
Go to the documentation of this file.
00001 /*******************************************************************************
00002 # Linux-UVC streaming input-plugin for MJPG-streamer                           #
00003 #                                                                              #
00004 # This package work with the Logitech UVC based webcams with the mjpeg feature #
00005 #                                                                              #
00006 # Copyright (C) 2005 2006 Laurent Pinchart &&  Michel Xhaard                   #
00007 #                    2007 Lucas van Staden                                     #
00008 #                    2007 Tom Stöveken                                         #
00009 #                                                                              #
00010 # This program is free software; you can redistribute it and/or modify         #
00011 # it under the terms of the GNU General Public License as published by         #
00012 # the Free Software Foundation; either version 2 of the License, or            #
00013 # (at your option) any later version.                                          #
00014 #                                                                              #
00015 # This program is distributed in the hope that it will be useful,              #
00016 # but WITHOUT ANY WARRANTY; without even the implied warranty of               #
00017 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the                #
00018 # GNU General Public License for more details.                                 #
00019 #                                                                              #
00020 # You should have received a copy of the GNU General Public License            #
00021 # along with this program; if not, write to the Free Software                  #
00022 # Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA    #
00023 #                                                                              #
00024 *******************************************************************************/
00025 
00026 #include <stdlib.h>
00027 #include <errno.h>
00028 #include "v4l2uvc.h"
00029 #include "huffman.h"
00030 #include "dynctrl.h"
00031 
00032 static int debug = 0;
00033 
00034 /* ioctl with a number of retries in the case of failure
00035 * args:
00036 * fd - device descriptor
00037 * IOCTL_X - ioctl reference
00038 * arg - pointer to ioctl data
00039 * returns - ioctl result
00040 */
00041 int xioctl(int fd, int IOCTL_X, void *arg)
00042 {
00043     int ret = 0;
00044     int tries = IOCTL_RETRY;
00045     do {
00046         ret = IOCTL_VIDEO(fd, IOCTL_X, arg);
00047     } while(ret && tries-- &&
00048             ((errno == EINTR) || (errno == EAGAIN) || (errno == ETIMEDOUT)));
00049 
00050     if(ret && (tries <= 0)) fprintf(stderr, "ioctl (%i) retried %i times - giving up: %s)\n", IOCTL_X, IOCTL_RETRY, strerror(errno));
00051 
00052     return (ret);
00053 }
00054 
00055 static int init_v4l2(struct vdIn *vd);
00056 
00057 int init_videoIn(struct vdIn *vd, char *device, int width,
00058                  int height, int fps, int format, int grabmethod, globals *pglobal, int id, v4l2_std_id vstd)
00059 {
00060     if(vd == NULL || device == NULL)
00061         return -1;
00062     if(width == 0 || height == 0)
00063         return -1;
00064     if(grabmethod < 0 || grabmethod > 1)
00065         grabmethod = 1;     //mmap by default;
00066     vd->videodevice = NULL;
00067     vd->status = NULL;
00068     vd->pictName = NULL;
00069     if((vd->fd = OPEN_VIDEO(vd->videodevice, 0))) {
00070     vd->videodevice = (char *) calloc(1, 16 * sizeof(char));
00071     vd->status = (char *) calloc(1, 100 * sizeof(char));
00072     vd->pictName = (char *) calloc(1, 80 * sizeof(char));
00073     snprintf(vd->videodevice, 12, "%s", device);
00074     vd->toggleAvi = 0;
00075     vd->getPict = 0;
00076     vd->signalquit = 1;
00077     vd->width = width;
00078     vd->height = height;
00079     vd->fps = fps;
00080     vd->formatIn = format;
00081         vd->vstd = vstd;
00082     vd->grabmethod = grabmethod;
00083     vd->soft_framedrop = 0;
00084     if(init_v4l2(vd) < 0) {
00085         fprintf(stderr, " Init v4L2 failed !! exit fatal \n");
00086         goto error;;
00087     }
00088     }
00089     // getting the name of the input source
00090     struct v4l2_input in_struct;
00091     memset(&in_struct, 0, sizeof(struct v4l2_input));
00092     in_struct.index = 0;
00093     if (xioctl(vd->fd, VIDIOC_ENUMINPUT,  &in_struct) == 0) {
00094         int nameLength = strlen((char*)&in_struct.name);
00095         pglobal->in[id].handle = malloc((1+nameLength)*sizeof(char));
00096         sprintf(pglobal->in[id].handle, "%s", in_struct.name);
00097         DBG("Input name: %s\n", in_struct.name);
00098     } else {
00099         DBG("VIDIOC_ENUMINPUT failed\n");
00100     }
00101 
00102     // enumerating formats
00103 
00104     struct v4l2_format currentFormat;
00105     memset(&currentFormat, 0, sizeof(struct v4l2_format));
00106     currentFormat.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00107     if (xioctl(vd->fd, VIDIOC_G_FMT, &currentFormat) == 0) {
00108         DBG("Current size: %dx%d\n",
00109              currentFormat.fmt.pix.width,
00110              currentFormat.fmt.pix.height);
00111     }
00112 
00113     pglobal->in[id].in_formats = NULL;
00114     for(pglobal->in[id].formatCount = 0; 1; pglobal->in[id].formatCount++) {
00115         struct v4l2_fmtdesc fmtdesc;
00116         memset(&fmtdesc, 0, sizeof(struct v4l2_fmtdesc));
00117         fmtdesc.index = pglobal->in[id].formatCount;
00118         fmtdesc.type  = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00119         if(xioctl(vd->fd, VIDIOC_ENUM_FMT, &fmtdesc) < 0) {
00120             break;
00121         }
00122 
00123         if (pglobal->in[id].in_formats == NULL) {
00124             pglobal->in[id].in_formats = (input_format*)calloc(1, sizeof(input_format));
00125         } else {
00126             pglobal->in[id].in_formats = (input_format*)realloc(pglobal->in[id].in_formats, (pglobal->in[id].formatCount + 1) * sizeof(input_format));
00127         }
00128 
00129         if (pglobal->in[id].in_formats == NULL) {
00130             DBG("Calloc/realloc failed: %s\n", strerror(errno));
00131             return -1;
00132         }
00133 
00134         memcpy(&pglobal->in[id].in_formats[pglobal->in[id].formatCount], &fmtdesc, sizeof(input_format));
00135 
00136         if(fmtdesc.pixelformat == format)
00137             pglobal->in[id].currentFormat = pglobal->in[id].formatCount;
00138 
00139         DBG("Supported format: %s\n", fmtdesc.description);
00140         struct v4l2_frmsizeenum fsenum;
00141         memset(&fsenum, 0, sizeof(struct v4l2_frmsizeenum));
00142         fsenum.pixel_format = fmtdesc.pixelformat;
00143         int j = 0;
00144         pglobal->in[id].in_formats[pglobal->in[id].formatCount].supportedResolutions = NULL;
00145         pglobal->in[id].in_formats[pglobal->in[id].formatCount].resolutionCount = 0;
00146         pglobal->in[id].in_formats[pglobal->in[id].formatCount].currentResolution = -1;
00147         while(1) {
00148             fsenum.index = j;
00149             j++;
00150             if(xioctl(vd->fd, VIDIOC_ENUM_FRAMESIZES, &fsenum) == 0) {
00151                 pglobal->in[id].in_formats[pglobal->in[id].formatCount].resolutionCount++;
00152 
00153                 if (pglobal->in[id].in_formats[pglobal->in[id].formatCount].supportedResolutions == NULL) {
00154                     pglobal->in[id].in_formats[pglobal->in[id].formatCount].supportedResolutions = (input_resolution*)
00155                             calloc(1, sizeof(input_resolution));
00156                 } else {
00157                     pglobal->in[id].in_formats[pglobal->in[id].formatCount].supportedResolutions = (input_resolution*)
00158                             realloc(pglobal->in[id].in_formats[pglobal->in[id].formatCount].supportedResolutions, j * sizeof(input_resolution));
00159                 }
00160 
00161                 if (pglobal->in[id].in_formats[pglobal->in[id].formatCount].supportedResolutions == NULL) {
00162                     DBG("Calloc/realloc failed\n");
00163                     return -1;
00164                 }
00165 
00166                 pglobal->in[id].in_formats[pglobal->in[id].formatCount].supportedResolutions[j-1].width = fsenum.discrete.width;
00167                 pglobal->in[id].in_formats[pglobal->in[id].formatCount].supportedResolutions[j-1].height = fsenum.discrete.height;
00168                 if(format == fmtdesc.pixelformat) {
00169                     pglobal->in[id].in_formats[pglobal->in[id].formatCount].currentResolution = (j - 1);
00170                     DBG("\tSupported size with the current format: %dx%d\n", fsenum.discrete.width, fsenum.discrete.height);
00171                 } else {
00172                     DBG("\tSupported size: %dx%d\n", fsenum.discrete.width, fsenum.discrete.height);
00173                 }
00174             } else {
00175                 break;
00176             }
00177         }
00178     }
00179 
00180     /* alloc a temp buffer to reconstruct the pict */
00181     vd->framesizeIn = (vd->width * vd->height << 1);
00182     switch(vd->formatIn) {
00183     case V4L2_PIX_FMT_MJPEG: // in JPG mode the frame size is varies at every frame, so we allocate a bit bigger buffer
00184         vd->tmpbuffer = (unsigned char *) calloc(1, (size_t) vd->framesizeIn);
00185         if(!vd->tmpbuffer)
00186             goto error;
00187         vd->framebuffer =
00188             (unsigned char *) calloc(1, (size_t) vd->width * (vd->height + 8) * 2);
00189         break;
00190     case V4L2_PIX_FMT_RGB565: // buffer allocation for non varies on frame size formats
00191     case V4L2_PIX_FMT_YUYV:
00192     case V4L2_PIX_FMT_RGB24:
00193         vd->framebuffer =
00194             (unsigned char *) calloc(1, (size_t) vd->framesizeIn);
00195         break;
00196     default:
00197         fprintf(stderr, " should never arrive exit fatal !!\n");
00198         goto error;
00199         break;
00200 
00201     }
00202 
00203     if(!vd->framebuffer)
00204         goto error;
00205     return 0;
00206 error:
00207     free(pglobal->in[id].in_parameters);
00208     free(vd->videodevice);
00209     free(vd->status);
00210     free(vd->pictName);
00211     CLOSE_VIDEO(vd->fd);
00212     return -1;
00213 }
00214 
00215 static int init_v4l2(struct vdIn *vd)
00216 {
00217     int i;
00218     int ret = 0;
00219     if((vd->fd = OPEN_VIDEO(vd->videodevice, O_RDWR)) == -1) {
00220         perror("ERROR opening V4L interface");
00221         DBG("errno: %d", errno);
00222         return -1;
00223     }
00224 
00225     memset(&vd->cap, 0, sizeof(struct v4l2_capability));
00226     ret = xioctl(vd->fd, VIDIOC_QUERYCAP, &vd->cap);
00227     if(ret < 0) {
00228         fprintf(stderr, "Error opening device %s: unable to query device.\n", vd->videodevice);
00229         goto fatal;
00230     }
00231 
00232     if((vd->cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
00233         fprintf(stderr, "Error opening device %s: video capture not supported.\n",
00234                 vd->videodevice);
00235         goto fatal;;
00236     }
00237 
00238     if(vd->grabmethod) {
00239         if(!(vd->cap.capabilities & V4L2_CAP_STREAMING)) {
00240             fprintf(stderr, "%s does not support streaming i/o\n", vd->videodevice);
00241             goto fatal;
00242         }
00243     } else {
00244         if(!(vd->cap.capabilities & V4L2_CAP_READWRITE)) {
00245             fprintf(stderr, "%s does not support read i/o\n", vd->videodevice);
00246             goto fatal;
00247         }
00248     }
00249 
00250     if (vd->vstd != V4L2_STD_UNKNOWN) {
00251         if (ioctl(vd->fd, VIDIOC_S_STD, &vd->vstd) == -1) {
00252             fprintf(stderr, "Can't set video standard: %s\n",strerror(errno));
00253             goto fatal;
00254         }
00255     }
00256 
00257     /*
00258      * set format in
00259      */
00260     memset(&vd->fmt, 0, sizeof(struct v4l2_format));
00261     vd->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00262     vd->fmt.fmt.pix.width = vd->width;
00263     vd->fmt.fmt.pix.height = vd->height;
00264     vd->fmt.fmt.pix.pixelformat = vd->formatIn;
00265     vd->fmt.fmt.pix.field = V4L2_FIELD_ANY;
00266     ret = xioctl(vd->fd, VIDIOC_S_FMT, &vd->fmt);
00267     if(ret < 0) {
00268         fprintf(stderr, "Unable to set format: %d res: %dx%d\n", vd->formatIn, vd->width, vd->height);
00269         goto fatal;
00270     }
00271 
00272     if((vd->fmt.fmt.pix.width != vd->width) ||
00273             (vd->fmt.fmt.pix.height != vd->height)) {
00274         fprintf(stderr, "i: The format asked unavailable, so the width %d height %d \n", vd->fmt.fmt.pix.width, vd->fmt.fmt.pix.height);
00275         vd->width = vd->fmt.fmt.pix.width;
00276         vd->height = vd->fmt.fmt.pix.height;
00277         /*
00278          * look the format is not part of the deal ???
00279          */
00280         if(vd->formatIn != vd->fmt.fmt.pix.pixelformat) {
00281             if(vd->formatIn == V4L2_PIX_FMT_MJPEG) {
00282                 fprintf(stderr, "The input device does not supports MJPEG mode\n"
00283                                 "You may also try the YUV mode (-yuv option), \n"
00284                                 "or the you can set another supported formats using the -fourcc argument.\n"
00285                                 "Note: streaming using uncompressed formats will require much more CPU power on your server\n");
00286                 goto fatal;
00287             } else if(vd->formatIn == V4L2_PIX_FMT_YUYV) {
00288                 fprintf(stderr, "The input device does not supports YUV format\n");
00289                 goto fatal;
00290             } else if (vd->formatIn == V4L2_PIX_FMT_RGB565) {
00291                 fprintf(stderr, "The input device does not supports RGB565 format\n");
00292                 goto fatal;
00293             } else if (vd->formatIn == V4L2_PIX_FMT_RGB24) {
00294                 fprintf(stderr, "The input device does not supports RGB3 format\n");
00295                 goto fatal;
00296             }
00297         } else {
00298             vd->formatIn = vd->fmt.fmt.pix.pixelformat;
00299         }
00300     }
00301 
00302     /*
00303      * set framerate
00304      */
00305 
00306     if (vd->fps != -1) {
00307         struct v4l2_streamparm *setfps;
00308         setfps = (struct v4l2_streamparm *) calloc(1, sizeof(struct v4l2_streamparm));
00309         memset(setfps, 0, sizeof(struct v4l2_streamparm));
00310         setfps->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00311 
00312         /*
00313         * first query streaming parameters to determine that the FPS selection is supported
00314         */
00315         ret = xioctl(vd->fd, VIDIOC_G_PARM, setfps);
00316         if (ret == 0) {
00317             if (setfps->parm.capture.capability & V4L2_CAP_TIMEPERFRAME) {
00318                 memset(setfps, 0, sizeof(struct v4l2_streamparm));
00319                 setfps->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00320                 setfps->parm.capture.timeperframe.numerator = 1;
00321                 setfps->parm.capture.timeperframe.denominator = vd->fps==-1?255:vd->fps; // if no default fps set set it to maximum
00322 
00323                 ret = xioctl(vd->fd, VIDIOC_S_PARM, setfps);
00324                 if (ret) {
00325                     perror("Unable to set the FPS\n");
00326                 } else {
00327                     if (vd->fps != setfps->parm.capture.timeperframe.denominator) {
00328                         IPRINT("FPS coerced ......: from %d to %d\n", vd->fps, setfps->parm.capture.timeperframe.denominator);
00329                     }
00330 
00331                     // if we selecting lower FPS than the allowed then we will use software framedropping
00332                     if (vd->fps < setfps->parm.capture.timeperframe.denominator) {
00333                         vd->soft_framedrop = 1;
00334                         vd->frame_period_time = 1000/vd->fps; // calcualate frame period time in ms
00335                         IPRINT("Frame period time ......: %ld ms\n", vd->frame_period_time);
00336 
00337                         // set FPS to maximum in order to minimize the lagging
00338                         memset(setfps, 0, sizeof(struct v4l2_streamparm));
00339                         setfps->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00340                         setfps->parm.capture.timeperframe.numerator = 1;
00341                         setfps->parm.capture.timeperframe.denominator = 255;
00342                         ret = xioctl(vd->fd, VIDIOC_S_PARM, setfps);
00343                         if (ret) {
00344                             perror("Unable to set the FPS\n");
00345                         }
00346                     }
00347                 }
00348             } else {
00349                 perror("Setting FPS on the capture device is not supported, fallback to software framedropping\n");
00350                 vd->soft_framedrop = 1;
00351                 vd->frame_period_time = 1000/vd->fps; // calcualate frame period time in ms
00352                 IPRINT("Frame period time ......: %ld ms\n", vd->frame_period_time);
00353             }
00354         } else {
00355             perror("Unable to query that the FPS change is supported\n");
00356         }
00357     }
00358 
00359     /*
00360      * request buffers
00361      */
00362     memset(&vd->rb, 0, sizeof(struct v4l2_requestbuffers));
00363     vd->rb.count = NB_BUFFER;
00364     vd->rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00365     vd->rb.memory = V4L2_MEMORY_MMAP;
00366 
00367     ret = xioctl(vd->fd, VIDIOC_REQBUFS, &vd->rb);
00368     if(ret < 0) {
00369         perror("Unable to allocate buffers");
00370         goto fatal;
00371     }
00372 
00373     /*
00374      * map the buffers
00375      */
00376     for(i = 0; i < NB_BUFFER; i++) {
00377         memset(&vd->buf, 0, sizeof(struct v4l2_buffer));
00378         vd->buf.index = i;
00379         vd->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00380         vd->buf.memory = V4L2_MEMORY_MMAP;
00381         ret = xioctl(vd->fd, VIDIOC_QUERYBUF, &vd->buf);
00382         if(ret < 0) {
00383             perror("Unable to query buffer");
00384             goto fatal;
00385         }
00386 
00387         if(debug)
00388             fprintf(stderr, "length: %u offset: %u\n", vd->buf.length, vd->buf.m.offset);
00389 
00390         vd->mem[i] = mmap(0 /* start anywhere */ ,
00391                           vd->buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, vd->fd,
00392                           vd->buf.m.offset);
00393         if(vd->mem[i] == MAP_FAILED) {
00394             perror("Unable to map buffer");
00395             goto fatal;
00396         }
00397         if(debug)
00398             fprintf(stderr, "Buffer mapped at address %p.\n", vd->mem[i]);
00399     }
00400 
00401     /*
00402      * Queue the buffers.
00403      */
00404     for(i = 0; i < NB_BUFFER; ++i) {
00405         memset(&vd->buf, 0, sizeof(struct v4l2_buffer));
00406         vd->buf.index = i;
00407         vd->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00408         vd->buf.memory = V4L2_MEMORY_MMAP;
00409         ret = xioctl(vd->fd, VIDIOC_QBUF, &vd->buf);
00410         if(ret < 0) {
00411             perror("Unable to queue buffer");
00412             goto fatal;;
00413         }
00414     }
00415     return 0;
00416 fatal:
00417     return -1;
00418 
00419 }
00420 
00421 static int video_enable(struct vdIn *vd)
00422 {
00423     int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00424     int ret;
00425 
00426     ret = xioctl(vd->fd, VIDIOC_STREAMON, &type);
00427     if(ret < 0) {
00428         perror("Unable to start capture");
00429         return ret;
00430     }
00431     vd->streamingState = STREAMING_ON;
00432     return 0;
00433 }
00434 
00435 static int video_disable(struct vdIn *vd, streaming_state disabledState)
00436 {
00437     int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00438     int ret;
00439     DBG("STopping capture\n");
00440     ret = xioctl(vd->fd, VIDIOC_STREAMOFF, &type);
00441     if(ret != 0) {
00442         perror("Unable to stop capture");
00443         return ret;
00444     }
00445     DBG("STopping capture done\n");
00446     vd->streamingState = disabledState;
00447     return 0;
00448 }
00449 
00450 /******************************************************************************
00451 Description.:
00452 Input Value.:
00453 Return Value:
00454 ******************************************************************************/
00455 int is_huffman(unsigned char *buf)
00456 {
00457     unsigned char *ptbuf;
00458     int i = 0;
00459     ptbuf = buf;
00460     while(((ptbuf[0] << 8) | ptbuf[1]) != 0xffda) {
00461         if(i++ > 2048)
00462             return 0;
00463         if(((ptbuf[0] << 8) | ptbuf[1]) == 0xffc4)
00464             return 1;
00465         ptbuf++;
00466     }
00467     return 0;
00468 }
00469 
00470 /******************************************************************************
00471 Description.:
00472 Input Value.:
00473 Return Value:
00474 ******************************************************************************/
00475 int memcpy_picture(unsigned char *out, unsigned char *buf, int size)
00476 {
00477     unsigned char *ptdeb, *ptlimit, *ptcur = buf;
00478     int sizein, pos = 0;
00479 
00480     if(!is_huffman(buf)) {
00481         ptdeb = ptcur = buf;
00482         ptlimit = buf + size;
00483         while((((ptcur[0] << 8) | ptcur[1]) != 0xffc0) && (ptcur < ptlimit))
00484             ptcur++;
00485         if(ptcur >= ptlimit)
00486             return pos;
00487         sizein = ptcur - ptdeb;
00488 
00489         memcpy(out + pos, buf, sizein); pos += sizein;
00490         memcpy(out + pos, dht_data, sizeof(dht_data)); pos += sizeof(dht_data);
00491         memcpy(out + pos, ptcur, size - sizein); pos += size - sizein;
00492     } else {
00493         memcpy(out + pos, ptcur, size); pos += size;
00494     }
00495     return pos;
00496 }
00497 
00498 int uvcGrab(struct vdIn *vd)
00499 {
00500 #define HEADERFRAME1 0xaf
00501     int ret;
00502 
00503     if(vd->streamingState == STREAMING_OFF) {
00504         if(video_enable(vd))
00505             goto err;
00506     }
00507     memset(&vd->buf, 0, sizeof(struct v4l2_buffer));
00508     vd->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00509     vd->buf.memory = V4L2_MEMORY_MMAP;
00510 
00511     ret = xioctl(vd->fd, VIDIOC_DQBUF, &vd->buf);
00512     if(ret < 0) {
00513         perror("Unable to dequeue buffer");
00514         goto err;
00515     }
00516 
00517     switch(vd->formatIn) {
00518     case V4L2_PIX_FMT_MJPEG:
00519         if(vd->buf.bytesused <= HEADERFRAME1) {
00520             /* Prevent crash
00521                                                         * on empty image */
00522             fprintf(stderr, "Ignoring empty buffer ...\n");
00523             return 0;
00524         }
00525 
00526         /* memcpy(vd->tmpbuffer, vd->mem[vd->buf.index], vd->buf.bytesused);
00527 
00528         memcpy (vd->tmpbuffer, vd->mem[vd->buf.index], HEADERFRAME1);
00529         memcpy (vd->tmpbuffer + HEADERFRAME1, dht_data, sizeof(dht_data));
00530         memcpy (vd->tmpbuffer + HEADERFRAME1 + sizeof(dht_data), vd->mem[vd->buf.index] + HEADERFRAME1, (vd->buf.bytesused - HEADERFRAME1));
00531         */
00532 
00533         memcpy(vd->tmpbuffer, vd->mem[vd->buf.index], vd->buf.bytesused);
00534 
00535         if(debug)
00536             fprintf(stderr, "bytes in used %d \n", vd->buf.bytesused);
00537         break;
00538     case V4L2_PIX_FMT_RGB565:
00539     case V4L2_PIX_FMT_YUYV:
00540     case V4L2_PIX_FMT_RGB24:
00541         if(vd->buf.bytesused > vd->framesizeIn)
00542             memcpy(vd->framebuffer, vd->mem[vd->buf.index], (size_t) vd->framesizeIn);
00543         else
00544             memcpy(vd->framebuffer, vd->mem[vd->buf.index], (size_t) vd->buf.bytesused);
00545         break;
00546 
00547     default:
00548         goto err;
00549         break;
00550     }
00551 
00552     ret = xioctl(vd->fd, VIDIOC_QBUF, &vd->buf);
00553     if(ret < 0) {
00554         perror("Unable to requeue buffer");
00555         goto err;
00556     }
00557 
00558     return 0;
00559 
00560 err:
00561     vd->signalquit = 0;
00562     return -1;
00563 }
00564 
00565 int close_v4l2(struct vdIn *vd)
00566 {
00567     if(vd->streamingState == STREAMING_ON)
00568         video_disable(vd, STREAMING_OFF);
00569     if(vd->tmpbuffer)
00570         free(vd->tmpbuffer);
00571     vd->tmpbuffer = NULL;
00572     free(vd->framebuffer);
00573     vd->framebuffer = NULL;
00574     free(vd->videodevice);
00575     free(vd->status);
00576     free(vd->pictName);
00577     vd->videodevice = NULL;
00578     vd->status = NULL;
00579     vd->pictName = NULL;
00580 
00581     return 0;
00582 }
00583 
00584 /* return >= 0 ok otherwhise -1 */
00585 static int isv4l2Control(struct vdIn *vd, int control, struct v4l2_queryctrl *queryctrl)
00586 {
00587     int err = 0;
00588 
00589     queryctrl->id = control;
00590     if((err = xioctl(vd->fd, VIDIOC_QUERYCTRL, queryctrl)) < 0) {
00591         //fprintf(stderr, "ioctl querycontrol error %d \n",errno);
00592         return -1;
00593     }
00594 
00595     if(queryctrl->flags & V4L2_CTRL_FLAG_DISABLED) {
00596         //fprintf(stderr, "control %s disabled \n", (char *) queryctrl->name);
00597         return -1;
00598     }
00599 
00600     if(queryctrl->type & V4L2_CTRL_TYPE_BOOLEAN) {
00601         return 1;
00602     }
00603 
00604     if(queryctrl->type & V4L2_CTRL_TYPE_INTEGER) {
00605         return 0;
00606     }
00607 
00608     fprintf(stderr, "contol %s unsupported  \n", (char *) queryctrl->name);
00609     return -1;
00610 }
00611 
00612 int v4l2GetControl(struct vdIn *vd, int control)
00613 {
00614     struct v4l2_queryctrl queryctrl;
00615     struct v4l2_control control_s;
00616     int err;
00617 
00618     if((err = isv4l2Control(vd, control, &queryctrl)) < 0) {
00619         return -1;
00620     }
00621 
00622     control_s.id = control;
00623     if((err = xioctl(vd->fd, VIDIOC_G_CTRL, &control_s)) < 0) {
00624         return -1;
00625     }
00626 
00627     return control_s.value;
00628 }
00629 
00630 int v4l2SetControl(struct vdIn *vd, int control_id, int value, int plugin_number, globals *pglobal)
00631 {
00632     struct v4l2_control control_s;
00633     int min, max;
00634     int ret = 0;
00635     int err;
00636     int i;
00637     int got = -1;
00638     DBG("Looking for the %d V4L2 control\n", control_id);
00639     for (i = 0; i<pglobal->in[plugin_number].parametercount; i++) {
00640         if (pglobal->in[plugin_number].in_parameters[i].ctrl.id == control_id) {
00641             got = 0;
00642             break;
00643         }
00644     }
00645 
00646     if (got == 0) { // we have found the control with the specified id
00647         DBG("V4L2 ctrl %d found\n", control_id);
00648         if (pglobal->in[plugin_number].in_parameters[i].class_id == V4L2_CTRL_CLASS_USER) {
00649             DBG("Control type: USER\n");
00650             min = pglobal->in[plugin_number].in_parameters[i].ctrl.minimum;
00651             max = pglobal->in[plugin_number].in_parameters[i].ctrl.maximum;
00652 
00653             if((value >= min) && (value <= max)) {
00654                 control_s.id = control_id;
00655                 control_s.value = value;
00656                 if((err = xioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
00657                     DBG("VIDIOC_S_CTRL failed\n");
00658                     return -1;
00659                 } else {
00660                     DBG("V4L2 ctrl %d new value: %d\n", control_id, value);
00661                     pglobal->in[plugin_number].in_parameters[i].value = value;
00662                 }
00663             } else {
00664                 DBG("Value (%d) out of range (%d .. %d)\n", value, min, max);
00665             }
00666             return 0;
00667         } else { // not user class controls
00668             DBG("Control type: EXTENDED\n");
00669             struct v4l2_ext_controls ext_ctrls = {0};
00670             struct v4l2_ext_control ext_ctrl = {0};
00671             ext_ctrl.id = pglobal->in[plugin_number].in_parameters[i].ctrl.id;
00672 
00673             switch(pglobal->in[plugin_number].in_parameters[i].ctrl.type) {
00674 #ifdef V4L2_CTRL_TYPE_STRING
00675                 case V4L2_CTRL_TYPE_STRING:
00676                     //string gets set on VIDIOC_G_EXT_CTRLS
00677                     //add the maximum size to value
00678                     ext_ctrl.size = value;
00679                     DBG("STRING extended controls are currently broken\n");
00680                     //ext_ctrl.string = control->string; // FIXMEE
00681                     break;
00682 #endif
00683                 case V4L2_CTRL_TYPE_INTEGER64:
00684                     ext_ctrl.value64 = value;
00685                     break;
00686                 default:
00687                     ext_ctrl.value = value;
00688                     break;
00689             }
00690 
00691             ext_ctrls.count = 1;
00692             ext_ctrls.controls = &ext_ctrl;
00693             ret = xioctl(vd->fd, VIDIOC_S_EXT_CTRLS, &ext_ctrls);
00694             if(ret) {
00695                 DBG("control id: 0x%08x failed to set value (error %i)\n", ext_ctrl.id, ret);
00696                 return -1;
00697             } else {
00698                 DBG("control id: %d new value: %d\n", ext_ctrl.id, ext_ctrl.value);
00699             }
00700             return 0;
00701         }
00702     } else {
00703         DBG("Invalid V4L2_set_control request for the id: %d. Control cannot be found in the list\n", control_id);
00704         return -1;
00705     }
00706 }
00707 
00708 int v4l2ResetControl(struct vdIn *vd, int control)
00709 {
00710     struct v4l2_control control_s;
00711     struct v4l2_queryctrl queryctrl;
00712     int val_def;
00713     int err;
00714 
00715     if(isv4l2Control(vd, control, &queryctrl) < 0)
00716         return -1;
00717 
00718     val_def = queryctrl.default_value;
00719     control_s.id = control;
00720     control_s.value = val_def;
00721 
00722     if((err = xioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
00723         return -1;
00724     }
00725 
00726     return 0;
00727 }
00728 
00729 void control_readed(struct vdIn *vd, struct v4l2_queryctrl *ctrl, globals *pglobal, int id)
00730 {
00731     struct v4l2_control c;
00732     memset(&c, 0, sizeof(struct v4l2_control));
00733     c.id = ctrl->id;
00734 
00735     if (pglobal->in[id].in_parameters == NULL) {
00736         pglobal->in[id].in_parameters = (control*)calloc(1, sizeof(control));
00737     } else {
00738         pglobal->in[id].in_parameters =
00739         (control*)realloc(pglobal->in[id].in_parameters,(pglobal->in[id].parametercount + 1) * sizeof(control));
00740     }
00741 
00742     if (pglobal->in[id].in_parameters == NULL) {
00743         DBG("Calloc failed\n");
00744         return;
00745     }
00746 
00747     memcpy(&pglobal->in[id].in_parameters[pglobal->in[id].parametercount].ctrl, ctrl, sizeof(struct v4l2_queryctrl));
00748     pglobal->in[id].in_parameters[pglobal->in[id].parametercount].group = IN_CMD_V4L2;
00749     pglobal->in[id].in_parameters[pglobal->in[id].parametercount].value = c.value;
00750     if(ctrl->type == V4L2_CTRL_TYPE_MENU) {
00751         pglobal->in[id].in_parameters[pglobal->in[id].parametercount].menuitems =
00752             (struct v4l2_querymenu*)malloc((ctrl->maximum + 1) * sizeof(struct v4l2_querymenu));
00753         int i;
00754         for(i = ctrl->minimum; i <= ctrl->maximum; i++) {
00755             struct v4l2_querymenu qm;
00756             memset(&qm, 0 , sizeof(struct v4l2_querymenu));
00757             qm.id = ctrl->id;
00758             qm.index = i;
00759             if(xioctl(vd->fd, VIDIOC_QUERYMENU, &qm) == 0) {
00760                 memcpy(&pglobal->in[id].in_parameters[pglobal->in[id].parametercount].menuitems[i], &qm, sizeof(struct v4l2_querymenu));
00761                 DBG("Menu item %d: %s\n", qm.index, qm.name);
00762             } else {
00763                 DBG("Unable to get menu item for %s, index=%d\n", ctrl->name, qm.index);
00764             }
00765         }
00766     } else {
00767         pglobal->in[id].in_parameters[pglobal->in[id].parametercount].menuitems = NULL;
00768     }
00769 
00770     pglobal->in[id].in_parameters[pglobal->in[id].parametercount].value = 0;
00771     pglobal->in[id].in_parameters[pglobal->in[id].parametercount].class_id = (ctrl->id & 0xFFFF0000);
00772 #ifndef V4L2_CTRL_FLAG_NEXT_CTRL
00773     pglobal->in[id].in_parameters[pglobal->in[id].parametercount].class_id = V4L2_CTRL_CLASS_USER;
00774 #endif
00775 
00776     int ret = -1;
00777     if (pglobal->in[id].in_parameters[pglobal->in[id].parametercount].class_id == V4L2_CTRL_CLASS_USER) {
00778         DBG("V4L2 parameter found: %s value %d Class: USER \n", ctrl->name, c.value);
00779         ret = xioctl(vd->fd, VIDIOC_G_CTRL, &c);
00780         if(ret == 0) {
00781             pglobal->in[id].in_parameters[pglobal->in[id].parametercount].value = c.value;
00782         } else {
00783             DBG("Unable to get the value of %s retcode: %d  %s\n", ctrl->name, ret, strerror(errno));
00784         }
00785     } else {
00786         DBG("V4L2 parameter found: %s value %d Class: EXTENDED \n", ctrl->name, c.value);
00787         struct v4l2_ext_controls ext_ctrls = {0};
00788         struct v4l2_ext_control ext_ctrl = {0};
00789         ext_ctrl.id = ctrl->id;
00790 #ifdef V4L2_CTRL_TYPE_STRING
00791         ext_ctrl.size = 0;
00792         if(ctrl.type == V4L2_CTRL_TYPE_STRING) {
00793             ext_ctrl.size = ctrl->maximum + 1;
00794             // FIXMEEEEext_ctrl.string = control->string;
00795         }
00796 #endif
00797         ext_ctrls.count = 1;
00798         ext_ctrls.controls = &ext_ctrl;
00799         ret = xioctl(vd->fd, VIDIOC_G_EXT_CTRLS, &ext_ctrls);
00800         if(ret) {
00801             switch (ext_ctrl.id) {
00802                 case V4L2_CID_PAN_RESET:
00803                     pglobal->in[id].in_parameters[pglobal->in[id].parametercount].value = 1;
00804                     DBG("Setting PAN reset value to 1\n");
00805                     break;
00806                 case V4L2_CID_TILT_RESET:
00807                     pglobal->in[id].in_parameters[pglobal->in[id].parametercount].value = 1;
00808                     DBG("Setting the Tilt reset value to 2\n");
00809                     break;
00810                 default:
00811                     DBG("control id: 0x%08x failed to get value (error %i)\n", ext_ctrl.id, ret);
00812             }
00813         } else {
00814             switch(ctrl->type)
00815             {
00816 #ifdef V4L2_CTRL_TYPE_STRING
00817                 case V4L2_CTRL_TYPE_STRING:
00818                     //string gets set on VIDIOC_G_EXT_CTRLS
00819                     //add the maximum size to value
00820                     pglobal->in[id].in_parameters[pglobal->in[id].parametercount].value = ext_ctrl.size;
00821                     break;
00822 #endif
00823                 case V4L2_CTRL_TYPE_INTEGER64:
00824                     pglobal->in[id].in_parameters[pglobal->in[id].parametercount].value = ext_ctrl.value64;
00825                     break;
00826                 default:
00827                     pglobal->in[id].in_parameters[pglobal->in[id].parametercount].value = ext_ctrl.value;
00828                     break;
00829             }
00830         }
00831     }
00832 
00833     pglobal->in[id].parametercount++;
00834 }
00835 
00836 /*  It should set the capture resolution
00837     Cheated from the openCV cap_libv4l.cpp the method is the following:
00838     Turn off the stream (video_disable)
00839     Unmap buffers
00840     Close the filedescriptor
00841     Initialize the camera again with the new resolution
00842 */
00843 int setResolution(struct vdIn *vd, int width, int height)
00844 {
00845     int ret;
00846     DBG("setResolution(%d, %d)\n", width, height);
00847 
00848     vd->streamingState = STREAMING_PAUSED;
00849     if(video_disable(vd, STREAMING_PAUSED) == 0) {  // do streamoff
00850         DBG("Unmap buffers\n");
00851         int i;
00852         for(i = 0; i < NB_BUFFER; i++)
00853             munmap(vd->mem[i], vd->buf.length);
00854 
00855         if(CLOSE_VIDEO(vd->fd) == 0) {
00856             DBG("Device closed successfully\n");
00857         }
00858 
00859         vd->width = width;
00860         vd->height = height;
00861         if(init_v4l2(vd) < 0) {
00862             fprintf(stderr, " Init v4L2 failed !! exit fatal \n");
00863             return -1;
00864         } else {
00865             DBG("reinit done\n");
00866             video_enable(vd);
00867             return 0;
00868         }
00869     } else {
00870         DBG("Unable to disable streaming\n");
00871         return -1;
00872     }
00873     return ret;
00874 }
00875 
00876 /*
00877  *
00878  * Enumarates all V4L2 controls using various methods.
00879  * It places them to the
00880  *
00881  */
00882 
00883 void enumerateControls(struct vdIn *vd, globals *pglobal, int id)
00884 {
00885     // enumerating v4l2 controls
00886     struct v4l2_queryctrl ctrl;
00887     memset(&ctrl, 0, sizeof(struct v4l2_queryctrl));
00888     pglobal->in[id].parametercount = 0;
00889     pglobal->in[id].in_parameters = malloc(0 * sizeof(control));
00890     /* Enumerate the v4l2 controls
00891      Try the extended control API first */
00892 #ifdef V4L2_CTRL_FLAG_NEXT_CTRL
00893     DBG("V4L2 API's V4L2_CTRL_FLAG_NEXT_CTRL is supported\n");
00894     ctrl.id = V4L2_CTRL_FLAG_NEXT_CTRL;
00895     if(0 == IOCTL_VIDEO(vd->fd, VIDIOC_QUERYCTRL, &ctrl)) {
00896         do {
00897             control_readed(vd, &ctrl, pglobal, id);
00898             ctrl.id |= V4L2_CTRL_FLAG_NEXT_CTRL;
00899         } while(0 == IOCTL_VIDEO(vd->fd, VIDIOC_QUERYCTRL, &ctrl));
00900         // note: use simple ioctl or v4l2_ioctl instead of the xioctl
00901     } else
00902 #endif
00903     {
00904         DBG("V4L2 API's V4L2_CTRL_FLAG_NEXT_CTRL is NOT supported\n");
00905         /* Fall back on the standard API */
00906         /* Check all the standard controls */
00907         int i;
00908         for(i = V4L2_CID_BASE; i < V4L2_CID_LASTP1; i++) {
00909             ctrl.id = i;
00910             if(IOCTL_VIDEO(vd->fd, VIDIOC_QUERYCTRL, &ctrl) == 0) {
00911                 control_readed(vd, &ctrl, pglobal, id);
00912             }
00913         }
00914 
00915         /* Check any custom controls */
00916         for(i = V4L2_CID_PRIVATE_BASE; ; i++) {
00917             ctrl.id = i;
00918             if(IOCTL_VIDEO(vd->fd, VIDIOC_QUERYCTRL, &ctrl) == 0) {
00919                 control_readed(vd, &ctrl, pglobal, id);
00920             } else {
00921                 break;
00922             }
00923         }
00924     }
00925 
00926     memset(&pglobal->in[id].jpegcomp, 0, sizeof(struct v4l2_jpegcompression));
00927     if(xioctl(vd->fd, VIDIOC_G_JPEGCOMP, &pglobal->in[id].jpegcomp) != EINVAL) {
00928         DBG("JPEG compression details:\n");
00929         DBG("Quality: %d\n", pglobal->in[id].jpegcomp.quality);
00930         DBG("APPn: %d\n", pglobal->in[id].jpegcomp.APPn);
00931         DBG("APP length: %d\n", pglobal->in[id].jpegcomp.APP_len);
00932         DBG("APP data: %s\n", pglobal->in[id].jpegcomp.APP_data);
00933         DBG("COM length: %d\n", pglobal->in[id].jpegcomp.COM_len);
00934         DBG("COM data: %s\n", pglobal->in[id].jpegcomp.COM_data);
00935         struct v4l2_queryctrl ctrl_jpeg;
00936         ctrl_jpeg.id = 1;
00937         sprintf((char*)&ctrl_jpeg.name, "JPEG quality");
00938         ctrl_jpeg.minimum = 0;
00939         ctrl_jpeg.maximum = 100;
00940         ctrl_jpeg.step = 1;
00941         ctrl_jpeg.default_value = 50;
00942         ctrl_jpeg.flags = 0;
00943         ctrl_jpeg.type = V4L2_CTRL_TYPE_INTEGER;
00944         if (pglobal->in[id].in_parameters == NULL) {
00945             pglobal->in[id].in_parameters = (control*)calloc(1, sizeof(control));
00946         } else {
00947             pglobal->in[id].in_parameters = (control*)realloc(pglobal->in[id].in_parameters,(pglobal->in[id].parametercount + 1) * sizeof(control));
00948         }
00949 
00950         if (pglobal->in[id].in_parameters == NULL) {
00951             DBG("Calloc/realloc failed\n");
00952             return;
00953         }
00954 
00955         memcpy(&pglobal->in[id].in_parameters[pglobal->in[id].parametercount].ctrl, &ctrl_jpeg, sizeof(struct v4l2_queryctrl));
00956         pglobal->in[id].in_parameters[pglobal->in[id].parametercount].group = IN_CMD_JPEG_QUALITY;
00957         pglobal->in[id].in_parameters[pglobal->in[id].parametercount].value = pglobal->in[id].jpegcomp.quality;
00958         pglobal->in[id].parametercount++;
00959     } else {
00960         DBG("Modifying the setting of the JPEG compression is not supported\n");
00961         pglobal->in[id].jpegcomp.quality = -1;
00962     }
00963 }


corobot_pantilt
Author(s):
autogenerated on Wed Aug 26 2015 11:10:04