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

Revision 4115, 39.5 KB (checked in by douglask, 7 years ago)

Corrected ioctl for VIDIOC_QUERYCTRL

  • 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, 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", 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                have_YUV420P = 1;
337                debug_msg("\nDevice capture V4L2_PIX_FMT_YUV420\n");
338        }
339
340        fmt.fmt.pix.width = CIF_WIDTH;
341        fmt.fmt.pix.height = CIF_HEIGHT;
342        fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV422P;
343        if (-1 != ioctl(fd_, VIDIOC_S_FMT, &fmt) ) {
344                have_YUV422P = 1;
345                debug_msg("\nDevice capture V4L2_PIX_FMT_YUV422\n");
346        }
347
348        fmt.fmt.pix.width = CIF_WIDTH;
349        fmt.fmt.pix.height = CIF_HEIGHT;
350        fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
351        if (-1 != ioctl(fd_, VIDIOC_S_FMT, &fmt) ) {
352                if (fmt.fmt.pix.width <= CIF_WIDTH && fmt.fmt.pix.height <= CIF_HEIGHT) {
353                        have_YUV422 = 1;
354                        debug_msg("\nDevice capture V4L2_PIX_FMT_YUYV (YUV 4:2:2)\n");
355                }
356        }
357
358        fmt.fmt.pix.width = CIF_WIDTH;
359        fmt.fmt.pix.height = CIF_HEIGHT;
360        fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
361        if (-1 != ioctl(fd_, VIDIOC_S_FMT, &fmt) ) {
362                have_MJPEG = 1;
363                debug_msg("\nDevice capture V4L2_PIX_FMT_MJPEG\n");
364        }
365
366        fmt.fmt.pix.width = CIF_WIDTH;
367        fmt.fmt.pix.height = CIF_HEIGHT;
368        fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_JPEG;
369        if (-1 != ioctl(fd_, VIDIOC_S_FMT, &fmt) ) {
370                have_MJPEG = 1;
371                debug_msg("\nDevice capture V4L2_PIX_FMT_JPEG\n");
372        }
373
374        if( !( have_YUV422P || have_YUV422 || have_YUV420P || have_MJPEG || have_JPEG)){
375                debug_msg("No suitable palette found\n");
376                close(fd_);
377                status_=-1;
378                return;
379        }
380
381        struct v4l2_requestbuffers reqbuf;
382        memset(&reqbuf,0,sizeof(reqbuf));
383        reqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
384        reqbuf.memory = V4L2_MEMORY_MMAP;
385        reqbuf.count = 20;
386        have_mmap = 1;
387        if (-1 == ioctl (fd_, VIDIOC_REQBUFS, &reqbuf)) {
388                if (errno == EINVAL) {
389                        printf("Video capturing or mmap-streaming is not supported\n");
390                        have_mmap = 0;
391                        status_=-1;
392                } else {
393                        perror ("VIDIOC_REQBUFS");
394                        have_mmap = 0;
395                }
396        }
397
398        /* fill in defaults */
399        if(!strcmp(cformat, "411"))
400                cformat_ = CF_411;
401        if(!strcmp(cformat, "422"))
402                cformat_ = CF_422;
403        if(!strcmp(cformat, "cif"))
404                cformat_ = CF_CIF;
405
406        port_      = 0;
407        norm_      = 0;
408        decimate_  = 2;
409        running_   = 0;
410
411}
412
413
414V4l2Grabber::~V4l2Grabber()
415{
416        int i;
417        debug_msg("V4L2: destructor\n");
418
419        if (have_mmap) {
420                for (i = 0; i < STREAMBUFS; ++i) {
421                        if (vimage[i].data)
422                                munmap(vimage[i].data,
423                                       vimage[i].vidbuf.length);
424                        vimage[i].data = NULL;
425                }
426        } else {
427                if (vimage[0].data)
428                        free(vimage[0].data);
429                vimage[0].data = NULL;
430        }
431        close(fd_);
432
433        if (jpegdec_) {
434                tinyjpeg_free(jpegdec_);
435        }
436
437}
438
439int V4l2Grabber::command(int argc, const char*const* argv)
440{
441        int i, err;
442
443        Tcl &tcl = Tcl::instance();
444
445        byteorder_ = 0;
446
447        if ( tcl.attr("yuv_byteOrder") != NULL )
448                byteorder_ = atoi( tcl.attr("yuv_byteOrder") );
449
450        if ( ! ((byteorder_ >= 0) && (byteorder_ <= 3)) ) byteorder_=0;
451
452        if (argc == 3) {
453                if (strcmp(argv[1], "decimate") == 0) {
454                        decimate_ = atoi(argv[2]);
455
456                        if (running_) {
457                                stop(); start();
458                        }
459                        return (TCL_OK);
460                }
461
462                if (strcmp(argv[1], "port") == 0) {
463
464                        for (i = 0, err = 0; err == 0; ++i) {
465                                struct v4l2_input inp;
466                                memset(&inp,0,sizeof(inp));
467                                inp.index = i;
468                                err = ioctl(fd_, VIDIOC_ENUMINPUT, &inp);
469                                if (!err) {
470
471                                        char input[32];
472                                        unsigned int s;
473                                        for ( s=0 ; s<= strlen((const char*)inp.name) ; s++ )
474                                                        if (inp.name[s]==' ') input[s]='-'; else input[s]=inp.name[s];
475
476                                        if ( !strcmp(input,argv[2])) {
477                                                port_ = i;
478                                                break;
479                                        }
480                                }
481                        }
482                        if (running_) {
483                                stop(); start();
484                        }
485
486                        return (TCL_OK);
487                }
488
489
490                if (strcmp(argv[1], "brightness") == 0) {
491                        setctrl(atoi(argv[2]), V4L2_CID_BRIGHTNESS, "Brightness", 0);
492                        return (TCL_OK);
493                }
494
495                if (strcmp(argv[1], "hue") == 0) {
496                        setctrl(atoi(argv[2]), V4L2_CID_HUE, "Hue", 0);
497                        return (TCL_OK);
498                }
499
500                if (strcmp(argv[1], "contrast") == 0) {
501                        setctrl(atoi(argv[2]), V4L2_CID_CONTRAST, "Contrast", 0);
502                        return (TCL_OK);
503                }
504
505                if (strcmp(argv[1], "saturation") == 0) {
506                        setctrl(atoi(argv[2]), V4L2_CID_SATURATION, "Saturation", 0);
507                        return (TCL_OK);
508                }
509
510                if (strcmp(argv[1], "gamma") == 0) {
511                        setctrl(atoi(argv[2]), V4L2_CID_GAMMA, "Gamma", 0);
512                        return (TCL_OK);
513                }
514
515                if (strcmp(argv[1], "gain") == 0) {
516                        setctrl(atoi(argv[2]), V4L2_CID_GAIN, "Gain", 0);
517                        return (TCL_OK);
518                }
519
520                if (strcmp(argv[1], "antiflicker") == 0) {
521                        struct v4l2_queryctrl qctrl;
522                        struct v4l2_control ctrl;
523
524                        memset (&qctrl, 0, sizeof(qctrl));
525                        qctrl.id = V4L2_CID_POWER_LINE_FREQUENCY;
526                        if (-1 != ioctl(fd_, VIDIOC_QUERYCTRL, &qctrl)) {
527                                if (strcmp((char *)qctrl.name, "Power Line Frequency") == 0) {
528                                        ctrl.id = qctrl.id;
529                                        ctrl.value = atoi(argv[2]);
530                                        if (-1 == ioctl(fd_, VIDIOC_S_CTRL, &ctrl))
531                                                perror("ioctl  VIDIOC_S_CTRL");
532                                        else
533                                                debug_msg( "V4L2: V4L2_CID_POWER_LINE_FREQUENCY = %d\n", ctrl.value);
534                                 }
535                        }
536                        return (TCL_OK);
537                }
538
539                if (strcmp(argv[1], "controls") == 0) {
540                        if (strcmp(argv[2], "reset") == 0) {
541                                debug_msg( "V4L2: Resetting controls\n");
542
543                                setctrl(atoi(argv[2]), V4L2_CID_BRIGHTNESS, "Brightness", 1);
544                                setctrl(atoi(argv[2]), V4L2_CID_HUE, "Hue", 1);
545                                setctrl(atoi(argv[2]), V4L2_CID_CONTRAST, "Contrast", 1);
546                                setctrl(atoi(argv[2]), V4L2_CID_SATURATION, "Saturation", 1);
547                                setctrl(atoi(argv[2]), V4L2_CID_GAMMA, "Gamma", 1);
548                                setctrl(atoi(argv[2]), V4L2_CID_GAIN, "Gain", 1);
549                                return (TCL_OK);
550                        }
551                }
552
553                if (strcmp(argv[1], "yuv_byteorder") == 0) {
554                        debug_msg("V4L2: asked for yuv_byteorder\n");
555                        return (TCL_OK);
556                }
557
558                if (strcmp(argv[1], "fps") == 0) {
559                        debug_msg("V4L2: fps %s\n",argv[2]);
560                }
561
562
563                if (strcmp(argv[1], "type") == 0 || strcmp(argv[1], "format") == 0) {
564
565                        for (int k = 0, err = 0; err == 0; ++k)
566                        {
567                                struct v4l2_standard     estd;
568                                memset(&estd,0,sizeof(estd));
569                                estd.index = k;
570                                if ( !(err = ioctl(fd_, VIDIOC_ENUMSTD, &estd)) )
571                                        if ( strcasecmp(argv[2], (const char *)estd.name) == 0)
572                                                norm_ = k;
573                        }
574
575                        if (running_) {
576                                stop(); start();
577                        }
578
579                        return (TCL_OK);
580                }
581
582        } else if (argc == 2) {
583                if (strcmp(argv[1], "format") == 0 ||
584                        strcmp(argv[1], "type") == 0) {
585                        return (TCL_OK);
586                }
587        }
588
589        return (Grabber::command(argc, argv));
590}
591
592void V4l2Grabber::start()
593{
594        int     err;
595        int     i;
596
597        if (fd_ > 0 ) {
598                debug_msg("\nV4L2: start\n");
599
600                format();
601
602                if (have_mmap) {
603                        buf_sync = 1;
604
605                        memset(&req, 0, sizeof(req));
606                        req.count = STREAMBUFS;
607                        req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
608                        req.memory = V4L2_MEMORY_MMAP;
609                        err = ioctl(fd_, VIDIOC_REQBUFS, &req);
610                        if (err < 0 || req.count < 1) {
611                                debug_msg("REQBUFS returned error %d, count %d\n", errno,req.count);
612                                return;
613                        }
614
615                        for (i = 0; i < (int)req.count; ++i) {
616                                vimage[i].vidbuf.index = i;
617                                vimage[i].vidbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
618                                vimage[i].vidbuf.memory = V4L2_MEMORY_MMAP;
619                                err = ioctl(fd_, VIDIOC_QUERYBUF, &vimage[i].vidbuf);
620                                if (err < 0) {
621                                        debug_msg("QUERYBUF returned error %d\n",errno);
622                                        return;
623                                }
624                                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);
625
626                                if ((long)vimage[i].data == -1) {
627                                        debug_msg("V4L2: mmap() returned error %l\n", errno);
628                                        return;
629                                } else debug_msg("V4L2: mmap()'ed buffer at 0x%x (%d bytes)\n", (long)vimage[i].data, vimage[i].vidbuf.length);
630                        }
631
632                        for (i = 0; i < (int)req.count; ++i)
633                                if ((err = ioctl(fd_, VIDIOC_QBUF, &vimage[i].vidbuf))) {
634                                        debug_msg("QBUF returned error %d\n",errno);
635                                        return;
636                                }
637
638                        err = ioctl(fd_, VIDIOC_STREAMON, &vimage[0].vidbuf.type);
639                        if (err)
640                                debug_msg("STREAMON returned error %d\n",errno);
641
642                } else {
643                        vimage[0].data = (typeof(vimage[0].data)) malloc(fmt.fmt.pix.sizeimage);
644
645                        if (vimage[0].data == NULL) {
646                                debug_msg("malloc(%d) failed\n", fmt.fmt.pix.sizeimage);
647                                return;
648                        } else debug_msg("V4L2: malloc()'ed buffer (%d bytes)\n",  fmt.fmt.pix.sizeimage);
649                }
650
651                Grabber::start();
652                running_ = 1;
653        }
654}
655
656void V4l2Grabber::stop()
657{
658        debug_msg("V4L2: stop\n");
659        int i, err;
660
661        if (have_mmap) {
662                if ( fd_ > 0 ) {
663                        i = V4L2_BUF_TYPE_VIDEO_CAPTURE;
664                        if ( (err = ioctl(fd_, VIDIOC_STREAMOFF, &i) ) )
665                                debug_msg("V4L2: VIDIOC_STREAMOFF failed\n");
666                }
667
668                tempbuf.type = vimage[0].vidbuf.type;
669                while ((err = ioctl(fd_, VIDIOC_DQBUF, &tempbuf)) < 0 &&
670                       (errno == EINTR));
671
672                if (err < 0) {
673                        debug_msg("V4L2: VIDIOC_DQBUF failed: %s\n", strerror(errno));
674                }
675
676                for (i = 0; i < STREAMBUFS; ++i) {
677                        if (vimage[i].data)
678                                err = munmap(vimage[i].data, vimage[i].vidbuf.length);
679                        vimage[i].data = NULL;
680                }
681
682
683        } else {
684                if (vimage[0].data)
685                        free(vimage[0].data);
686                vimage[0].data = NULL;
687        }
688
689        Grabber::stop();
690        running_ = 0;
691}
692
693int V4l2Grabber::grab()
694{
695        char  *fr=NULL;
696        fd_set                  rdset;
697        struct timeval          timeout;
698        int                     n;
699
700        bytesused_ = 0;
701        if (have_mmap) {
702                FD_ZERO(&rdset);
703                FD_SET(fd_, &rdset);
704                timeout.tv_sec = 0;
705                timeout.tv_usec = 0;
706                n = select(fd_+1, &rdset, NULL, NULL, &timeout);
707
708                if (n == 0)
709                        return (0);
710                else if (FD_ISSET(fd_, &rdset)) {
711                        memset(&tempbuf, 0, sizeof(struct v4l2_buffer));
712                        tempbuf.type = vimage[0].vidbuf.type;
713                        tempbuf.memory = vimage[0].vidbuf.memory;
714                        if (-1 == ioctl(fd_, VIDIOC_DQBUF, &tempbuf))
715                                perror("ioctl  VIDIOC_DQBUF");
716
717                        if (  (req.count > 1) ) buf_sync++;
718                        fr = vimage[buf_sync%2].data;
719                        bytesused_ = tempbuf.bytesused;
720
721                }
722
723        } else {
724                fr = vimage[0].data;
725                read(fd_, vimage[0].data, fmt.fmt.pix.sizeimage);
726                bytesused_ = fmt.fmt.pix.sizeimage;
727        }
728
729        switch (cformat_) {
730        case CF_411:
731        case CF_CIF:
732                if( have_YUV420P )
733                       memcpy((void *)frame_, (const void *)fr, (size_t)height_*width_*3/2)
734;
735                else if( have_YUV422 )
736                       packed422_to_planar420((char*)frame_,fr);
737                else if( have_MJPEG || have_JPEG) {
738                       jpeg_to_planar420((char*)frame_,fr);
739                }
740                break;
741
742        case CF_422:
743                if (have_YUV422P)
744                       memcpy((void *)frame_, (const void *)fr, (size_t)height_*width_*2);
745                else if( have_YUV422 )
746                       packed422_to_planar422((char*)frame_,fr);
747                else if( have_MJPEG  || have_JPEG)
748                       // jpeg_to_planar422((char*)frame_,fr);
749                break;
750        }
751
752        if (have_mmap)
753                ioctl(fd_, VIDIOC_QBUF, &vimage[buf_sync%2].vidbuf);
754
755        suppress(frame_);
756        saveblks(frame_);
757        YuvFrame f(media_ts(), frame_, crvec_, outw_, outh_);
758        return (target_->consume(&f));
759}
760
761
762void V4l2Grabber::packed422_to_planar422(char *dest, const char *src)
763{
764    int i;
765    char *y,*u,*v;
766    unsigned int a, *srca;
767
768    srca = (unsigned int *)src;
769
770    i = (width_ * height_)/2;
771    y = dest;
772    u = y + width_ * height_;
773    v = u + width_ * height_ / 2;
774
775    switch (byteorder_) {
776    case BYTE_ORDER_YUYV:
777        while (--i) {
778                a = *(srca++);
779#if BYTE_ORDER == LITTLE_ENDIAN 
780                *(y++) = a & 0xff;
781                a >>= 8;
782                *(u++) = a & 0xff;
783                a >>= 8;
784                *(y++) = a & 0xff;
785                a >>= 8;
786                *(v++) = a & 0xff;
787#else
788                *(v++) = a & 0xff;
789                a >>= 8;
790                *(y++) = a & 0xff;
791                a >>= 8;
792                *(u++) = a & 0xff;
793                a >>= 8;
794                *(y++) = a & 0xff;
795#endif
796        }
797        break;
798
799    case BYTE_ORDER_YVYU:
800        while (--i) {
801                a = *(srca++);
802#if BYTE_ORDER == LITTLE_ENDIAN 
803                *(y++) = a & 0xff;
804                a >>= 8;
805                *(v++) = a & 0xff;
806                a >>= 8;
807                *(y++) = a & 0xff;
808                a >>= 8;
809                *(u++) = a & 0xff;
810#else
811                *(u++) = a & 0xff;
812                a >>= 8;
813                *(y++) = a & 0xff;
814                a >>= 8;
815                *(v++) = a & 0xff;
816                a >>= 8;
817                *(y++) = a & 0xff;
818#endif
819        }
820        break;
821
822    case BYTE_ORDER_UYVY:
823        while (--i) {
824                a = *(srca++);
825#if BYTE_ORDER == LITTLE_ENDIAN 
826                *(u++) = a & 0xff;
827                a >>= 8;
828                *(y++) = a & 0xff;
829                a >>= 8;
830                *(v++) = a & 0xff;
831                a >>= 8;
832                *(y++) = a & 0xff;
833#else
834                *(y++) = a & 0xff;
835                a >>= 8;
836                *(v++) = a & 0xff;
837                a >>= 8;
838                *(y++) = a & 0xff;
839                a >>= 8;
840                *(u++) = a & 0xff;
841#endif
842        }
843        break;
844
845    case BYTE_ORDER_VYUY:
846        while (--i) {
847                a = *(srca++);
848#if BYTE_ORDER == LITTLE_ENDIAN 
849                *(v++) = a & 0xff;
850                a >>= 8;
851                *(y++) = a & 0xff;
852                a >>= 8;
853                *(u++) = a & 0xff;
854                a >>= 8;
855                *(y++) = a & 0xff;
856#else
857                *(y++) = a & 0xff;
858                a >>= 8;
859                *(u++) = a & 0xff;
860                a >>= 8;
861                *(y++) = a & 0xff;
862                a >>= 8;
863                *(v++) = a & 0xff;
864#endif
865        }
866        break;
867    }
868
869}
870
871void V4l2Grabber::packed422_to_planar420(char *dest, const char *src)
872{
873    int  a1,b;
874    char *y,*u,*v;
875    unsigned int a, *srca;
876
877    srca = (unsigned int *)src;
878
879    y = dest;
880    u = y + width_ * height_;
881    v = u + width_ * height_ / 4;
882
883    switch (byteorder_) {
884    case BYTE_ORDER_YUYV:
885        for (a1 = height_; a1 > 0; a1 -= 2) {
886                for (b = width_; b > 0; b -= 2) {
887                        a = *(srca++);
888#if BYTE_ORDER == LITTLE_ENDIAN 
889                        *(y++) = a & 0xff; a >>= 8;
890                        *(u++) = a & 0xff; a >>= 8;
891                        *(y++) = a & 0xff; a >>= 8;
892                        *(v++) = a & 0xff;
893#else
894                        *(v++) = a & 0xff; a >>= 8;
895                        *(y+1) = a & 0xff; a >>= 8;
896                        *(u++) = a & 0xff; a >>= 8;
897                        *(y) = a;  y += 2;
898#endif
899                }
900                for (b = width_; b > 0; b -= 2) {
901                        a = *(srca++);
902#if BYTE_ORDER == LITTLE_ENDIAN 
903                        *(y++) = a & 0xff; a >>= 16;
904                        *(y++) = a & 0xff;
905#else
906                        a >>= 8;
907                        *(y+1) = a & 0xff; a >>= 16;
908                        *(y) = a; y += 2;
909#endif
910                }
911        }
912        break;
913
914    case BYTE_ORDER_YVYU:
915        for (a1 = height_; a1 > 0; a1 -= 2) {
916                for (b = width_; b > 0; b -= 2) {
917                        a = *(srca++);
918#if BYTE_ORDER == LITTLE_ENDIAN 
919                        *(y++) = a & 0xff; a >>= 8;
920                        *(v++) = a & 0xff; a >>= 8;
921                        *(y++) = a & 0xff; a >>= 8;
922                        *(u++) = a & 0xff;
923#else
924                        *(u++) = a & 0xff; a >>= 8;
925                        *(y+1) = a & 0xff; a >>= 8;
926                        *(v++) = a & 0xff; a >>= 8;
927                        *(y) = a;  y += 2;
928#endif
929                }
930                for (b = width_; b > 0; b -= 2) {
931                        a = *(srca++);
932#if BYTE_ORDER == LITTLE_ENDIAN 
933                        *(y++) = a & 0xff; a >>= 16;
934                        *(y++) = a & 0xff;
935#else
936                        a >>= 8;
937                        *(y+1) = a & 0xff; a >>= 16;
938                        *(y) = a; y += 2;
939#endif
940                }
941        }
942        break;
943
944    case BYTE_ORDER_UYVY:
945        for (a1 = height_; a1 > 0; a1 -= 2) {
946                for (b = width_; b > 0; b -= 2) {
947                        a = *(srca++);
948#if BYTE_ORDER == LITTLE_ENDIAN 
949                        *(u++) = a & 0xff; a >>= 8;
950                        *(y++) = a & 0xff; a >>= 8;
951                        *(v++) = a & 0xff; a >>= 8;
952                        *(y++) = a & 0xff;
953#else
954                        *(y+1) = a & 0xff; a >>= 8;
955                        *(v++) = a & 0xff; a >>= 8;
956                        *(y) = a & 0xff; a >>= 8;
957                        *(u++) = a & 0xff;
958                        y += 2;
959#endif
960                }
961                for (b = width_; b > 0; b -= 2) {
962                        a = *(srca++);
963#if BYTE_ORDER == LITTLE_ENDIAN 
964                        a >>= 8;
965                        *(y++) = a & 0xff; a >>= 16;
966                        *(y++) = a & 0xff;
967#else
968                        *(y+1) = a & 0xff; a >>= 16;
969                        *(y) = a; y += 2;
970#endif
971                }
972        }
973        break;
974
975    case BYTE_ORDER_VYUY:
976        for (a1 = height_; a1 > 0; a1 -= 2) {
977                for (b = width_; b > 0; b -= 2) {
978                        a = *(srca++);
979#if BYTE_ORDER == LITTLE_ENDIAN 
980                        *(v++) = a & 0xff; a >>= 8;
981                        *(y++) = a & 0xff; a >>= 8;
982                        *(u++) = a & 0xff; a >>= 8;
983                        *(y++) = a & 0xff;
984#else
985                        *(y+1) = a & 0xff; a >>= 8;
986                        *(u++) = a & 0xff; a >>= 8;
987                        *(y) = a & 0xff; a >>= 8;
988                        *(v++) = a & 0xff;
989                        y += 2;
990#endif
991                }
992                for (b = width_; b > 0; b -= 2) {
993                        a = *(srca++);
994#if BYTE_ORDER == LITTLE_ENDIAN 
995                        a >>= 8;
996                        *(y++) = a & 0xff; a >>= 16;
997                        *(y++) = a & 0xff;
998#else
999                        *(y+1) = a & 0xff; a >>= 16;
1000                        *(y) = a; y += 2;
1001#endif
1002                }
1003        }
1004        break;
1005    }
1006}
1007
1008void V4l2Grabber::jpeg_to_planar420(char *dest, const char *src)
1009{
1010        char *y,*u,*v;
1011        unsigned char *components[4];
1012
1013        y = dest;
1014        u = y + width_ * height_;
1015        v = u + width_ * height_ /4;
1016
1017        components[0] = (unsigned char *) y;
1018        components[1] = (unsigned char *) u;
1019        components[2] = (unsigned char *) v;
1020
1021        if (jpegdec_ == NULL) {
1022                jpegdec_ = tinyjpeg_init();
1023                if (jpegdec_ == NULL) {
1024                        return;
1025                }
1026                tinyjpeg_set_flags(jpegdec_, TINYJPEG_FLAGS_MJPEG_TABLE);
1027        }
1028        tinyjpeg_set_components(jpegdec_, components, 4);
1029        if (tinyjpeg_parse_header(jpegdec_, (unsigned char *)src, bytesused_) < 0) {
1030               debug_msg("V4L2 Jpeg error: %s", tinyjpeg_get_errorstring(jpegdec_));
1031               return;
1032        }
1033        if (tinyjpeg_decode(jpegdec_, TINYJPEG_FMT_YUV420P) < 0) {
1034               debug_msg("V4L2 Jpeg error: %s", tinyjpeg_get_errorstring(jpegdec_));
1035               return;
1036        }
1037}
1038
1039
1040
1041void V4l2Grabber::format()
1042{
1043
1044        struct v4l2_standard    standard;
1045
1046        int i, err;
1047        int input;
1048        int format_ok = 0;
1049
1050        switch (cformat_) {
1051        case CF_411:
1052        case CF_CIF:
1053                if( have_YUV420P )
1054                       pixelformat = V4L2_PIX_FMT_YUV420;
1055                else if( have_YUV422 )
1056                       pixelformat = V4L2_PIX_FMT_YUYV;
1057                else if( have_MJPEG )
1058                       pixelformat = V4L2_PIX_FMT_MJPEG;
1059                else if( have_JPEG )
1060                       pixelformat = V4L2_PIX_FMT_JPEG;
1061                break;
1062
1063        case CF_422:
1064                if (have_YUV422P)
1065                       pixelformat = V4L2_PIX_FMT_YUV422P;
1066                else if( have_YUV422 )
1067                       pixelformat = V4L2_PIX_FMT_YUYV;
1068                else if( have_MJPEG )
1069                       pixelformat = V4L2_PIX_FMT_MJPEG;
1070                else if( have_JPEG )
1071                       pixelformat = V4L2_PIX_FMT_JPEG;
1072                break;
1073        }
1074
1075        while ( !format_ok ) {
1076                width_  = CIF_WIDTH  *2  / decimate_;
1077                height_ = CIF_HEIGHT *2  / decimate_;
1078
1079                debug_msg("V4L2: format");
1080                switch (cformat_) {
1081                case CF_CIF:
1082                        set_size_411(width_, height_);
1083                        debug_msg(" cif");
1084                        break;
1085                case CF_411:
1086                        set_size_411(width_, height_);
1087                        debug_msg(" 411");
1088                        break;
1089                case CF_422:
1090                        set_size_422(width_, height_);
1091                        debug_msg(" 422");
1092                        break;
1093                }
1094                debug_msg("decimate: %d\n",decimate_);
1095
1096                memset(&standard,0,sizeof(standard));
1097                standard.index = norm_;
1098                err = ioctl(fd_, VIDIOC_ENUMSTD, &standard);
1099                if (!err) {
1100                        if (-1 == ioctl(fd_, VIDIOC_S_STD, &standard.id))
1101                                perror("ioctl VIDIOC_S_STD");
1102                        else debug_msg("V4L2: setting norm to %s\n",standard.name);
1103                }
1104
1105                input = port_;
1106                if ((err = ioctl(fd_, VIDIOC_S_INPUT, &input)) )
1107                        debug_msg("S_INPUT returned error %d\n",errno);
1108                else debug_msg("V4L2: setting input port to %d\n", port_);
1109
1110                for (i = 0, err = 0; err == 0; ++i) {
1111                        struct v4l2_fmtdesc     fmtd;
1112                        memset(&fmtd,0,sizeof(fmtd));
1113                        fmtd.index = i;
1114                        fmtd.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1115                        err = ioctl(fd_, VIDIOC_ENUM_FMT, &fmtd);
1116                        if (!err) {
1117
1118                                if (fmtd.pixelformat == pixelformat) {
1119                                        memset(&fmt,0,sizeof(fmt));
1120                                        fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1121                                        ioctl(fd_, VIDIOC_G_FMT, &fmt);
1122                                        fmt.fmt.pix.width = width_;
1123                                        fmt.fmt.pix.height = height_;
1124                                        fmt.fmt.pix.field = V4L2_FIELD_ANY;
1125                                        fmt.fmt.pix.pixelformat = pixelformat;
1126
1127                                        if ( (err = ioctl(fd_, VIDIOC_S_FMT, &fmt) ) )
1128                                                debug_msg("\nV4L2: Failed to set format\n");
1129
1130                                        if ( ( fmt.fmt.pix.width != (unsigned int)width_ ) ||
1131                                                                ( fmt.fmt.pix.height !=  (unsigned int)height_ ) ) {
1132
1133                                                debug_msg("V4L2: failed to set format! requested %dx%d, got %dx%d\n", width_, height_, fmt.fmt.pix.width, fmt.fmt.pix.height);
1134
1135
1136                                                switch(decimate_) {
1137                                                case 2:
1138                                                        debug_msg("V4L2: trying resolution under ...\n");
1139                                                        decimate_ = 4;
1140                                                        break;
1141                                                case 1:
1142                                                        debug_msg("V4L2: trying resolution under ...\n");
1143                                                        decimate_ = 2;
1144                                                        break;
1145                                                default:
1146                                                        debug_msg("V4L2: giving up ...\n");
1147                                                        format_ok = 1;
1148                                                }
1149
1150                                        } else {
1151                                                debug_msg("V4L2: setting format: width=%d height=%d\n", fmt.fmt.pix.width, fmt.fmt.pix.height);
1152                                                format_ok = 1;
1153                                        }
1154                                        break;
1155                                }
1156
1157                        }
1158                }
1159        }
1160
1161
1162        allocref();
1163}
1164
1165void V4l2Grabber::setctrl(int val, int cid, char *controlname, int reset)
1166{
1167        struct v4l2_queryctrl qctrl;
1168        struct v4l2_control ctrl;
1169
1170        qctrl.id = cid;
1171        if (-1 != ioctl(fd_, VIDIOC_QUERYCTRL, &qctrl)) {
1172                if ( !(qctrl.flags & V4L2_CTRL_FLAG_DISABLED) ) {
1173
1174                        ctrl.id = cid;
1175                        if ( reset )
1176                                ctrl.value = qctrl.default_value;
1177                        else
1178                                ctrl.value = qctrl.minimum + (qctrl.maximum - qctrl.minimum) * val/256;
1179                        if (-1 == ioctl(fd_, VIDIOC_S_CTRL,&ctrl))
1180                                perror("ioctl  VIDIOC_S_CTRL");
1181                        else debug_msg( "V4L2: %s = %d\n", controlname, val);
1182                }
1183        }
1184        return;
1185}
Note: See TracBrowser for help on using the browser.