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

Revision 4339, 45.0 KB (checked in by douglask, 5 years ago)

Forgot to add debug code for V4L2_PIX_FMT_YUV420
  • 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 various YUV byte orders.
13     by Jean-Marc Orliaguet <jmo@medialab.chalmers.se>
14
15     Added support for NTSC/PAL/SECAM video norm selection. (14/10/99)
16     by Jean-Marc Orliaguet <jmo@medialab.chalmers.se>
17
18     Early stage V4L2 implementation. (v0.92a)
19     by Jean-Marc Orliaguet <jmo@medialab.chalmers.se>
20     Capture code taken from xcaptest.c V4L2 demo app (Bill Dirks)
21     Some code contributed by Malcolm Caldwell <malcolm@it.ntu.edu.au>
22
23     Added support for MJPEG/JPEG.
24     Added gamma and gain controls.
25     by Douglas Kosovic <douglask@itee.uq.edu.au>
26     MJPEG/JPEG support uses Tiny Jpeg Decoder from :
27     http://www.saillard.org/programs_and_patches/tinyjpegdecoder/
28
29   ========================================================================= */
30
31#include <stdio.h>
32#include <stdlib.h>
33#include <unistd.h>
34#include <string.h>
35#include <signal.h>
36#include <errno.h>
37#include <endian.h>
38
39#include <sys/types.h>
40#include <sys/fcntl.h>
41#include <sys/ioctl.h>
42#include <sys/time.h>
43#include <sys/mman.h>
44
45#include <X11/Xlib.h>
46#include <X11/Xutil.h>
47#include <X11/keysym.h>
48
49extern "C"
50{
51#include <asm/types.h>
52#include <linux/videodev2.h>
53}
54
55#ifdef HAVE_LIBV4L
56#include <libv4l2.h>
57#else
58#define v4l2_open open
59#define v4l2_close close
60#define v4l2_ioctl ioctl
61#define v4l2_read read
62#define v4l2_mmap mmap
63#define v4l2_munmap munmap
64#include "tinyjpeg.h"
65#endif
66
67#include "grabber.h"
68#include "vic_tcl.h"
69#include "device-input.h"
70#include "module.h"
71
72/* here you can tune the device names */
73static const char *devlist[] = {
74    "/dev/video0", "/dev/video1", "/dev/video2", "/dev/video3",
75    "/dev/video4", "/dev/video5", "/dev/video6", "/dev/video7",
76    "/dev/video8", "/dev/video9", "/dev/video10", "/dev/video11",
77    NULL
78};
79
80#define NTSC_WIDTH  640
81#define NTSC_HEIGHT 480
82#define PAL_WIDTH   768
83#define PAL_HEIGHT  576
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/* YUV Byte order */
92#define BYTE_ORDER_YUYV 0
93#define BYTE_ORDER_YVYU 1
94#define BYTE_ORDER_UYVY 2
95#define BYTE_ORDER_VYUY 3
96
97/* V4L2 driver specific controls */
98#ifndef V4L2_CID_POWER_LINE_FREQUENCY
99#define V4L2_CID_POWER_LINE_FREQUENCY (V4L2_CID_PRIVATE_BASE + 1)
100#endif
101#ifndef V4L2_CID_FOCUS_AUTO
102#define V4L2_CID_FOCUS_AUTO           (V4L2_CID_PRIVATE_BASE + 4)
103#endif
104#ifndef V4L2_CID_FOCUS_ABSOLUTE
105#define V4L2_CID_FOCUS_ABSOLUTE       (V4L2_CID_PRIVATE_BASE + 5)
106#endif
107#ifndef V4L2_CID_FOCUS_RELATIVE
108#define V4L2_CID_FOCUS_RELATIVE       (V4L2_CID_PRIVATE_BASE + 6)
109#endif
110
111typedef struct tag_vimage
112{
113        struct v4l2_buffer      vidbuf;
114        char                    *data;
115}       VIMAGE;
116
117
118#define STREAMBUFS 2
119
120class V4l2Grabber : public Grabber
121{
122public:
123        V4l2Grabber(const char * cformat, const char *dev);
124        virtual ~V4l2Grabber();
125
126        virtual int  command(int argc, const char*const* argv);
127        virtual void start();
128        virtual void stop();
129        virtual int  grab();
130
131
132protected:
133        void format();
134        void setsize();
135
136        void packed422_to_planar422(char *, const char*);
137        void packed422_to_planar420(char *, const char*);
138
139#ifndef HAVE_LIBV4L
140        void jpeg_to_planar420(char *, const char*);
141#endif
142
143        void setctrl(int, int, const char *, int);
144
145        struct v4l2_capability   capability;
146        struct v4l2_input        *inputs;
147        struct v4l2_input        input;
148        struct v4l2_queryctrl    qctrl;
149        struct v4l2_format       fmt;
150        struct v4l2_requestbuffers req;
151
152        /* mmap */
153        int                      have_mmap;
154        VIMAGE                   vimage[STREAMBUFS];
155        struct v4l2_buffer       tempbuf;
156        int                      buf_sync;
157
158        __u32   pixelformat;
159        int fd_;
160        int format_;
161        int have_YUV422;
162        int have_YUV422P;
163        int have_YUV420P;
164        int have_MJPEG;
165        int have_JPEG;
166
167        int byteorder_;
168        int cformat_;
169        int port_;
170        int norm_;
171
172        unsigned char *tm_;
173        int width_;
174        int height_;
175        int max_width_;
176        int max_height_;
177        int decimate_;
178        int bytesused_;
179
180#ifndef HAVE_LIBV4L
181        struct jdec_private *jpegdec_;
182#endif
183};
184
185/* ----------------------------------------------------------------- */
186
187class V4l2Device : public InputDevice
188{
189public:
190        V4l2Device(const char *dev, const char*, char *attr);
191        virtual int command(int argc, const char*const* argv);
192
193private:
194        const char *dev_;
195};
196
197
198V4l2Device::V4l2Device(const char *dev, const char *name, char *attr) : InputDevice(name)
199{
200        dev_ = dev;
201        attributes_ = attr;
202        debug_msg("V4L2:  ==> %s\n",attr);
203}
204
205int V4l2Device::command(int argc, const char*const* argv)
206{
207        Tcl& tcl = Tcl::instance();
208
209
210        if (argc == 3) {
211                if (strcmp(argv[1], "open") == 0) {
212                        TclObject* o = 0;
213                        o = new V4l2Grabber(argv[2],dev_);
214                        if (o != 0)
215                                tcl.result(o->name());
216                        return (TCL_OK);
217                }
218
219        }
220        return (InputDevice::command(argc, argv));
221}
222
223/* ----------------------------------------------------------------- */
224
225class V4l2Scanner
226{
227public:
228        V4l2Scanner(const char **dev);
229};
230
231static V4l2Scanner find_video4linux_devices(devlist);
232
233V4l2Scanner::V4l2Scanner(const char **dev)
234{
235        struct v4l2_capability   capability;
236        struct v4l2_input        input;
237
238        int  k,i,err,fd;
239        char *nick, *attr;
240
241        // VIC_DEVICE env variable selects V4L device with AGTk 3.02 and earlier
242        // but doesn't work if V4L2 devices are listed before the V4L device
243        // so don't list V4L2 devices if VIC_DEVICE env variable is set.
244        const char *myDev = getenv("VIC_DEVICE");
245        if (myDev != 0)
246        {
247                 return;
248        }
249
250        for (i = 0; dev[i] != NULL; i++) {
251                debug_msg("V4L2: trying %s... ",dev[i]);
252                if (-1 == (fd = v4l2_open(dev[i],O_RDWR))) {
253                        debug_msg("Error opening: %s : %s\n", dev[i], strerror(errno));
254                        continue;
255                }
256#ifdef HAVE_LIBV4L
257                if (-1 == v4l2_fd_open(fd,V4L2_ENABLE_ENUM_FMT_EMULATION)) {
258                        perror("V4L2: v4l2_fd_open V4L2_ENABLE_ENUM_FMT_EMULATION");
259                        v4l2_close(fd);
260                        continue;
261                }
262#endif
263                memset(&capability,0,sizeof(capability));
264                if (-1 == v4l2_ioctl(fd,VIDIOC_QUERYCAP,&capability)) {
265                        perror("V4L2: ioctl VIDIOC_QUERYCAP");
266                        v4l2_close(fd);
267                        continue;
268                }
269
270                if (capability.capabilities & V4L2_CAP_VIDEO_CAPTURE == 0) {
271                        debug_msg("%s, %s can't capture\n",capability.card,capability.bus_info);
272                        v4l2_close(fd);
273                        continue;
274                }
275
276                attr = new char[512];
277                strcpy(attr,"format { 420 422 cif } ");
278                strcat(attr,"size { small large cif } ");
279
280                debug_msg("V4L2:   ports:");
281                strcat(attr,"port { ");
282                //input.index = 0;
283                memset(&input, 0, sizeof(input));
284                while (-1 != v4l2_ioctl(fd, VIDIOC_ENUMINPUT, &input)) {
285                        if (-1 == v4l2_ioctl(fd,VIDIOC_S_INPUT,&input.index)) {
286                                debug_msg("ioctl VIDIOC_S_INPUT: %s", strerror(errno));
287                        } else {
288
289                                debug_msg(" %s: ", (char*)input.name);
290                                for (unsigned int s=0 ; s<strlen((char*)input.name) ; s++)
291                                        if (input.name[s]==' ') input.name[s]='-';
292                                strcat(attr,(const char*)input.name);
293                                strcat(attr," ");
294                        }
295                        input.index++;
296                }
297                debug_msg("\n");
298                strcat(attr,"} ");
299
300                debug_msg("V4L2:   norms: ");
301                strcat(attr,"type { ");
302
303                for (k = 0, err = 0; err == 0; ++k)
304                {
305                        struct v4l2_standard     estd;
306                        memset(&estd,0,sizeof(estd));
307                        estd.index = k;
308                        err = v4l2_ioctl(fd, VIDIOC_ENUMSTD, &estd);
309                        if (!err) {
310                                strcat(attr, (const char*)estd.name);
311                                strcat(attr," ");
312                                debug_msg("%s ", estd.name);
313                        }
314                }
315                debug_msg("\n");
316
317                strcat(attr,"} ");
318
319                nick = new char[strlen((char*)capability.card)+strlen(dev[i])+7];
320                sprintf(nick,"V4L2-%s %s",capability.card, dev[i]);
321                new V4l2Device(dev[i],nick,attr);
322                fprintf(stderr, "Attached to V4L2 device: %s\n", nick);
323                v4l2_close(fd);
324        }
325}
326
327/* ----------------------------------------------------------------- */
328
329V4l2Grabber::V4l2Grabber(const char *cformat, const char *dev)
330{
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, 320, 0};
365        unsigned int test_height[] = {CIF_HEIGHT, 240, 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]) {
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 {
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]) {
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 {
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]) {
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 {
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]) {
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 {
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        Tcl &tcl = Tcl::instance();
502
503        byteorder_ = 0;
504
505        if ( tcl.attr("yuv_byteOrder") != NULL )
506                byteorder_ = atoi( tcl.attr("yuv_byteOrder") );
507
508        if ( ! ((byteorder_ >= 0) && (byteorder_ <= 3)) ) byteorder_=0;
509
510        if (argc == 3) {
511                if (strcmp(argv[1], "decimate") == 0) {
512                        decimate_ = atoi(argv[2]);
513
514                        if (running_) {
515                                stop(); start();
516                        }
517                        return (TCL_OK);
518                }
519
520                if (strcmp(argv[1], "port") == 0) {
521
522                        for (i = 0, err = 0; err == 0; ++i) {
523                                struct v4l2_input inp;
524                                memset(&inp,0,sizeof(inp));
525                                inp.index = i;
526                                err = v4l2_ioctl(fd_, VIDIOC_ENUMINPUT, &inp);
527                                if (!err) {
528
529                                        char input[32];
530                                        unsigned int s;
531                                        for ( s=0 ; s<= strlen((const char*)inp.name) ; s++ )
532                                                        if (inp.name[s]==' ') input[s]='-'; else input[s]=inp.name[s];
533
534                                        if ( !strcmp(input,argv[2])) {
535                                                port_ = i;
536                                                break;
537                                        }
538                                }
539                        }
540                        if (running_) {
541                                stop(); start();
542                        }
543
544                        return (TCL_OK);
545                }
546
547
548                if (strcmp(argv[1], "brightness") == 0) {
549                        setctrl(atoi(argv[2]), V4L2_CID_BRIGHTNESS, "Brightness", 0);
550                        return (TCL_OK);
551                }
552
553                if (strcmp(argv[1], "hue") == 0) {
554                        setctrl(atoi(argv[2]), V4L2_CID_HUE, "Hue", 0);
555                        return (TCL_OK);
556                }
557
558                if (strcmp(argv[1], "contrast") == 0) {
559                        setctrl(atoi(argv[2]), V4L2_CID_CONTRAST, "Contrast", 0);
560                        return (TCL_OK);
561                }
562
563                if (strcmp(argv[1], "saturation") == 0) {
564                        setctrl(atoi(argv[2]), V4L2_CID_SATURATION, "Saturation", 0);
565                        return (TCL_OK);
566                }
567
568                if (strcmp(argv[1], "gamma") == 0) {
569                        setctrl(atoi(argv[2]), V4L2_CID_GAMMA, "Gamma", 0);
570                        return (TCL_OK);
571                }
572
573                if (strcmp(argv[1], "gain") == 0) {
574                        setctrl(atoi(argv[2]), V4L2_CID_GAIN, "Gain", 0);
575                        return (TCL_OK);
576                }
577
578                if (strcmp(argv[1], "antiflicker") == 0) {
579                        struct v4l2_queryctrl qctrl;
580                        struct v4l2_control ctrl;
581
582                        memset (&qctrl, 0, sizeof(qctrl));
583                        qctrl.id = V4L2_CID_POWER_LINE_FREQUENCY;
584                        if (-1 != v4l2_ioctl(fd_, VIDIOC_QUERYCTRL, &qctrl)) {
585                                if (strcmp((char *)qctrl.name, "Power Line Frequency") == 0) {
586                                        ctrl.id = qctrl.id;
587                                        ctrl.value = atoi(argv[2]);
588                                        if (-1 == v4l2_ioctl(fd_, VIDIOC_S_CTRL, &ctrl))
589                                                perror("ioctl  VIDIOC_S_CTRL");
590                                        else
591                                                debug_msg( "V4L2: V4L2_CID_POWER_LINE_FREQUENCY = %d\n", ctrl.value);
592                                 }
593                        }
594                        return (TCL_OK);
595                }
596
597                if (strcmp(argv[1], "controls") == 0) {
598                        if (strcmp(argv[2], "reset") == 0) {
599                                debug_msg( "V4L2: Resetting controls\n");
600
601                                setctrl(atoi(argv[2]), V4L2_CID_BRIGHTNESS, "Brightness", 1);
602                                setctrl(atoi(argv[2]), V4L2_CID_HUE, "Hue", 1);
603                                setctrl(atoi(argv[2]), V4L2_CID_CONTRAST, "Contrast", 1);
604                                setctrl(atoi(argv[2]), V4L2_CID_SATURATION, "Saturation", 1);
605                                setctrl(atoi(argv[2]), V4L2_CID_GAMMA, "Gamma", 1);
606                                setctrl(atoi(argv[2]), V4L2_CID_GAIN, "Gain", 1);
607                                return (TCL_OK);
608                        }
609                }
610
611                if (strcmp(argv[1], "yuv_byteorder") == 0) {
612                        debug_msg("V4L2: asked for yuv_byteorder\n");
613                        return (TCL_OK);
614                }
615
616                if (strcmp(argv[1], "fps") == 0) {
617                        debug_msg("V4L2: fps %s\n",argv[2]);
618                }
619
620
621                if (strcmp(argv[1], "type") == 0 || strcmp(argv[1], "format") == 0) {
622
623                        for (int k = 0, err = 0; err == 0; ++k)
624                        {
625                                struct v4l2_standard     estd;
626                                memset(&estd,0,sizeof(estd));
627                                estd.index = k;
628                                if ( !(err = v4l2_ioctl(fd_, VIDIOC_ENUMSTD, &estd)) )
629                                        if ( strcasecmp(argv[2], (const char *)estd.name) == 0)
630                                                norm_ = k;
631                        }
632
633                        if (running_) {
634                                stop(); start();
635                        }
636
637                        return (TCL_OK);
638                }
639
640        } else if (argc == 2) {
641                if (strcmp(argv[1], "format") == 0 ||
642                        strcmp(argv[1], "type") == 0) {
643                        return (TCL_OK);
644                }
645        }
646
647        return (Grabber::command(argc, argv));
648}
649
650void V4l2Grabber::start()
651{
652        int     err;
653        int     i;
654
655        if (fd_ > 0 ) {
656                debug_msg("\nV4L2: start\n");
657
658                format();
659
660                if (have_mmap) {
661                        buf_sync = 1;
662
663                        memset(&req, 0, sizeof(req));
664                        req.count = STREAMBUFS;
665                        req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
666                        req.memory = V4L2_MEMORY_MMAP;
667                        err = v4l2_ioctl(fd_, VIDIOC_REQBUFS, &req);
668                        if (err < 0 || req.count < 1) {
669                                debug_msg("REQBUFS returned error %d, count %d\n", errno,req.count);
670                                return;
671                        }
672
673                        for (i = 0; i < (int)req.count; ++i) {
674                                vimage[i].vidbuf.index = i;
675                                vimage[i].vidbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
676                                vimage[i].vidbuf.memory = V4L2_MEMORY_MMAP;
677                                err = v4l2_ioctl(fd_, VIDIOC_QUERYBUF, &vimage[i].vidbuf);
678                                if (err < 0) {
679                                        debug_msg("QUERYBUF returned error %d\n", errno);
680                                        return;
681                                }
682                                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);
683
684                                if ((long)vimage[i].data == -1) {
685                                        debug_msg("V4L2: mmap() returned error %d\n", errno);
686                                        return;
687                                } else debug_msg("V4L2: mmap()'ed buffer at 0x%lx (%u bytes)\n", (unsigned long)vimage[i].data, vimage[i].vidbuf.length);
688                        }
689
690                        for (i = 0; i < (int)req.count; ++i)
691                                if ((err = v4l2_ioctl(fd_, VIDIOC_QBUF, &vimage[i].vidbuf))) {
692                                        debug_msg("QBUF returned error %d\n",errno);
693                                        return;
694                                }
695
696                        err = v4l2_ioctl(fd_, VIDIOC_STREAMON, &vimage[0].vidbuf.type);
697                        if (err)
698                                debug_msg("STREAMON returned error %d\n",errno);
699
700                } else {
701                        vimage[0].data = (typeof(vimage[0].data)) malloc(fmt.fmt.pix.sizeimage);
702
703                        if (vimage[0].data == NULL) {
704                                debug_msg("malloc(%d) failed\n", fmt.fmt.pix.sizeimage);
705                                return;
706                        } else debug_msg("V4L2: malloc()'ed buffer (%d bytes)\n", fmt.fmt.pix.sizeimage);
707                }
708
709                Grabber::start();
710                running_ = 1;
711        }
712}
713
714void V4l2Grabber::stop()
715{
716        debug_msg("V4L2: stop\n");
717        int i, err;
718
719        if (have_mmap) {
720                if ( fd_ > 0 ) {
721                        i = V4L2_BUF_TYPE_VIDEO_CAPTURE;
722                        if ( (err = v4l2_ioctl(fd_, VIDIOC_STREAMOFF, &i) ) )
723                                debug_msg("V4L2: VIDIOC_STREAMOFF failed\n");
724                }
725
726                tempbuf.type = vimage[0].vidbuf.type;
727                while ((err = v4l2_ioctl(fd_, VIDIOC_DQBUF, &tempbuf)) < 0 &&
728                       (errno == EINTR));
729
730                if (err < 0) {
731                        debug_msg("V4L2: VIDIOC_DQBUF failed: %s\n", strerror(errno));
732                }
733
734                for (i = 0; i < STREAMBUFS; ++i) {
735                        if (vimage[i].data)
736                                err = v4l2_munmap(vimage[i].data, vimage[i].vidbuf.length);
737                        vimage[i].data = NULL;
738                }
739
740
741        } else {
742                if (vimage[0].data)
743                        free(vimage[0].data);
744                vimage[0].data = NULL;
745        }
746
747        Grabber::stop();
748        running_ = 0;
749}
750
751int V4l2Grabber::grab()
752{
753        char  *fr=NULL;
754        fd_set                  rdset;
755        struct timeval          timeout;
756        int                     n;
757
758        bytesused_ = 0;
759        if (have_mmap) {
760                FD_ZERO(&rdset);
761                FD_SET(fd_, &rdset);
762                timeout.tv_sec = 0;
763                timeout.tv_usec = 0;
764                n = select(fd_+1, &rdset, NULL, NULL, &timeout);
765
766                if (n == 0)
767                        return (0);
768                else if (FD_ISSET(fd_, &rdset)) {
769                        memset(&tempbuf, 0, sizeof(struct v4l2_buffer));
770                        tempbuf.type = vimage[0].vidbuf.type;
771                        tempbuf.memory = vimage[0].vidbuf.memory;
772                        if (-1 == v4l2_ioctl(fd_, VIDIOC_DQBUF, &tempbuf))
773                                perror("ioctl  VIDIOC_DQBUF");
774
775                        if (  (req.count > 1) ) buf_sync++;
776                        fr = vimage[buf_sync%2].data;
777                        bytesused_ = tempbuf.bytesused;
778
779                }
780
781        } else {
782                fr = vimage[0].data;
783                v4l2_read(fd_, vimage[0].data, fmt.fmt.pix.sizeimage);
784                bytesused_ = fmt.fmt.pix.sizeimage;
785        }
786
787        switch (cformat_) {
788        case CF_420:
789        case CF_CIF:
790                if( have_YUV420P )
791                       memcpy((void *)frame_, (const void *)fr, (size_t)height_*width_*3/2)
792;
793                else if( have_YUV422 )
794                       packed422_to_planar420((char*)frame_,fr);
795#ifndef HAVE_LIBV4L
796                else if( have_MJPEG || have_JPEG) {
797                       jpeg_to_planar420((char*)frame_,fr);
798                }
799#endif
800                break;
801
802        case CF_422:
803                if (have_YUV422P)
804                       memcpy((void *)frame_, (const void *)fr, (size_t)height_*width_*2);
805                else if( have_YUV422 )
806                       packed422_to_planar422((char*)frame_,fr);
807#ifndef HAVE_LIBV4L
808                else if( have_MJPEG  || have_JPEG)
809                       // jpeg_to_planar422((char*)frame_,fr);
810#endif
811                break;
812        }
813
814        if (have_mmap)
815                v4l2_ioctl(fd_, VIDIOC_QBUF, &vimage[buf_sync%2].vidbuf);
816
817        suppress(frame_);
818        saveblks(frame_);
819        YuvFrame f(media_ts(), frame_, crvec_, outw_, outh_);
820        return (target_->consume(&f));
821}
822
823
824void V4l2Grabber::packed422_to_planar422(char *dest, const char *src)
825{
826    int i;
827    char *y,*u,*v;
828    unsigned int a, *srca;
829
830    srca = (unsigned int *)src;
831
832    i = (width_ * height_)/2;
833    y = dest;
834    u = y + width_ * height_;
835    v = u + width_ * height_ / 2;
836
837    switch (byteorder_) {
838    case BYTE_ORDER_YUYV:
839        while (--i) {
840                a = *(srca++);
841#if BYTE_ORDER == LITTLE_ENDIAN 
842                *(y++) = a & 0xff;
843                a >>= 8;
844                *(u++) = a & 0xff;
845                a >>= 8;
846                *(y++) = a & 0xff;
847                a >>= 8;
848                *(v++) = a & 0xff;
849#else
850                *(v++) = a & 0xff;
851                a >>= 8;
852                *(y++) = a & 0xff;
853                a >>= 8;
854                *(u++) = a & 0xff;
855                a >>= 8;
856                *(y++) = a & 0xff;
857#endif
858        }
859        break;
860
861    case BYTE_ORDER_YVYU:
862        while (--i) {
863                a = *(srca++);
864#if BYTE_ORDER == LITTLE_ENDIAN 
865                *(y++) = a & 0xff;
866                a >>= 8;
867                *(v++) = a & 0xff;
868                a >>= 8;
869                *(y++) = a & 0xff;
870                a >>= 8;
871                *(u++) = a & 0xff;
872#else
873                *(u++) = a & 0xff;
874                a >>= 8;
875                *(y++) = a & 0xff;
876                a >>= 8;
877                *(v++) = a & 0xff;
878                a >>= 8;
879                *(y++) = a & 0xff;
880#endif
881        }
882        break;
883
884    case BYTE_ORDER_UYVY:
885        while (--i) {
886                a = *(srca++);
887#if BYTE_ORDER == LITTLE_ENDIAN 
888                *(u++) = a & 0xff;
889                a >>= 8;
890                *(y++) = a & 0xff;
891                a >>= 8;
892                *(v++) = a & 0xff;
893                a >>= 8;
894                *(y++) = a & 0xff;
895#else
896                *(y++) = a & 0xff;
897                a >>= 8;
898                *(v++) = a & 0xff;
899                a >>= 8;
900                *(y++) = a & 0xff;
901                a >>= 8;
902                *(u++) = a & 0xff;
903#endif
904        }
905        break;
906
907    case BYTE_ORDER_VYUY:
908        while (--i) {
909                a = *(srca++);
910#if BYTE_ORDER == LITTLE_ENDIAN 
911                *(v++) = a & 0xff;
912                a >>= 8;
913                *(y++) = a & 0xff;
914                a >>= 8;
915                *(u++) = a & 0xff;
916                a >>= 8;
917                *(y++) = a & 0xff;
918#else
919                *(y++) = a & 0xff;
920                a >>= 8;
921                *(u++) = a & 0xff;
922                a >>= 8;
923                *(y++) = a & 0xff;
924                a >>= 8;
925                *(v++) = a & 0xff;
926#endif
927        }
928        break;
929    }
930
931}
932
933void V4l2Grabber::packed422_to_planar420(char *dest, const char *src)
934{
935    int  a1,b;
936    char *y,*u,*v;
937    unsigned int a, *srca;
938
939    srca = (unsigned int *)src;
940
941    y = dest;
942    u = y + width_ * height_;
943    v = u + width_ * height_ / 4;
944
945    switch (byteorder_) {
946    case BYTE_ORDER_YUYV:
947        for (a1 = height_; a1 > 0; a1 -= 2) {
948                for (b = width_; b > 0; b -= 2) {
949                        a = *(srca++);
950#if BYTE_ORDER == LITTLE_ENDIAN 
951                        *(y++) = a & 0xff; a >>= 8;
952                        *(u++) = a & 0xff; a >>= 8;
953                        *(y++) = a & 0xff; a >>= 8;
954                        *(v++) = a & 0xff;
955#else
956                        *(v++) = a & 0xff; a >>= 8;
957                        *(y+1) = a & 0xff; a >>= 8;
958                        *(u++) = a & 0xff; a >>= 8;
959                        *(y) = a;  y += 2;
960#endif
961                }
962                for (b = width_; b > 0; b -= 2) {
963                        a = *(srca++);
964#if BYTE_ORDER == LITTLE_ENDIAN 
965                        *(y++) = a & 0xff; a >>= 16;
966                        *(y++) = a & 0xff;
967#else
968                        a >>= 8;
969                        *(y+1) = a & 0xff; a >>= 16;
970                        *(y) = a; y += 2;
971#endif
972                }
973        }
974        break;
975
976    case BYTE_ORDER_YVYU:
977        for (a1 = height_; a1 > 0; a1 -= 2) {
978                for (b = width_; b > 0; b -= 2) {
979                        a = *(srca++);
980#if BYTE_ORDER == LITTLE_ENDIAN 
981                        *(y++) = a & 0xff; a >>= 8;
982                        *(v++) = a & 0xff; a >>= 8;
983                        *(y++) = a & 0xff; a >>= 8;
984                        *(u++) = a & 0xff;
985#else
986                        *(u++) = a & 0xff; a >>= 8;
987                        *(y+1) = a & 0xff; a >>= 8;
988                        *(v++) = a & 0xff; a >>= 8;
989                        *(y) = a;  y += 2;
990#endif
991                }
992                for (b = width_; b > 0; b -= 2) {
993                        a = *(srca++);
994#if BYTE_ORDER == LITTLE_ENDIAN 
995                        *(y++) = a & 0xff; a >>= 16;
996                        *(y++) = a & 0xff;
997#else
998                        a >>= 8;
999                        *(y+1) = a & 0xff; a >>= 16;
1000                        *(y) = a; y += 2;
1001#endif
1002                }
1003        }
1004        break;
1005
1006    case BYTE_ORDER_UYVY:
1007        for (a1 = height_; a1 > 0; a1 -= 2) {
1008                for (b = width_; b > 0; b -= 2) {
1009                        a = *(srca++);
1010#if BYTE_ORDER == LITTLE_ENDIAN 
1011                        *(u++) = a & 0xff; a >>= 8;
1012                        *(y++) = a & 0xff; a >>= 8;
1013                        *(v++) = a & 0xff; a >>= 8;
1014                        *(y++) = a & 0xff;
1015#else
1016                        *(y+1) = a & 0xff; a >>= 8;
1017                        *(v++) = a & 0xff; a >>= 8;
1018                        *(y) = a & 0xff; a >>= 8;
1019                        *(u++) = a & 0xff;
1020                        y += 2;
1021#endif
1022                }
1023                for (b = width_; b > 0; b -= 2) {
1024                        a = *(srca++);
1025#if BYTE_ORDER == LITTLE_ENDIAN 
1026                        a >>= 8;
1027                        *(y++) = a & 0xff; a >>= 16;
1028                        *(y++) = a & 0xff;
1029#else
1030                        *(y+1) = a & 0xff; a >>= 16;
1031                        *(y) = a; y += 2;
1032#endif
1033                }
1034        }
1035        break;
1036
1037    case BYTE_ORDER_VYUY:
1038        for (a1 = height_; a1 > 0; a1 -= 2) {
1039                for (b = width_; b > 0; b -= 2) {
1040                        a = *(srca++);
1041#if BYTE_ORDER == LITTLE_ENDIAN 
1042                        *(v++) = a & 0xff; a >>= 8;
1043                        *(y++) = a & 0xff; a >>= 8;
1044                        *(u++) = a & 0xff; a >>= 8;
1045                        *(y++) = a & 0xff;
1046#else
1047                        *(y+1) = a & 0xff; a >>= 8;
1048                        *(u++) = a & 0xff; a >>= 8;
1049                        *(y) = a & 0xff; a >>= 8;
1050                        *(v++) = a & 0xff;
1051                        y += 2;
1052#endif
1053                }
1054                for (b = width_; b > 0; b -= 2) {
1055                        a = *(srca++);
1056#if BYTE_ORDER == LITTLE_ENDIAN 
1057                        a >>= 8;
1058                        *(y++) = a & 0xff; a >>= 16;
1059                        *(y++) = a & 0xff;
1060#else
1061                        *(y+1) = a & 0xff; a >>= 16;
1062                        *(y) = a; y += 2;
1063#endif
1064                }
1065        }
1066        break;
1067    }
1068}
1069
1070#ifndef HAVE_LIBV4L
1071void V4l2Grabber::jpeg_to_planar420(char *dest, const char *src)
1072{
1073        char *y,*u,*v;
1074        unsigned char *components[4];
1075
1076        y = dest;
1077        u = y + width_ * height_;
1078        v = u + width_ * height_ /4;
1079
1080        components[0] = (unsigned char *) y;
1081        components[1] = (unsigned char *) u;
1082        components[2] = (unsigned char *) v;
1083
1084        if (jpegdec_ == NULL) {
1085                jpegdec_ = tinyjpeg_init();
1086                if (jpegdec_ == NULL) {
1087                        return;
1088                }
1089                tinyjpeg_set_flags(jpegdec_, TINYJPEG_FLAGS_MJPEG_TABLE);
1090        }
1091        tinyjpeg_set_components(jpegdec_, components, 4);
1092        if (tinyjpeg_parse_header(jpegdec_, (unsigned char *)src, bytesused_) < 0) {
1093               debug_msg("V4L2 Jpeg error: %s", tinyjpeg_get_errorstring(jpegdec_));
1094               return;
1095        }
1096        if (tinyjpeg_decode(jpegdec_, TINYJPEG_FMT_YUV420P) < 0) {
1097               debug_msg("V4L2 Jpeg error: %s", tinyjpeg_get_errorstring(jpegdec_));
1098               return;
1099        }
1100}
1101#endif
1102
1103
1104void V4l2Grabber::format()
1105{
1106
1107        struct v4l2_standard    standard;
1108
1109        int i, err;
1110        int input;
1111        int format_ok = 0;
1112        int try_ntsc = 0;
1113
1114        switch (cformat_) {
1115        case CF_420:
1116        case CF_CIF:
1117                if( have_YUV420P )
1118                       pixelformat = V4L2_PIX_FMT_YUV420;
1119                else if( have_YUV422 )
1120                       pixelformat = V4L2_PIX_FMT_YUYV;
1121#ifndef HAVE_LIBV4L
1122                else if( have_MJPEG )
1123                       pixelformat = V4L2_PIX_FMT_MJPEG;
1124                else if( have_JPEG )
1125                       pixelformat = V4L2_PIX_FMT_JPEG;
1126#endif
1127                break;
1128
1129        case CF_422:
1130                if (have_YUV422P)
1131                       pixelformat = V4L2_PIX_FMT_YUV422P;
1132                else if( have_YUV422 )
1133                       pixelformat = V4L2_PIX_FMT_YUYV;
1134#ifndef HAVE_LIBV4L
1135                else if( have_MJPEG )
1136                       pixelformat = V4L2_PIX_FMT_MJPEG;
1137                else if( have_JPEG )
1138                       pixelformat = V4L2_PIX_FMT_JPEG;
1139#endif
1140                break;
1141        }
1142
1143        while ( !format_ok ) {
1144                width_  = CIF_WIDTH  *2  / decimate_;
1145                height_ = CIF_HEIGHT *2  / decimate_;
1146
1147                debug_msg("V4L2: format");
1148                switch (cformat_) {
1149                case CF_CIF:
1150                        set_size_420(width_, height_);
1151                        debug_msg(" cif\n");
1152                        break;
1153                case CF_420:
1154                        set_size_420(width_, height_);
1155                        debug_msg(" 420\n");
1156                        break;
1157                case CF_422:
1158                        set_size_422(width_, height_);
1159                        debug_msg(" 422\n");
1160                        break;
1161                }
1162                debug_msg("decimate: %d\n",decimate_);
1163
1164                memset(&standard,0,sizeof(standard));
1165                standard.index = norm_;
1166                err = v4l2_ioctl(fd_, VIDIOC_ENUMSTD, &standard);
1167                if (!err) {
1168                        if (-1 == v4l2_ioctl(fd_, VIDIOC_S_STD, &standard.id))
1169                                perror("ioctl VIDIOC_S_STD");
1170                        else debug_msg("V4L2: setting norm to %s\n",standard.name);
1171                }
1172
1173                input = port_;
1174                if ((err = v4l2_ioctl(fd_, VIDIOC_S_INPUT, &input)) )
1175                        debug_msg("S_INPUT returned error %d\n",errno);
1176                else debug_msg("V4L2: setting input port to %d\n", port_);
1177
1178                for (i = 0, err = 0; err == 0; ++i) {
1179                        struct v4l2_fmtdesc     fmtd;
1180                        memset(&fmtd,0,sizeof(fmtd));
1181                        fmtd.index = i;
1182                        fmtd.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1183                        err = v4l2_ioctl(fd_, VIDIOC_ENUM_FMT, &fmtd);
1184                        if (err) {
1185                                debug_msg("VIDIOC_ENUM_FMT returned error %d\n",errno);
1186                        } else {
1187                                if (fmtd.pixelformat == pixelformat) {
1188                                        memset(&fmt,0,sizeof(fmt));
1189                                        fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1190                                        v4l2_ioctl(fd_, VIDIOC_G_FMT, &fmt);
1191                                        fmt.fmt.pix.width = width_;
1192                                        fmt.fmt.pix.height = height_;
1193                                        fmt.fmt.pix.field = V4L2_FIELD_ANY;
1194                                        fmt.fmt.pix.pixelformat = pixelformat;
1195
1196                                        if ( (err = v4l2_ioctl(fd_, VIDIOC_S_FMT, &fmt) ) )
1197                                                debug_msg("\nV4L2: Failed to set format\n");
1198
1199                                        if ( ( fmt.fmt.pix.width != (unsigned int)width_ ) ||
1200                                                                ( fmt.fmt.pix.height !=  (unsigned int)height_ ) ) {
1201
1202                                                debug_msg("V4L2: failed to set format! requested %dx%d, got %dx%d\n", width_, height_, fmt.fmt.pix.width, fmt.fmt.pix.height);
1203
1204                                                switch(decimate_) {
1205                                                case 1:
1206                                                        if (!try_ntsc) {
1207                                                                debug_msg("V4L2: trying NTSC resolution ...\n");
1208                                                                width_ = NTSC_WIDTH;
1209                                                                height_ = NTSC_HEIGHT;
1210                                                                try_ntsc = 1;
1211                                                        } else {
1212                                                                debug_msg("V4L2: trying resolution under ...\n");
1213                                                                decimate_ = 2;
1214                                                                try_ntsc = 0;
1215                                                        }
1216                                                        break;
1217                                                case 2:
1218                                                        if (!try_ntsc) {
1219                                                                debug_msg("V4L2: trying 1/4 NTSC resolution ...\n");
1220                                                                width_ = NTSC_WIDTH / 2;
1221                                                                height_ = NTSC_HEIGHT / 2;
1222                                                                try_ntsc = 1;
1223                                                        } else {
1224                                                                debug_msg("V4L2: trying resolution under ...\n");
1225                                                                decimate_ = 4;
1226                                                                try_ntsc = 0;
1227                                                        }
1228                                                        break;
1229                                                default:
1230                                                        if (!try_ntsc) {
1231                                                                debug_msg("V4L2: trying 1/16 NTSC resolution ...\n");
1232                                                                width_ = NTSC_WIDTH / 4;
1233                                                                height_ = NTSC_HEIGHT / 4;
1234                                                                try_ntsc = 0;
1235                                                        } else {
1236                                                                debug_msg("V4L2: giving up ...\n");
1237                                                                format_ok = 1;
1238                                                        }
1239                                                        break;
1240
1241                                                }
1242
1243                                        } else {
1244                                                debug_msg("V4L2: setting format: width=%d height=%d\n", fmt.fmt.pix.width, fmt.fmt.pix.height);
1245                                                format_ok = 1;
1246                                        }
1247                                        break;
1248                                }
1249
1250                        }
1251                }
1252        }
1253
1254
1255        allocref();
1256}
1257
1258void V4l2Grabber::setctrl(int val, int cid, const char *controlname, int reset)
1259{
1260        struct v4l2_queryctrl qctrl;
1261        struct v4l2_control ctrl;
1262
1263        qctrl.id = cid;
1264        if (-1 != v4l2_ioctl(fd_, VIDIOC_QUERYCTRL, &qctrl)) {
1265                if ( !(qctrl.flags & V4L2_CTRL_FLAG_DISABLED) ) {
1266
1267                        ctrl.id = cid;
1268                        if ( reset )
1269                                ctrl.value = qctrl.default_value;
1270                        else
1271                                ctrl.value = qctrl.minimum + (qctrl.maximum - qctrl.minimum) * val/256;
1272                        if (-1 == v4l2_ioctl(fd_, VIDIOC_S_CTRL,&ctrl))
1273                                perror("ioctl  VIDIOC_S_CTRL");
1274                        else debug_msg( "V4L2: %s = %d\n", controlname, val);
1275                }
1276        }
1277        return;
1278}
Note: See TracBrowser for help on using the browser.