root/vic/branches/mpeg4/video/grabber-v4l2.cpp @ 4365

Revision 4365, 43.0 KB (checked in by douglask, 6 years ago)

Add support for V4L2_PIX_FMT_YUV420 only webcams with VIC's 422 based codecs

  • Property svn:eol-style set to native
  • Property svn:keywords set to Author Date Id Revision
Line 
1/* =========================================================================
2
3     Copyright (c) 1997 Regents of Koji OKAMURA, oka@kobe-u.ac.jp
4     All rights reserved.
5
6     largely rewritten for new bttv/video4linux interface
7     by Gerd Knorr <kraxel@cs.tu-berlin.de>
8
9     Added brightness, contrast, hue and saturation controls.
10     by Jean-Marc Orliaguet <jmo@medialab.chalmers.se>
11
12     Added support for NTSC/PAL/SECAM video norm selection. (14/10/99)
13     by Jean-Marc Orliaguet <jmo@medialab.chalmers.se>
14
15     Early stage V4L2 implementation. (v0.92a)
16     by Jean-Marc Orliaguet <jmo@medialab.chalmers.se>
17     Capture code taken from xcaptest.c V4L2 demo app (Bill Dirks)
18     Some code contributed by Malcolm Caldwell <malcolm@it.ntu.edu.au>
19
20     Added support for MJPEG/JPEG.
21     Added gamma and gain controls.
22     by Douglas Kosovic <douglask@itee.uq.edu.au>
23     MJPEG/JPEG support uses Tiny Jpeg Decoder from :
24     http://www.saillard.org/programs_and_patches/tinyjpegdecoder/
25
26   ========================================================================= */
27
28#include <stdio.h>
29#include <stdlib.h>
30#include <unistd.h>
31#include <string.h>
32#include <signal.h>
33#include <errno.h>
34#include <endian.h>
35
36#include <sys/types.h>
37#include <sys/fcntl.h>
38#include <sys/ioctl.h>
39#include <sys/time.h>
40#include <sys/mman.h>
41
42#include <X11/Xlib.h>
43#include <X11/Xutil.h>
44#include <X11/keysym.h>
45
46extern "C"
47{
48#include <asm/types.h>
49#include <linux/videodev2.h>
50}
51
52#ifdef HAVE_LIBV4L
53#include <libv4l2.h>
54#else
55#define v4l2_open open
56#define v4l2_close close
57#define v4l2_ioctl ioctl
58#define v4l2_read read
59#define v4l2_mmap mmap
60#define v4l2_munmap munmap
61#include "tinyjpeg.h"
62#endif
63
64#include "grabber.h"
65#include "vic_tcl.h"
66#include "device-input.h"
67#include "module.h"
68#include "yuv_convert.h"
69
70/* here you can tune the device names */
71static const char *devlist[] = {
72    "/dev/video0", "/dev/video1", "/dev/video2", "/dev/video3",
73    "/dev/video4", "/dev/video5", "/dev/video6", "/dev/video7",
74    "/dev/video8", "/dev/video9", "/dev/video10", "/dev/video11",
75    NULL
76};
77
78#define PAL_BT601_WIDTH   720
79#define PAL_BT601_HEIGHT  576
80#define NTSC_BT601_WIDTH  720
81#define NTSC_BT601_HEIGHT 480
82#define VGA_WIDTH   640
83#define VGA_HEIGHT  480
84#define CIF_WIDTH   352
85#define CIF_HEIGHT  288
86
87#define CF_422 0
88#define CF_420 1
89#define CF_CIF 2
90
91#define CS_VC         0 /* 4CIF (704x576), CIF (352x288), QCIF (176x144) */
92#define CS_VGA        1 /* VGA (640x480), 1/4 VGA (320x240), 1/16 VGA (160x120) */
93#define CS_BT601_NTSC 2 /* ITU-R Recommendation BT.601 720x480 (plus 360x240 & 180x120) */
94#define CS_BT601_PAL  3 /* ITU-R Recommendation BT.601 720x576 (plus 360x288 & 180x144) */
95
96/* YUV Byte order */
97#define BYTE_ORDER_YUYV 0
98#define BYTE_ORDER_YVYU 1
99#define BYTE_ORDER_UYVY 2
100#define BYTE_ORDER_VYUY 3
101
102/* V4L2 driver specific controls */
103#ifndef V4L2_CID_POWER_LINE_FREQUENCY
104#define V4L2_CID_POWER_LINE_FREQUENCY (V4L2_CID_PRIVATE_BASE + 1)
105#endif
106#ifndef V4L2_CID_FOCUS_AUTO
107#define V4L2_CID_FOCUS_AUTO           (V4L2_CID_PRIVATE_BASE + 4)
108#endif
109#ifndef V4L2_CID_FOCUS_ABSOLUTE
110#define V4L2_CID_FOCUS_ABSOLUTE       (V4L2_CID_PRIVATE_BASE + 5)
111#endif
112#ifndef V4L2_CID_FOCUS_RELATIVE
113#define V4L2_CID_FOCUS_RELATIVE       (V4L2_CID_PRIVATE_BASE + 6)
114#endif
115
116typedef struct tag_vimage
117{
118        struct v4l2_buffer      vidbuf;
119        char                    *data;
120}       VIMAGE;
121
122
123#define STREAMBUFS 2
124
125class V4l2Grabber : public Grabber
126{
127public:
128        V4l2Grabber(const char * cformat, const char *dev);
129        virtual ~V4l2Grabber();
130
131        virtual int  command(int argc, const char*const* argv);
132        virtual void start();
133        virtual void stop();
134        virtual int  grab();
135
136
137protected:
138        void format();
139        void setsize();
140
141#ifndef HAVE_LIBV4L
142        void jpeg_to_planar420(char *, const char*);
143#endif
144
145        void setctrl(int, int, const char *, int);
146
147        struct v4l2_capability   capability;
148        struct v4l2_input        *inputs;
149        struct v4l2_input        input;
150        struct v4l2_queryctrl    qctrl;
151        struct v4l2_format       fmt;
152        struct v4l2_requestbuffers req;
153
154        /* mmap */
155        int                      have_mmap;
156        VIMAGE                   vimage[STREAMBUFS];
157        struct v4l2_buffer       tempbuf;
158        int                      buf_sync;
159
160        __u32   pixelformat;
161        int fd_;
162        int format_;
163        int have_YUV422;
164        int have_YUV422P;
165        int have_YUV420P;
166        int have_MJPEG;
167        int have_JPEG;
168
169        int cformat_;
170        int port_;
171        int norm_;
172
173        unsigned char *tm_;
174        int width_;
175        int height_;
176        int max_width_;
177        int max_height_;
178        int decimate_;
179        int bytesused_;
180
181#ifndef HAVE_LIBV4L
182        struct jdec_private *jpegdec_;
183#endif
184};
185
186/* ----------------------------------------------------------------- */
187
188class V4l2Device : public InputDevice
189{
190public:
191        V4l2Device(const char *dev, const char*, char *attr);
192        virtual int command(int argc, const char*const* argv);
193
194private:
195        const char *dev_;
196};
197
198
199V4l2Device::V4l2Device(const char *dev, const char *name, char *attr) : InputDevice(name)
200{
201        dev_ = dev;
202        attributes_ = attr;
203        debug_msg("V4L2:  ==> %s\n",attr);
204}
205
206int V4l2Device::command(int argc, const char*const* argv)
207{
208        Tcl& tcl = Tcl::instance();
209
210
211        if (argc == 3) {
212                if (strcmp(argv[1], "open") == 0) {
213                        TclObject* o = 0;
214                        o = new V4l2Grabber(argv[2],dev_);
215                        if (o != 0)
216                                tcl.result(o->name());
217                        return (TCL_OK);
218                }
219
220        }
221        return (InputDevice::command(argc, argv));
222}
223
224/* ----------------------------------------------------------------- */
225
226class V4l2Scanner
227{
228public:
229        V4l2Scanner(const char **dev);
230};
231
232static V4l2Scanner find_video4linux_devices(devlist);
233
234V4l2Scanner::V4l2Scanner(const char **dev)
235{
236        struct v4l2_capability   capability;
237        struct v4l2_input        input;
238
239        int  k,i,err,fd;
240        char *nick, *attr;
241
242        // VIC_DEVICE env variable selects V4L device with AGTk 3.02 and earlier
243        // but doesn't work if V4L2 devices are listed before the V4L device
244        // so don't list V4L2 devices if VIC_DEVICE env variable is set.
245        const char *myDev = getenv("VIC_DEVICE");
246        if (myDev != 0)
247        {
248                 return;
249        }
250
251        for (i = 0; dev[i] != NULL; i++) {
252                debug_msg("V4L2: trying %s... ",dev[i]);
253                if (-1 == (fd = v4l2_open(dev[i],O_RDWR))) {
254                        debug_msg("Error opening: %s : %s\n", dev[i], strerror(errno));
255                        continue;
256                }
257#ifdef HAVE_LIBV4L
258                if (-1 == v4l2_fd_open(fd,V4L2_ENABLE_ENUM_FMT_EMULATION)) {
259                        perror("V4L2: v4l2_fd_open V4L2_ENABLE_ENUM_FMT_EMULATION");
260                        v4l2_close(fd);
261                        continue;
262                }
263#endif
264                memset(&capability,0,sizeof(capability));
265                if (-1 == v4l2_ioctl(fd,VIDIOC_QUERYCAP,&capability)) {
266                        perror("V4L2: ioctl VIDIOC_QUERYCAP");
267                        v4l2_close(fd);
268                        continue;
269                }
270
271                if ((capability.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
272                        debug_msg("%s, %s can't capture\n",capability.card,capability.bus_info);
273                        v4l2_close(fd);
274                        continue;
275                }
276
277                attr = new char[512];
278                strcpy(attr,"format { 420 422 cif } ");
279                strcat(attr,"size { small large cif } ");
280
281                debug_msg("V4L2:   ports:");
282                strcat(attr,"port { ");
283                //input.index = 0;
284                memset(&input, 0, sizeof(input));
285                while (-1 != v4l2_ioctl(fd, VIDIOC_ENUMINPUT, &input)) {
286                        if (-1 == v4l2_ioctl(fd,VIDIOC_S_INPUT,&input.index)) {
287                                debug_msg("ioctl VIDIOC_S_INPUT: %s", strerror(errno));
288                        } else {
289
290                                debug_msg(" %s: ", (char*)input.name);
291                                for (unsigned int s=0 ; s<strlen((char*)input.name) ; s++)
292                                        if (input.name[s]==' ') input.name[s]='-';
293                                strcat(attr,(const char*)input.name);
294                                strcat(attr," ");
295                        }
296                        input.index++;
297                }
298                debug_msg("\n");
299                strcat(attr,"} ");
300
301                debug_msg("V4L2:   norms: ");
302                strcat(attr,"type { ");
303
304                for (k = 0, err = 0; err == 0; ++k)
305                {
306                        struct v4l2_standard     estd;
307                        memset(&estd,0,sizeof(estd));
308                        estd.index = k;
309                        err = v4l2_ioctl(fd, VIDIOC_ENUMSTD, &estd);
310                        if (!err) {
311                                strcat(attr, (const char*)estd.name);
312                                strcat(attr," ");
313                                debug_msg("%s ", estd.name);
314                        }
315                }
316                debug_msg("\n");
317
318                strcat(attr,"} ");
319
320                nick = new char[strlen((char*)capability.card)+strlen(dev[i])+7];
321                sprintf(nick,"V4L2-%s %s",capability.card, dev[i]);
322                new V4l2Device(dev[i],nick,attr);
323                fprintf(stderr, "Attached to V4L2 device: %s\n", nick);
324                v4l2_close(fd);
325        }
326}
327
328/* ----------------------------------------------------------------- */
329
330V4l2Grabber::V4l2Grabber(const char *cformat, const char *dev)
331{
332        fd_ = open(dev, O_RDWR);
333        if (fd_ < 0) {
334                perror("open");
335                return;
336        }
337        have_mmap = 0;
338        have_YUV422 = 0;
339        have_YUV422P = 0;
340        have_YUV420P = 0;
341        have_MJPEG = 0;
342        have_JPEG = 0;
343
344#ifndef HAVE_LIBV4L
345        jpegdec_ = NULL;
346#endif
347
348        struct v4l2_capability cap;
349        memset(&cap,0,sizeof(cap));
350        if (-1 == v4l2_ioctl(fd_,VIDIOC_QUERYCAP,&cap)) {
351                perror("ioctl VIDIOC_QUERYCAP");
352        } else {
353                if ( !(cap.capabilities & V4L2_CAP_READWRITE) && !(cap.capabilities & V4L2_CAP_STREAMING)) {
354                        debug_msg("V4L2: fatal: device does not support read()/write() calls or streaming capture.\n");
355                        status_=-1;
356                        return;
357                }
358        }
359
360        memset(&fmt,0,sizeof(fmt));
361        fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
362        v4l2_ioctl(fd_, VIDIOC_G_FMT, &fmt);
363
364        unsigned int test_width[] =  {CIF_WIDTH,  PAL_BT601_WIDTH/2,  NTSC_BT601_WIDTH/2,  VGA_WIDTH/2,  0};
365        unsigned int test_height[] = {CIF_HEIGHT, PAL_BT601_HEIGHT/2, NTSC_BT601_HEIGHT/2, VGA_HEIGHT/2, 0};
366        for (unsigned int i = 0; test_width[i] != 0; i++) {
367                fmt.fmt.pix.width = test_width[i];
368                fmt.fmt.pix.height = test_height[i];
369                fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;
370                if (-1 != v4l2_ioctl(fd_, VIDIOC_S_FMT, &fmt) ) {
371                        if (fmt.fmt.pix.height == test_height[i] && fmt.fmt.pix.width >= test_width[i] && fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420) {
372                                have_YUV420P = 1;
373                                debug_msg("Device supports V4L2_PIX_FMT_YUV420 capture at %dx%d\n",test_width[i],test_height[i]);
374                        } else if (fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420) {
375                                debug_msg("V4L2_PIX_FMT_YUV420 capture at %dx%d not supported, returned %dx%d\n",test_width[i],test_height[i],fmt.fmt.pix.width,fmt.fmt.pix.height);
376                        }
377                }
378
379                fmt.fmt.pix.width = test_width[i];
380                fmt.fmt.pix.height = test_height[i];
381                fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV422P;
382                if (-1 != v4l2_ioctl(fd_, VIDIOC_S_FMT, &fmt) ) {
383                        if (fmt.fmt.pix.height == test_height[i] && fmt.fmt.pix.width >= test_width[i] && fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV422P) {
384                                have_YUV422P = 1;
385                                debug_msg("Device supports V4L2_PIX_FMT_YUV422 capture at %dx%d\n",test_width[i],test_height[i]);
386                        } else {
387                                debug_msg("V4L2_PIX_FMT_YUV422 capture at %dx%d not supported, returned %dx%d\n",test_width[i],test_height[i],fmt.fmt.pix.width,fmt.fmt.pix.height);
388                        }
389                }
390
391                fmt.fmt.pix.width = test_width[i];
392                fmt.fmt.pix.height = test_height[i];
393                fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
394                if (-1 != v4l2_ioctl(fd_, VIDIOC_S_FMT, &fmt) ) {
395                        if (fmt.fmt.pix.height == test_height[i] && fmt.fmt.pix.width >= test_width[i]) {
396                                have_YUV422 = 1;
397                                debug_msg("Device supports V4L2_PIX_FMT_YUYV (YUV 4:2:2) capture at %dx%d\n",test_width[i],test_height[i]);
398                        } else if (fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV) {
399                                debug_msg("V4L2_PIX_FMT_YUYV (YUV 4:2:2) capture at %dx%d not supported, returned %dx%d\n",test_width[i],test_height[i],fmt.fmt.pix.width,fmt.fmt.pix.height);
400                        }
401                }
402
403#ifdef HAVE_LIBV4L
404                fmt.fmt.pix.width = test_width[i];
405                fmt.fmt.pix.height = test_height[i];
406                fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
407                if (-1 != v4l2_ioctl(fd_, VIDIOC_S_FMT, &fmt) ) {
408                        if (fmt.fmt.pix.height == test_height[i] && fmt.fmt.pix.width >= test_width[i] && fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG) {
409                                have_MJPEG = 1;
410                                debug_msg("Device supports V4L2_PIX_FMT_MJPEG capture at %dx%d\n",test_width[i],test_height[i]);
411                        } else if (fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG) {
412                                debug_msg("V4L2_PIX_FMT_MJPEG capture at %dx%d not supported, returned %dx%d\n",test_width[i],test_height[i],fmt.fmt.pix.width,fmt.fmt.pix.height);
413                        }
414                }
415
416                fmt.fmt.pix.width = test_width[i];
417                fmt.fmt.pix.height = test_height[i];
418                fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_JPEG;
419                if (-1 != v4l2_ioctl(fd_, VIDIOC_S_FMT, &fmt) ) {
420                        if (fmt.fmt.pix.height == test_height[i] && fmt.fmt.pix.width >= test_width[i] && fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_JPEG) {
421                                have_MJPEG = 1;
422                                debug_msg("Device supports V4L2_PIX_FMT_JPEG capture at %dx%d\n",test_width[i],test_height[i]);
423                        } else if (fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_JPEG) {
424                                debug_msg("V4L2_PIX_FMT_JPEG capture at %dx%d not supported, returned %dx%d\n",test_width[i],test_height[i],fmt.fmt.pix.width,fmt.fmt.pix.height);
425                        }
426                }
427#endif
428        }
429
430        if( !( have_YUV422P || have_YUV422 || have_YUV420P || have_MJPEG || have_JPEG)){
431                debug_msg("No suitable pixelformat found\n");
432                v4l2_close(fd_);
433                status_=-1;
434                return;
435        }
436
437        struct v4l2_requestbuffers reqbuf;
438        memset(&reqbuf,0,sizeof(reqbuf));
439        reqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
440        reqbuf.memory = V4L2_MEMORY_MMAP;
441        reqbuf.count = 20;
442        have_mmap = 1;
443        if (-1 == v4l2_ioctl(fd_, VIDIOC_REQBUFS, &reqbuf)) {
444                if (errno == EINVAL) {
445                        printf("Video capturing or mmap-streaming is not supported\n");
446                        have_mmap = 0;
447                        status_=-1;
448                } else {
449                        perror ("VIDIOC_REQBUFS");
450                        have_mmap = 0;
451                }
452        }
453
454        /* fill in defaults */
455        if(!strcmp(cformat, "420"))
456                cformat_ = CF_420;
457        if(!strcmp(cformat, "422"))
458                cformat_ = CF_422;
459        if(!strcmp(cformat, "cif"))
460                cformat_ = CF_CIF;
461
462        port_      = 0;
463        norm_      = 0;
464        decimate_  = 2;
465        running_   = 0;
466
467}
468
469
470V4l2Grabber::~V4l2Grabber()
471{
472        int i;
473        debug_msg("V4L2: destructor\n");
474
475        if (have_mmap) {
476                for (i = 0; i < STREAMBUFS; ++i) {
477                        if (vimage[i].data)
478                                v4l2_munmap(vimage[i].data,
479                                            vimage[i].vidbuf.length);
480                        vimage[i].data = NULL;
481                }
482        } else {
483                if (vimage[0].data)
484                        free(vimage[0].data);
485                vimage[0].data = NULL;
486        }
487        v4l2_close(fd_);
488
489#ifndef HAVE_LIBV4L
490        if (jpegdec_) {
491                tinyjpeg_free(jpegdec_);
492        }
493#endif
494
495}
496
497int V4l2Grabber::command(int argc, const char*const* argv)
498{
499        int i, err;
500
501        if (argc == 3) {
502                if (strcmp(argv[1], "decimate") == 0) {
503                        decimate_ = atoi(argv[2]);
504
505                        if (running_) {
506                                stop(); start();
507                        }
508                        return (TCL_OK);
509                }
510
511                if (strcmp(argv[1], "port") == 0) {
512
513                        for (i = 0, err = 0; err == 0; ++i) {
514                                struct v4l2_input inp;
515                                memset(&inp,0,sizeof(inp));
516                                inp.index = i;
517                                err = v4l2_ioctl(fd_, VIDIOC_ENUMINPUT, &inp);
518                                if (!err) {
519
520                                        char input[32];
521                                        unsigned int s;
522                                        for ( s=0 ; s<= strlen((const char*)inp.name) ; s++ )
523                                                        if (inp.name[s]==' ') input[s]='-'; else input[s]=inp.name[s];
524
525                                        if ( !strcmp(input,argv[2])) {
526                                                port_ = i;
527                                                break;
528                                        }
529                                }
530                        }
531                        if (running_) {
532                                stop(); start();
533                        }
534
535                        return (TCL_OK);
536                }
537
538
539                if (strcmp(argv[1], "brightness") == 0) {
540                        setctrl(atoi(argv[2]), V4L2_CID_BRIGHTNESS, "Brightness", 0);
541                        return (TCL_OK);
542                }
543
544                if (strcmp(argv[1], "hue") == 0) {
545                        setctrl(atoi(argv[2]), V4L2_CID_HUE, "Hue", 0);
546                        return (TCL_OK);
547                }
548
549                if (strcmp(argv[1], "contrast") == 0) {
550                        setctrl(atoi(argv[2]), V4L2_CID_CONTRAST, "Contrast", 0);
551                        return (TCL_OK);
552                }
553
554                if (strcmp(argv[1], "saturation") == 0) {
555                        setctrl(atoi(argv[2]), V4L2_CID_SATURATION, "Saturation", 0);
556                        return (TCL_OK);
557                }
558
559                if (strcmp(argv[1], "gamma") == 0) {
560                        setctrl(atoi(argv[2]), V4L2_CID_GAMMA, "Gamma", 0);
561                        return (TCL_OK);
562                }
563
564                if (strcmp(argv[1], "gain") == 0) {
565                        setctrl(atoi(argv[2]), V4L2_CID_GAIN, "Gain", 0);
566                        return (TCL_OK);
567                }
568
569                if (strcmp(argv[1], "antiflicker") == 0) {
570                        struct v4l2_queryctrl qctrl;
571                        struct v4l2_control ctrl;
572
573                        memset (&qctrl, 0, sizeof(qctrl));
574                        qctrl.id = V4L2_CID_POWER_LINE_FREQUENCY;
575                        if (-1 != v4l2_ioctl(fd_, VIDIOC_QUERYCTRL, &qctrl)) {
576                                if (strcmp((char *)qctrl.name, "Power Line Frequency") == 0) {
577                                        ctrl.id = qctrl.id;
578                                        ctrl.value = atoi(argv[2]);
579                                        if (-1 == v4l2_ioctl(fd_, VIDIOC_S_CTRL, &ctrl))
580                                                perror("ioctl  VIDIOC_S_CTRL");
581                                        else
582                                                debug_msg( "V4L2: V4L2_CID_POWER_LINE_FREQUENCY = %d\n", ctrl.value);
583                                 }
584                        }
585                        return (TCL_OK);
586                }
587
588                if (strcmp(argv[1], "controls") == 0) {
589                        if (strcmp(argv[2], "reset") == 0) {
590                                debug_msg( "V4L2: Resetting controls\n");
591
592                                setctrl(atoi(argv[2]), V4L2_CID_BRIGHTNESS, "Brightness", 1);
593                                setctrl(atoi(argv[2]), V4L2_CID_HUE, "Hue", 1);
594                                setctrl(atoi(argv[2]), V4L2_CID_CONTRAST, "Contrast", 1);
595                                setctrl(atoi(argv[2]), V4L2_CID_SATURATION, "Saturation", 1);
596                                setctrl(atoi(argv[2]), V4L2_CID_GAMMA, "Gamma", 1);
597                                setctrl(atoi(argv[2]), V4L2_CID_GAIN, "Gain", 1);
598                                return (TCL_OK);
599                        }
600                }
601
602                if (strcmp(argv[1], "fps") == 0) {
603                        debug_msg("V4L2: fps %s\n",argv[2]);
604                }
605
606
607                if (strcmp(argv[1], "type") == 0 || strcmp(argv[1], "format") == 0) {
608
609                        for (int k = 0, err = 0; err == 0; ++k)
610                        {
611                                struct v4l2_standard     estd;
612                                memset(&estd,0,sizeof(estd));
613                                estd.index = k;
614                                if ( !(err = v4l2_ioctl(fd_, VIDIOC_ENUMSTD, &estd)) )
615                                        if ( strcasecmp(argv[2], (const char *)estd.name) == 0)
616                                                norm_ = k;
617                        }
618
619                        if (running_) {
620                                stop(); start();
621                        }
622
623                        return (TCL_OK);
624                }
625
626        } else if (argc == 2) {
627                if (strcmp(argv[1], "format") == 0 ||
628                        strcmp(argv[1], "type") == 0) {
629                        return (TCL_OK);
630                }
631        }
632
633        return (Grabber::command(argc, argv));
634}
635
636void V4l2Grabber::start()
637{
638        int     err;
639        int     i;
640
641        if (fd_ > 0 ) {
642                debug_msg("\nV4L2: start\n");
643
644                format();
645
646                if (have_mmap) {
647                        buf_sync = 1;
648
649                        memset(&req, 0, sizeof(req));
650                        req.count = STREAMBUFS;
651                        req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
652                        req.memory = V4L2_MEMORY_MMAP;
653                        err = v4l2_ioctl(fd_, VIDIOC_REQBUFS, &req);
654                        if (err < 0 || req.count < 1) {
655                                debug_msg("REQBUFS returned error %d, count %d\n", errno,req.count);
656                                return;
657                        }
658
659                        for (i = 0; i < (int)req.count; ++i) {
660                                vimage[i].vidbuf.index = i;
661                                vimage[i].vidbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
662                                vimage[i].vidbuf.memory = V4L2_MEMORY_MMAP;
663                                err = v4l2_ioctl(fd_, VIDIOC_QUERYBUF, &vimage[i].vidbuf);
664                                if (err < 0) {
665                                        debug_msg("QUERYBUF returned error %d\n", errno);
666                                        return;
667                                }
668                                vimage[i].data = (typeof(vimage[0].data)) v4l2_mmap(0,  vimage[i].vidbuf.length, PROT_READ|PROT_WRITE, MAP_SHARED, fd_, vimage[i].vidbuf.m.offset);
669
670                                if ((long)vimage[i].data == -1) {
671                                        debug_msg("V4L2: mmap() returned error %d\n", errno);
672                                        return;
673                                } else debug_msg("V4L2: mmap()'ed buffer at 0x%lx (%u bytes)\n", (unsigned long)vimage[i].data, vimage[i].vidbuf.length);
674                        }
675
676                        for (i = 0; i < (int)req.count; ++i)
677                                if ((err = v4l2_ioctl(fd_, VIDIOC_QBUF, &vimage[i].vidbuf))) {
678                                        debug_msg("QBUF returned error %d\n",errno);
679                                        return;
680                                }
681
682                        err = v4l2_ioctl(fd_, VIDIOC_STREAMON, &vimage[0].vidbuf.type);
683                        if (err)
684                                debug_msg("STREAMON returned error %d\n",errno);
685
686                } else {
687                        vimage[0].data = (typeof(vimage[0].data)) malloc(fmt.fmt.pix.sizeimage);
688
689                        if (vimage[0].data == NULL) {
690                                debug_msg("malloc(%d) failed\n", fmt.fmt.pix.sizeimage);
691                                return;
692                        } else debug_msg("V4L2: malloc()'ed buffer (%d bytes)\n", fmt.fmt.pix.sizeimage);
693                }
694
695                Grabber::start();
696                running_ = 1;
697        }
698}
699
700void V4l2Grabber::stop()
701{
702        debug_msg("V4L2: stop\n");
703        int i, err;
704
705        if (have_mmap) {
706                if ( fd_ > 0 ) {
707                        i = V4L2_BUF_TYPE_VIDEO_CAPTURE;
708                        if ( (err = v4l2_ioctl(fd_, VIDIOC_STREAMOFF, &i) ) )
709                                debug_msg("V4L2: VIDIOC_STREAMOFF failed\n");
710                }
711
712                tempbuf.type = vimage[0].vidbuf.type;
713                while ((err = v4l2_ioctl(fd_, VIDIOC_DQBUF, &tempbuf)) < 0 &&
714                       (errno == EINTR)) ;
715
716                if (err < 0) {
717                        debug_msg("V4L2: VIDIOC_DQBUF failed: %s\n", strerror(errno));
718                }
719
720                for (i = 0; i < STREAMBUFS; ++i) {
721                        if (vimage[i].data)
722                                err = v4l2_munmap(vimage[i].data, vimage[i].vidbuf.length);
723                        vimage[i].data = NULL;
724                }
725
726
727        } else {
728                if (vimage[0].data)
729                        free(vimage[0].data);
730                vimage[0].data = NULL;
731        }
732
733        Grabber::stop();
734        running_ = 0;
735}
736
737int V4l2Grabber::grab()
738{
739        char  *fr=NULL;
740        fd_set                  rdset;
741        struct timeval          timeout;
742        int                     n;
743
744        bytesused_ = 0;
745        if (have_mmap) {
746                FD_ZERO(&rdset);
747                FD_SET(fd_, &rdset);
748                timeout.tv_sec = 0;
749                timeout.tv_usec = 0;
750                n = select(fd_+1, &rdset, NULL, NULL, &timeout);
751
752                if (n == 0)
753                        return (0);
754                else if (FD_ISSET(fd_, &rdset)) {
755                        memset(&tempbuf, 0, sizeof(struct v4l2_buffer));
756                        tempbuf.type = vimage[0].vidbuf.type;
757                        tempbuf.memory = vimage[0].vidbuf.memory;
758                        if (-1 == v4l2_ioctl(fd_, VIDIOC_DQBUF, &tempbuf))
759                                perror("ioctl  VIDIOC_DQBUF");
760
761                        if (  (req.count > 1) ) buf_sync++;
762                        fr = vimage[buf_sync%2].data;
763                        bytesused_ = tempbuf.bytesused;
764
765                }
766
767        } else {
768                fr = vimage[0].data;
769                v4l2_read(fd_, vimage[0].data, fmt.fmt.pix.sizeimage);
770                bytesused_ = fmt.fmt.pix.sizeimage;
771        }
772
773        switch (cformat_) {
774        case CF_420:
775        case CF_CIF:
776                if (have_YUV420P)
777                       planarYUYV420_to_planarYUYV420((char *)frame_, outw_, outh_, fr, inw_, inh_);
778                else if (have_YUV422)
779                       packedUYVY422_to_planarYUYV420((char *)frame_, outw_, outh_, fr, inw_, inh_);
780                else if (have_YUV422P)
781                       planarYUYV422_to_planarYUYV420((char *)frame_, outw_, outh_, fr, inw_, inh_);
782#ifndef HAVE_LIBV4L
783                else if (have_MJPEG || have_JPEG) {
784                       jpeg_to_planar420((char*)frame_,fr);
785                }
786#endif
787                break;
788
789        case CF_422:
790                if (have_YUV422P)
791                       planarYUYV422_to_planarYUYV422((char *)frame_, outw_, outh_, fr, inw_, inh_);
792                else if (have_YUV422)
793                       packedUYVY422_to_planarYUYV422((char *)frame_, outw_, outh_, fr, inw_, inh_);
794                else if (have_YUV420P)
795                       planarYUYV420_to_planarYUYV422((char *)frame_, outw_, outh_, fr, inw_, inh_);
796
797#ifndef HAVE_LIBV4L
798                else if (have_MJPEG  || have_JPEG)
799                       // jpeg_to_planar422((char*)frame_,fr);
800#endif
801                break;
802        }
803
804        if (have_mmap)
805                v4l2_ioctl(fd_, VIDIOC_QBUF, &vimage[buf_sync%2].vidbuf);
806
807        suppress(frame_);
808        saveblks(frame_);
809        YuvFrame f(media_ts(), frame_, crvec_, outw_, outh_);
810        return (target_->consume(&f));
811}
812
813#ifndef HAVE_LIBV4L
814void V4l2Grabber::jpeg_to_planar420(char *dest, const char *src)
815{
816        char *y,*u,*v;
817        unsigned char *components[4];
818
819        y = dest;
820        u = y + width_ * height_;
821        v = u + width_ * height_ /4;
822
823        components[0] = (unsigned char *) y;
824        components[1] = (unsigned char *) u;
825        components[2] = (unsigned char *) v;
826
827        if (jpegdec_ == NULL) {
828                jpegdec_ = tinyjpeg_init();
829                if (jpegdec_ == NULL) {
830                        return;
831                }
832                tinyjpeg_set_flags(jpegdec_, TINYJPEG_FLAGS_MJPEG_TABLE);
833        }
834        tinyjpeg_set_components(jpegdec_, components, 4);
835        if (tinyjpeg_parse_header(jpegdec_, (unsigned char *)src, bytesused_) < 0) {
836               debug_msg("V4L2 Jpeg error: %s", tinyjpeg_get_errorstring(jpegdec_));
837               return;
838        }
839        if (tinyjpeg_decode(jpegdec_, TINYJPEG_FMT_YUV420P) < 0) {
840               debug_msg("V4L2 Jpeg error: %s", tinyjpeg_get_errorstring(jpegdec_));
841               return;
842        }
843}
844#endif
845
846
847void V4l2Grabber::format()
848{
849
850        struct v4l2_standard    standard;
851
852        int i, err;
853        int input;
854        int format_ok = 0;
855        int capture_standard = CS_VC; // initially try video conferencing resolutions
856
857        switch (cformat_) {
858        case CF_420:
859        case CF_CIF:
860                if( have_YUV420P )
861                       pixelformat = V4L2_PIX_FMT_YUV420;
862                else if( have_YUV422 )
863                       pixelformat = V4L2_PIX_FMT_YUYV;
864#ifndef HAVE_LIBV4L
865                else if( have_MJPEG )
866                       pixelformat = V4L2_PIX_FMT_MJPEG;
867                else if( have_JPEG )
868                       pixelformat = V4L2_PIX_FMT_JPEG;
869#endif
870                break;
871
872        case CF_422:
873                if (have_YUV422P)
874                       pixelformat = V4L2_PIX_FMT_YUV422P;
875                else if( have_YUV422 )
876                       pixelformat = V4L2_PIX_FMT_YUYV;
877                else if( have_YUV420P )
878                       pixelformat = V4L2_PIX_FMT_YUV420;
879#ifndef HAVE_LIBV4L
880                else if( have_MJPEG )
881                       pixelformat = V4L2_PIX_FMT_MJPEG;
882                else if( have_JPEG )
883                       pixelformat = V4L2_PIX_FMT_JPEG;
884#endif
885                break;
886        }
887
888        while ( !format_ok ) {
889                if (capture_standard == CS_VC) {
890                        width_  = CIF_WIDTH  *2  / decimate_;
891                        height_ = CIF_HEIGHT *2  / decimate_;
892                }
893
894                debug_msg("V4L2: format");
895                switch (cformat_) {
896                case CF_CIF:
897                        set_size_cif(width_, height_);
898                        debug_msg(" cif\n");
899                        break;
900                case CF_420:
901                        set_size_420(width_, height_);
902                        debug_msg(" 420\n");
903                        break;
904                case CF_422:
905                        set_size_422(width_, height_);
906                        debug_msg(" 422\n");
907                        break;
908                }
909                debug_msg("decimate: %d\n",decimate_);
910
911                memset(&standard,0,sizeof(standard));
912                standard.index = norm_;
913                err = v4l2_ioctl(fd_, VIDIOC_ENUMSTD, &standard);
914                if (!err) {
915                        if (-1 == v4l2_ioctl(fd_, VIDIOC_S_STD, &standard.id))
916                                perror("ioctl VIDIOC_S_STD");
917                        else debug_msg("V4L2: setting norm to %s\n",standard.name);
918                }
919
920                input = port_;
921                if ((err = v4l2_ioctl(fd_, VIDIOC_S_INPUT, &input)) )
922                        debug_msg("S_INPUT returned error %d\n",errno);
923                else debug_msg("V4L2: setting input port to %d\n", port_);
924
925                for (i = 0, err = 0; err == 0; ++i) {
926                        struct v4l2_fmtdesc     fmtd;
927                        memset(&fmtd,0,sizeof(fmtd));
928                        fmtd.index = i;
929                        fmtd.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
930                        err = v4l2_ioctl(fd_, VIDIOC_ENUM_FMT, &fmtd);
931                        if (err) {
932                                debug_msg("V4L2: VIDIOC_ENUM_FMT returned unexpected EINVAL error code\n");
933                                debug_msg("V4L2: giving up ...\n");
934                                format_ok = 1;
935
936                        } else {
937                                if (fmtd.pixelformat == pixelformat) {
938                                        memset(&fmt,0,sizeof(fmt));
939                                        fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
940                                        v4l2_ioctl(fd_, VIDIOC_G_FMT, &fmt);
941                                        fmt.fmt.pix.width = width_;
942                                        fmt.fmt.pix.height = height_;
943                                        fmt.fmt.pix.field = V4L2_FIELD_ANY;
944                                        fmt.fmt.pix.pixelformat = pixelformat;
945
946                                        if ( (err = v4l2_ioctl(fd_, VIDIOC_S_FMT, &fmt) ) )
947                                                debug_msg("\nV4L2: Failed to set format\n");
948
949                                        if ( fmt.fmt.pix.width > (unsigned int)width_ )
950                                                inw_ = width_ = fmt.fmt.pix.width;
951
952
953                                        if ( ( fmt.fmt.pix.width == (unsigned int)width_ ) ||
954                                                                ( fmt.fmt.pix.height == (unsigned int)height_ ) )  {
955                                                debug_msg("V4L2: setting format: width=%d height=%d\n", fmt.fmt.pix.width, fmt.fmt.pix.height);
956                                                format_ok = 1;
957                                                break;
958                                        } else {
959
960                                                debug_msg("V4L2: failed to set format! requested %dx%d, got %dx%d\n", width_, height_, fmt.fmt.pix.width, fmt.fmt.pix.height);
961
962                                                switch(decimate_) {
963                                                case 1:
964                                                        switch (capture_standard) {
965                                                        case CS_VC :
966                                                                debug_msg("V4L2: trying VGA resolution ...\n");
967                                                                width_ = VGA_WIDTH;
968                                                                height_ = VGA_HEIGHT;
969                                                                capture_standard = CS_VGA;
970                                                                break;
971                                                        case CS_VGA :
972                                                                debug_msg("V4L2: trying ITU-R BT.601 NTSC resolution ...\n");
973                                                                width_ = NTSC_BT601_WIDTH;
974                                                                height_ = NTSC_BT601_HEIGHT;
975                                                                capture_standard = CS_BT601_NTSC;
976                                                                break;
977                                                        case CS_BT601_NTSC :
978                                                                debug_msg("V4L2: trying ITU-R BT.601 PAL resolution ...\n");
979                                                                width_ = PAL_BT601_WIDTH;
980                                                                height_ = PAL_BT601_HEIGHT;
981                                                                capture_standard = CS_BT601_PAL;
982                                                                break;
983                                                        default :
984                                                                debug_msg("V4L2: trying resolution under ...\n");
985                                                                decimate_ = 2;
986                                                                capture_standard = CS_VC;
987                                                        }
988                                                        break;
989                                                case 2:
990                                                        switch (capture_standard) {
991                                                        case CS_VC :
992                                                                debug_msg("V4L2: trying 1/4 VGA resolution ...\n");
993                                                                width_ = VGA_WIDTH / 2;
994                                                                height_ = VGA_HEIGHT / 2;
995                                                                capture_standard = CS_VGA;
996                                                                break;
997                                                        case CS_VGA :
998                                                                debug_msg("V4L2: trying 1/4 ITU-R BT.601 NTSC resolution ...\n");
999                                                                width_ = NTSC_BT601_WIDTH / 2;
1000                                                                height_ = NTSC_BT601_HEIGHT / 2;
1001                                                                capture_standard = CS_BT601_NTSC;
1002                                                                break;
1003                                                        case CS_BT601_NTSC :
1004                                                                debug_msg("V4L2: trying 1/4 ITU-R BT.601 PAL resolution ...\n");
1005                                                                width_ = PAL_BT601_WIDTH / 2;
1006                                                                height_ = PAL_BT601_HEIGHT / 2;
1007                                                                capture_standard = CS_BT601_PAL;
1008                                                                break;
1009                                                        default :
1010                                                                debug_msg("V4L2: trying resolution under ...\n");
1011                                                                decimate_ = 4;
1012                                                                capture_standard = CS_VC;
1013                                                        }
1014                                                        break;
1015                                                default:
1016                                                        switch (capture_standard) {
1017                                                        case CS_VC :
1018                                                                debug_msg("V4L2: trying 1/16 VGA resolution ...\n");
1019                                                                width_ = VGA_WIDTH / 4;
1020                                                                height_ = VGA_HEIGHT / 4;
1021                                                                capture_standard = CS_VGA;
1022                                                                break;
1023                                                        case CS_VGA :
1024                                                                debug_msg("V4L2: trying 1/16 ITU-R BT.601 NTSC resolution ...\n");
1025                                                                width_ = NTSC_BT601_WIDTH / 4;
1026                                                                height_ = NTSC_BT601_HEIGHT / 4;
1027                                                                capture_standard = CS_BT601_NTSC;
1028                                                                break;
1029                                                        case CS_BT601_NTSC :
1030                                                                debug_msg("V4L2: trying 1/16 ITU-R BT.601 PAL resolution ...\n");
1031                                                                width_ = PAL_BT601_WIDTH / 4;
1032                                                                height_ = PAL_BT601_HEIGHT / 4;
1033                                                                capture_standard = CS_BT601_PAL;
1034                                                                break;
1035                                                        default :
1036                                                                debug_msg("V4L2: giving up ...\n");
1037                                                                format_ok = 1;
1038                                                        }
1039                                                        break;
1040                                                }
1041                                        }
1042                                }
1043                        }
1044                }
1045        }
1046
1047        allocref();
1048}
1049
1050void V4l2Grabber::setctrl(int val, int cid, const char *controlname, int reset)
1051{
1052        struct v4l2_queryctrl qctrl;
1053        struct v4l2_control ctrl;
1054
1055        qctrl.id = cid;
1056        if (-1 != v4l2_ioctl(fd_, VIDIOC_QUERYCTRL, &qctrl)) {
1057                if ( !(qctrl.flags & V4L2_CTRL_FLAG_DISABLED) ) {
1058
1059                        ctrl.id = cid;
1060                        if ( reset )
1061                                ctrl.value = qctrl.default_value;
1062                        else
1063                                ctrl.value = qctrl.minimum + (qctrl.maximum - qctrl.minimum) * val/256;
1064                        if (-1 == v4l2_ioctl(fd_, VIDIOC_S_CTRL,&ctrl))
1065                                perror("ioctl  VIDIOC_S_CTRL");
1066                        else debug_msg( "V4L2: %s = %d\n", controlname, val);
1067                }
1068        }
1069        return;
1070}
Note: See TracBrowser for help on using the browser.