modules/highgui/src/cap_v4l.cpp

   1 /* This is the contributed code:
   2
   3 File:             cvcap_v4l.cpp
   4 Current Location: ../opencv-0.9.6/otherlibs/highgui
   5
   6 Original Version: 2003-03-12  Magnus Lundin lundin@mlu.mine.nu
   7 Original Comments:
   8
   9 ML:This set of files adds support for firevre and usb cameras.
  10 First it tries to install a firewire camera,
  11 if that fails it tries a v4l/USB camera
  12 It has been tested with the motempl sample program
  13
  14 First Patch:  August 24, 2004 Travis Wood   TravisOCV@tkwood.com
  15 For Release:  OpenCV-Linux Beta4  opencv-0.9.6
  16 Tested On:    LMLBT44 with 8 video inputs
  17 Problems?     Post your questions at answers.opencv.org,
  18               Report bugs at code.opencv.org,
  19               Submit your fixes at https://github.com/Itseez/opencv/
  20 Patched Comments:
  21
  22 TW: The cv cam utils that came with the initial release of OpenCV for LINUX Beta4
  23 were not working.  I have rewritten them so they work for me. At the same time, trying
  24 to keep the original code as ML wrote it as unchanged as possible.  No one likes to debug
  25 someone elses code, so I resisted changes as much as possible.  I have tried to keep the
  26 same "ideas" where applicable, that is, where I could figure out what the previous author
  27 intended. Some areas I just could not help myself and had to "spiffy-it-up" my way.
  28
  29 These drivers should work with other V4L frame capture cards other then my bttv
  30 driven frame capture card.
  31
  32 Re Written driver for standard V4L mode. Tested using LMLBT44 video capture card.
  33 Standard bttv drivers are on the LMLBT44 with up to 8 Inputs.
  34
  35 This utility was written with the help of the document:
  36 http://pages.cpsc.ucalgary.ca/~sayles/VFL_HowTo
  37 as a general guide for interfacing into the V4l standard.
  38
  39 Made the index value passed for icvOpenCAM_V4L(index) be the number of the
  40 video device source in the /dev tree. The -1 uses original /dev/video.
  41
  42 Index  Device
  43   0    /dev/video0
  44   1    /dev/video1
  45   2    /dev/video2
  46   3    /dev/video3
  47   ...
  48   7    /dev/video7
  49 with
  50   -1   /dev/video
  51
  52 TW: You can select any video source, but this package was limited from the start to only
  53 ONE camera opened at any ONE time.
  54 This is an original program limitation.
  55 If you are interested, I will make my version available to other OpenCV users.  The big
  56 difference in mine is you may pass the camera number as part of the cv argument, but this
  57 convention is non standard for current OpenCV calls and the camera number is not currently
  58 passed into the called routine.
  59
  60 Second Patch:   August 28, 2004 Sfuncia Fabio fiblan@yahoo.it
  61 For Release:  OpenCV-Linux Beta4 Opencv-0.9.6
  62
  63 FS: this patch fix not sequential index of device (unplugged device), and real numCameras.
  64     for -1 index (icvOpenCAM_V4L) i dont use /dev/video but real device available, because
  65     if /dev/video is a link to /dev/video0 and i unplugged device on /dev/video0, /dev/video
  66     is a bad link. I search the first available device with indexList.
  67
  68 Third Patch:   December 9, 2004 Frederic Devernay Frederic.Devernay@inria.fr
  69 For Release:  OpenCV-Linux Beta4 Opencv-0.9.6
  70
  71 [FD] I modified the following:
  72  - handle YUV420P, YUV420, and YUV411P palettes (for many webcams) without using floating-point
  73  - cvGrabFrame should not wait for the end of the first frame, and should return quickly
  74    (see highgui doc)
  75  - cvRetrieveFrame should in turn wait for the end of frame capture, and should not
  76    trigger the capture of the next frame (the user choses when to do it using GrabFrame)
  77    To get the old behavior, re-call cvRetrieveFrame just after cvGrabFrame.
  78  - having global bufferIndex and FirstCapture variables makes the code non-reentrant
  79  (e.g. when using several cameras), put these in the CvCapture struct.
  80  - according to V4L HowTo, incrementing the buffer index must be done before VIDIOCMCAPTURE.
  81  - the VID_TYPE_SCALES stuff from V4L HowTo is wrong: image size can be changed
  82    even if the hardware does not support scaling (e.g. webcams can have several
  83    resolutions available). Just don't try to set the size at 640x480 if the hardware supports
  84    scaling: open with the default (probably best) image size, and let the user scale it
  85    using SetProperty.
  86  - image size can be changed by two subsequent calls to SetProperty (for width and height)
  87  - bug fix: if the image size changes, realloc the new image only when it is grabbed
  88  - issue errors only when necessary, fix error message formatting.
  89
  90 Fourth Patch: Sept 7, 2005 Csaba Kertesz sign@freemail.hu
  91 For Release:  OpenCV-Linux Beta5 OpenCV-0.9.7
  92
  93 I modified the following:
  94   - Additional Video4Linux2 support :)
  95   - Use mmap functions (v4l2)
  96   - New methods are internal:
  97     try_palette_v4l2 -> rewrite try_palette for v4l2
  98     mainloop_v4l2, read_image_v4l2 -> this methods are moved from official v4l2 capture.c example
  99     try_init_v4l -> device v4l initialisation
 100     try_init_v4l2 -> device v4l2 initialisation
 101     autosetup_capture_mode_v4l -> autodetect capture modes for v4l
 102     autosetup_capture_mode_v4l2 -> autodetect capture modes for v4l2
 103   - Modifications are according with Video4Linux old codes
 104   - Video4Linux handling is automatically if it does not recognize a Video4Linux2 device
 105   - Tested succesful with Logitech Quickcam Express (V4L), Creative Vista (V4L) and Genius VideoCam Notebook (V4L2)
 106   - Correct source lines with compiler warning messages
 107   - Information message from v4l/v4l2 detection
 108
 109 Fifth Patch: Sept 7, 2005 Csaba Kertesz sign@freemail.hu
 110 For Release:  OpenCV-Linux Beta5 OpenCV-0.9.7
 111
 112 I modified the following:
 113   - SN9C10x chip based webcams support
 114   - New methods are internal:
 115     bayer2rgb24, sonix_decompress -> decoder routines for SN9C10x decoding from Takafumi Mizuno <taka-qce@ls-a.jp> with his pleasure :)
 116   - Tested succesful with Genius VideoCam Notebook (V4L2)
 117
 118 Sixth Patch: Sept 10, 2005 Csaba Kertesz sign@freemail.hu
 119 For Release:  OpenCV-Linux Beta5 OpenCV-0.9.7
 120
 121 I added the following:
 122   - Add capture control support (hue, saturation, brightness, contrast, gain)
 123   - Get and change V4L capture controls (hue, saturation, brightness, contrast)
 124   - New method is internal:
 125     icvSetControl -> set capture controls
 126   - Tested succesful with Creative Vista (V4L)
 127
 128 Seventh Patch: Sept 10, 2005 Csaba Kertesz sign@freemail.hu
 129 For Release:  OpenCV-Linux Beta5 OpenCV-0.9.7
 130
 131 I added the following:
 132   - Detect, get and change V4L2 capture controls (hue, saturation, brightness, contrast, gain)
 133   - New methods are internal:
 134     v4l2_scan_controls_enumerate_menu, v4l2_scan_controls -> detect capture control intervals
 135   - Tested succesful with Genius VideoCam Notebook (V4L2)
 136
 137 8th patch: Jan 5, 2006, Olivier.Bornet@idiap.ch
 138 Add support of V4L2_PIX_FMT_YUYV and V4L2_PIX_FMT_MJPEG.
 139 With this patch, new webcams of Logitech, like QuickCam Fusion works.
 140 Note: For use these webcams, look at the UVC driver at
 141 http://linux-uvc.berlios.de/
 142
 143 9th patch: Mar 4, 2006, Olivier.Bornet@idiap.ch
 144 - try V4L2 before V4L, because some devices are V4L2 by default,
 145   but they try to implement the V4L compatibility layer.
 146   So, I think this is better to support V4L2 before V4L.
 147 - better separation between V4L2 and V4L initialization. (this was needed to support
 148   some drivers working, but not fully with V4L2. (so, we do not know when we
 149   need to switch from V4L2 to V4L.
 150
 151 10th patch: July 02, 2008, Mikhail Afanasyev fopencv@theamk.com
 152 Fix reliability problems with high-resolution UVC cameras on linux
 153 the symptoms were damaged image and 'Corrupt JPEG data: premature end of data segment' on stderr
 154 - V4L_ABORT_BADJPEG detects JPEG warnings and turns them into errors, so bad images
 155   could be filtered out
 156 - USE_TEMP_BUFFER fixes the main problem (improper buffer management) and
 157   prevents bad images in the first place
 158
 159 11th patch: April 2, 2013, Forrest Reiling forrest.reiling@gmail.com
 160 Added v4l2 support for getting capture property CV_CAP_PROP_POS_MSEC.
 161 Returns the millisecond timestamp of the last frame grabbed or 0 if no frames have been grabbed
 162 Used to successfully synchonize 2 Logitech C310 USB webcams to within 16 ms of one another
 163
 164
 165 make & enjoy!
 166
 167 */
 168
 169 /*M///////////////////////////////////////////////////////////////////////////////////////
 170 //
 171 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
 172 //
 173 //  By downloading, copying, installing or using the software you agree to this license.
 174 //  If you do not agree to this license, do not download, install,
 175 //  copy or use the software.
 176 //
 177 //
 178 //                        Intel License Agreement
 179 //                For Open Source Computer Vision Library
 180 //
 181 // Copyright (C) 2000, Intel Corporation, all rights reserved.
 182 // Third party copyrights are property of their respective owners.
 183 //
 184 // Redistribution and use in source and binary forms, with or without modification,
 185 // are permitted provided that the following conditions are met:
 186 //
 187 //   * Redistribution's of source code must retain the above copyright notice,
 188 //     this list of conditions and the following disclaimer.
 189 //
 190 //   * Redistribution's in binary form must reproduce the above copyright notice,
 191 //     this list of conditions and the following disclaimer in the documentation
 192 //     and/or other materials provided with the distribution.
 193 //
 194 //   * The name of Intel Corporation may not be used to endorse or promote products
 195 //     derived from this software without specific prior written permission.
 196 //
 197 // This software is provided by the copyright holders and contributors "as is" and
 198 // any express or implied warranties, including, but not limited to, the implied
 199 // warranties of merchantability and fitness for a particular purpose are disclaimed.
 200 // In no event shall the Intel Corporation or contributors be liable for any direct,
 201 // indirect, incidental, special, exemplary, or consequential damages
 202 // (including, but not limited to, procurement of substitute goods or services;
 203 // loss of use, data, or profits; or business interruption) however caused
 204 // and on any theory of liability, whether in contract, strict liability,
 205 // or tort (including negligence or otherwise) arising in any way out of
 206 // the use of this software, even if advised of the possibility of such damage.
 207 //
 208 //M*/
 209
 210 #include "precomp.hpp"
 211
 212 #if !defined WIN32 && (defined HAVE_CAMV4L || defined HAVE_CAMV4L2 || defined HAVE_VIDEOIO)
 213
 214 #define CLEAR(x) memset (&(x), 0, sizeof (x))
 215
 216 #include <stdio.h>
 217 #include <unistd.h>
 218 #include <fcntl.h>
 219 #include <errno.h>
 220 #include <sys/ioctl.h>
 221 #include <sys/types.h>
 222 #include <sys/mman.h>
 223
 224 #ifdef HAVE_CAMV4L
 225 #include <linux/videodev.h>
 226 #endif
 227
 228 #include <string.h>
 229 #include <stdlib.h>
 230 #include <assert.h>
 231 #include <sys/stat.h>
 232 #include <sys/ioctl.h>
 233
 234 #ifdef HAVE_CAMV4L2
 235 #include <asm/types.h>          /* for videodev2.h */
 236 #include <linux/videodev2.h>
 237 #endif
 238
 239 #ifdef HAVE_VIDEOIO
 240 #include <sys/videoio.h>
 241 #define HAVE_CAMV4L2
 242 #endif
 243
 244 /* Defaults - If your board can do better, set it here.  Set for the most common type inputs. */
 245 #define DEFAULT_V4L_WIDTH  640
 246 #define DEFAULT_V4L_HEIGHT 480
 247
 248 #define CHANNEL_NUMBER 1
 249 #define MAX_CAMERAS 8
 250
 251
 252 // default and maximum number of V4L buffers, not including last, 'special' buffer
 253 #define MAX_V4L_BUFFERS 10
 254 #define DEFAULT_V4L_BUFFERS 4
 255
 256 // if enabled, then bad JPEG warnings become errors and cause NULL returned instead of image
 257 #define V4L_ABORT_BADJPEG
 258
 259 #define MAX_DEVICE_DRIVER_NAME 80
 260
 261 /* Device Capture Objects */
 262
 263 #ifdef HAVE_CAMV4L2
 264
 265 /* V4L2 structure */
 266 struct buffer
 267 {
 268   void *  start;
 269   size_t  length;
 270 };
 271
 272 static unsigned int n_buffers = 0;
 273
 274 /* Additional V4L2 pixelformats support for Sonix SN9C10x base webcams */
 275 #ifndef V4L2_PIX_FMT_SBGGR8
 276 #define V4L2_PIX_FMT_SBGGR8  v4l2_fourcc('B','A','8','1') /* 8 BGBG.. GRGR.. */
 277 #endif
 278 #ifndef V4L2_PIX_FMT_SN9C10X
 279 #define V4L2_PIX_FMT_SN9C10X  v4l2_fourcc('S','9','1','0') /* SN9C10x cmpr. */
 280 #endif
 281
 282 #ifndef V4L2_PIX_FMT_SGBRG
 283 #define V4L2_PIX_FMT_SGBRG v4l2_fourcc('G','B','R','G') /* bayer GBRG   GBGB.. RGRG.. */
 284 #endif
 285
 286 #endif  /* HAVE_CAMV4L2 */
 287
 288 enum PALETTE_TYPE {
 289   PALETTE_BGR24 = 1,
 290   PALETTE_YVU420,
 291   PALETTE_YUV411P,
 292   PALETTE_YUYV,
 293   PALETTE_UYVY,
 294   PALETTE_SBGGR8,
 295   PALETTE_SN9C10X,
 296   PALETTE_MJPEG,
 297   PALETTE_SGBRG
 298 };
 299
 300 typedef struct CvCaptureCAM_V4L
 301 {
 302     int deviceHandle;
 303     int bufferIndex;
 304     int FirstCapture;
 305 #ifdef HAVE_CAMV4L
 306     struct video_capability capability;
 307     struct video_window     captureWindow;
 308     struct video_picture    imageProperties;
 309     struct video_mbuf       memoryBuffer;
 310     struct video_mmap       *mmaps;
 311 #endif /* HAVE_CAMV4L */
 312     char *memoryMap;
 313     IplImage frame;
 314
 315 #ifdef HAVE_CAMV4L2
 316    enum PALETTE_TYPE palette;
 317    /* V4L2 variables */
 318    buffer buffers[MAX_V4L_BUFFERS + 1];
 319    struct v4l2_capability cap;
 320    struct v4l2_input inp;
 321    struct v4l2_format form;
 322    struct v4l2_crop crop;
 323    struct v4l2_cropcap cropcap;
 324    struct v4l2_requestbuffers req;
 325    struct v4l2_control control;
 326    enum v4l2_buf_type type;
 327    struct v4l2_queryctrl queryctrl;
 328    struct v4l2_querymenu querymenu;
 329
 330    struct timeval timestamp;
 331
 332    /* V4L2 control variables */
 333    int v4l2_brightness, v4l2_brightness_min, v4l2_brightness_max;
 334    int v4l2_contrast, v4l2_contrast_min, v4l2_contrast_max;
 335    int v4l2_saturation, v4l2_saturation_min, v4l2_saturation_max;
 336    int v4l2_hue, v4l2_hue_min, v4l2_hue_max;
 337    int v4l2_gain, v4l2_gain_min, v4l2_gain_max;
 338    int v4l2_exposure, v4l2_exposure_min, v4l2_exposure_max;
 339
 340 #endif /* HAVE_CAMV4L2 */
 341
 342 }
 343 CvCaptureCAM_V4L;
 344
 345 #ifdef HAVE_CAMV4L2
 346
 347 int V4L2_SUPPORT = 0;
 348
 349 #endif /* HAVE_CAMV4L2 */
 350
 351 static void icvCloseCAM_V4L( CvCaptureCAM_V4L* capture );
 352
 353 static int icvGrabFrameCAM_V4L( CvCaptureCAM_V4L* capture );
 354 static IplImage* icvRetrieveFrameCAM_V4L( CvCaptureCAM_V4L* capture, int );
 355
 356 static double icvGetPropertyCAM_V4L( CvCaptureCAM_V4L* capture, int property_id );
 357 static int    icvSetPropertyCAM_V4L( CvCaptureCAM_V4L* capture, int property_id, double value );
 358
 359 static int icvSetVideoSize( CvCaptureCAM_V4L* capture, int w, int h);
 360
 361 /***********************   Implementations  ***************************************/
 362
 363 static int numCameras = 0;
 364 static int indexList = 0;
 365
 366 /* Simple test program: Find number of Video Sources available.
 367    Start from 0 and go to MAX_CAMERAS while checking for the device with that name.
 368    If it fails on the first attempt of /dev/video0, then check if /dev/video is valid.
 369    Returns the global numCameras with the correct value (we hope) */
 370
 371 static void icvInitCapture_V4L() {
 372    int deviceHandle;
 373    int CameraNumber;
 374    char deviceName[MAX_DEVICE_DRIVER_NAME];
 375
 376    CameraNumber = 0;
 377    while(CameraNumber < MAX_CAMERAS) {
 378       /* Print the CameraNumber at the end of the string with a width of one character */
 379       sprintf(deviceName, "/dev/video%1d", CameraNumber);
 380       /* Test using an open to see if this new device name really does exists. */
 381       deviceHandle = open(deviceName, O_RDONLY);
 382       if (deviceHandle != -1) {
 383          /* This device does indeed exist - add it to the total so far */
 384     // add indexList
 385     indexList|=(1 << CameraNumber);
 386         numCameras++;
 387     }
 388     if (deviceHandle != -1)
 389       close(deviceHandle);
 390       /* Set up to test the next /dev/video source in line */
 391       CameraNumber++;
 392    } /* End while */
 393
 394 }; /* End icvInitCapture_V4L */
 395
 396 #ifdef HAVE_CAMV4L
 397
 398 static int
 399 try_palette(int fd,
 400             struct video_picture *cam_pic,
 401             int pal,
 402             int depth)
 403 {
 404   cam_pic->palette = pal;
 405   cam_pic->depth = depth;
 406   if (ioctl(fd, VIDIOCSPICT, cam_pic) < 0)
 407     return 0;
 408   if (ioctl(fd, VIDIOCGPICT, cam_pic) < 0)
 409     return 0;
 410   if (cam_pic->palette == pal)
 411     return 1;
 412   return 0;
 413 }
 414
 415 #endif /* HAVE_CAMV4L */
 416
 417 #ifdef HAVE_CAMV4L2
 418
 419 static int try_palette_v4l2(CvCaptureCAM_V4L* capture, unsigned long colorspace)
 420 {
 421   CLEAR (capture->form);
 422
 423   capture->form.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 424   capture->form.fmt.pix.pixelformat = colorspace;
 425   capture->form.fmt.pix.field       = V4L2_FIELD_ANY;
 426   capture->form.fmt.pix.width = DEFAULT_V4L_WIDTH;
 427   capture->form.fmt.pix.height = DEFAULT_V4L_HEIGHT;
 428
 429   if (-1 == ioctl (capture->deviceHandle, VIDIOC_S_FMT, &capture->form))
 430       return -1;
 431
 432
 433   if (colorspace != capture->form.fmt.pix.pixelformat)
 434     return -1;
 435   else
 436     return 0;
 437 }
 438
 439 #endif /* HAVE_CAMV4L2 */
 440
 441 #ifdef HAVE_CAMV4L
 442
 443 static int try_init_v4l(CvCaptureCAM_V4L* capture, char *deviceName)
 444 {
 445
 446   // if detect = -1 then unable to open device
 447   // if detect = 0 then detected nothing
 448   // if detect = 1 then V4L device
 449   int detect = 0;
 450
 451
 452   // Test device for V4L compability
 453
 454   /* Test using an open to see if this new device name really does exists. */
 455   /* No matter what the name - it still must be opened! */
 456   capture->deviceHandle = open(deviceName, O_RDWR);
 457
 458   if (capture->deviceHandle == 0)
 459   {
 460     detect = -1;
 461
 462     icvCloseCAM_V4L(capture);
 463   }
 464
 465   if (detect == 0)
 466   {
 467     /* Query the newly opened device for its capabilities */
 468     if (ioctl(capture->deviceHandle, VIDIOCGCAP, &capture->capability) < 0)
 469     {
 470       detect = 0;
 471       icvCloseCAM_V4L(capture);
 472     }
 473       else
 474     {
 475       detect = 1;
 476     }
 477   }
 478
 479   return detect;
 480
 481 }
 482
 483 #endif /* HAVE_CAMV4L */
 484
 485 #ifdef HAVE_CAMV4L2
 486
 487 static int try_init_v4l2(CvCaptureCAM_V4L* capture, char *deviceName)
 488 {
 489   // Test device for V4L2 compability
 490   // Return value:
 491   // -1 then unable to open device
 492   //  0 then detected nothing
 493   //  1 then V4L2 device
 494
 495   int deviceIndex;
 496
 497   /* Open and test V4L2 device */
 498   capture->deviceHandle = open (deviceName, O_RDWR /* required */ | O_NONBLOCK, 0);
 499   if (-1 == capture->deviceHandle)
 500   {
 501 #ifndef NDEBUG
 502     fprintf(stderr, "(DEBUG) try_init_v4l2 open \"%s\": %s\n", deviceName, strerror(errno));
 503 #endif
 504     icvCloseCAM_V4L(capture);
 505     return -1;
 506   }
 507
 508   CLEAR (capture->cap);
 509   if (-1 == ioctl (capture->deviceHandle, VIDIOC_QUERYCAP, &capture->cap))
 510   {
 511 #ifndef NDEBUG
 512     fprintf(stderr, "(DEBUG) try_init_v4l2 VIDIOC_QUERYCAP \"%s\": %s\n", deviceName, strerror(errno));
 513 #endif
 514     icvCloseCAM_V4L(capture);
 515     return 0;
 516   }
 517
 518   /* Query channels number */
 519   if (-1 == ioctl (capture->deviceHandle, VIDIOC_G_INPUT, &deviceIndex))
 520   {
 521 #ifndef NDEBUG
 522     fprintf(stderr, "(DEBUG) try_init_v4l2 VIDIOC_G_INPUT \"%s\": %s\n", deviceName, strerror(errno));
 523 #endif
 524     icvCloseCAM_V4L(capture);
 525     return 0;
 526   }
 527
 528   /* Query information about current input */
 529   CLEAR (capture->inp);
 530   capture->inp.index = deviceIndex;
 531   if (-1 == ioctl (capture->deviceHandle, VIDIOC_ENUMINPUT, &capture->inp))
 532   {
 533 #ifndef NDEBUG
 534     fprintf(stderr, "(DEBUG) try_init_v4l2 VIDIOC_ENUMINPUT \"%s\": %s\n", deviceName, strerror(errno));
 535 #endif
 536     icvCloseCAM_V4L(capture);
 537     return 0;
 538   }
 539
 540   return 1;
 541
 542 }
 543
 544 static int autosetup_capture_mode_v4l2(CvCaptureCAM_V4L* capture)
 545 {
 546   if (try_palette_v4l2(capture, V4L2_PIX_FMT_BGR24) == 0)
 547   {
 548     capture->palette = PALETTE_BGR24;
 549   }
 550   else
 551   if (try_palette_v4l2(capture, V4L2_PIX_FMT_YVU420) == 0)
 552   {
 553     capture->palette = PALETTE_YVU420;
 554   }
 555   else
 556   if (try_palette_v4l2(capture, V4L2_PIX_FMT_YUV411P) == 0)
 557   {
 558     capture->palette = PALETTE_YUV411P;
 559   }
 560   else
 561
 562 #ifdef HAVE_JPEG
 563   if (try_palette_v4l2(capture, V4L2_PIX_FMT_MJPEG) == 0 ||
 564       try_palette_v4l2(capture, V4L2_PIX_FMT_JPEG) == 0)
 565   {
 566     capture->palette = PALETTE_MJPEG;
 567   }
 568   else
 569 #endif
 570
 571   if (try_palette_v4l2(capture, V4L2_PIX_FMT_YUYV) == 0)
 572   {
 573     capture->palette = PALETTE_YUYV;
 574   }
 575   else if (try_palette_v4l2(capture, V4L2_PIX_FMT_UYVY) == 0)
 576   {
 577     capture->palette = PALETTE_UYVY;
 578   }
 579   else
 580   if (try_palette_v4l2(capture, V4L2_PIX_FMT_SN9C10X) == 0)
 581   {
 582     capture->palette = PALETTE_SN9C10X;
 583   } else
 584   if (try_palette_v4l2(capture, V4L2_PIX_FMT_SBGGR8) == 0)
 585   {
 586     capture->palette = PALETTE_SBGGR8;
 587   } else
 588   if (try_palette_v4l2(capture, V4L2_PIX_FMT_SGBRG) == 0)
 589   {
 590     capture->palette = PALETTE_SGBRG;
 591   }
 592       else
 593   {
 594     fprintf(stderr, "HIGHGUI ERROR: V4L2: Pixel format of incoming image is unsupported by OpenCV\n");
 595     icvCloseCAM_V4L(capture);
 596     return -1;
 597   }
 598
 599   return 0;
 600
 601 }
 602
 603 #endif /* HAVE_CAMV4L2 */
 604
 605 #ifdef HAVE_CAMV4L
 606
 607 static int autosetup_capture_mode_v4l(CvCaptureCAM_V4L* capture)
 608 {
 609
 610   if(ioctl(capture->deviceHandle, VIDIOCGPICT, &capture->imageProperties) < 0) {
 611      fprintf( stderr, "HIGHGUI ERROR: V4L: Unable to determine size of incoming image\n");
 612      icvCloseCAM_V4L(capture);
 613      return -1;
 614   }
 615
 616   /* Yet MORE things that might have to be changes with your frame capture card */
 617   /* This sets the scale to the center of a 2^16 number */
 618   if (try_palette(capture->deviceHandle, &capture->imageProperties, VIDEO_PALETTE_RGB24, 24)) {
 619       //printf("negotiated palette RGB24\n");
 620   }
 621   else if (try_palette(capture->deviceHandle, &capture->imageProperties, VIDEO_PALETTE_YUV420P, 16)) {
 622       //printf("negotiated palette YUV420P\n");
 623   }
 624   else if (try_palette(capture->deviceHandle, &capture->imageProperties, VIDEO_PALETTE_YUV420, 16)) {
 625       //printf("negotiated palette YUV420\n");
 626   }
 627   else if (try_palette(capture->deviceHandle, &capture->imageProperties, VIDEO_PALETTE_YUV411P, 16)) {
 628       //printf("negotiated palette YUV420P\n");
 629   }
 630   else {
 631     fprintf(stderr, "HIGHGUI ERROR: V4L: Pixel format of incoming image is unsupported by OpenCV\n");
 632     icvCloseCAM_V4L(capture);
 633     return -1;
 634   }
 635
 636   return 0;
 637
 638 }
 639
 640 #endif /* HAVE_CAMV4L */
 641
 642 #ifdef HAVE_CAMV4L2
 643
 644 static void v4l2_scan_controls_enumerate_menu(CvCaptureCAM_V4L* capture)
 645 {
 646 //  printf (" Menu items:\n");
 647   CLEAR (capture->querymenu);
 648   capture->querymenu.id = capture->queryctrl.id;
 649   for (capture->querymenu.index = capture->queryctrl.minimum;
 650        (int)capture->querymenu.index <= capture->queryctrl.maximum;
 651        capture->querymenu.index++)
 652   {
 653     if (0 == ioctl (capture->deviceHandle, VIDIOC_QUERYMENU,
 654                      &capture->querymenu))
 655     {
 656 //      printf (" %s\n", capture->querymenu.name);
 657     } else {
 658         perror ("VIDIOC_QUERYMENU");
 659     }
 660   }
 661 }
 662
 663 static void v4l2_scan_controls(CvCaptureCAM_V4L* capture)
 664 {
 665
 666   __u32 ctrl_id;
 667
 668   for (ctrl_id = V4L2_CID_BASE;
 669        ctrl_id < V4L2_CID_LASTP1;
 670        ctrl_id++)
 671   {
 672
 673     /* set the id we will query now */
 674     CLEAR (capture->queryctrl);
 675     capture->queryctrl.id = ctrl_id;
 676
 677     if (0 == ioctl (capture->deviceHandle, VIDIOC_QUERYCTRL,
 678                      &capture->queryctrl))
 679     {
 680
 681       if (capture->queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
 682         continue;
 683
 684       if (capture->queryctrl.id == V4L2_CID_BRIGHTNESS)
 685       {
 686         capture->v4l2_brightness = 1;
 687         capture->v4l2_brightness_min = capture->queryctrl.minimum;
 688         capture->v4l2_brightness_max = capture->queryctrl.maximum;
 689       }
 690
 691       if (capture->queryctrl.id == V4L2_CID_CONTRAST)
 692       {
 693         capture->v4l2_contrast = 1;
 694         capture->v4l2_contrast_min = capture->queryctrl.minimum;
 695         capture->v4l2_contrast_max = capture->queryctrl.maximum;
 696       }
 697
 698       if (capture->queryctrl.id == V4L2_CID_SATURATION)
 699       {
 700         capture->v4l2_saturation = 1;
 701         capture->v4l2_saturation_min = capture->queryctrl.minimum;
 702         capture->v4l2_saturation_max = capture->queryctrl.maximum;
 703       }
 704
 705       if (capture->queryctrl.id == V4L2_CID_HUE)
 706       {
 707         capture->v4l2_hue = 1;
 708         capture->v4l2_hue_min = capture->queryctrl.minimum;
 709         capture->v4l2_hue_max = capture->queryctrl.maximum;
 710       }
 711
 712       if (capture->queryctrl.id == V4L2_CID_GAIN)
 713       {
 714         capture->v4l2_gain = 1;
 715         capture->v4l2_gain_min = capture->queryctrl.minimum;
 716         capture->v4l2_gain_max = capture->queryctrl.maximum;
 717       }
 718
 719       if (capture->queryctrl.id == V4L2_CID_EXPOSURE)
 720       {
 721         capture->v4l2_exposure = 1;
 722         capture->v4l2_exposure_min = capture->queryctrl.minimum;
 723         capture->v4l2_exposure_max = capture->queryctrl.maximum;
 724       }
 725
 726       if (capture->queryctrl.type == V4L2_CTRL_TYPE_MENU)
 727         v4l2_scan_controls_enumerate_menu(capture);
 728
 729     } else {
 730
 731       if (errno == EINVAL)
 732         continue;
 733
 734       perror ("VIDIOC_QUERYCTRL");
 735
 736     }
 737
 738   }
 739
 740   for (ctrl_id = V4L2_CID_PRIVATE_BASE;;ctrl_id++)
 741   {
 742
 743     /* set the id we will query now */
 744     CLEAR (capture->queryctrl);
 745     capture->queryctrl.id = ctrl_id;
 746
 747     if (0 == ioctl (capture->deviceHandle, VIDIOC_QUERYCTRL,
 748                      &capture->queryctrl))
 749     {
 750
 751       if (capture->queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
 752         continue;
 753
 754       if (capture->queryctrl.id == V4L2_CID_BRIGHTNESS)
 755       {
 756         capture->v4l2_brightness = 1;
 757         capture->v4l2_brightness_min = capture->queryctrl.minimum;
 758         capture->v4l2_brightness_max = capture->queryctrl.maximum;
 759       }
 760
 761       if (capture->queryctrl.id == V4L2_CID_CONTRAST)
 762       {
 763         capture->v4l2_contrast = 1;
 764         capture->v4l2_contrast_min = capture->queryctrl.minimum;
 765         capture->v4l2_contrast_max = capture->queryctrl.maximum;
 766       }
 767
 768       if (capture->queryctrl.id == V4L2_CID_SATURATION)
 769       {
 770         capture->v4l2_saturation = 1;
 771         capture->v4l2_saturation_min = capture->queryctrl.minimum;
 772         capture->v4l2_saturation_max = capture->queryctrl.maximum;
 773       }
 774
 775       if (capture->queryctrl.id == V4L2_CID_HUE)
 776       {
 777         capture->v4l2_hue = 1;
 778         capture->v4l2_hue_min = capture->queryctrl.minimum;
 779         capture->v4l2_hue_max = capture->queryctrl.maximum;
 780       }
 781
 782       if (capture->queryctrl.id == V4L2_CID_GAIN)
 783       {
 784         capture->v4l2_gain = 1;
 785         capture->v4l2_gain_min = capture->queryctrl.minimum;
 786         capture->v4l2_gain_max = capture->queryctrl.maximum;
 787       }
 788
 789       if (capture->queryctrl.id == V4L2_CID_EXPOSURE)
 790       {
 791         capture->v4l2_exposure = 1;
 792         capture->v4l2_exposure_min = capture->queryctrl.minimum;
 793         capture->v4l2_exposure_max = capture->queryctrl.maximum;
 794       }
 795
 796       if (capture->queryctrl.type == V4L2_CTRL_TYPE_MENU)
 797         v4l2_scan_controls_enumerate_menu(capture);
 798
 799     } else {
 800
 801       if (errno == EINVAL)
 802         break;
 803
 804       perror ("VIDIOC_QUERYCTRL");
 805
 806     }
 807
 808   }
 809
 810 }
 811
 812 static int _capture_V4L2 (CvCaptureCAM_V4L *capture, char *deviceName)
 813 {
 814    int detect_v4l2 = 0;
 815
 816    detect_v4l2 = try_init_v4l2(capture, deviceName);
 817
 818    if (detect_v4l2 != 1) {
 819        /* init of the v4l2 device is not OK */
 820        return -1;
 821    }
 822
 823    /* starting from here, we assume we are in V4L2 mode */
 824    V4L2_SUPPORT = 1;
 825
 826    /* Init V4L2 control variables */
 827    capture->v4l2_brightness = 0;
 828    capture->v4l2_contrast = 0;
 829    capture->v4l2_saturation = 0;
 830    capture->v4l2_hue = 0;
 831    capture->v4l2_gain = 0;
 832    capture->v4l2_exposure = 0;
 833
 834    capture->v4l2_brightness_min = 0;
 835    capture->v4l2_contrast_min = 0;
 836    capture->v4l2_saturation_min = 0;
 837    capture->v4l2_hue_min = 0;
 838    capture->v4l2_gain_min = 0;
 839    capture->v4l2_exposure_min = 0;
 840
 841    capture->v4l2_brightness_max = 0;
 842    capture->v4l2_contrast_max = 0;
 843    capture->v4l2_saturation_max = 0;
 844    capture->v4l2_hue_max = 0;
 845    capture->v4l2_gain_max = 0;
 846    capture->v4l2_exposure_max = 0;
 847
 848    capture->timestamp.tv_sec = 0;
 849    capture->timestamp.tv_usec = 0;
 850
 851    /* Scan V4L2 controls */
 852    v4l2_scan_controls(capture);
 853
 854    if ((capture->cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
 855       /* Nope. */
 856       fprintf( stderr, "HIGHGUI ERROR: V4L2: device %s is unable to capture video memory.\n",deviceName);
 857       icvCloseCAM_V4L(capture);
 858       return -1;
 859    }
 860
 861    /* The following code sets the CHANNEL_NUMBER of the video input.  Some video sources
 862    have sub "Channel Numbers".  For a typical V4L TV capture card, this is usually 1.
 863    I myself am using a simple NTSC video input capture card that uses the value of 1.
 864    If you are not in North America or have a different video standard, you WILL have to change
 865    the following settings and recompile/reinstall.  This set of settings is based on
 866    the most commonly encountered input video source types (like my bttv card) */
 867
 868    if(capture->inp.index > 0) {
 869        CLEAR (capture->inp);
 870        capture->inp.index = CHANNEL_NUMBER;
 871        /* Set only channel number to CHANNEL_NUMBER */
 872        /* V4L2 have a status field from selected video mode */
 873        if (-1 == ioctl (capture->deviceHandle, VIDIOC_ENUMINPUT, &capture->inp))
 874        {
 875          fprintf (stderr, "HIGHGUI ERROR: V4L2: Aren't able to set channel number\n");
 876          icvCloseCAM_V4L (capture);
 877          return -1;
 878        }
 879    } /* End if */
 880
 881    /* Find Window info */
 882    CLEAR (capture->form);
 883    capture->form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 884
 885    if (-1 == ioctl (capture->deviceHandle, VIDIOC_G_FMT, &capture->form)) {
 886        fprintf( stderr, "HIGHGUI ERROR: V4L2: Could not obtain specifics of capture window.\n\n");
 887        icvCloseCAM_V4L(capture);
 888        return -1;
 889    }
 890
 891    if (V4L2_SUPPORT == 0)
 892    {
 893    }
 894
 895    if (autosetup_capture_mode_v4l2(capture) == -1)
 896        return -1;
 897
 898    icvSetVideoSize(capture, DEFAULT_V4L_WIDTH, DEFAULT_V4L_HEIGHT);
 899
 900    unsigned int min;
 901
 902    /* Buggy driver paranoia. */
 903    min = capture->form.fmt.pix.width * 2;
 904
 905    if (capture->form.fmt.pix.bytesperline < min)
 906        capture->form.fmt.pix.bytesperline = min;
 907
 908    min = capture->form.fmt.pix.bytesperline * capture->form.fmt.pix.height;
 909
 910    if (capture->form.fmt.pix.sizeimage < min)
 911        capture->form.fmt.pix.sizeimage = min;
 912
 913    CLEAR (capture->req);
 914
 915    unsigned int buffer_number = DEFAULT_V4L_BUFFERS;
 916
 917    try_again:
 918
 919    capture->req.count = buffer_number;
 920    capture->req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 921    capture->req.memory = V4L2_MEMORY_MMAP;
 922
 923    if (-1 == ioctl (capture->deviceHandle, VIDIOC_REQBUFS, &capture->req))
 924    {
 925        if (EINVAL == errno)
 926        {
 927          fprintf (stderr, "%s does not support memory mapping\n", deviceName);
 928        } else {
 929          perror ("VIDIOC_REQBUFS");
 930        }
 931        /* free capture, and returns an error code */
 932        icvCloseCAM_V4L (capture);
 933        return -1;
 934    }
 935
 936    if (capture->req.count < buffer_number)
 937    {
 938        if (buffer_number == 1)
 939        {
 940            fprintf (stderr, "Insufficient buffer memory on %s\n", deviceName);
 941
 942            /* free capture, and returns an error code */
 943            icvCloseCAM_V4L (capture);
 944            return -1;
 945        } else {
 946          buffer_number--;
 947      fprintf (stderr, "Insufficient buffer memory on %s -- decreaseing buffers\n", deviceName);
 948
 949      goto try_again;
 950        }
 951    }
 952
 953    for (n_buffers = 0; n_buffers < capture->req.count; ++n_buffers)
 954    {
 955        struct v4l2_buffer buf;
 956
 957        CLEAR (buf);
 958
 959        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 960        buf.memory = V4L2_MEMORY_MMAP;
 961        buf.index = n_buffers;
 962
 963        if (-1 == ioctl (capture->deviceHandle, VIDIOC_QUERYBUF, &buf)) {
 964            perror ("VIDIOC_QUERYBUF");
 965
 966            /* free capture, and returns an error code */
 967            icvCloseCAM_V4L (capture);
 968            return -1;
 969        }
 970
 971        capture->buffers[n_buffers].length = buf.length;
 972        capture->buffers[n_buffers].start =
 973          mmap (NULL /* start anywhere */,
 974                buf.length,
 975                PROT_READ | PROT_WRITE /* required */,
 976                MAP_SHARED /* recommended */,
 977                capture->deviceHandle, buf.m.offset);
 978
 979        if (MAP_FAILED == capture->buffers[n_buffers].start) {
 980            perror ("mmap");
 981
 982            /* free capture, and returns an error code */
 983            icvCloseCAM_V4L (capture);
 984            return -1;
 985        }
 986
 987        if (n_buffers == 0) {
 988      capture->buffers[MAX_V4L_BUFFERS].start = malloc( buf.length );
 989      capture->buffers[MAX_V4L_BUFFERS].length = buf.length;
 990        }
 991    }
 992
 993    /* Set up Image data */
 994    cvInitImageHeader( &capture->frame,
 995                       cvSize( capture->form.fmt.pix.width,
 996                               capture->form.fmt.pix.height ),
 997                       IPL_DEPTH_8U, 3, IPL_ORIGIN_TL, 4 );
 998    /* Allocate space for RGBA data */
 999    capture->frame.imageData = (char *)cvAlloc(capture->frame.imageSize);
1000
1001    return 1;
1002 }; /* End _capture_V4L2 */
1003
1004 #endif /* HAVE_CAMV4L2 */
1005
1006 #ifdef HAVE_CAMV4L
1007
1008 static int _capture_V4L (CvCaptureCAM_V4L *capture, char *deviceName)
1009 {
1010    int detect_v4l = 0;
1011
1012    detect_v4l = try_init_v4l(capture, deviceName);
1013
1014    if ((detect_v4l == -1)
1015        )
1016    {
1017      fprintf (stderr, "HIGHGUI ERROR: V4L"
1018               ": device %s: Unable to open for READ ONLY\n", deviceName);
1019
1020      return -1;
1021    }
1022
1023    if ((detect_v4l <= 0)
1024        )
1025    {
1026      fprintf (stderr, "HIGHGUI ERROR: V4L"
1027               ": device %s: Unable to query number of channels\n", deviceName);
1028
1029      return -1;
1030    }
1031
1032    {
1033      if ((capture->capability.type & VID_TYPE_CAPTURE) == 0) {
1034        /* Nope. */
1035        fprintf( stderr, "HIGHGUI ERROR: V4L: "
1036                 "device %s is unable to capture video memory.\n",deviceName);
1037        icvCloseCAM_V4L(capture);
1038        return -1;
1039      }
1040
1041    }
1042
1043
1044    /* The following code sets the CHANNEL_NUMBER of the video input.  Some video sources
1045    have sub "Channel Numbers".  For a typical V4L TV capture card, this is usually 1.
1046    I myself am using a simple NTSC video input capture card that uses the value of 1.
1047    If you are not in North America or have a different video standard, you WILL have to change
1048    the following settings and recompile/reinstall.  This set of settings is based on
1049    the most commonly encountered input video source types (like my bttv card) */
1050
1051    {
1052
1053      if(capture->capability.channels>0) {
1054
1055        struct video_channel selectedChannel;
1056        memset(&selectedChannel, 0, sizeof(selectedChannel));
1057
1058        selectedChannel.channel=CHANNEL_NUMBER;
1059        if (ioctl(capture->deviceHandle, VIDIOCGCHAN , &selectedChannel) != -1) {
1060           /* set the video mode to ( VIDEO_MODE_PAL, VIDEO_MODE_NTSC, VIDEO_MODE_SECAM) */
1061 //           selectedChannel.norm = VIDEO_MODE_NTSC;
1062           if (ioctl(capture->deviceHandle, VIDIOCSCHAN , &selectedChannel) == -1) {
1063              /* Could not set selected channel - Oh well */
1064              //printf("\n%d, %s not NTSC capable.\n",selectedChannel.channel, selectedChannel.name);
1065           } /* End if */
1066        } /* End if */
1067      } /* End if */
1068
1069    }
1070
1071    {
1072
1073      if(ioctl(capture->deviceHandle, VIDIOCGWIN, &capture->captureWindow) == -1) {
1074        fprintf( stderr, "HIGHGUI ERROR: V4L: "
1075                 "Could not obtain specifics of capture window.\n\n");
1076        icvCloseCAM_V4L(capture);
1077        return -1;
1078      }
1079
1080    }
1081
1082    {
1083
1084      if (autosetup_capture_mode_v4l(capture) == -1)
1085        return -1;
1086
1087    }
1088
1089    {
1090
1091      ioctl(capture->deviceHandle, VIDIOCGMBUF, &capture->memoryBuffer);
1092      capture->memoryMap  = (char *)mmap(0,
1093                                    capture->memoryBuffer.size,
1094                                    PROT_READ | PROT_WRITE,
1095                                    MAP_SHARED,
1096                                    capture->deviceHandle,
1097                                    0);
1098      if (capture->memoryMap == MAP_FAILED) {
1099         fprintf( stderr, "HIGHGUI ERROR: V4L: Mapping Memmory from video source error: %s\n", strerror(errno));
1100         icvCloseCAM_V4L(capture);
1101      }
1102
1103      /* Set up video_mmap structure pointing to this memory mapped area so each image may be
1104         retrieved from an index value */
1105      capture->mmaps = (struct video_mmap *)
1106                  (malloc(capture->memoryBuffer.frames * sizeof(struct video_mmap)));
1107      if (!capture->mmaps) {
1108         fprintf( stderr, "HIGHGUI ERROR: V4L: Could not memory map video frames.\n");
1109         icvCloseCAM_V4L(capture);
1110         return -1;
1111      }
1112
1113    }
1114
1115    /* Set up Image data */
1116    cvInitImageHeader( &capture->frame,
1117                       cvSize( capture->captureWindow.width,
1118                               capture->captureWindow.height ),
1119                       IPL_DEPTH_8U, 3, IPL_ORIGIN_TL, 4 );
1120    /* Allocate space for RGBA data */
1121    capture->frame.imageData = (char *)cvAlloc(capture->frame.imageSize);
1122
1123    return 1;
1124 }; /* End _capture_V4L */
1125
1126 #endif /* HAVE_CAMV4L */
1127
1128 static CvCaptureCAM_V4L * icvCaptureFromCAM_V4L (int index)
1129 {
1130    static int autoindex;
1131    autoindex = 0;
1132
1133    char deviceName[MAX_DEVICE_DRIVER_NAME];
1134
1135    if (!numCameras)
1136       icvInitCapture_V4L(); /* Havent called icvInitCapture yet - do it now! */
1137    if (!numCameras)
1138      return NULL; /* Are there any /dev/video input sources? */
1139
1140    //search index in indexList
1141    if ( (index>-1) && ! ((1 << index) & indexList) )
1142    {
1143      fprintf( stderr, "HIGHGUI ERROR: V4L: index %d is not correct!\n",index);
1144      return NULL; /* Did someone ask for not correct video source number? */
1145    }
1146    /* Allocate memory for this humongus CvCaptureCAM_V4L structure that contains ALL
1147       the handles for V4L processing */
1148    CvCaptureCAM_V4L * capture = (CvCaptureCAM_V4L*)cvAlloc(sizeof(CvCaptureCAM_V4L));
1149    if (!capture) {
1150       fprintf( stderr, "HIGHGUI ERROR: V4L: Could not allocate memory for capture process.\n");
1151       return NULL;
1152    }
1153    /* Select camera, or rather, V4L video source */
1154    if (index<0) { // Asking for the first device available
1155      for (; autoindex<MAX_CAMERAS;autoindex++)
1156     if (indexList & (1<<autoindex))
1157         break;
1158      if (autoindex==MAX_CAMERAS)
1159     return NULL;
1160      index=autoindex;
1161      autoindex++;// i can recall icvOpenCAM_V4l with index=-1 for next camera
1162    }
1163    /* Print the CameraNumber at the end of the string with a width of one character */
1164    sprintf(deviceName, "/dev/video%1d", index);
1165
1166    /* w/o memset some parts  arent initialized - AKA: Fill it with zeros so it is clean */
1167    memset(capture,0,sizeof(CvCaptureCAM_V4L));
1168    /* Present the routines needed for V4L funtionality.  They are inserted as part of
1169       the standard set of cv calls promoting transparency.  "Vector Table" insertion. */
1170    capture->FirstCapture = 1;
1171
1172 #ifdef HAVE_CAMV4L2
1173    if (_capture_V4L2 (capture, deviceName) == -1) {
1174        icvCloseCAM_V4L(capture);
1175        V4L2_SUPPORT = 0;
1176 #endif  /* HAVE_CAMV4L2 */
1177 #ifdef HAVE_CAMV4L
1178        if (_capture_V4L (capture, deviceName) == -1) {
1179            icvCloseCAM_V4L(capture);
1180            return NULL;
1181        }
1182 #endif  /* HAVE_CAMV4L */
1183 #ifdef HAVE_CAMV4L2
1184    } else {
1185        V4L2_SUPPORT = 1;
1186    }
1187 #endif  /* HAVE_CAMV4L2 */
1188
1189    return capture;
1190 }; /* End icvOpenCAM_V4L */
1191
1192 #ifdef HAVE_CAMV4L2
1193
1194 static int read_frame_v4l2(CvCaptureCAM_V4L* capture) {
1195     struct v4l2_buffer buf;
1196
1197     CLEAR (buf);
1198
1199     buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1200     buf.memory = V4L2_MEMORY_MMAP;
1201
1202     if (-1 == ioctl (capture->deviceHandle, VIDIOC_DQBUF, &buf)) {
1203         switch (errno) {
1204         case EAGAIN:
1205             return 0;
1206
1207         case EIO:
1208         if (!(buf.flags & (V4L2_BUF_FLAG_QUEUED | V4L2_BUF_FLAG_DONE)))
1209         {
1210           if (ioctl(capture->deviceHandle, VIDIOC_QBUF, &buf) == -1)
1211           {
1212             return 0;
1213           }
1214         }
1215         return 0;
1216
1217         default:
1218             /* display the error and stop processing */
1219             perror ("VIDIOC_DQBUF");
1220             return 1;
1221         }
1222    }
1223
1224    assert(buf.index < capture->req.count);
1225
1226    memcpy(capture->buffers[MAX_V4L_BUFFERS].start,
1227       capture->buffers[buf.index].start,
1228       capture->buffers[MAX_V4L_BUFFERS].length );
1229    capture->bufferIndex = MAX_V4L_BUFFERS;
1230    //printf("got data in buff %d, len=%d, flags=0x%X, seq=%d, used=%d)\n",
1231    //     buf.index, buf.length, buf.flags, buf.sequence, buf.bytesused);
1232
1233    if (-1 == ioctl (capture->deviceHandle, VIDIOC_QBUF, &buf))
1234        perror ("VIDIOC_QBUF");
1235
1236    //set timestamp in capture struct to be timestamp of most recent frame
1237    capture->timestamp = buf.timestamp;
1238
1239    return 1;
1240 }
1241
1242 static void mainloop_v4l2(CvCaptureCAM_V4L* capture) {
1243     unsigned int count;
1244
1245     count = 1;
1246
1247     while (count-- > 0) {
1248         for (;;) {
1249             fd_set fds;
1250             struct timeval tv;
1251             int r;
1252
1253             FD_ZERO (&fds);
1254             FD_SET (capture->deviceHandle, &fds);
1255
1256             /* Timeout. */
1257             tv.tv_sec = 10;
1258             tv.tv_usec = 0;
1259
1260             r = select (capture->deviceHandle+1, &fds, NULL, NULL, &tv);
1261
1262             if (-1 == r) {
1263                 if (EINTR == errno)
1264                     continue;
1265
1266                 perror ("select");
1267             }
1268
1269             if (0 == r) {
1270                 fprintf (stderr, "select timeout\n");
1271
1272                 /* end the infinite loop */
1273                 break;
1274             }
1275
1276             if (read_frame_v4l2 (capture))
1277                 break;
1278         }
1279     }
1280 }
1281
1282 #endif /* HAVE_CAMV4L2 */
1283
1284 static int icvGrabFrameCAM_V4L(CvCaptureCAM_V4L* capture) {
1285
1286    if (capture->FirstCapture) {
1287       /* Some general initialization must take place the first time through */
1288
1289       /* This is just a technicality, but all buffers must be filled up before any
1290          staggered SYNC is applied.  SO, filler up. (see V4L HowTo) */
1291
1292 #ifdef HAVE_CAMV4L2
1293
1294 #ifdef HAVE_CAMV4L
1295       if (V4L2_SUPPORT == 1)
1296 #endif
1297       {
1298
1299         for (capture->bufferIndex = 0;
1300              capture->bufferIndex < ((int)capture->req.count);
1301              ++capture->bufferIndex)
1302         {
1303
1304           struct v4l2_buffer buf;
1305
1306           CLEAR (buf);
1307
1308           buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1309           buf.memory      = V4L2_MEMORY_MMAP;
1310           buf.index       = (unsigned long)capture->bufferIndex;
1311
1312           if (-1 == ioctl (capture->deviceHandle, VIDIOC_QBUF, &buf)) {
1313               perror ("VIDIOC_QBUF");
1314               return 0;
1315           }
1316         }
1317
1318         /* enable the streaming */
1319         capture->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1320         if (-1 == ioctl (capture->deviceHandle, VIDIOC_STREAMON,
1321                           &capture->type)) {
1322             /* error enabling the stream */
1323             perror ("VIDIOC_STREAMON");
1324             return 0;
1325         }
1326       }
1327 #endif /* HAVE_CAMV4L2 */
1328 #if defined(HAVE_CAMV4L) && defined(HAVE_CAMV4L2)
1329       else
1330 #endif /* HAVE_CAMV4L && HAVE_CAMV4L2 */
1331 #ifdef HAVE_CAMV4L
1332       {
1333
1334         for (capture->bufferIndex = 0;
1335          capture->bufferIndex < (capture->memoryBuffer.frames-1);
1336          ++capture->bufferIndex) {
1337
1338           capture->mmaps[capture->bufferIndex].frame  = capture->bufferIndex;
1339           capture->mmaps[capture->bufferIndex].width  = capture->captureWindow.width;
1340           capture->mmaps[capture->bufferIndex].height = capture->captureWindow.height;
1341           capture->mmaps[capture->bufferIndex].format = capture->imageProperties.palette;
1342
1343           if (ioctl(capture->deviceHandle, VIDIOCMCAPTURE, &capture->mmaps[capture->bufferIndex]) == -1) {
1344             fprintf( stderr, "HIGHGUI ERROR: V4L: Initial Capture Error: Unable to load initial memory buffers.\n");
1345             return 0;
1346           }
1347         }
1348
1349       }
1350 #endif /* HAVE_CAMV4L */
1351
1352 #if defined(V4L_ABORT_BADJPEG) && defined(HAVE_CAMV4L2)
1353      if (V4L2_SUPPORT == 1)
1354      {
1355         // skip first frame. it is often bad -- this is unnotied in traditional apps,
1356         //  but could be fatal if bad jpeg is enabled
1357         mainloop_v4l2(capture);
1358      }
1359 #endif
1360
1361       /* preparation is ok */
1362       capture->FirstCapture = 0;
1363    }
1364
1365 #ifdef HAVE_CAMV4L2
1366
1367    if (V4L2_SUPPORT == 1)
1368    {
1369
1370      mainloop_v4l2(capture);
1371
1372    }
1373 #endif /* HAVE_CAMV4L2 */
1374 #if defined(HAVE_CAMV4L) && defined(HAVE_CAMV4L2)
1375      else
1376 #endif /* HAVE_CAMV4L && HAVE_CAMV4L2 */
1377 #ifdef HAVE_CAMV4L
1378    {
1379
1380      capture->mmaps[capture->bufferIndex].frame  = capture->bufferIndex;
1381      capture->mmaps[capture->bufferIndex].width  = capture->captureWindow.width;
1382      capture->mmaps[capture->bufferIndex].height = capture->captureWindow.height;
1383      capture->mmaps[capture->bufferIndex].format = capture->imageProperties.palette;
1384
1385      if (ioctl (capture->deviceHandle, VIDIOCMCAPTURE,
1386         &capture->mmaps[capture->bufferIndex]) == -1) {
1387      /* capture is on the way, so just exit */
1388      return 1;
1389      }
1390
1391      ++capture->bufferIndex;
1392      if (capture->bufferIndex == capture->memoryBuffer.frames) {
1393         capture->bufferIndex = 0;
1394      }
1395
1396    }
1397 #endif /* HAVE_CAMV4L */
1398
1399    return(1);
1400 }
1401
1402 /*
1403  * Turn a YUV4:2:0 block into an RGB block
1404  *
1405  * Video4Linux seems to use the blue, green, red channel
1406  * order convention-- rgb[0] is blue, rgb[1] is green, rgb[2] is red.
1407  *
1408  * Color space conversion coefficients taken from the excellent
1409  * http://www.inforamp.net/~poynton/ColorFAQ.html
1410  * In his terminology, this is a CCIR 601.1 YCbCr -> RGB.
1411  * Y values are given for all 4 pixels, but the U (Pb)
1412  * and V (Pr) are assumed constant over the 2x2 block.
1413  *
1414  * To avoid floating point arithmetic, the color conversion
1415  * coefficients are scaled into 16.16 fixed-point integers.
1416  * They were determined as follows:
1417  *
1418  *  double brightness = 1.0;  (0->black; 1->full scale)
1419  *  double saturation = 1.0;  (0->greyscale; 1->full color)
1420  *  double fixScale = brightness * 256 * 256;
1421  *  int rvScale = (int)(1.402 * saturation * fixScale);
1422  *  int guScale = (int)(-0.344136 * saturation * fixScale);
1423  *  int gvScale = (int)(-0.714136 * saturation * fixScale);
1424  *  int buScale = (int)(1.772 * saturation * fixScale);
1425  *  int yScale = (int)(fixScale);
1426  */
1427
1428 /* LIMIT: convert a 16.16 fixed-point value to a byte, with clipping. */
1429 #define LIMIT(x) ((x)>0xffffff?0xff: ((x)<=0xffff?0:((x)>>16)))
1430
1431 static inline void
1432 move_420_block(int yTL, int yTR, int yBL, int yBR, int u, int v,
1433                int rowPixels, unsigned char * rgb)
1434 {
1435     const int rvScale = 91881;
1436     const int guScale = -22553;
1437     const int gvScale = -46801;
1438     const int buScale = 116129;
1439     const int yScale  = 65536;
1440     int r, g, b;
1441
1442     g = guScale * u + gvScale * v;
1443 //  if (force_rgb) {
1444 //      r = buScale * u;
1445 //      b = rvScale * v;
1446 //  } else {
1447         r = rvScale * v;
1448         b = buScale * u;
1449 //  }
1450
1451     yTL *= yScale; yTR *= yScale;
1452     yBL *= yScale; yBR *= yScale;
1453
1454     /* Write out top two pixels */
1455     rgb[0] = LIMIT(b+yTL); rgb[1] = LIMIT(g+yTL);
1456     rgb[2] = LIMIT(r+yTL);
1457
1458     rgb[3] = LIMIT(b+yTR); rgb[4] = LIMIT(g+yTR);
1459     rgb[5] = LIMIT(r+yTR);
1460
1461     /* Skip down to next line to write out bottom two pixels */
1462     rgb += 3 * rowPixels;
1463     rgb[0] = LIMIT(b+yBL); rgb[1] = LIMIT(g+yBL);
1464     rgb[2] = LIMIT(r+yBL);
1465
1466     rgb[3] = LIMIT(b+yBR); rgb[4] = LIMIT(g+yBR);
1467     rgb[5] = LIMIT(r+yBR);
1468 }
1469
1470 static inline void
1471 move_411_block(int yTL, int yTR, int yBL, int yBR, int u, int v,
1472                int /*rowPixels*/, unsigned char * rgb)
1473 {
1474     const int rvScale = 91881;
1475     const int guScale = -22553;
1476     const int gvScale = -46801;
1477     const int buScale = 116129;
1478     const int yScale  = 65536;
1479     int r, g, b;
1480
1481     g = guScale * u + gvScale * v;
1482 //  if (force_rgb) {
1483 //      r = buScale * u;
1484 //      b = rvScale * v;
1485 //  } else {
1486         r = rvScale * v;
1487         b = buScale * u;
1488 //  }
1489
1490     yTL *= yScale; yTR *= yScale;
1491     yBL *= yScale; yBR *= yScale;
1492
1493     /* Write out top two first pixels */
1494     rgb[0] = LIMIT(b+yTL); rgb[1] = LIMIT(g+yTL);
1495     rgb[2] = LIMIT(r+yTL);
1496
1497     rgb[3] = LIMIT(b+yTR); rgb[4] = LIMIT(g+yTR);
1498     rgb[5] = LIMIT(r+yTR);
1499
1500     /* Write out top two last pixels */
1501     rgb += 6;
1502     rgb[0] = LIMIT(b+yBL); rgb[1] = LIMIT(g+yBL);
1503     rgb[2] = LIMIT(r+yBL);
1504
1505     rgb[3] = LIMIT(b+yBR); rgb[4] = LIMIT(g+yBR);
1506     rgb[5] = LIMIT(r+yBR);
1507 }
1508
1509 // Consider a YUV420P image of 8x2 pixels.
1510 //
1511 // A plane of Y values    A B C D E F G H
1512 //                        I J K L M N O P
1513 //
1514 // A plane of U values    1   2   3   4
1515 // A plane of V values    1   2   3   4 ....
1516 //
1517 // The U1/V1 samples correspond to the ABIJ pixels.
1518 //     U2/V2 samples correspond to the CDKL pixels.
1519 //
1520 /* Converts from planar YUV420P to RGB24. */
1521 static void
1522 yuv420p_to_rgb24(int width, int height,
1523            unsigned char *pIn0, unsigned char *pOut0)
1524 {
1525     const int numpix = width * height;
1526     const int bytes = 24 >> 3;
1527     int i, j, y00, y01, y10, y11, u, v;
1528     unsigned char *pY = pIn0;
1529     unsigned char *pU = pY + numpix;
1530     unsigned char *pV = pU + numpix / 4;
1531     unsigned char *pOut = pOut0;
1532
1533     for (j = 0; j <= height - 2; j += 2) {
1534         for (i = 0; i <= width - 2; i += 2) {
1535             y00 = *pY;
1536             y01 = *(pY + 1);
1537             y10 = *(pY + width);
1538             y11 = *(pY + width + 1);
1539             u = (*pU++) - 128;
1540             v = (*pV++) - 128;
1541
1542             move_420_block(y00, y01, y10, y11, u, v,
1543                        width, pOut);
1544
1545             pY += 2;
1546             pOut += 2 * bytes;
1547
1548         }
1549         pY += width;
1550         pOut += width * bytes;
1551     }
1552 }
1553
1554 // Consider a YUV420 image of 6x2 pixels.
1555 //
1556 // A B C D U1 U2
1557 // I J K L V1 V2
1558 //
1559 // The U1/V1 samples correspond to the ABIJ pixels.
1560 //     U2/V2 samples correspond to the CDKL pixels.
1561 //
1562 /* Converts from interlaced YUV420 to RGB24. */
1563 /* [FD] untested... */
1564 #ifdef HAVE_CAMV4L
1565 static void
1566 yuv420_to_rgb24(int width, int height,
1567         unsigned char *pIn0, unsigned char *pOut0)
1568 {
1569     const int bytes = 24 >> 3;
1570     int i, j, y00, y01, y10, y11, u, v;
1571     unsigned char *pY = pIn0;
1572     unsigned char *pU = pY + 4;
1573     unsigned char *pV = pU + width;
1574     unsigned char *pOut = pOut0;
1575
1576     for (j = 0; j <= height - 2; j += 2) {
1577         for (i = 0; i <= width - 4; i += 4) {
1578             y00 = *pY;
1579             y01 = *(pY + 1);
1580             y10 = *(pY + width);
1581             y11 = *(pY + width + 1);
1582             u = (*pU++) - 128;
1583             v = (*pV++) - 128;
1584
1585             move_420_block(y00, y01, y10, y11, u, v,
1586                        width, pOut);
1587
1588             pY += 2;
1589             pOut += 2 * bytes;
1590
1591             y00 = *pY;
1592             y01 = *(pY + 1);
1593             y10 = *(pY + width);
1594             y11 = *(pY + width + 1);
1595             u = (*pU++) - 128;
1596             v = (*pV++) - 128;
1597
1598             move_420_block(y00, y01, y10, y11, u, v,
1599                        width, pOut);
1600
1601             pY += 4; // skip UV
1602             pOut += 2 * bytes;
1603
1604         }
1605         pY += width;
1606         pOut += width * bytes;
1607     }
1608 }
1609 #endif //HAVE_CAMV4L
1610
1611 // Consider a YUV411P image of 8x2 pixels.
1612 //
1613 // A plane of Y values as before.
1614 //
1615 // A plane of U values    1       2
1616 //                        3       4
1617 //
1618 // A plane of V values    1       2
1619 //                        3       4
1620 //
1621 // The U1/V1 samples correspond to the ABCD pixels.
1622 //     U2/V2 samples correspond to the EFGH pixels.
1623 //
1624 /* Converts from planar YUV411P to RGB24. */
1625 /* [FD] untested... */
1626 static void
1627 yuv411p_to_rgb24(int width, int height,
1628            unsigned char *pIn0, unsigned char *pOut0)
1629 {
1630     const int numpix = width * height;
1631     const int bytes = 24 >> 3;
1632     int i, j, y00, y01, y10, y11, u, v;
1633     unsigned char *pY = pIn0;
1634     unsigned char *pU = pY + numpix;
1635     unsigned char *pV = pU + numpix / 4;
1636     unsigned char *pOut = pOut0;
1637
1638     for (j = 0; j <= height; j++) {
1639         for (i = 0; i <= width - 4; i += 4) {
1640             y00 = *pY;
1641             y01 = *(pY + 1);
1642             y10 = *(pY + 2);
1643             y11 = *(pY + 3);
1644             u = (*pU++) - 128;
1645             v = (*pV++) - 128;
1646
1647             move_411_block(y00, y01, y10, y11, u, v,
1648                        width, pOut);
1649
1650             pY += 4;
1651             pOut += 4 * bytes;
1652
1653         }
1654     }
1655 }
1656
1657 /* convert from 4:2:2 YUYV interlaced to RGB24 */
1658 /* based on ccvt_yuyv_bgr32() from camstream */
1659 #define SAT(c) \
1660         if (c & (~255)) { if (c < 0) c = 0; else c = 255; }
1661
1662 #ifdef HAVE_CAMV4L2
1663 static void
1664 yuyv_to_rgb24 (int width, int height, unsigned char *src, unsigned char *dst)
1665 {
1666    unsigned char *s;
1667    unsigned char *d;
1668    int l, c;
1669    int r, g, b, cr, cg, cb, y1, y2;
1670
1671    l = height;
1672    s = src;
1673    d = dst;
1674    while (l--) {
1675       c = width >> 1;
1676       while (c--) {
1677          y1 = *s++;
1678          cb = ((*s - 128) * 454) >> 8;
1679          cg = (*s++ - 128) * 88;
1680          y2 = *s++;
1681          cr = ((*s - 128) * 359) >> 8;
1682          cg = (cg + (*s++ - 128) * 183) >> 8;
1683
1684          r = y1 + cr;
1685          b = y1 + cb;
1686          g = y1 - cg;
1687          SAT(r);
1688          SAT(g);
1689          SAT(b);
1690
1691      *d++ = b;
1692      *d++ = g;
1693      *d++ = r;
1694
1695          r = y2 + cr;
1696          b = y2 + cb;
1697          g = y2 - cg;
1698          SAT(r);
1699          SAT(g);
1700          SAT(b);
1701
1702      *d++ = b;
1703      *d++ = g;
1704      *d++ = r;
1705       }
1706    }
1707 }
1708
1709 static void
1710 uyvy_to_rgb24 (int width, int height, unsigned char *src, unsigned char *dst)
1711 {
1712    unsigned char *s;
1713    unsigned char *d;
1714    int l, c;
1715    int r, g, b, cr, cg, cb, y1, y2;
1716
1717    l = height;
1718    s = src;
1719    d = dst;
1720    while (l--) {
1721       c = width >> 1;
1722       while (c--) {
1723          cb = ((*s - 128) * 454) >> 8;
1724          cg = (*s++ - 128) * 88;
1725          y1 = *s++;
1726          cr = ((*s - 128) * 359) >> 8;
1727          cg = (cg + (*s++ - 128) * 183) >> 8;
1728          y2 = *s++;
1729
1730          r = y1 + cr;
1731          b = y1 + cb;
1732          g = y1 - cg;
1733          SAT(r);
1734          SAT(g);
1735          SAT(b);
1736
1737      *d++ = b;
1738      *d++ = g;
1739      *d++ = r;
1740
1741          r = y2 + cr;
1742          b = y2 + cb;
1743          g = y2 - cg;
1744          SAT(r);
1745          SAT(g);
1746          SAT(b);
1747
1748      *d++ = b;
1749      *d++ = g;
1750      *d++ = r;
1751       }
1752    }
1753 }
1754 #endif //HAVE_CAMV4L2
1755
1756 #ifdef HAVE_JPEG
1757
1758 /* convert from mjpeg to rgb24 */
1759 static bool
1760 mjpeg_to_rgb24 (int width, int height,
1761         unsigned char *src, int length,
1762         unsigned char *dst)
1763 {
1764   cv::Mat temp=cv::imdecode(cv::Mat(std::vector<uchar>(src, src + length)), 1);
1765   if( !temp.data || temp.cols != width || temp.rows != height )
1766     return false;
1767   memcpy(dst, temp.data, width*height*3);
1768   return true;
1769 }
1770
1771 #endif
1772
1773 /*
1774  * BAYER2RGB24 ROUTINE TAKEN FROM:
1775  *
1776  * Sonix SN9C10x based webcam basic I/F routines
1777  * Takafumi Mizuno <taka-qce@ls-a.jp>
1778  *
1779  */
1780
1781 #ifdef HAVE_CAMV4L2
1782 static void bayer2rgb24(long int WIDTH, long int HEIGHT, unsigned char *src, unsigned char *dst)
1783 {
1784     long int i;
1785     unsigned char *rawpt, *scanpt;
1786     long int size;
1787
1788     rawpt = src;
1789     scanpt = dst;
1790     size = WIDTH*HEIGHT;
1791
1792     for ( i = 0; i < size; i++ ) {
1793   if ( (i/WIDTH) % 2 == 0 ) {
1794       if ( (i % 2) == 0 ) {
1795     /* B */
1796     if ( (i > WIDTH) && ((i % WIDTH) > 0) ) {
1797         *scanpt++ = (*(rawpt-WIDTH-1)+*(rawpt-WIDTH+1)+
1798          *(rawpt+WIDTH-1)+*(rawpt+WIDTH+1))/4;  /* R */
1799         *scanpt++ = (*(rawpt-1)+*(rawpt+1)+
1800          *(rawpt+WIDTH)+*(rawpt-WIDTH))/4;      /* G */
1801         *scanpt++ = *rawpt;                                     /* B */
1802     } else {
1803         /* first line or left column */
1804         *scanpt++ = *(rawpt+WIDTH+1);           /* R */
1805         *scanpt++ = (*(rawpt+1)+*(rawpt+WIDTH))/2;      /* G */
1806         *scanpt++ = *rawpt;                             /* B */
1807     }
1808       } else {
1809     /* (B)G */
1810     if ( (i > WIDTH) && ((i % WIDTH) < (WIDTH-1)) ) {
1811         *scanpt++ = (*(rawpt+WIDTH)+*(rawpt-WIDTH))/2;  /* R */
1812         *scanpt++ = *rawpt;                                     /* G */
1813         *scanpt++ = (*(rawpt-1)+*(rawpt+1))/2;          /* B */
1814     } else {
1815         /* first line or right column */
1816         *scanpt++ = *(rawpt+WIDTH);     /* R */
1817         *scanpt++ = *rawpt;             /* G */
1818         *scanpt++ = *(rawpt-1); /* B */
1819     }
1820       }
1821   } else {
1822       if ( (i % 2) == 0 ) {
1823     /* G(R) */
1824     if ( (i < (WIDTH*(HEIGHT-1))) && ((i % WIDTH) > 0) ) {
1825         *scanpt++ = (*(rawpt-1)+*(rawpt+1))/2;          /* R */
1826         *scanpt++ = *rawpt;                                     /* G */
1827         *scanpt++ = (*(rawpt+WIDTH)+*(rawpt-WIDTH))/2;  /* B */
1828     } else {
1829         /* bottom line or left column */
1830         *scanpt++ = *(rawpt+1);         /* R */
1831         *scanpt++ = *rawpt;                     /* G */
1832         *scanpt++ = *(rawpt-WIDTH);             /* B */
1833     }
1834       } else {
1835     /* R */
1836     if ( i < (WIDTH*(HEIGHT-1)) && ((i % WIDTH) < (WIDTH-1)) ) {
1837         *scanpt++ = *rawpt;                                     /* R */
1838         *scanpt++ = (*(rawpt-1)+*(rawpt+1)+
1839          *(rawpt-WIDTH)+*(rawpt+WIDTH))/4;      /* G */
1840         *scanpt++ = (*(rawpt-WIDTH-1)+*(rawpt-WIDTH+1)+
1841          *(rawpt+WIDTH-1)+*(rawpt+WIDTH+1))/4;  /* B */
1842     } else {
1843         /* bottom line or right column */
1844         *scanpt++ = *rawpt;                             /* R */
1845         *scanpt++ = (*(rawpt-1)+*(rawpt-WIDTH))/2;      /* G */
1846         *scanpt++ = *(rawpt-WIDTH-1);           /* B */
1847     }
1848       }
1849   }
1850   rawpt++;
1851     }
1852
1853 }
1854
1855 // SGBRG to RGB24
1856 // for some reason, red and blue needs to be swapped
1857 // at least for  046d:092f Logitech, Inc. QuickCam Express Plus to work
1858 //see: http://www.siliconimaging.com/RGB%20Bayer.htm
1859 //and 4.6 at http://tldp.org/HOWTO/html_single/libdc1394-HOWTO/
1860 static void sgbrg2rgb24(long int WIDTH, long int HEIGHT, unsigned char *src, unsigned char *dst)
1861 {
1862     long int i;
1863     unsigned char *rawpt, *scanpt;
1864     long int size;
1865
1866     rawpt = src;
1867     scanpt = dst;
1868     size = WIDTH*HEIGHT;
1869
1870     for ( i = 0; i < size; i++ )
1871     {
1872         if ( (i/WIDTH) % 2 == 0 ) //even row
1873         {
1874             if ( (i % 2) == 0 ) //even pixel
1875             {
1876                 if ( (i > WIDTH) && ((i % WIDTH) > 0) )
1877                 {
1878                     *scanpt++ = (*(rawpt-1)+*(rawpt+1))/2;       /* R */
1879                     *scanpt++ = *(rawpt);                        /* G */
1880                     *scanpt++ = (*(rawpt-WIDTH) + *(rawpt+WIDTH))/2;      /* B */
1881                 } else
1882                 {
1883                   /* first line or left column */
1884
1885                   *scanpt++ = *(rawpt+1);           /* R */
1886                   *scanpt++ = *(rawpt);             /* G */
1887                   *scanpt++ =  *(rawpt+WIDTH);      /* B */
1888                 }
1889             } else //odd pixel
1890             {
1891                 if ( (i > WIDTH) && ((i % WIDTH) < (WIDTH-1)) )
1892                 {
1893                     *scanpt++ = *(rawpt);       /* R */
1894                     *scanpt++ = (*(rawpt-1)+*(rawpt+1)+*(rawpt-WIDTH)+*(rawpt+WIDTH))/4; /* G */
1895                     *scanpt++ = (*(rawpt-WIDTH-1) + *(rawpt-WIDTH+1) + *(rawpt+WIDTH-1) + *(rawpt+WIDTH+1))/4;      /* B */
1896                 } else
1897                 {
1898                     /* first line or right column */
1899
1900                     *scanpt++ = *(rawpt);       /* R */
1901                     *scanpt++ = (*(rawpt-1)+*(rawpt+WIDTH))/2; /* G */
1902                     *scanpt++ = *(rawpt+WIDTH-1);      /* B */
1903                 }
1904             }
1905         } else
1906         { //odd row
1907             if ( (i % 2) == 0 ) //even pixel
1908             {
1909                 if ( (i < (WIDTH*(HEIGHT-1))) && ((i % WIDTH) > 0) )
1910                 {
1911                     *scanpt++ =  (*(rawpt-WIDTH-1)+*(rawpt-WIDTH+1)+*(rawpt+WIDTH-1)+*(rawpt+WIDTH+1))/4;          /* R */
1912                     *scanpt++ =  (*(rawpt-1)+*(rawpt+1)+*(rawpt-WIDTH)+*(rawpt+WIDTH))/4;      /* G */
1913                     *scanpt++ =  *(rawpt); /* B */
1914                 } else
1915                 {
1916                     /* bottom line or left column */
1917
1918                     *scanpt++ =  *(rawpt-WIDTH+1);          /* R */
1919                     *scanpt++ =  (*(rawpt+1)+*(rawpt-WIDTH))/2;      /* G */
1920                     *scanpt++ =  *(rawpt); /* B */
1921                 }
1922             } else
1923             { //odd pixel
1924                 if ( i < (WIDTH*(HEIGHT-1)) && ((i % WIDTH) < (WIDTH-1)) )
1925                 {
1926                     *scanpt++ = (*(rawpt-WIDTH)+*(rawpt+WIDTH))/2;  /* R */
1927                     *scanpt++ = *(rawpt);      /* G */
1928                     *scanpt++ = (*(rawpt-1)+*(rawpt+1))/2; /* B */
1929                 } else
1930                 {
1931                     /* bottom line or right column */
1932
1933                     *scanpt++ = (*(rawpt-WIDTH));  /* R */
1934                     *scanpt++ = *(rawpt);      /* G */
1935                     *scanpt++ = (*(rawpt-1)); /* B */
1936                 }
1937             }
1938         }
1939         rawpt++;
1940     }
1941 }
1942
1943 #define CLAMP(x)        ((x)<0?0:((x)>255)?255:(x))
1944
1945 typedef struct {
1946   int is_abs;
1947   int len;
1948   int val;
1949 } code_table_t;
1950
1951
1952 /* local storage */
1953 static code_table_t table[256];
1954 static int init_done = 0;
1955
1956
1957 /*
1958   sonix_decompress_init
1959   =====================
1960     pre-calculates a locally stored table for efficient huffman-decoding.
1961
1962   Each entry at index x in the table represents the codeword
1963   present at the MSB of byte x.
1964
1965 */
1966 static void sonix_decompress_init(void)
1967 {
1968   int i;
1969   int is_abs, val, len;
1970
1971   for (i = 0; i < 256; i++) {
1972     is_abs = 0;
1973     val = 0;
1974     len = 0;
1975     if ((i & 0x80) == 0) {
1976       /* code 0 */
1977       val = 0;
1978       len = 1;
1979     }
1980     else if ((i & 0xE0) == 0x80) {
1981       /* code 100 */
1982       val = +4;
1983       len = 3;
1984     }
1985     else if ((i & 0xE0) == 0xA0) {
1986       /* code 101 */
1987       val = -4;
1988       len = 3;
1989     }
1990     else if ((i & 0xF0) == 0xD0) {
1991       /* code 1101 */
1992       val = +11;
1993       len = 4;
1994     }
1995     else if ((i & 0xF0) == 0xF0) {
1996       /* code 1111 */
1997       val = -11;
1998       len = 4;
1999     }
2000     else if ((i & 0xF8) == 0xC8) {
2001       /* code 11001 */
2002       val = +20;
2003       len = 5;
2004     }
2005     else if ((i & 0xFC) == 0xC0) {
2006       /* code 110000 */
2007       val = -20;
2008       len = 6;
2009     }
2010     else if ((i & 0xFC) == 0xC4) {
2011       /* code 110001xx: unknown */
2012       val = 0;
2013       len = 8;
2014     }
2015     else if ((i & 0xF0) == 0xE0) {
2016       /* code 1110xxxx */
2017       is_abs = 1;
2018       val = (i & 0x0F) << 4;
2019       len = 8;
2020     }
2021     table[i].is_abs = is_abs;
2022     table[i].val = val;
2023     table[i].len = len;
2024   }
2025
2026   init_done = 1;
2027 }
2028
2029
2030 /*
2031   sonix_decompress
2032   ================
2033     decompresses an image encoded by a SN9C101 camera controller chip.
2034
2035   IN    width
2036     height
2037     inp         pointer to compressed frame (with header already stripped)
2038   OUT   outp    pointer to decompressed frame
2039
2040   Returns 0 if the operation was successful.
2041   Returns <0 if operation failed.
2042
2043 */
2044 static int sonix_decompress(int width, int height, unsigned char *inp, unsigned char *outp)
2045 {
2046   int row, col;
2047   int val;
2048   int bitpos;
2049   unsigned char code;
2050   unsigned char *addr;
2051
2052   if (!init_done) {
2053     /* do sonix_decompress_init first! */
2054     return -1;
2055   }
2056
2057   bitpos = 0;
2058   for (row = 0; row < height; row++) {
2059
2060     col = 0;
2061
2062
2063
2064     /* first two pixels in first two rows are stored as raw 8-bit */
2065     if (row < 2) {
2066       addr = inp + (bitpos >> 3);
2067       code = (addr[0] << (bitpos & 7)) | (addr[1] >> (8 - (bitpos & 7)));
2068       bitpos += 8;
2069       *outp++ = code;
2070
2071       addr = inp + (bitpos >> 3);
2072       code = (addr[0] << (bitpos & 7)) | (addr[1] >> (8 - (bitpos & 7)));
2073       bitpos += 8;
2074       *outp++ = code;
2075
2076       col += 2;
2077     }
2078
2079     while (col < width) {
2080       /* get bitcode from bitstream */
2081       addr = inp + (bitpos >> 3);
2082       code = (addr[0] << (bitpos & 7)) | (addr[1] >> (8 - (bitpos & 7)));
2083
2084       /* update bit position */
2085       bitpos += table[code].len;
2086
2087       /* calculate pixel value */
2088       val = table[code].val;
2089       if (!table[code].is_abs) {
2090         /* value is relative to top and left pixel */
2091         if (col < 2) {
2092           /* left column: relative to top pixel */
2093           val += outp[-2*width];
2094         }
2095         else if (row < 2) {
2096           /* top row: relative to left pixel */
2097           val += outp[-2];
2098         }
2099         else {
2100           /* main area: average of left pixel and top pixel */
2101           val += (outp[-2] + outp[-2*width]) / 2;
2102         }
2103       }
2104
2105       /* store pixel */
2106       *outp++ = CLAMP(val);
2107       col++;
2108     }
2109   }
2110
2111   return 0;
2112 }
2113 #endif //HAVE_CAMV4L2
2114
2115 static IplImage* icvRetrieveFrameCAM_V4L( CvCaptureCAM_V4L* capture, int) {
2116
2117 #ifdef HAVE_CAMV4L2
2118   if (V4L2_SUPPORT == 0)
2119 #endif /* HAVE_CAMV4L2 */
2120 #ifdef HAVE_CAMV4L
2121   {
2122
2123     /* [FD] this really belongs here */
2124     if (ioctl(capture->deviceHandle, VIDIOCSYNC, &capture->mmaps[capture->bufferIndex].frame) == -1) {
2125       fprintf( stderr, "HIGHGUI ERROR: V4L: Could not SYNC to video stream. %s\n", strerror(errno));
2126     }
2127
2128   }
2129 #endif /* HAVE_CAMV4L */
2130
2131    /* Now get what has already been captured as a IplImage return */
2132
2133    /* First, reallocate imageData if the frame size changed */
2134
2135 #ifdef HAVE_CAMV4L2
2136
2137   if (V4L2_SUPPORT == 1)
2138   {
2139
2140     if(((unsigned long)capture->frame.width != capture->form.fmt.pix.width)
2141        || ((unsigned long)capture->frame.height != capture->form.fmt.pix.height)) {
2142         cvFree(&capture->frame.imageData);
2143         cvInitImageHeader( &capture->frame,
2144               cvSize( capture->form.fmt.pix.width,
2145                   capture->form.fmt.pix.height ),
2146               IPL_DEPTH_8U, 3, IPL_ORIGIN_TL, 4 );
2147        capture->frame.imageData = (char *)cvAlloc(capture->frame.imageSize);
2148     }
2149
2150   }
2151 #endif /* HAVE_CAMV4L2 */
2152 #if defined(HAVE_CAMV4L) && defined(HAVE_CAMV4L2)
2153     else
2154 #endif /* HAVE_CAMV4L && HAVE_CAMV4L2 */
2155 #ifdef HAVE_CAMV4L
2156   {
2157
2158     if((capture->frame.width != capture->mmaps[capture->bufferIndex].width)
2159       || (capture->frame.height != capture->mmaps[capture->bufferIndex].height)) {
2160        cvFree(&capture->frame.imageData);
2161        cvInitImageHeader( &capture->frame,
2162               cvSize( capture->captureWindow.width,
2163                   capture->captureWindow.height ),
2164               IPL_DEPTH_8U, 3, IPL_ORIGIN_TL, 4 );
2165        capture->frame.imageData = (char *)cvAlloc(capture->frame.imageSize);
2166     }
2167
2168   }
2169 #endif /* HAVE_CAMV4L */
2170
2171 #ifdef HAVE_CAMV4L2
2172
2173   if (V4L2_SUPPORT == 1)
2174   {
2175     switch (capture->palette)
2176     {
2177     case PALETTE_BGR24:
2178         memcpy((char *)capture->frame.imageData,
2179                (char *)capture->buffers[capture->bufferIndex].start,
2180                capture->frame.imageSize);
2181         break;
2182
2183     case PALETTE_YVU420:
2184         yuv420p_to_rgb24(capture->form.fmt.pix.width,
2185                  capture->form.fmt.pix.height,
2186                  (unsigned char*)(capture->buffers[capture->bufferIndex].start),
2187                  (unsigned char*)capture->frame.imageData);
2188         break;
2189
2190     case PALETTE_YUV411P:
2191         yuv411p_to_rgb24(capture->form.fmt.pix.width,
2192                  capture->form.fmt.pix.height,
2193                  (unsigned char*)(capture->buffers[capture->bufferIndex].start),
2194                  (unsigned char*)capture->frame.imageData);
2195         break;
2196 #ifdef HAVE_JPEG
2197     case PALETTE_MJPEG:
2198         if (!mjpeg_to_rgb24(capture->form.fmt.pix.width,
2199                     capture->form.fmt.pix.height,
2200                     (unsigned char*)(capture->buffers[capture->bufferIndex]
2201                              .start),
2202                     capture->buffers[capture->bufferIndex].length,
2203                     (unsigned char*)capture->frame.imageData))
2204           return 0;
2205         break;
2206 #endif
2207
2208     case PALETTE_YUYV:
2209         yuyv_to_rgb24(capture->form.fmt.pix.width,
2210                   capture->form.fmt.pix.height,
2211                   (unsigned char*)(capture->buffers[capture->bufferIndex].start),
2212                   (unsigned char*)capture->frame.imageData);
2213         break;
2214     case PALETTE_UYVY:
2215         uyvy_to_rgb24(capture->form.fmt.pix.width,
2216                   capture->form.fmt.pix.height,
2217                   (unsigned char*)(capture->buffers[capture->bufferIndex].start),
2218                   (unsigned char*)capture->frame.imageData);
2219         break;
2220     case PALETTE_SBGGR8:
2221         bayer2rgb24(capture->form.fmt.pix.width,
2222                 capture->form.fmt.pix.height,
2223                 (unsigned char*)capture->buffers[capture->bufferIndex].start,
2224                 (unsigned char*)capture->frame.imageData);
2225         break;
2226
2227     case PALETTE_SN9C10X:
2228         sonix_decompress_init();
2229         sonix_decompress(capture->form.fmt.pix.width,
2230                  capture->form.fmt.pix.height,
2231                  (unsigned char*)capture->buffers[capture->bufferIndex].start,
2232                  (unsigned char*)capture->buffers[(capture->bufferIndex+1) % capture->req.count].start);
2233
2234         bayer2rgb24(capture->form.fmt.pix.width,
2235                 capture->form.fmt.pix.height,
2236                 (unsigned char*)capture->buffers[(capture->bufferIndex+1) % capture->req.count].start,
2237                 (unsigned char*)capture->frame.imageData);
2238         break;
2239
2240     case PALETTE_SGBRG:
2241         sgbrg2rgb24(capture->form.fmt.pix.width,
2242                 capture->form.fmt.pix.height,
2243                 (unsigned char*)capture->buffers[(capture->bufferIndex+1) % capture->req.count].start,
2244                 (unsigned char*)capture->frame.imageData);
2245         break;
2246     }
2247   }
2248 #endif /* HAVE_CAMV4L2 */
2249 #if defined(HAVE_CAMV4L) && defined(HAVE_CAMV4L2)
2250     else
2251 #endif /* HAVE_CAMV4L && HAVE_CAMV4L2 */
2252 #ifdef HAVE_CAMV4L
2253   {
2254
2255     switch(capture->imageProperties.palette)
2256     {
2257     case VIDEO_PALETTE_RGB24:
2258         memcpy((char *)capture->frame.imageData,
2259            (char *)(capture->memoryMap + capture->memoryBuffer.offsets[capture->bufferIndex]),
2260            capture->frame.imageSize);
2261         break;
2262     case VIDEO_PALETTE_YUV420P:
2263         yuv420p_to_rgb24(capture->captureWindow.width,
2264              capture->captureWindow.height,
2265              (unsigned char*)(capture->memoryMap + capture->memoryBuffer.offsets[capture->bufferIndex]),
2266              (unsigned char*)capture->frame.imageData);
2267         break;
2268     case VIDEO_PALETTE_YUV420:
2269         yuv420_to_rgb24(capture->captureWindow.width,
2270           capture->captureWindow.height,
2271           (unsigned char*)(capture->memoryMap + capture->memoryBuffer.offsets[capture->bufferIndex]),
2272           (unsigned char*)capture->frame.imageData);
2273         break;
2274     case VIDEO_PALETTE_YUV411P:
2275         yuv411p_to_rgb24(capture->captureWindow.width,
2276           capture->captureWindow.height,
2277           (unsigned char*)(capture->memoryMap + capture->memoryBuffer.offsets[capture->bufferIndex]),
2278           (unsigned char*)capture->frame.imageData);
2279         break;
2280     default:
2281         fprintf( stderr,
2282                  "HIGHGUI ERROR: V4L: Cannot convert from palette %d to RGB\n",
2283                  capture->imageProperties.palette);
2284
2285         return 0;
2286     }
2287
2288   }
2289 #endif /* HAVE_CAMV4L */
2290
2291    return(&capture->frame);
2292 }
2293
2294 static double icvGetPropertyCAM_V4L (CvCaptureCAM_V4L* capture,
2295                                      int property_id ) {
2296
2297 #ifdef HAVE_CAMV4L2
2298
2299 #ifdef HAVE_CAMV4L
2300   if (V4L2_SUPPORT == 1)
2301 #endif
2302   {
2303
2304       /* default value for min and max */
2305       int v4l2_min = 0;
2306       int v4l2_max = 255;
2307
2308       CLEAR (capture->form);
2309       capture->form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
2310       if (-1 == ioctl (capture->deviceHandle, VIDIOC_G_FMT, &capture->form)) {
2311           /* display an error message, and return an error code */
2312           perror ("VIDIOC_G_FMT");
2313           return -1;
2314       }
2315
2316       switch (property_id) {
2317       case CV_CAP_PROP_FRAME_WIDTH:
2318           return capture->form.fmt.pix.width;
2319       case CV_CAP_PROP_FRAME_HEIGHT:
2320           return capture->form.fmt.pix.height;
2321       }
2322
2323       /* initialize the control structure */
2324
2325       switch (property_id) {
2326       case CV_CAP_PROP_POS_MSEC:
2327           if (capture->FirstCapture) {
2328             return 0;
2329           } else {
2330             return 1000 * capture->timestamp.tv_sec + ((double) capture->timestamp.tv_usec) / 1000;
2331           }
2332           break;
2333       case CV_CAP_PROP_BRIGHTNESS:
2334           capture->control.id = V4L2_CID_BRIGHTNESS;
2335           break;
2336       case CV_CAP_PROP_CONTRAST:
2337           capture->control.id = V4L2_CID_CONTRAST;
2338           break;
2339       case CV_CAP_PROP_SATURATION:
2340           capture->control.id = V4L2_CID_SATURATION;
2341           break;
2342       case CV_CAP_PROP_HUE:
2343           capture->control.id = V4L2_CID_HUE;
2344           break;
2345       case CV_CAP_PROP_GAIN:
2346           capture->control.id = V4L2_CID_GAIN;
2347           break;
2348       case CV_CAP_PROP_EXPOSURE:
2349           capture->control.id = V4L2_CID_EXPOSURE;
2350           break;
2351       default:
2352         fprintf(stderr,
2353                 "HIGHGUI ERROR: V4L2: getting property #%d is not supported\n",
2354                 property_id);
2355         return -1;
2356       }
2357
2358       if (-1 == ioctl (capture->deviceHandle, VIDIOC_G_CTRL,
2359                         &capture->control)) {
2360
2361           fprintf( stderr, "HIGHGUI ERROR: V4L2: ");
2362           switch (property_id) {
2363           case CV_CAP_PROP_BRIGHTNESS:
2364               fprintf (stderr, "Brightness");
2365               break;
2366           case CV_CAP_PROP_CONTRAST:
2367               fprintf (stderr, "Contrast");
2368               break;
2369           case CV_CAP_PROP_SATURATION:
2370               fprintf (stderr, "Saturation");
2371               break;
2372           case CV_CAP_PROP_HUE:
2373               fprintf (stderr, "Hue");
2374               break;
2375           case CV_CAP_PROP_GAIN:
2376               fprintf (stderr, "Gain");
2377               break;
2378           case CV_CAP_PROP_EXPOSURE:
2379               fprintf (stderr, "Exposure");
2380               break;
2381           }
2382           fprintf (stderr, " is not supported by your device\n");
2383
2384           return -1;
2385       }
2386
2387       /* get the min/max values */
2388       switch (property_id) {
2389
2390       case CV_CAP_PROP_BRIGHTNESS:
2391           v4l2_min = capture->v4l2_brightness_min;
2392           v4l2_max = capture->v4l2_brightness_max;
2393           break;
2394       case CV_CAP_PROP_CONTRAST:
2395           v4l2_min = capture->v4l2_contrast_min;
2396           v4l2_max = capture->v4l2_contrast_max;
2397           break;
2398       case CV_CAP_PROP_SATURATION:
2399           v4l2_min = capture->v4l2_saturation_min;
2400           v4l2_max = capture->v4l2_saturation_max;
2401           break;
2402       case CV_CAP_PROP_HUE:
2403           v4l2_min = capture->v4l2_hue_min;
2404           v4l2_max = capture->v4l2_hue_max;
2405           break;
2406       case CV_CAP_PROP_GAIN:
2407           v4l2_min = capture->v4l2_gain_min;
2408           v4l2_max = capture->v4l2_gain_max;
2409           break;
2410       case CV_CAP_PROP_EXPOSURE:
2411           v4l2_min = capture->v4l2_exposure_min;
2412           v4l2_max = capture->v4l2_exposure_max;
2413           break;
2414       }
2415
2416       /* all was OK, so convert to 0.0 - 1.0 range, and return the value */
2417       return ((float)capture->control.value - v4l2_min + 1) / (v4l2_max - v4l2_min);
2418
2419   }
2420 #endif /* HAVE_CAMV4L2 */
2421 #if defined(HAVE_CAMV4L) && defined(HAVE_CAMV4L2)
2422     else
2423 #endif /* HAVE_CAMV4L && HAVE_CAMV4L2 */
2424 #ifdef HAVE_CAMV4L
2425   {
2426
2427     int retval = -1;
2428
2429     if (ioctl (capture->deviceHandle,
2430                VIDIOCGWIN, &capture->captureWindow) < 0) {
2431         fprintf (stderr,
2432                  "HIGHGUI ERROR: V4L: "
2433                  "Unable to determine size of incoming image\n");
2434         icvCloseCAM_V4L(capture);
2435         return -1;
2436     }
2437
2438     switch (property_id) {
2439     case CV_CAP_PROP_FRAME_WIDTH:
2440         retval = capture->captureWindow.width;
2441         break;
2442     case CV_CAP_PROP_FRAME_HEIGHT:
2443         retval = capture->captureWindow.height;
2444         break;
2445     case CV_CAP_PROP_BRIGHTNESS:
2446         retval = capture->imageProperties.brightness;
2447         break;
2448     case CV_CAP_PROP_CONTRAST:
2449         retval = capture->imageProperties.contrast;
2450         break;
2451     case CV_CAP_PROP_SATURATION:
2452         retval = capture->imageProperties.colour;
2453         break;
2454     case CV_CAP_PROP_HUE:
2455         retval = capture->imageProperties.hue;
2456         break;
2457     case CV_CAP_PROP_GAIN:
2458         fprintf(stderr,
2459                 "HIGHGUI ERROR: V4L: Gain control in V4L is not supported\n");
2460         return -1;
2461         break;
2462     case CV_CAP_PROP_EXPOSURE:
2463         fprintf(stderr,
2464                 "HIGHGUI ERROR: V4L: Exposure control in V4L is not supported\n");
2465         return -1;
2466         break;
2467     default:
2468         fprintf(stderr,
2469                 "HIGHGUI ERROR: V4L: getting property #%d is not supported\n",
2470                 property_id);
2471     }
2472
2473     if (retval == -1) {
2474         /* there was a problem */
2475         return -1;
2476     }
2477
2478     /* all was OK, so convert to 0.0 - 1.0 range, and return the value */
2479     return float (retval) / 0xFFFF;
2480
2481   }
2482 #endif /* HAVE_CAMV4L */
2483
2484 };
2485
2486 static int icvSetVideoSize( CvCaptureCAM_V4L* capture, int w, int h) {
2487
2488 #ifdef HAVE_CAMV4L2
2489
2490   if (V4L2_SUPPORT == 1)
2491   {
2492
2493     CLEAR (capture->cropcap);
2494     capture->cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
2495
2496     if (ioctl (capture->deviceHandle, VIDIOC_CROPCAP, &capture->cropcap) < 0) {
2497         fprintf(stderr, "HIGHGUI ERROR: V4L/V4L2: VIDIOC_CROPCAP\n");
2498     } else {
2499
2500         CLEAR (capture->crop);
2501         capture->crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
2502         capture->crop.c= capture->cropcap.defrect;
2503
2504         /* set the crop area, but don't exit if the device don't support croping */
2505         if (ioctl (capture->deviceHandle, VIDIOC_S_CROP, &capture->crop) < 0) {
2506             fprintf(stderr, "HIGHGUI ERROR: V4L/V4L2: VIDIOC_S_CROP\n");
2507         }
2508     }
2509
2510     CLEAR (capture->form);
2511     capture->form.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
2512
2513     /* read the current setting, mainly to retreive the pixelformat information */
2514     ioctl (capture->deviceHandle, VIDIOC_G_FMT, &capture->form);
2515
2516     /* set the values we want to change */
2517     capture->form.fmt.pix.width = w;
2518     capture->form.fmt.pix.height = h;
2519     capture->form.fmt.win.chromakey = 0;
2520     capture->form.fmt.win.field = V4L2_FIELD_ANY;
2521     capture->form.fmt.win.clips = 0;
2522     capture->form.fmt.win.clipcount = 0;
2523     capture->form.fmt.pix.field = V4L2_FIELD_ANY;
2524
2525     /* ask the device to change the size
2526      * don't test if the set of the size is ok, because some device
2527      * don't allow changing the size, and we will get the real size
2528      * later */
2529     ioctl (capture->deviceHandle, VIDIOC_S_FMT, &capture->form);
2530
2531     /* try to set framerate to 30 fps */
2532     struct v4l2_streamparm setfps;
2533     memset (&setfps, 0, sizeof(struct v4l2_streamparm));
2534     setfps.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
2535     setfps.parm.capture.timeperframe.numerator = 1;
2536     setfps.parm.capture.timeperframe.denominator = 30;
2537     ioctl (capture->deviceHandle, VIDIOC_S_PARM, &setfps);
2538
2539     /* we need to re-initialize some things, like buffers, because the size has
2540      * changed */
2541     capture->FirstCapture = 1;
2542
2543     /* Get window info again, to get the real value */
2544     if (-1 == ioctl (capture->deviceHandle, VIDIOC_G_FMT, &capture->form))
2545     {
2546       fprintf(stderr, "HIGHGUI ERROR: V4L/V4L2: Could not obtain specifics of capture window.\n\n");
2547
2548       icvCloseCAM_V4L(capture);
2549
2550       return 0;
2551     }
2552
2553     return 0;
2554
2555   }
2556 #endif /* HAVE_CAMV4L2 */
2557 #if defined(HAVE_CAMV4L) && defined(HAVE_CAMV4L2)
2558     else
2559 #endif /* HAVE_CAMV4L && HAVE_CAMV4L2 */
2560 #ifdef HAVE_CAMV4L
2561   {
2562
2563     if (capture==0) return 0;
2564      if (w>capture->capability.maxwidth) {
2565        w=capture->capability.maxwidth;
2566      }
2567      if (h>capture->capability.maxheight) {
2568        h=capture->capability.maxheight;
2569      }
2570
2571      capture->captureWindow.width=w;
2572      capture->captureWindow.height=h;
2573
2574      if (ioctl(capture->deviceHandle, VIDIOCSWIN, &capture->captureWindow) < 0) {
2575        icvCloseCAM_V4L(capture);
2576        return 0;
2577      }
2578
2579      if (ioctl(capture->deviceHandle, VIDIOCGWIN, &capture->captureWindow) < 0) {
2580        icvCloseCAM_V4L(capture);
2581        return 0;
2582      }
2583
2584      capture->FirstCapture = 1;
2585
2586   }
2587 #endif /* HAVE_CAMV4L */
2588
2589   return 0;
2590
2591 }
2592
2593 static int icvSetControl (CvCaptureCAM_V4L* capture,
2594                           int property_id, double value) {
2595
2596   /* limitation of the input value */
2597   if (value < 0.0) {
2598     value = 0.0;
2599   } else if (value > 1.0) {
2600     value = 1.0;
2601   }
2602
2603 #ifdef HAVE_CAMV4L2
2604
2605   if (V4L2_SUPPORT == 1)
2606   {
2607
2608     /* default value for min and max */
2609     int v4l2_min = 0;
2610     int v4l2_max = 255;
2611
2612     /* initialisations */
2613     CLEAR (capture->control);
2614
2615     /* set which control we want to set */
2616     switch (property_id) {
2617
2618     case CV_CAP_PROP_BRIGHTNESS:
2619         capture->control.id = V4L2_CID_BRIGHTNESS;
2620         break;
2621     case CV_CAP_PROP_CONTRAST:
2622         capture->control.id = V4L2_CID_CONTRAST;
2623         break;
2624     case CV_CAP_PROP_SATURATION:
2625         capture->control.id = V4L2_CID_SATURATION;
2626         break;
2627     case CV_CAP_PROP_HUE:
2628         capture->control.id = V4L2_CID_HUE;
2629         break;
2630     case CV_CAP_PROP_GAIN:
2631         capture->control.id = V4L2_CID_GAIN;
2632         break;
2633     case CV_CAP_PROP_EXPOSURE:
2634         capture->control.id = V4L2_CID_EXPOSURE;
2635         break;
2636     default:
2637         fprintf(stderr,
2638                 "HIGHGUI ERROR: V4L2: setting property #%d is not supported\n",
2639                 property_id);
2640         return -1;
2641     }
2642
2643     /* get the min and max values */
2644     if (-1 == ioctl (capture->deviceHandle,
2645                       VIDIOC_G_CTRL, &capture->control)) {
2646 //          perror ("VIDIOC_G_CTRL for getting min/max values");
2647           return -1;
2648     }
2649
2650     /* get the min/max values */
2651     switch (property_id) {
2652
2653     case CV_CAP_PROP_BRIGHTNESS:
2654         v4l2_min = capture->v4l2_brightness_min;
2655         v4l2_max = capture->v4l2_brightness_max;
2656         break;
2657     case CV_CAP_PROP_CONTRAST:
2658         v4l2_min = capture->v4l2_contrast_min;
2659         v4l2_max = capture->v4l2_contrast_max;
2660         break;
2661     case CV_CAP_PROP_SATURATION:
2662         v4l2_min = capture->v4l2_saturation_min;
2663         v4l2_max = capture->v4l2_saturation_max;
2664         break;
2665     case CV_CAP_PROP_HUE:
2666         v4l2_min = capture->v4l2_hue_min;
2667         v4l2_max = capture->v4l2_hue_max;
2668         break;
2669     case CV_CAP_PROP_GAIN:
2670         v4l2_min = capture->v4l2_gain_min;
2671         v4l2_max = capture->v4l2_gain_max;
2672         break;
2673     case CV_CAP_PROP_EXPOSURE:
2674         v4l2_min = capture->v4l2_exposure_min;
2675         v4l2_max = capture->v4l2_exposure_max;
2676         break;
2677     }
2678
2679     /* initialisations */
2680     CLEAR (capture->control);
2681
2682     /* set which control we want to set */
2683     switch (property_id) {
2684
2685     case CV_CAP_PROP_BRIGHTNESS:
2686         capture->control.id = V4L2_CID_BRIGHTNESS;
2687         break;
2688     case CV_CAP_PROP_CONTRAST:
2689         capture->control.id = V4L2_CID_CONTRAST;
2690         break;
2691     case CV_CAP_PROP_SATURATION:
2692         capture->control.id = V4L2_CID_SATURATION;
2693         break;
2694     case CV_CAP_PROP_HUE:
2695         capture->control.id = V4L2_CID_HUE;
2696         break;
2697     case CV_CAP_PROP_GAIN:
2698         capture->control.id = V4L2_CID_GAIN;
2699         break;
2700     case CV_CAP_PROP_EXPOSURE:
2701         capture->control.id = V4L2_CID_EXPOSURE;
2702         break;
2703     default:
2704         fprintf(stderr,
2705                 "HIGHGUI ERROR: V4L2: setting property #%d is not supported\n",
2706                 property_id);
2707         return -1;
2708     }
2709
2710     /* set the value we want to set to the scaled the value */
2711     capture->control.value = (int)(value * (v4l2_max - v4l2_min) + v4l2_min);
2712
2713     /* The driver may clamp the value or return ERANGE, ignored here */
2714     if (-1 == ioctl (capture->deviceHandle,
2715                       VIDIOC_S_CTRL, &capture->control) && errno != ERANGE) {
2716         perror ("VIDIOC_S_CTRL");
2717         return -1;
2718     }
2719   }
2720 #endif /* HAVE_CAMV4L2 */
2721 #if defined(HAVE_CAMV4L) && defined(HAVE_CAMV4L2)
2722     else
2723 #endif /* HAVE_CAMV4L && HAVE_CAMV4L2 */
2724 #ifdef HAVE_CAMV4L
2725   {
2726
2727     int v4l_value;
2728
2729     /* scale the value to the wanted integer one */
2730     v4l_value = (int)(0xFFFF * value);
2731
2732     switch (property_id) {
2733     case CV_CAP_PROP_BRIGHTNESS:
2734       capture->imageProperties.brightness = v4l_value;
2735       break;
2736     case CV_CAP_PROP_CONTRAST:
2737       capture->imageProperties.contrast = v4l_value;
2738       break;
2739     case CV_CAP_PROP_SATURATION:
2740       capture->imageProperties.colour = v4l_value;
2741       break;
2742     case CV_CAP_PROP_HUE:
2743       capture->imageProperties.hue = v4l_value;
2744       break;
2745     case CV_CAP_PROP_GAIN:
2746         fprintf(stderr,
2747                 "HIGHGUI ERROR: V4L: Gain control in V4L is not supported\n");
2748         return -1;
2749     case CV_CAP_PROP_EXPOSURE:
2750         fprintf(stderr,
2751                 "HIGHGUI ERROR: V4L: Exposure control in V4L is not supported\n");
2752         return -1;
2753     default:
2754         fprintf(stderr,
2755                 "HIGHGUI ERROR: V4L: property #%d is not supported\n",
2756                 property_id);
2757         return -1;
2758     }
2759
2760     if (ioctl(capture->deviceHandle, VIDIOCSPICT, &capture->imageProperties)
2761         < 0)
2762     {
2763        fprintf(stderr,
2764                "HIGHGUI ERROR: V4L: Unable to set video informations\n");
2765        icvCloseCAM_V4L(capture);
2766        return -1;
2767     }
2768   }
2769 #endif /* HAVE_CAMV4L */
2770
2771   /* all was OK */
2772   return 0;
2773
2774 }
2775
2776 static int icvSetPropertyCAM_V4L( CvCaptureCAM_V4L* capture,
2777                                   int property_id, double value ){
2778     static int width = 0, height = 0;
2779     int retval;
2780
2781     /* initialization */
2782     retval = 0;
2783
2784     /* two subsequent calls setting WIDTH and HEIGHT will change
2785        the video size */
2786     /* the first one will return an error, though. */
2787
2788     switch (property_id) {
2789     case CV_CAP_PROP_FRAME_WIDTH:
2790         width = cvRound(value);
2791         if(width !=0 && height != 0) {
2792             retval = icvSetVideoSize( capture, width, height);
2793             width = height = 0;
2794         }
2795         break;
2796     case CV_CAP_PROP_FRAME_HEIGHT:
2797         height = cvRound(value);
2798         if(width !=0 && height != 0) {
2799             retval = icvSetVideoSize( capture, width, height);
2800             width = height = 0;
2801         }
2802         break;
2803     case CV_CAP_PROP_BRIGHTNESS:
2804     case CV_CAP_PROP_CONTRAST:
2805     case CV_CAP_PROP_SATURATION:
2806     case CV_CAP_PROP_HUE:
2807     case CV_CAP_PROP_GAIN:
2808     case CV_CAP_PROP_EXPOSURE:
2809         retval = icvSetControl(capture, property_id, value);
2810         break;
2811     default:
2812         fprintf(stderr,
2813                 "HIGHGUI ERROR: V4L: setting property #%d is not supported\n",
2814                 property_id);
2815     }
2816
2817     /* return the the status */
2818     return retval;
2819 }
2820
2821 static void icvCloseCAM_V4L( CvCaptureCAM_V4L* capture ){
2822    /* Deallocate space - Hopefully, no leaks */
2823
2824    if (capture)
2825    {
2826
2827 #ifdef HAVE_CAMV4L2
2828      if (V4L2_SUPPORT == 0)
2829 #endif /* HAVE_CAMV4L2 */
2830 #ifdef HAVE_CAMV4L
2831      {
2832
2833        if (capture->mmaps)
2834          free(capture->mmaps);
2835        if (capture->memoryMap)
2836          munmap(capture->memoryMap, capture->memoryBuffer.size);
2837
2838      }
2839 #endif /* HAVE_CAMV4L */
2840 #if defined(HAVE_CAMV4L) && defined(HAVE_CAMV4L2)
2841      else
2842 #endif /* HAVE_CAMV4L && HAVE_CAMV4L2 */
2843 #ifdef HAVE_CAMV4L2
2844        {
2845        capture->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
2846        if (-1 == ioctl(capture->deviceHandle, VIDIOC_STREAMOFF, &capture->type)) {
2847            perror ("Unable to stop the stream.");
2848        }
2849
2850        for (unsigned int n_buffers_ = 0; n_buffers_ < capture->req.count; ++n_buffers_)
2851        {
2852            if (-1 == munmap (capture->buffers[n_buffers_].start, capture->buffers[n_buffers_].length)) {
2853                perror ("munmap");
2854            }
2855        }
2856
2857        if (capture->buffers[MAX_V4L_BUFFERS].start)
2858        {
2859            free(capture->buffers[MAX_V4L_BUFFERS].start);
2860            capture->buffers[MAX_V4L_BUFFERS].start = 0;
2861        }
2862      }
2863 #endif /* HAVE_CAMV4L2 */
2864
2865      if (capture->deviceHandle != -1)
2866        close(capture->deviceHandle);
2867
2868      if (capture->frame.imageData) cvFree(&capture->frame.imageData);
2869       //cvFree((void **)capture);
2870    }
2871 };
2872
2873
2874 class CvCaptureCAM_V4L_CPP : CvCapture
2875 {
2876 public:
2877     CvCaptureCAM_V4L_CPP() { captureV4L = 0; }
2878     virtual ~CvCaptureCAM_V4L_CPP() { close(); }
2879
2880     virtual bool open( int index );
2881     virtual void close();
2882
2883     virtual double getProperty(int);
2884     virtual bool setProperty(int, double);
2885     virtual bool grabFrame();
2886     virtual IplImage* retrieveFrame(int);
2887 protected:
2888
2889     CvCaptureCAM_V4L* captureV4L;
2890 };
2891
2892 bool CvCaptureCAM_V4L_CPP::open( int index )
2893 {
2894     close();
2895     captureV4L = icvCaptureFromCAM_V4L(index);
2896     return captureV4L != 0;
2897 }
2898
2899 void CvCaptureCAM_V4L_CPP::close()
2900 {
2901     if( captureV4L )
2902     {
2903         icvCloseCAM_V4L( captureV4L );
2904         cvFree( &captureV4L );
2905     }
2906 }
2907
2908 bool CvCaptureCAM_V4L_CPP::grabFrame()
2909 {
2910     return captureV4L ? icvGrabFrameCAM_V4L( captureV4L ) != 0 : false;
2911 }
2912
2913 IplImage* CvCaptureCAM_V4L_CPP::retrieveFrame(int)
2914 {
2915     return captureV4L ? icvRetrieveFrameCAM_V4L( captureV4L, 0 ) : 0;
2916 }
2917
2918 double CvCaptureCAM_V4L_CPP::getProperty( int propId )
2919 {
2920     return captureV4L ? icvGetPropertyCAM_V4L( captureV4L, propId ) : 0.0;
2921 }
2922
2923 bool CvCaptureCAM_V4L_CPP::setProperty( int propId, double value )
2924 {
2925     return captureV4L ? icvSetPropertyCAM_V4L( captureV4L, propId, value ) != 0 : false;
2926 }
2927
2928 CvCapture* cvCreateCameraCapture_V4L( int index )
2929 {
2930     CvCaptureCAM_V4L_CPP* capture = new CvCaptureCAM_V4L_CPP;
2931
2932     if( capture->open( index ))
2933         return (CvCapture*)capture;
2934
2935     delete capture;
2936     return 0;
2937 }
2938
2939 #endif