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