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   avframe_camera_size = avpicture_get_size(PIX_FMT_YUV422P, image_width, image_height);
00303   avframe_rgb_size = avpicture_get_size(PIX_FMT_RGB24, image_width, image_height);
00304 
00305   /* open it */
00306   if (avcodec_open(avcodec_context, avcodec) < 0)
00307   {
00308     fprintf(stderr,"Could not open MJPEG Decoder\n");
00309     return 0;
00310   }
00311   return 1;
00312 }
00313 
00314 static void
00315 mjpeg2rgb(char *MJPEG, int len, char *RGB, int NumPixels)
00316 {
00317   int got_picture;
00318 
00319   memset(RGB, 0, avframe_rgb_size);
00320 
00321   avcodec_decode_video(avcodec_context, avframe_camera, &got_picture, (uint8_t *) MJPEG, len);
00322 
00323   if (!got_picture) {
00324     fprintf(stderr,"Webcam: expected picture but didn't get it...\n");
00325     return;
00326   }
00327 
00328   int xsize = avcodec_context->width;
00329   int ysize = avcodec_context->height;
00330   int pic_size = avpicture_get_size(avcodec_context->pix_fmt, xsize, ysize);
00331   if (pic_size != avframe_camera_size) {
00332     fprintf(stderr,"outbuf size mismatch.  pic_size: %d bufsize: %d\n",pic_size,avframe_camera_size);
00333     return;
00334   }
00335 
00336   video_sws = sws_getContext( xsize, ysize, avcodec_context->pix_fmt, xsize, ysize, PIX_FMT_RGB24, SWS_BILINEAR, NULL, NULL, NULL);
00337   sws_scale(video_sws, avframe_camera->data, avframe_camera->linesize, 0, ysize, avframe_rgb->data, avframe_rgb->linesize );
00338   sws_freeContext(video_sws);  
00339 
00340   int size = avpicture_layout((AVPicture *) avframe_rgb, PIX_FMT_RGB24, xsize, ysize, (uint8_t *)RGB, avframe_rgb_size);
00341   if (size != avframe_rgb_size) {
00342     fprintf(stderr,"webcam: avpicture_layout error: %d\n",size);
00343     return;
00344   }
00345 }
00346 
00347 static void process_image(const void * src, int len, usb_cam_camera_image_t *dest)
00348 {
00349   if(pixelformat==V4L2_PIX_FMT_YUYV)
00350     yuyv2rgb((char*)src, dest->image, dest->width*dest->height);
00351   else if(pixelformat==V4L2_PIX_FMT_UYVY)
00352     uyvy2rgb((char*)src, dest->image, dest->width*dest->height);
00353   else if(pixelformat==V4L2_PIX_FMT_MJPEG)
00354     mjpeg2rgb((char*)src, len, dest->image, dest->width*dest->height);
00355 }
00356 
00357 static int read_frame(usb_cam_camera_image_t *image)
00358 {
00359   struct v4l2_buffer buf;
00360   unsigned int i;
00361   int len;
00362 
00363   switch (io) {
00364   case IO_METHOD_READ:
00365     len = read(fd, buffers[0].start, buffers[0].length);
00366     if (len==-1) {
00367       switch (errno) {
00368       case EAGAIN:
00369         return 0;
00370 
00371       case EIO:
00372         /* Could ignore EIO, see spec. */
00373 
00374         /* fall through */
00375 
00376       default:
00377         errno_exit("read");
00378       }
00379     }
00380 
00381     process_image(buffers[0].start, len, image);
00382 
00383     break;
00384 
00385   case IO_METHOD_MMAP:
00386     CLEAR (buf);
00387 
00388     buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00389     buf.memory = V4L2_MEMORY_MMAP;
00390 
00391     if (-1==xioctl(fd, VIDIOC_DQBUF, &buf)) {
00392       switch (errno) {
00393       case EAGAIN:
00394         return 0;
00395 
00396       case EIO:
00397         /* Could ignore EIO, see spec. */
00398 
00399         /* fall through */
00400 
00401       default:
00402         errno_exit("VIDIOC_DQBUF");
00403       }
00404     }
00405 
00406     assert (buf.index < n_buffers);
00407     len = buf.bytesused;
00408     process_image(buffers[buf.index].start, len, image);
00409 
00410     if (-1==xioctl(fd, VIDIOC_QBUF, &buf))
00411       errno_exit("VIDIOC_QBUF");
00412 
00413     break;
00414 
00415   case IO_METHOD_USERPTR:
00416     CLEAR (buf);
00417 
00418     buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00419     buf.memory = V4L2_MEMORY_USERPTR;
00420 
00421     if (-1==xioctl(fd, VIDIOC_DQBUF, &buf)) {
00422       switch (errno) {
00423       case EAGAIN:
00424         return 0;
00425 
00426       case EIO:
00427         /* Could ignore EIO, see spec. */
00428 
00429         /* fall through */
00430 
00431       default:
00432         errno_exit("VIDIOC_DQBUF");
00433       }
00434     }
00435 
00436     for(i = 0; i<n_buffers; ++i)
00437       if (buf.m.userptr==(unsigned long) buffers[i].start&&buf.length==buffers[i].length)
00438         break;
00439 
00440     assert (i < n_buffers);
00441     len = buf.bytesused;
00442     process_image((void *) buf.m.userptr, len, image);
00443 
00444     if (-1==xioctl(fd, VIDIOC_QBUF, &buf))
00445       errno_exit("VIDIOC_QBUF");
00446 
00447     break;
00448   }
00449 
00450   return 1;
00451 }
00452 
00453 static void stop_capturing(void)
00454 {
00455   enum v4l2_buf_type type;
00456 
00457   switch (io) {
00458   case IO_METHOD_READ:
00459     /* Nothing to do. */
00460     break;
00461 
00462   case IO_METHOD_MMAP:
00463   case IO_METHOD_USERPTR:
00464     type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00465 
00466     if (-1==xioctl(fd, VIDIOC_STREAMOFF, &type))
00467       errno_exit("VIDIOC_STREAMOFF");
00468 
00469     break;
00470   }
00471 }
00472 
00473 static void start_capturing(void)
00474 {
00475   unsigned int i;
00476   enum v4l2_buf_type type;
00477 
00478   switch (io) {
00479   case IO_METHOD_READ:
00480     /* Nothing to do. */
00481     break;
00482 
00483   case IO_METHOD_MMAP:
00484     for(i = 0; i<n_buffers; ++i) {
00485       struct v4l2_buffer buf;
00486 
00487       CLEAR (buf);
00488 
00489       buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00490       buf.memory = V4L2_MEMORY_MMAP;
00491       buf.index = i;
00492 
00493       if (-1==xioctl(fd, VIDIOC_QBUF, &buf))
00494         errno_exit("VIDIOC_QBUF");
00495     }
00496 
00497     type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00498 
00499     if (-1==xioctl(fd, VIDIOC_STREAMON, &type))
00500       errno_exit("VIDIOC_STREAMON");
00501 
00502     break;
00503 
00504   case IO_METHOD_USERPTR:
00505     for(i = 0; i<n_buffers; ++i) {
00506       struct v4l2_buffer buf;
00507 
00508       CLEAR (buf);
00509 
00510       buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00511       buf.memory = V4L2_MEMORY_USERPTR;
00512       buf.index = i;
00513       buf.m.userptr = (unsigned long) buffers[i].start;
00514       buf.length = buffers[i].length;
00515 
00516       if (-1==xioctl(fd, VIDIOC_QBUF, &buf))
00517         errno_exit("VIDIOC_QBUF");
00518     }
00519 
00520     type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00521 
00522     if (-1==xioctl(fd, VIDIOC_STREAMON, &type))
00523       errno_exit("VIDIOC_STREAMON");
00524 
00525     break;
00526   }
00527 }
00528 
00529 static void uninit_device(void)
00530 {
00531   unsigned int i;
00532 
00533   switch (io) {
00534   case IO_METHOD_READ:
00535     free(buffers[0].start);
00536     break;
00537 
00538   case IO_METHOD_MMAP:
00539     for(i = 0; i<n_buffers; ++i)
00540       if (-1==munmap(buffers[i].start, buffers[i].length))
00541         errno_exit("munmap");
00542     break;
00543 
00544   case IO_METHOD_USERPTR:
00545     for(i = 0; i<n_buffers; ++i)
00546       free(buffers[i].start);
00547     break;
00548   }
00549 
00550   free(buffers);
00551 }
00552 
00553 static void init_read(unsigned int buffer_size)
00554 {
00555   buffers = (buffer*)calloc(1, sizeof(*buffers));
00556 
00557   if (!buffers) {
00558     fprintf(stderr, "Out of memory\n");
00559     exit(EXIT_FAILURE);
00560   }
00561 
00562   buffers[0].length = buffer_size;
00563   buffers[0].start = malloc(buffer_size);
00564 
00565   if (!buffers[0].start) {
00566     fprintf(stderr, "Out of memory\n");
00567     exit(EXIT_FAILURE);
00568   }
00569 }
00570 
00571 static void init_mmap(void)
00572 {
00573   struct v4l2_requestbuffers req;
00574 
00575   CLEAR (req);
00576 
00577   req.count = 4;
00578   req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00579   req.memory = V4L2_MEMORY_MMAP;
00580 
00581   if (-1==xioctl(fd, VIDIOC_REQBUFS, &req)) {
00582     if (EINVAL==errno) {
00583       fprintf(stderr, "%s does not support memory mapping\n", camera_dev);
00584       exit(EXIT_FAILURE);
00585     } else {
00586       errno_exit("VIDIOC_REQBUFS");
00587     }
00588   }
00589 
00590   if (req.count<2) {
00591     fprintf(stderr, "Insufficient buffer memory on %s\n", camera_dev);
00592     exit(EXIT_FAILURE);
00593   }
00594 
00595   buffers = (buffer*)calloc(req.count, sizeof(*buffers));
00596 
00597   if (!buffers) {
00598     fprintf(stderr, "Out of memory\n");
00599     exit(EXIT_FAILURE);
00600   }
00601 
00602   for(n_buffers = 0; n_buffers<req.count; ++n_buffers) {
00603     struct v4l2_buffer buf;
00604 
00605     CLEAR (buf);
00606 
00607     buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00608     buf.memory = V4L2_MEMORY_MMAP;
00609     buf.index = n_buffers;
00610 
00611     if (-1==xioctl(fd, VIDIOC_QUERYBUF, &buf))
00612       errno_exit("VIDIOC_QUERYBUF");
00613 
00614     buffers[n_buffers].length = buf.length;
00615     buffers[n_buffers].start = mmap(NULL /* start anywhere */, buf.length, PROT_READ|PROT_WRITE /* required */, MAP_SHARED /* recommended */, fd, buf.m.offset);
00616 
00617     if (MAP_FAILED==buffers[n_buffers].start)
00618       errno_exit("mmap");
00619   }
00620 }
00621 
00622 static void init_userp(unsigned int buffer_size)
00623 {
00624   struct v4l2_requestbuffers req;
00625   unsigned int page_size;
00626 
00627   page_size = getpagesize();
00628   buffer_size = (buffer_size+page_size-1)&~(page_size-1);
00629 
00630   CLEAR (req);
00631 
00632   req.count = 4;
00633   req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00634   req.memory = V4L2_MEMORY_USERPTR;
00635 
00636   if (-1==xioctl(fd, VIDIOC_REQBUFS, &req)) {
00637     if (EINVAL==errno) {
00638       fprintf(stderr, "%s does not support "
00639         "user pointer i/o\n", camera_dev);
00640       exit(EXIT_FAILURE);
00641     } else {
00642       errno_exit("VIDIOC_REQBUFS");
00643     }
00644   }
00645 
00646   buffers = (buffer*)calloc(4, sizeof(*buffers));
00647 
00648   if (!buffers) {
00649     fprintf(stderr, "Out of memory\n");
00650     exit(EXIT_FAILURE);
00651   }
00652 
00653   for(n_buffers = 0; n_buffers<4; ++n_buffers) {
00654     buffers[n_buffers].length = buffer_size;
00655     buffers[n_buffers].start = memalign(/* boundary */page_size, buffer_size);
00656 
00657     if (!buffers[n_buffers].start) {
00658       fprintf(stderr, "Out of memory\n");
00659       exit(EXIT_FAILURE);
00660     }
00661   }
00662 }
00663 
00664 static void init_device(int image_width, int image_height)
00665 {
00666   struct v4l2_capability cap;
00667   struct v4l2_cropcap cropcap;
00668   struct v4l2_crop crop;
00669   struct v4l2_format fmt;
00670   unsigned int min;
00671 
00672   if (-1==xioctl(fd, VIDIOC_QUERYCAP, &cap)) {
00673     if (EINVAL==errno) {
00674       fprintf(stderr, "%s is no V4L2 device\n", camera_dev);
00675       exit(EXIT_FAILURE);
00676     } else {
00677       errno_exit("VIDIOC_QUERYCAP");
00678     }
00679   }
00680 
00681   if (!(cap.capabilities&V4L2_CAP_VIDEO_CAPTURE)) {
00682     fprintf(stderr, "%s is no video capture device\n", camera_dev);
00683     exit(EXIT_FAILURE);
00684   }
00685 
00686   switch (io) {
00687   case IO_METHOD_READ:
00688     if (!(cap.capabilities&V4L2_CAP_READWRITE)) {
00689       fprintf(stderr, "%s does not support read i/o\n", camera_dev);
00690       exit(EXIT_FAILURE);
00691     }
00692 
00693     break;
00694 
00695   case IO_METHOD_MMAP:
00696   case IO_METHOD_USERPTR:
00697     if (!(cap.capabilities&V4L2_CAP_STREAMING)) {
00698       fprintf(stderr, "%s does not support streaming i/o\n", camera_dev);
00699       exit(EXIT_FAILURE);
00700     }
00701 
00702     break;
00703   }
00704 
00705   /* Select video input, video standard and tune here. */
00706 
00707   CLEAR (cropcap);
00708 
00709   cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00710 
00711   if (0==xioctl(fd, VIDIOC_CROPCAP, &cropcap)) {
00712     crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00713     crop.c = cropcap.defrect; /* reset to default */
00714 
00715     if (-1==xioctl(fd, VIDIOC_S_CROP, &crop)) {
00716       switch (errno) {
00717       case EINVAL:
00718         /* Cropping not supported. */
00719         break;
00720       default:
00721         /* Errors ignored. */
00722         break;
00723       }
00724     }
00725   } else {
00726     /* Errors ignored. */
00727   }
00728 
00729   CLEAR (fmt);
00730 
00731 //  fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00732 //  fmt.fmt.pix.width = 640;
00733 //  fmt.fmt.pix.height = 480;
00734 //  fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
00735 //  fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
00736 
00737   fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00738   fmt.fmt.pix.width = image_width;
00739   fmt.fmt.pix.height = image_height;
00740   fmt.fmt.pix.pixelformat = pixelformat;
00741   fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
00742 
00743 
00744   if (-1==xioctl(fd, VIDIOC_S_FMT, &fmt))
00745     errno_exit("VIDIOC_S_FMT");
00746 
00747   /* Note VIDIOC_S_FMT may change width and height. */
00748 
00749   /* Buggy driver paranoia. */
00750   min = fmt.fmt.pix.width*2;
00751   if (fmt.fmt.pix.bytesperline<min)
00752     fmt.fmt.pix.bytesperline = min;
00753   min = fmt.fmt.pix.bytesperline*fmt.fmt.pix.height;
00754   if (fmt.fmt.pix.sizeimage<min)
00755     fmt.fmt.pix.sizeimage = min;
00756 
00757   image_width = fmt.fmt.pix.width;
00758   image_height = fmt.fmt.pix.height;
00759 
00760   switch (io) {
00761   case IO_METHOD_READ:
00762     init_read(fmt.fmt.pix.sizeimage);
00763     break;
00764 
00765   case IO_METHOD_MMAP:
00766     init_mmap();
00767     break;
00768 
00769   case IO_METHOD_USERPTR:
00770     init_userp(fmt.fmt.pix.sizeimage);
00771     break;
00772   }
00773 }
00774 
00775 static void close_device(void)
00776 {
00777   if (-1==close(fd))
00778     errno_exit("close");
00779 
00780   fd = -1;
00781 }
00782 
00783 static void open_device(void)
00784 {
00785   struct stat st;
00786 
00787   if (-1==stat(camera_dev, &st)) {
00788     fprintf(stderr, "Cannot identify '%s': %d, %s\n", camera_dev, errno, strerror(errno));
00789     exit(EXIT_FAILURE);
00790   }
00791 
00792   if (!S_ISCHR (st.st_mode)) {
00793     fprintf(stderr, "%s is no device\n", camera_dev);
00794     exit(EXIT_FAILURE);
00795   }
00796 
00797   fd = open(camera_dev, O_RDWR /* required */|O_NONBLOCK, 0);
00798 
00799   if (-1==fd) {
00800     fprintf(stderr, "Cannot open '%s': %d, %s\n", camera_dev, errno, strerror(errno));
00801     exit(EXIT_FAILURE);
00802   }
00803 }
00804 
00805 usb_cam_camera_image_t *usb_cam_camera_start(const char* dev, usb_cam_io_method io_method,
00806     usb_cam_pixel_format pixel_format, int image_width, int image_height)
00807 {
00808   camera_dev = (char*)calloc(1,strlen(dev)+1);
00809   strcpy(camera_dev,dev);
00810 
00811   usb_cam_camera_image_t *image;
00812   io = io_method;
00813   if(pixel_format == PIXEL_FORMAT_YUYV)
00814     pixelformat = V4L2_PIX_FMT_YUYV;
00815   else if(pixel_format == PIXEL_FORMAT_UYVY)
00816     pixelformat = V4L2_PIX_FMT_UYVY;
00817   else if(pixel_format == PIXEL_FORMAT_MJPEG) {
00818     pixelformat = V4L2_PIX_FMT_MJPEG;
00819     init_mjpeg_decoder(image_width, image_height);
00820   }
00821   else {
00822     fprintf(stderr, "Unknown pixelformat.\n");
00823     exit(EXIT_FAILURE);
00824   }
00825 
00826   open_device();
00827   init_device(image_width, image_height);
00828   start_capturing();
00829 
00830   image = (usb_cam_camera_image_t *) calloc(1, sizeof(usb_cam_camera_image_t));
00831 
00832   image->width = image_width;
00833   image->height = image_height;
00834   image->bytes_per_pixel = 24;
00835 
00836   image->image_size = image->width*image->height*image->bytes_per_pixel;
00837   image->is_new = 0;
00838   image->image = (char *) calloc(image->image_size, sizeof(char));
00839   memset(image->image, 0, image->image_size*sizeof(char));
00840 
00841   return image;
00842 }
00843 
00844 void usb_cam_camera_shutdown(void)
00845 {
00846   stop_capturing();
00847   uninit_device();
00848   close_device();
00849 
00850   if (avcodec_context) {
00851     avcodec_close(avcodec_context);
00852     av_free(avcodec_context);
00853     avcodec_context = NULL;
00854   }
00855   if (avframe_camera)
00856     av_free(avframe_camera);
00857   avframe_camera = NULL;
00858   if (avframe_rgb)
00859     av_free(avframe_rgb);
00860   avframe_rgb = NULL;
00861 }
00862 
00863 void usb_cam_camera_grab_image(usb_cam_camera_image_t *image)
00864 {
00865   fd_set fds;
00866   struct timeval tv;
00867   int r;
00868 
00869   FD_ZERO (&fds);
00870   FD_SET (fd, &fds);
00871 
00872   /* Timeout. */
00873   tv.tv_sec = 2;
00874   tv.tv_usec = 0;
00875 
00876   r = select(fd+1, &fds, NULL, NULL, &tv);
00877 
00878   if (-1==r) {
00879     if (EINTR==errno)
00880       return;
00881 
00882     errno_exit("select");
00883   }
00884 
00885   if (0==r) {
00886     fprintf(stderr, "select timeout\n");
00887     exit(EXIT_FAILURE);
00888   }
00889 
00890   read_frame(image);
00891   image->is_new = 1;
00892 }
00893