usb_cam: usb_cam: usb_cam.cpp Source File

00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2009, Robert Bosch LLC.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Robert Bosch nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *********************************************************************/
00036 #define __STDC_CONSTANT_MACROS
00037 #include <stdio.h>
00038 #include <stdlib.h>
00039 #include <string.h>
00040 #include <assert.h>
00041 #include <fcntl.h>              /* low-level i/o */
00042 #include <unistd.h>
00043 #include <errno.h>
00044 #include <malloc.h>
00045 #include <sys/stat.h>
00046 #include <sys/types.h>
00047 #include <sys/time.h>
00048 #include <sys/mman.h>
00049 #include <sys/ioctl.h>
00050 
00051 #include <asm/types.h>          /* for videodev2.h */
00052 
00053 extern "C" {
00054 #include <linux/videodev2.h>
00055 #include <libavcodec/avcodec.h>
00056 #include <libswscale/swscale.h>
00057 }
00058 
00059 #include <usb_cam/usb_cam.h>
00060 
00061 #define CLEAR(x) memset (&(x), 0, sizeof (x))
00062 
00063 struct buffer {
00064   void * start;
00065   size_t length;
00066 };
00067 
00068 static char *camera_dev;
00069 static unsigned int pixelformat;
00070 static usb_cam_io_method io = IO_METHOD_MMAP;
00071 static int fd = -1;
00072 struct buffer * buffers = NULL;
00073 static unsigned int n_buffers = 0;
00074 static AVFrame *avframe_camera = NULL;
00075 static AVFrame *avframe_rgb = NULL;
00076 static AVCodec *avcodec = NULL;
00077 static AVCodecContext *avcodec_context = NULL;
00078 static int avframe_camera_size = 0;
00079 static int avframe_rgb_size = 0;
00080 
00081 struct SwsContext *video_sws = NULL;
00082 
00083 static void errno_exit(const char * s)
00084 {
00085   fprintf(stderr, "%s error %d, %s\n", s, errno, strerror(errno));
00086 
00087   exit(EXIT_FAILURE);
00088 }
00089 
00090 static int xioctl(int fd, int request, void * arg)
00091 {
00092   int r;
00093 
00094   do
00095     r = ioctl(fd, request, arg); while (-1==r&&EINTR==errno);
00096 
00097   return r;
00098 }
00099 
00100 const unsigned char uchar_clipping_table[] = {
00101   0,0,0,0,0,0,0,0, // -128 - -121
00102   0,0,0,0,0,0,0,0, // -120 - -113
00103   0,0,0,0,0,0,0,0, // -112 - -105
00104   0,0,0,0,0,0,0,0, // -104 -  -97
00105   0,0,0,0,0,0,0,0, //  -96 -  -89
00106   0,0,0,0,0,0,0,0, //  -88 -  -81
00107   0,0,0,0,0,0,0,0, //  -80 -  -73
00108   0,0,0,0,0,0,0,0, //  -72 -  -65
00109   0,0,0,0,0,0,0,0, //  -64 -  -57
00110   0,0,0,0,0,0,0,0, //  -56 -  -49
00111   0,0,0,0,0,0,0,0, //  -48 -  -41
00112   0,0,0,0,0,0,0,0, //  -40 -  -33
00113   0,0,0,0,0,0,0,0, //  -32 -  -25
00114   0,0,0,0,0,0,0,0, //  -24 -  -17
00115   0,0,0,0,0,0,0,0, //  -16 -   -9
00116   0,0,0,0,0,0,0,0, //   -8 -   -1
00117   0,1,2,3,4,5,6,7,
00118   8,9,10,11,12,13,14,15,
00119   16,17,18,19,20,21,22,23,
00120   24,25,26,27,28,29,30,31,
00121   32,33,34,35,36,37,38,39,
00122   40,41,42,43,44,45,46,47,
00123   48,49,50,51,52,53,54,55,
00124   56,57,58,59,60,61,62,63,
00125   64,65,66,67,68,69,70,71,
00126   72,73,74,75,76,77,78,79,
00127   80,81,82,83,84,85,86,87,
00128   88,89,90,91,92,93,94,95,
00129   96,97,98,99,100,101,102,103,
00130   104,105,106,107,108,109,110,111,
00131   112,113,114,115,116,117,118,119,
00132   120,121,122,123,124,125,126,127,
00133   128,129,130,131,132,133,134,135,
00134   136,137,138,139,140,141,142,143,
00135   144,145,146,147,148,149,150,151,
00136   152,153,154,155,156,157,158,159,
00137   160,161,162,163,164,165,166,167,
00138   168,169,170,171,172,173,174,175,
00139   176,177,178,179,180,181,182,183,
00140   184,185,186,187,188,189,190,191,
00141   192,193,194,195,196,197,198,199,
00142   200,201,202,203,204,205,206,207,
00143   208,209,210,211,212,213,214,215,
00144   216,217,218,219,220,221,222,223,
00145   224,225,226,227,228,229,230,231,
00146   232,233,234,235,236,237,238,239,
00147   240,241,242,243,244,245,246,247,
00148   248,249,250,251,252,253,254,255,
00149   255,255,255,255,255,255,255,255, // 256-263
00150   255,255,255,255,255,255,255,255, // 264-271
00151   255,255,255,255,255,255,255,255, // 272-279
00152   255,255,255,255,255,255,255,255, // 280-287
00153   255,255,255,255,255,255,255,255, // 288-295
00154   255,255,255,255,255,255,255,255, // 296-303
00155   255,255,255,255,255,255,255,255, // 304-311
00156   255,255,255,255,255,255,255,255, // 312-319
00157   255,255,255,255,255,255,255,255, // 320-327
00158   255,255,255,255,255,255,255,255, // 328-335
00159   255,255,255,255,255,255,255,255, // 336-343
00160   255,255,255,255,255,255,255,255, // 344-351
00161   255,255,255,255,255,255,255,255, // 352-359
00162   255,255,255,255,255,255,255,255, // 360-367
00163   255,255,255,255,255,255,255,255, // 368-375
00164   255,255,255,255,255,255,255,255, // 376-383
00165 };
00166 const int clipping_table_offset = 128;
00167 
00171 static unsigned char
00172 CLIPVALUE(int val)
00173 {
00174   // Old method (if)
00175 /*   val = val < 0 ? 0 : val; */
00176 /*   return val > 255 ? 255 : val; */
00177 
00178   // New method (array)
00179   return uchar_clipping_table[val+clipping_table_offset];
00180 }
00181 
00203 static void
00204 YUV2RGB(const unsigned char y,
00205         const unsigned char u,
00206         const unsigned char v,
00207         unsigned char* r,
00208         unsigned char* g,
00209         unsigned char* b)
00210 {
00211   const int y2=(int)y;
00212   const int u2=(int)u-128;
00213   const int v2=(int)v-128;
00214   //std::cerr << "YUV=("<<y2<<","<<u2<<","<<v2<<")"<<std::endl;
00215 
00216 
00217   // This is the normal YUV conversion, but
00218   // appears to be incorrect for the firewire cameras
00219   //   int r2 = y2 + ( (v2*91947) >> 16);
00220   //   int g2 = y2 - ( ((u2*22544) + (v2*46793)) >> 16 );
00221   //   int b2 = y2 + ( (u2*115999) >> 16);
00222   // This is an adjusted version (UV spread out a bit)
00223   int r2 = y2 + ( (v2*37221) >> 15);
00224   int g2 = y2 - ( ((u2*12975) + (v2*18949)) >> 15 );
00225   int b2 = y2 + ( (u2*66883) >> 15);
00226   //std::cerr << "   RGB=("<<r2<<","<<g2<<","<<b2<<")"<<std::endl;
00227 
00228 
00229   // Cap the values.
00230   *r=CLIPVALUE(r2);
00231   *g=CLIPVALUE(g2);
00232   *b=CLIPVALUE(b2);
00233 }
00234 
00235 void
00236 uyvy2rgb (char *YUV, char *RGB, int NumPixels) {
00237   int i, j;
00238   unsigned char y0, y1, u, v;
00239   unsigned char r, g, b;
00240   for (i = 0, j = 0; i < (NumPixels << 1); i += 4, j += 6)
00241     {
00242       u = (unsigned char) YUV[i + 0];
00243       y0 = (unsigned char) YUV[i + 1];
00244       v = (unsigned char) YUV[i + 2];
00245       y1 = (unsigned char) YUV[i + 3];
00246       YUV2RGB (y0, u, v, &r, &g, &b);
00247       RGB[j + 0] = r;
00248       RGB[j + 1] = g;
00249       RGB[j + 2] = b;
00250       YUV2RGB (y1, u, v, &r, &g, &b);
00251       RGB[j + 3] = r;
00252       RGB[j + 4] = g;
00253       RGB[j + 5] = b;
00254     }
00255 }
00256 
00257 static void
00258 yuyv2rgb(char *YUV, char *RGB, int NumPixels) {
00259   int i, j;
00260   unsigned char y0, y1, u, v;
00261   unsigned char r, g, b;
00262 
00263   for (i = 0, j = 0; i < (NumPixels << 1); i += 4, j += 6)
00264     {
00265       y0 = (unsigned char) YUV[i + 0];
00266       u = (unsigned char) YUV[i + 1];
00267       y1 = (unsigned char) YUV[i + 2];
00268       v = (unsigned char) YUV[i + 3];
00269       YUV2RGB (y0, u, v, &r, &g, &b);
00270       RGB[j + 0] = r;
00271       RGB[j + 1] = g;
00272       RGB[j + 2] = b;
00273       YUV2RGB (y1, u, v, &r, &g, &b);
00274       RGB[j + 3] = r;
00275       RGB[j + 4] = g;
00276       RGB[j + 5] = b;
00277     }
00278 }
00279 
00280 static int init_mjpeg_decoder(int image_width, int image_height)
00281 {
00282   avcodec_init();
00283   avcodec_register_all();
00284 
00285   avcodec = avcodec_find_decoder(CODEC_ID_MJPEG);
00286   if (!avcodec)
00287   {
00288     fprintf(stderr,"Could not find MJPEG decoder\n");
00289     return 0;
00290   }
00291 
00292   avcodec_context = avcodec_alloc_context();
00293   avframe_camera = avcodec_alloc_frame();
00294   avframe_rgb = avcodec_alloc_frame();
00295 
00296   avpicture_alloc((AVPicture *)avframe_rgb, PIX_FMT_RGB24, image_width, image_height);
00297 
00298   avcodec_context->codec_id = CODEC_ID_MJPEG;
00299   avcodec_context->width = image_width;
00300   avcodec_context->height = image_height;
00301 
00302 #if LIBAVCODEC_VERSION_MAJOR > 52
00303   avcodec_context->pix_fmt = PIX_FMT_YUV422P;
00304   avcodec_context->codec_type = AVMEDIA_TYPE_VIDEO;
00305 #endif
00306 
00307   avframe_camera_size = avpicture_get_size(PIX_FMT_YUV422P, image_width, image_height);
00308   avframe_rgb_size = avpicture_get_size(PIX_FMT_RGB24, image_width, image_height);
00309 
00310   /* open it */
00311   if (avcodec_open(avcodec_context, avcodec) < 0)
00312   {
00313     fprintf(stderr,"Could not open MJPEG Decoder\n");
00314     return 0;
00315   }
00316   return 1;
00317 }
00318 
00319 static void
00320 mjpeg2rgb(char *MJPEG, int len, char *RGB, int NumPixels)
00321 {
00322   int got_picture;
00323 
00324   memset(RGB, 0, avframe_rgb_size);
00325 
00326 #if LIBAVCODEC_VERSION_MAJOR > 52
00327   int decoded_len;
00328   AVPacket avpkt;
00329   av_init_packet(&avpkt);
00330   
00331   avpkt.size = len;
00332   avpkt.data = (unsigned char*)MJPEG;
00333   decoded_len = avcodec_decode_video2(avcodec_context, avframe_camera, &got_picture, &avpkt);
00334 
00335   if (decoded_len < 0) {
00336       fprintf(stderr, "Error while decoding frame.\n");
00337       return;
00338   }
00339 #else
00340   avcodec_decode_video(avcodec_context, avframe_camera, &got_picture, (uint8_t *) MJPEG, len);
00341 #endif
00342 
00343   if (!got_picture) {
00344     fprintf(stderr,"Webcam: expected picture but didn't get it...\n");
00345     return;
00346   }
00347 
00348   int xsize = avcodec_context->width;
00349   int ysize = avcodec_context->height;
00350   int pic_size = avpicture_get_size(avcodec_context->pix_fmt, xsize, ysize);
00351   if (pic_size != avframe_camera_size) {
00352     fprintf(stderr,"outbuf size mismatch.  pic_size: %d bufsize: %d\n",pic_size,avframe_camera_size);
00353     return;
00354   }
00355 
00356   video_sws = sws_getContext( xsize, ysize, avcodec_context->pix_fmt, xsize, ysize, PIX_FMT_RGB24, SWS_BILINEAR, NULL, NULL, NULL);
00357   sws_scale(video_sws, avframe_camera->data, avframe_camera->linesize, 0, ysize, avframe_rgb->data, avframe_rgb->linesize );
00358   sws_freeContext(video_sws);  
00359 
00360   int size = avpicture_layout((AVPicture *) avframe_rgb, PIX_FMT_RGB24, xsize, ysize, (uint8_t *)RGB, avframe_rgb_size);
00361   if (size != avframe_rgb_size) {
00362     fprintf(stderr,"webcam: avpicture_layout error: %d\n",size);
00363     return;
00364   }
00365 }
00366 
00367 static void process_image(const void * src, int len, usb_cam_camera_image_t *dest)
00368 {
00369   if(pixelformat==V4L2_PIX_FMT_YUYV)
00370     yuyv2rgb((char*)src, dest->image, dest->width*dest->height);
00371   else if(pixelformat==V4L2_PIX_FMT_UYVY)
00372     uyvy2rgb((char*)src, dest->image, dest->width*dest->height);
00373   else if(pixelformat==V4L2_PIX_FMT_MJPEG)
00374     mjpeg2rgb((char*)src, len, dest->image, dest->width*dest->height);
00375 }
00376 
00377 static int read_frame(usb_cam_camera_image_t *image)
00378 {
00379   struct v4l2_buffer buf;
00380   unsigned int i;
00381   int len;
00382 
00383   switch (io) {
00384   case IO_METHOD_READ:
00385     len = read(fd, buffers[0].start, buffers[0].length);
00386     if (len==-1) {
00387       switch (errno) {
00388       case EAGAIN:
00389         return 0;
00390 
00391       case EIO:
00392         /* Could ignore EIO, see spec. */
00393 
00394         /* fall through */
00395 
00396       default:
00397         errno_exit("read");
00398       }
00399     }
00400 
00401     process_image(buffers[0].start, len, image);
00402 
00403     break;
00404 
00405   case IO_METHOD_MMAP:
00406     CLEAR (buf);
00407 
00408     buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00409     buf.memory = V4L2_MEMORY_MMAP;
00410 
00411     if (-1==xioctl(fd, VIDIOC_DQBUF, &buf)) {
00412       switch (errno) {
00413       case EAGAIN:
00414         return 0;
00415 
00416       case EIO:
00417         /* Could ignore EIO, see spec. */
00418 
00419         /* fall through */
00420 
00421       default:
00422         errno_exit("VIDIOC_DQBUF");
00423       }
00424     }
00425 
00426     assert (buf.index < n_buffers);
00427     len = buf.bytesused;
00428     process_image(buffers[buf.index].start, len, image);
00429 
00430     if (-1==xioctl(fd, VIDIOC_QBUF, &buf))
00431       errno_exit("VIDIOC_QBUF");
00432 
00433     break;
00434 
00435   case IO_METHOD_USERPTR:
00436     CLEAR (buf);
00437 
00438     buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00439     buf.memory = V4L2_MEMORY_USERPTR;
00440 
00441     if (-1==xioctl(fd, VIDIOC_DQBUF, &buf)) {
00442       switch (errno) {
00443       case EAGAIN:
00444         return 0;
00445 
00446       case EIO:
00447         /* Could ignore EIO, see spec. */
00448 
00449         /* fall through */
00450 
00451       default:
00452         errno_exit("VIDIOC_DQBUF");
00453       }
00454     }
00455 
00456     for(i = 0; i<n_buffers; ++i)
00457       if (buf.m.userptr==(unsigned long) buffers[i].start&&buf.length==buffers[i].length)
00458         break;
00459 
00460     assert (i < n_buffers);
00461     len = buf.bytesused;
00462     process_image((void *) buf.m.userptr, len, image);
00463 
00464     if (-1==xioctl(fd, VIDIOC_QBUF, &buf))
00465       errno_exit("VIDIOC_QBUF");
00466 
00467     break;
00468   }
00469 
00470   return 1;
00471 }
00472 
00473 static void stop_capturing(void)
00474 {
00475   enum v4l2_buf_type type;
00476 
00477   switch (io) {
00478   case IO_METHOD_READ:
00479     /* Nothing to do. */
00480     break;
00481 
00482   case IO_METHOD_MMAP:
00483   case IO_METHOD_USERPTR:
00484     type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00485 
00486     if (-1==xioctl(fd, VIDIOC_STREAMOFF, &type))
00487       errno_exit("VIDIOC_STREAMOFF");
00488 
00489     break;
00490   }
00491 }
00492 
00493 static void start_capturing(void)
00494 {
00495   unsigned int i;
00496   enum v4l2_buf_type type;
00497 
00498   switch (io) {
00499   case IO_METHOD_READ:
00500     /* Nothing to do. */
00501     break;
00502 
00503   case IO_METHOD_MMAP:
00504     for(i = 0; i<n_buffers; ++i) {
00505       struct v4l2_buffer buf;
00506 
00507       CLEAR (buf);
00508 
00509       buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00510       buf.memory = V4L2_MEMORY_MMAP;
00511       buf.index = i;
00512 
00513       if (-1==xioctl(fd, VIDIOC_QBUF, &buf))
00514         errno_exit("VIDIOC_QBUF");
00515     }
00516 
00517     type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00518 
00519     if (-1==xioctl(fd, VIDIOC_STREAMON, &type))
00520       errno_exit("VIDIOC_STREAMON");
00521 
00522     break;
00523 
00524   case IO_METHOD_USERPTR:
00525     for(i = 0; i<n_buffers; ++i) {
00526       struct v4l2_buffer buf;
00527 
00528       CLEAR (buf);
00529 
00530       buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00531       buf.memory = V4L2_MEMORY_USERPTR;
00532       buf.index = i;
00533       buf.m.userptr = (unsigned long) buffers[i].start;
00534       buf.length = buffers[i].length;
00535 
00536       if (-1==xioctl(fd, VIDIOC_QBUF, &buf))
00537         errno_exit("VIDIOC_QBUF");
00538     }
00539 
00540     type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00541 
00542     if (-1==xioctl(fd, VIDIOC_STREAMON, &type))
00543       errno_exit("VIDIOC_STREAMON");
00544 
00545     break;
00546   }
00547 }
00548 
00549 static void uninit_device(void)
00550 {
00551   unsigned int i;
00552 
00553   switch (io) {
00554   case IO_METHOD_READ:
00555     free(buffers[0].start);
00556     break;
00557 
00558   case IO_METHOD_MMAP:
00559     for(i = 0; i<n_buffers; ++i)
00560       if (-1==munmap(buffers[i].start, buffers[i].length))
00561         errno_exit("munmap");
00562     break;
00563 
00564   case IO_METHOD_USERPTR:
00565     for(i = 0; i<n_buffers; ++i)
00566       free(buffers[i].start);
00567     break;
00568   }
00569 
00570   free(buffers);
00571 }
00572 
00573 static void init_read(unsigned int buffer_size)
00574 {
00575   buffers = (buffer*)calloc(1, sizeof(*buffers));
00576 
00577   if (!buffers) {
00578     fprintf(stderr, "Out of memory\n");
00579     exit(EXIT_FAILURE);
00580   }
00581 
00582   buffers[0].length = buffer_size;
00583   buffers[0].start = malloc(buffer_size);
00584 
00585   if (!buffers[0].start) {
00586     fprintf(stderr, "Out of memory\n");
00587     exit(EXIT_FAILURE);
00588   }
00589 }
00590 
00591 static void init_mmap(void)
00592 {
00593   struct v4l2_requestbuffers req;
00594 
00595   CLEAR (req);
00596 
00597   req.count = 4;
00598   req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00599   req.memory = V4L2_MEMORY_MMAP;
00600 
00601   if (-1==xioctl(fd, VIDIOC_REQBUFS, &req)) {
00602     if (EINVAL==errno) {
00603       fprintf(stderr, "%s does not support memory mapping\n", camera_dev);
00604       exit(EXIT_FAILURE);
00605     } else {
00606       errno_exit("VIDIOC_REQBUFS");
00607     }
00608   }
00609 
00610   if (req.count<2) {
00611     fprintf(stderr, "Insufficient buffer memory on %s\n", camera_dev);
00612     exit(EXIT_FAILURE);
00613   }
00614 
00615   buffers = (buffer*)calloc(req.count, sizeof(*buffers));
00616 
00617   if (!buffers) {
00618     fprintf(stderr, "Out of memory\n");
00619     exit(EXIT_FAILURE);
00620   }
00621 
00622   for(n_buffers = 0; n_buffers<req.count; ++n_buffers) {
00623     struct v4l2_buffer buf;
00624 
00625     CLEAR (buf);
00626 
00627     buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00628     buf.memory = V4L2_MEMORY_MMAP;
00629     buf.index = n_buffers;
00630 
00631     if (-1==xioctl(fd, VIDIOC_QUERYBUF, &buf))
00632       errno_exit("VIDIOC_QUERYBUF");
00633 
00634     buffers[n_buffers].length = buf.length;
00635     buffers[n_buffers].start = mmap(NULL /* start anywhere */, buf.length, PROT_READ|PROT_WRITE /* required */, MAP_SHARED /* recommended */, fd, buf.m.offset);
00636 
00637     if (MAP_FAILED==buffers[n_buffers].start)
00638       errno_exit("mmap");
00639   }
00640 }
00641 
00642 static void init_userp(unsigned int buffer_size)
00643 {
00644   struct v4l2_requestbuffers req;
00645   unsigned int page_size;
00646 
00647   page_size = getpagesize();
00648   buffer_size = (buffer_size+page_size-1)&~(page_size-1);
00649 
00650   CLEAR (req);
00651 
00652   req.count = 4;
00653   req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00654   req.memory = V4L2_MEMORY_USERPTR;
00655 
00656   if (-1==xioctl(fd, VIDIOC_REQBUFS, &req)) {
00657     if (EINVAL==errno) {
00658       fprintf(stderr, "%s does not support "
00659         "user pointer i/o\n", camera_dev);
00660       exit(EXIT_FAILURE);
00661     } else {
00662       errno_exit("VIDIOC_REQBUFS");
00663     }
00664   }
00665 
00666   buffers = (buffer*)calloc(4, sizeof(*buffers));
00667 
00668   if (!buffers) {
00669     fprintf(stderr, "Out of memory\n");
00670     exit(EXIT_FAILURE);
00671   }
00672 
00673   for(n_buffers = 0; n_buffers<4; ++n_buffers) {
00674     buffers[n_buffers].length = buffer_size;
00675     buffers[n_buffers].start = memalign(/* boundary */page_size, buffer_size);
00676 
00677     if (!buffers[n_buffers].start) {
00678       fprintf(stderr, "Out of memory\n");
00679       exit(EXIT_FAILURE);
00680     }
00681   }
00682 }
00683 
00684 static void init_device(int image_width, int image_height)
00685 {
00686   struct v4l2_capability cap;
00687   struct v4l2_cropcap cropcap;
00688   struct v4l2_crop crop;
00689   struct v4l2_format fmt;
00690   unsigned int min;
00691 
00692   if (-1==xioctl(fd, VIDIOC_QUERYCAP, &cap)) {
00693     if (EINVAL==errno) {
00694       fprintf(stderr, "%s is no V4L2 device\n", camera_dev);
00695       exit(EXIT_FAILURE);
00696     } else {
00697       errno_exit("VIDIOC_QUERYCAP");
00698     }
00699   }
00700 
00701   if (!(cap.capabilities&V4L2_CAP_VIDEO_CAPTURE)) {
00702     fprintf(stderr, "%s is no video capture device\n", camera_dev);
00703     exit(EXIT_FAILURE);
00704   }
00705 
00706   switch (io) {
00707   case IO_METHOD_READ:
00708     if (!(cap.capabilities&V4L2_CAP_READWRITE)) {
00709       fprintf(stderr, "%s does not support read i/o\n", camera_dev);
00710       exit(EXIT_FAILURE);
00711     }
00712 
00713     break;
00714 
00715   case IO_METHOD_MMAP:
00716   case IO_METHOD_USERPTR:
00717     if (!(cap.capabilities&V4L2_CAP_STREAMING)) {
00718       fprintf(stderr, "%s does not support streaming i/o\n", camera_dev);
00719       exit(EXIT_FAILURE);
00720     }
00721 
00722     break;
00723   }
00724 
00725   /* Select video input, video standard and tune here. */
00726 
00727   CLEAR (cropcap);
00728 
00729   cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00730 
00731   if (0==xioctl(fd, VIDIOC_CROPCAP, &cropcap)) {
00732     crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00733     crop.c = cropcap.defrect; /* reset to default */
00734 
00735     if (-1==xioctl(fd, VIDIOC_S_CROP, &crop)) {
00736       switch (errno) {
00737       case EINVAL:
00738         /* Cropping not supported. */
00739         break;
00740       default:
00741         /* Errors ignored. */
00742         break;
00743       }
00744     }
00745   } else {
00746     /* Errors ignored. */
00747   }
00748 
00749   CLEAR (fmt);
00750 
00751 //  fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00752 //  fmt.fmt.pix.width = 640;
00753 //  fmt.fmt.pix.height = 480;
00754 //  fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
00755 //  fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
00756 
00757   fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00758   fmt.fmt.pix.width = image_width;
00759   fmt.fmt.pix.height = image_height;
00760   fmt.fmt.pix.pixelformat = pixelformat;
00761   fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
00762 
00763 
00764   if (-1==xioctl(fd, VIDIOC_S_FMT, &fmt))
00765     errno_exit("VIDIOC_S_FMT");
00766 
00767   /* Note VIDIOC_S_FMT may change width and height. */
00768 
00769   /* Buggy driver paranoia. */
00770   min = fmt.fmt.pix.width*2;
00771   if (fmt.fmt.pix.bytesperline<min)
00772     fmt.fmt.pix.bytesperline = min;
00773   min = fmt.fmt.pix.bytesperline*fmt.fmt.pix.height;
00774   if (fmt.fmt.pix.sizeimage<min)
00775     fmt.fmt.pix.sizeimage = min;
00776 
00777   image_width = fmt.fmt.pix.width;
00778   image_height = fmt.fmt.pix.height;
00779 
00780   switch (io) {
00781   case IO_METHOD_READ:
00782     init_read(fmt.fmt.pix.sizeimage);
00783     break;
00784 
00785   case IO_METHOD_MMAP:
00786     init_mmap();
00787     break;
00788 
00789   case IO_METHOD_USERPTR:
00790     init_userp(fmt.fmt.pix.sizeimage);
00791     break;
00792   }
00793 }
00794 
00795 static void close_device(void)
00796 {
00797   if (-1==close(fd))
00798     errno_exit("close");
00799 
00800   fd = -1;
00801 }
00802 
00803 static void open_device(void)
00804 {
00805   struct stat st;
00806 
00807   if (-1==stat(camera_dev, &st)) {
00808     fprintf(stderr, "Cannot identify '%s': %d, %s\n", camera_dev, errno, strerror(errno));
00809     exit(EXIT_FAILURE);
00810   }
00811 
00812   if (!S_ISCHR (st.st_mode)) {
00813     fprintf(stderr, "%s is no device\n", camera_dev);
00814     exit(EXIT_FAILURE);
00815   }
00816 
00817   fd = open(camera_dev, O_RDWR /* required */|O_NONBLOCK, 0);
00818 
00819   if (-1==fd) {
00820     fprintf(stderr, "Cannot open '%s': %d, %s\n", camera_dev, errno, strerror(errno));
00821     exit(EXIT_FAILURE);
00822   }
00823 }
00824 
00825 usb_cam_camera_image_t *usb_cam_camera_start(const char* dev, usb_cam_io_method io_method,
00826     usb_cam_pixel_format pixel_format, int image_width, int image_height)
00827 {
00828   camera_dev = (char*)calloc(1,strlen(dev)+1);
00829   strcpy(camera_dev,dev);
00830 
00831   usb_cam_camera_image_t *image;
00832   io = io_method;
00833   if(pixel_format == PIXEL_FORMAT_YUYV)
00834     pixelformat = V4L2_PIX_FMT_YUYV;
00835   else if(pixel_format == PIXEL_FORMAT_UYVY)
00836     pixelformat = V4L2_PIX_FMT_UYVY;
00837   else if(pixel_format == PIXEL_FORMAT_MJPEG) {
00838     pixelformat = V4L2_PIX_FMT_MJPEG;
00839     init_mjpeg_decoder(image_width, image_height);
00840   }
00841   else {
00842     fprintf(stderr, "Unknown pixelformat.\n");
00843     exit(EXIT_FAILURE);
00844   }
00845 
00846   open_device();
00847   init_device(image_width, image_height);
00848   start_capturing();
00849 
00850   image = (usb_cam_camera_image_t *) calloc(1, sizeof(usb_cam_camera_image_t));
00851 
00852   image->width = image_width;
00853   image->height = image_height;
00854   image->bytes_per_pixel = 24;
00855 
00856   image->image_size = image->width*image->height*image->bytes_per_pixel;
00857   image->is_new = 0;
00858   image->image = (char *) calloc(image->image_size, sizeof(char));
00859   memset(image->image, 0, image->image_size*sizeof(char));
00860 
00861   return image;
00862 }
00863 
00864 void usb_cam_camera_shutdown(void)
00865 {
00866   stop_capturing();
00867   uninit_device();
00868   close_device();
00869 
00870   if (avcodec_context) {
00871     avcodec_close(avcodec_context);
00872     av_free(avcodec_context);
00873     avcodec_context = NULL;
00874   }
00875   if (avframe_camera)
00876     av_free(avframe_camera);
00877   avframe_camera = NULL;
00878   if (avframe_rgb)
00879     av_free(avframe_rgb);
00880   avframe_rgb = NULL;
00881 }
00882 
00883 void usb_cam_camera_grab_image(usb_cam_camera_image_t *image)
00884 {
00885   fd_set fds;
00886   struct timeval tv;
00887   int r;
00888 
00889   FD_ZERO (&fds);
00890   FD_SET (fd, &fds);
00891 
00892   /* Timeout. */
00893   tv.tv_sec = 5;
00894   tv.tv_usec = 0;
00895 
00896   r = select(fd+1, &fds, NULL, NULL, &tv);
00897 
00898   if (-1==r) {
00899     if (EINTR==errno)
00900       return;
00901 
00902     errno_exit("select");
00903   }
00904 
00905   if (0==r) {
00906     fprintf(stderr, "select timeout\n");
00907     exit(EXIT_FAILURE);
00908   }
00909 
00910   read_frame(image);
00911   image->is_new = 1;
00912 }
00913 
00914 // enables/disables auto focus
00915 void usb_cam_camera_set_auto_focus(int value)
00916 {
00917   struct v4l2_queryctrl queryctrl;
00918   struct v4l2_ext_control control;
00919 
00920   memset(&queryctrl, 0, sizeof(queryctrl));
00921   queryctrl.id = V4L2_CID_FOCUS_AUTO;
00922 
00923   if (-1 == xioctl(fd, VIDIOC_QUERYCTRL, &queryctrl)) {
00924     if (errno != EINVAL) {
00925       perror("VIDIOC_QUERYCTRL");
00926       return;
00927     } else {
00928       printf("V4L2_CID_FOCUS_AUTO is not supported\n");
00929       return;
00930     }
00931   } else if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) {
00932     printf("V4L2_CID_FOCUS_AUTO is not supported\n");
00933     return;
00934   } else {
00935     memset(&control, 0, sizeof(control));
00936     control.id = V4L2_CID_FOCUS_AUTO;
00937     control.value = value;
00938 
00939     if (-1 == xioctl(fd, VIDIOC_S_CTRL, &control)) {
00940       perror("VIDIOC_S_CTRL");
00941       return;
00942     }
00943   }
00944 }
00945 
00946 
00947