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