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

Revision 4037, 37.6 KB (checked in by piers, 7 years ago)

Video4Linux2 patches from Doug Kosovic (7may07)
- Updates to V4L2 grabber UI for control of Gamma and Gain
- Added support for cameras (e.g. UVC webcams) which supply MJPEG video
- Added tinyjpeg files from  http://www.saillard.org/programs_and_patches/tinyjpegdecoder/
- Updated configure.in to add these files to the build and correctly identify V4L2

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