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

Revision 4221, 40.2 KB (checked in by douglask, 6 years ago)

Apply patch posted by Claus Endres to the AG-tech mailing list, includes fix for resolution selection at large resolution.

Addition fixes include:
debug output fixes, including spelling mistake and missing newlines
fixed pixelformat selection for some UVC webcams that only support uncompressed video at <= QCIF resolution, but MJPEG at higher resolutions

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