1 /*******************************************************************************
2 # Linux-UVC streaming input-plugin for MJPG-streamer #
3 # #
4 # This package work with the Logitech UVC based webcams with the mjpeg feature #
5 # #
6 # Copyright (C) 2005 2006 Laurent Pinchart && Michel Xhaard #
7 # 2007 Lucas van Staden #
8 # 2007 Tom Stöveken #
9 # #
10 # This program is free software; you can redistribute it and/or modify #
11 # it under the terms of the GNU General Public License as published by #
12 # the Free Software Foundation; either version 2 of the License, or #
13 # (at your option) any later version. #
14 # #
15 # This program is distributed in the hope that it will be useful, #
16 # but WITHOUT ANY WARRANTY; without even the implied warranty of #
17 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the #
18 # GNU General Public License for more details. #
19 # #
20 # You should have received a copy of the GNU General Public License #
21 # along with this program; if not, write to the Free Software #
22 # Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA #
23 # #
24 *******************************************************************************/
25
26 #include <stdlib.h>
27 #include <errno.h>
28 #include "v4l2uvc.h"
29 #include "huffman.h"
30 #include "dynctrl.h"
31
32 static int debug = 0;
33
34 /* fcc2s - convert pixelformat to string
35 * (Obtained from vtl-utils: v4l2-ctl.cpp)
36 * args:
37 * fmsString - char* to hold string
38 * size - size of allocated memory for string
39 * pixelformat - v4l2 pixel format identidifier
40 */
fcc2s(char * fmtString,unsigned int size,unsigned int pixelformat)41 void fcc2s(char* fmtString, unsigned int size, unsigned int pixelformat)
42 {
43 if ( size < 8 )
44 {
45 fmtString[0] = '\0';
46 return;
47 }
48
49
50 fmtString[0] = pixelformat & 0x7f;
51 fmtString[1] = (pixelformat >> 8 ) & 0x7f;
52 fmtString[2] = (pixelformat >> 16 ) & 0x7f;
53 fmtString[3] = (pixelformat >> 24 ) & 0x7f;
54 if (pixelformat & (1 << 31))
55 {
56 fmtString[4] = '-';
57 fmtString[5] = 'B';
58 fmtString[6] = 'E';
59 fmtString[7] = '\0';
60 }
61 else
62 {
63 fmtString[4] = '\0';
64 }
65 return;
66 }
67
68 /* ioctl with a number of retries in the case of failure
69 * args:
70 * fd - device descriptor
71 * IOCTL_X - ioctl reference
72 * arg - pointer to ioctl data
73 * returns - ioctl result
74 */
xioctl(int fd,int IOCTL_X,void * arg)75 int xioctl(int fd, int IOCTL_X, void *arg)
76 {
77 int ret = 0;
78 int tries = IOCTL_RETRY;
79 do {
80 ret = IOCTL_VIDEO(fd, IOCTL_X, arg);
81 } while(ret && tries-- &&
82 ((errno == EINTR) || (errno == EAGAIN) || (errno == ETIMEDOUT)));
83
84 if(ret && (tries <= 0)) fprintf(stderr, "ioctl (%i) retried %i times - giving up: %s)\n", IOCTL_X, IOCTL_RETRY, strerror(errno));
85
86 return (ret);
87 }
88
89 static int init_v4l2(struct vdIn *vd);
90 static int init_framebuffer(struct vdIn *vd);
91 static void free_framebuffer(struct vdIn *vd);
92
init_videoIn(struct vdIn * vd,char * device,int width,int height,int fps,int format,int grabmethod,globals * pglobal,int id,v4l2_std_id vstd)93 int init_videoIn(struct vdIn *vd, char *device, int width,
94 int height, int fps, int format, int grabmethod, globals *pglobal, int id, v4l2_std_id vstd)
95 {
96 if(vd == NULL || device == NULL)
97 return -1;
98 if(width == 0 || height == 0)
99 return -1;
100 if(grabmethod < 0 || grabmethod > 1)
101 grabmethod = 1; //mmap by default;
102 vd->videodevice = NULL;
103 vd->status = NULL;
104 vd->pictName = NULL;
105 vd->videodevice = (char *) calloc(1, 16 * sizeof(char));
106 vd->status = (char *) calloc(1, 100 * sizeof(char));
107 vd->pictName = (char *) calloc(1, 80 * sizeof(char));
108 snprintf(vd->videodevice, (16 - 1), "%s", device);
109 vd->toggleAvi = 0;
110 vd->getPict = 0;
111 vd->signalquit = 1;
112 vd->width = width;
113 vd->height = height;
114 vd->fps = fps;
115 vd->formatIn = format;
116 vd->vstd = vstd;
117 vd->grabmethod = grabmethod;
118 vd->soft_framedrop = 0;
119
120 if(init_v4l2(vd) < 0) {
121 goto error;
122 }
123
124 // getting the name of the input source
125 struct v4l2_input in_struct;
126 memset(&in_struct, 0, sizeof(struct v4l2_input));
127 in_struct.index = 0;
128 if (xioctl(vd->fd, VIDIOC_ENUMINPUT, &in_struct) == 0) {
129 int nameLength = strlen((char*)&in_struct.name);
130 pglobal->in[id].name = malloc((1+nameLength)*sizeof(char));
131 sprintf(pglobal->in[id].name, "%s", in_struct.name);
132 DBG("Input name: %s\n", in_struct.name);
133 } else {
134 DBG("VIDIOC_ENUMINPUT failed\n");
135 }
136
137 // enumerating formats
138
139 struct v4l2_format currentFormat;
140 memset(¤tFormat, 0, sizeof(struct v4l2_format));
141 currentFormat.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
142 if (xioctl(vd->fd, VIDIOC_G_FMT, ¤tFormat) == 0) {
143 DBG("Current size: %dx%d\n",
144 currentFormat.fmt.pix.width,
145 currentFormat.fmt.pix.height);
146 }
147
148 pglobal->in[id].in_formats = NULL;
149 for(pglobal->in[id].formatCount = 0; 1; pglobal->in[id].formatCount++) {
150 struct v4l2_fmtdesc fmtdesc;
151 memset(&fmtdesc, 0, sizeof(struct v4l2_fmtdesc));
152 fmtdesc.index = pglobal->in[id].formatCount;
153 fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
154 if(xioctl(vd->fd, VIDIOC_ENUM_FMT, &fmtdesc) < 0) {
155 break;
156 }
157
158 if (pglobal->in[id].in_formats == NULL) {
159 pglobal->in[id].in_formats = (input_format*)calloc(1, sizeof(input_format));
160 } else {
161 pglobal->in[id].in_formats = (input_format*)realloc(pglobal->in[id].in_formats, (pglobal->in[id].formatCount + 1) * sizeof(input_format));
162 }
163
164 if (pglobal->in[id].in_formats == NULL) {
165 LOG("Calloc/realloc failed: %s\n", strerror(errno));
166 return -1;
167 }
168
169 memcpy(&pglobal->in[id].in_formats[pglobal->in[id].formatCount], &fmtdesc, sizeof(struct v4l2_fmtdesc));
170
171 if(fmtdesc.pixelformat == format)
172 pglobal->in[id].currentFormat = pglobal->in[id].formatCount;
173
174 DBG("Supported format: %s\n", fmtdesc.description);
175 struct v4l2_frmsizeenum fsenum;
176 memset(&fsenum, 0, sizeof(struct v4l2_frmsizeenum));
177 fsenum.pixel_format = fmtdesc.pixelformat;
178 int j = 0;
179 pglobal->in[id].in_formats[pglobal->in[id].formatCount].supportedResolutions = NULL;
180 pglobal->in[id].in_formats[pglobal->in[id].formatCount].resolutionCount = 0;
181 pglobal->in[id].in_formats[pglobal->in[id].formatCount].currentResolution = -1;
182 while(1) {
183 fsenum.index = j;
184 j++;
185 if(xioctl(vd->fd, VIDIOC_ENUM_FRAMESIZES, &fsenum) == 0) {
186 pglobal->in[id].in_formats[pglobal->in[id].formatCount].resolutionCount++;
187
188 if (pglobal->in[id].in_formats[pglobal->in[id].formatCount].supportedResolutions == NULL) {
189 pglobal->in[id].in_formats[pglobal->in[id].formatCount].supportedResolutions = (input_resolution*)
190 calloc(1, sizeof(input_resolution));
191 } else {
192 pglobal->in[id].in_formats[pglobal->in[id].formatCount].supportedResolutions = (input_resolution*)
193 realloc(pglobal->in[id].in_formats[pglobal->in[id].formatCount].supportedResolutions, j * sizeof(input_resolution));
194 }
195
196 if (pglobal->in[id].in_formats[pglobal->in[id].formatCount].supportedResolutions == NULL) {
197 LOG("Calloc/realloc failed\n");
198 return -1;
199 }
200
201 pglobal->in[id].in_formats[pglobal->in[id].formatCount].supportedResolutions[j-1].width = fsenum.discrete.width;
202 pglobal->in[id].in_formats[pglobal->in[id].formatCount].supportedResolutions[j-1].height = fsenum.discrete.height;
203 if(format == fmtdesc.pixelformat) {
204 pglobal->in[id].in_formats[pglobal->in[id].formatCount].currentResolution = (j - 1);
205 DBG("\tSupported size with the current format: %dx%d\n", fsenum.discrete.width, fsenum.discrete.height);
206 } else {
207 DBG("\tSupported size: %dx%d\n", fsenum.discrete.width, fsenum.discrete.height);
208 }
209 } else {
210 break;
211 }
212 }
213 }
214
215 if (init_framebuffer(vd) < 0) {
216 goto error;
217 }
218
219 return 0;
220 error:
221 free_framebuffer(vd);
222 free(pglobal->in[id].in_parameters);
223 free(vd->videodevice);
224 free(vd->status);
225 free(vd->pictName);
226 CLOSE_VIDEO(vd->fd);
227 return -1;
228 }
229
init_framebuffer(struct vdIn * vd)230 static int init_framebuffer(struct vdIn *vd) {
231 /* alloc a temp buffer to reconstruct the pict */
232 vd->framesizeIn = (vd->width * vd->height << 1);
233 switch (vd->formatIn) {
234 case V4L2_PIX_FMT_JPEG:
235 // Fall-through intentional
236 case V4L2_PIX_FMT_MJPEG: // in JPG mode the frame size is varies at every frame, so we allocate a bit bigger buffer
237 vd->tmpbuffer = (unsigned char *) calloc(1, (size_t) vd->framesizeIn);
238 if(!vd->tmpbuffer)
239 return -1;
240 vd->framebuffer =
241 (unsigned char *) calloc(1, (size_t) vd->width * (vd->height + 8) * 2);
242 break;
243 case V4L2_PIX_FMT_RGB24:
244 vd->framesizeIn = (vd->width * vd->height) * 3;
245 vd->framebuffer =
246 (unsigned char *) calloc(1, (size_t) vd->framesizeIn);
247 break;
248 case V4L2_PIX_FMT_RGB565: // buffer allocation for non varies on frame size formats
249 case V4L2_PIX_FMT_YUYV:
250 case V4L2_PIX_FMT_UYVY:
251 vd->framebuffer =
252 (unsigned char *) calloc(1, (size_t) vd->framesizeIn);
253 break;
254 default:
255 fprintf(stderr, "Unknown vd->formatIn\n");
256 return -1;
257 }
258 return -!vd->framebuffer;
259 }
260
free_framebuffer(struct vdIn * vd)261 static void free_framebuffer(struct vdIn *vd) {
262 if (vd->tmpbuffer) {
263 free(vd->tmpbuffer);
264 }
265 vd->tmpbuffer = NULL;
266 free(vd->framebuffer);
267 vd->framebuffer = NULL;
268 }
269
init_v4l2(struct vdIn * vd)270 static int init_v4l2(struct vdIn *vd)
271 {
272 int i;
273 int ret = 0;
274 if((vd->fd = OPEN_VIDEO(vd->videodevice, O_RDWR)) == -1) {
275 perror("ERROR opening V4L interface");
276 DBG("errno: %d", errno);
277 return -1;
278 }
279
280 memset(&vd->cap, 0, sizeof(struct v4l2_capability));
281 ret = xioctl(vd->fd, VIDIOC_QUERYCAP, &vd->cap);
282 if(ret < 0) {
283 fprintf(stderr, "Error opening device %s: unable to query device.\n", vd->videodevice);
284 goto fatal;
285 }
286
287 if((vd->cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
288 fprintf(stderr, "Error opening device %s: video capture not supported.\n",
289 vd->videodevice);
290 goto fatal;;
291 }
292
293 if(vd->grabmethod) {
294 if(!(vd->cap.capabilities & V4L2_CAP_STREAMING)) {
295 fprintf(stderr, "%s does not support streaming i/o\n", vd->videodevice);
296 goto fatal;
297 }
298 } else {
299 if(!(vd->cap.capabilities & V4L2_CAP_READWRITE)) {
300 fprintf(stderr, "%s does not support read i/o\n", vd->videodevice);
301 goto fatal;
302 }
303 }
304
305 if (vd->vstd != V4L2_STD_UNKNOWN) {
306 if (ioctl(vd->fd, VIDIOC_S_STD, &vd->vstd) == -1) {
307 fprintf(stderr, "Can't set video standard: %s\n",strerror(errno));
308 goto fatal;
309 }
310 }
311
312 if (vd->dv_timings) {
313 if (video_set_dv_timings(vd)) {
314 goto fatal;
315 }
316
317 struct v4l2_event_subscription sub;
318 memset(&sub, 0, sizeof(sub));
319 sub.type = V4L2_EVENT_SOURCE_CHANGE;
320 if (ioctl(vd->fd, VIDIOC_SUBSCRIBE_EVENT, &sub) < 0) {
321 IPRINT("Can\'t subscribe to V4L2_EVENT_SOURCE_CHANGE: %s\n", strerror(errno));
322 }
323 }
324
325 /*
326 * set format in
327 */
328 memset(&vd->fmt, 0, sizeof(struct v4l2_format));
329 vd->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
330 vd->fmt.fmt.pix.width = vd->width;
331 vd->fmt.fmt.pix.height = vd->height;
332 vd->fmt.fmt.pix.pixelformat = vd->formatIn;
333 vd->fmt.fmt.pix.field = V4L2_FIELD_ANY;
334 ret = xioctl(vd->fd, VIDIOC_S_FMT, &vd->fmt);
335 if(ret < 0) {
336 fprintf(stderr, "Unable to set format: %d res: %dx%d\n", vd->formatIn, vd->width, vd->height);
337 goto fatal;
338 }
339
340 /*
341 * Check reoslution
342 */
343 if((vd->fmt.fmt.pix.width != vd->width) ||
344 (vd->fmt.fmt.pix.height != vd->height)) {
345 fprintf(stderr, " i: The specified resolution is unavailable, using: width %d height %d instead \n", vd->fmt.fmt.pix.width, vd->fmt.fmt.pix.height);
346 vd->width = vd->fmt.fmt.pix.width;
347 vd->height = vd->fmt.fmt.pix.height;
348 }
349 /*
350 * Check format
351 */
352 if(vd->formatIn != vd->fmt.fmt.pix.pixelformat) {
353 char fmtStringRequested[8];
354 char fmtStringObtained[8];
355 fcc2s(fmtStringObtained,8,vd->fmt.fmt.pix.pixelformat);
356 fcc2s(fmtStringRequested,8,vd->formatIn);
357 fprintf(stderr, " i: Could not obtain the requested pixelformat: %s , driver gave us: %s\n",fmtStringRequested,fmtStringObtained);
358 fprintf(stderr, " ... will try to handle this by checking against supported formats. \n");
359
360 switch(vd->fmt.fmt.pix.pixelformat){
361 case V4L2_PIX_FMT_JPEG:
362 // Fall-through intentional
363 case V4L2_PIX_FMT_MJPEG:
364 fprintf(stderr, " ... Falling back to the faster MJPG mode (consider changing cmd line options).\n");
365 vd->formatIn = vd->fmt.fmt.pix.pixelformat;
366 break;
367 case V4L2_PIX_FMT_YUYV:
368 fprintf(stderr, " ... Falling back to YUV mode (consider using -yuv option). Note that this requires much more CPU power\n");
369 vd->formatIn = vd->fmt.fmt.pix.pixelformat;
370 break;
371 case V4L2_PIX_FMT_UYVY:
372 fprintf(stderr, " ... Falling back to UYVY mode (consider using -uyvy option). Note that this requires much more CPU power\n");
373 vd->formatIn = vd->fmt.fmt.pix.pixelformat;
374 break;
375 case V4L2_PIX_FMT_RGB24:
376 fprintf(stderr, " ... Falling back to RGB24 mode (consider using -fourcc RGB24 option). Note that this requires much more CPU power\n");
377 vd->formatIn = vd->fmt.fmt.pix.pixelformat;
378 break;
379 case V4L2_PIX_FMT_RGB565:
380 fprintf(stderr, " ... Falling back to RGB565 mode (consider using -fourcc RGBP option). Note that this requires much more CPU power\n");
381 vd->formatIn = vd->fmt.fmt.pix.pixelformat;
382 break;
383 default:
384 goto fatal;
385 break;
386 }
387 }
388
389 /*
390 * set framerate
391 */
392
393 if (vd->fps != -1) {
394 struct v4l2_streamparm *setfps;
395 setfps = (struct v4l2_streamparm *) calloc(1, sizeof(struct v4l2_streamparm));
396 memset(setfps, 0, sizeof(struct v4l2_streamparm));
397 setfps->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
398
399 /*
400 * first query streaming parameters to determine that the FPS selection is supported
401 */
402 ret = xioctl(vd->fd, VIDIOC_G_PARM, setfps);
403 if (ret == 0) {
404 if (setfps->parm.capture.capability & V4L2_CAP_TIMEPERFRAME) {
405 memset(setfps, 0, sizeof(struct v4l2_streamparm));
406 setfps->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
407 setfps->parm.capture.timeperframe.numerator = 1;
408 setfps->parm.capture.timeperframe.denominator = vd->fps==-1?255:vd->fps; // if no default fps set set it to maximum
409
410 ret = xioctl(vd->fd, VIDIOC_S_PARM, setfps);
411 if (ret) {
412 perror("Unable to set the FPS\n");
413 } else {
414 if (vd->fps != setfps->parm.capture.timeperframe.denominator) {
415 IPRINT("FPS coerced ......: from %d to %d\n", vd->fps, setfps->parm.capture.timeperframe.denominator);
416 }
417 }
418 } else {
419 perror("Setting FPS on the capture device is not supported, fallback to software framedropping\n");
420 vd->soft_framedrop = 1;
421 vd->frame_period_time = 1000/vd->fps; // calcualate frame period time in ms
422 IPRINT("Frame period time ......: %ld ms\n", vd->frame_period_time);
423 }
424 } else {
425 perror("Unable to query that the FPS change is supported\n");
426 }
427 }
428
429 /*
430 * request buffers
431 */
432 memset(&vd->rb, 0, sizeof(struct v4l2_requestbuffers));
433 vd->rb.count = NB_BUFFER;
434 vd->rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
435 vd->rb.memory = V4L2_MEMORY_MMAP;
436
437 ret = xioctl(vd->fd, VIDIOC_REQBUFS, &vd->rb);
438 if(ret < 0) {
439 perror("Unable to allocate buffers");
440 goto fatal;
441 }
442
443 /*
444 * map the buffers
445 */
446 for(i = 0; i < NB_BUFFER; i++) {
447 memset(&vd->buf, 0, sizeof(struct v4l2_buffer));
448 vd->buf.index = i;
449 vd->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
450 vd->buf.memory = V4L2_MEMORY_MMAP;
451 ret = xioctl(vd->fd, VIDIOC_QUERYBUF, &vd->buf);
452 if(ret < 0) {
453 perror("Unable to query buffer");
454 goto fatal;
455 }
456
457 if(debug)
458 fprintf(stderr, "length: %u offset: %u\n", vd->buf.length, vd->buf.m.offset);
459
460 vd->mem[i] = mmap(0 /* start anywhere */ ,
461 vd->buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, vd->fd,
462 vd->buf.m.offset);
463 if(vd->mem[i] == MAP_FAILED) {
464 perror("Unable to map buffer");
465 goto fatal;
466 }
467 if(debug)
468 fprintf(stderr, "Buffer mapped at address %p.\n", vd->mem[i]);
469 }
470
471 /*
472 * Queue the buffers.
473 */
474 for(i = 0; i < NB_BUFFER; ++i) {
475 memset(&vd->buf, 0, sizeof(struct v4l2_buffer));
476 vd->buf.index = i;
477 vd->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
478 vd->buf.memory = V4L2_MEMORY_MMAP;
479 ret = xioctl(vd->fd, VIDIOC_QBUF, &vd->buf);
480 if(ret < 0) {
481 perror("Unable to queue buffer");
482 goto fatal;;
483 }
484 }
485 return 0;
486 fatal:
487 fprintf(stderr, "Init v4L2 failed !! exit fatal\n");
488 return -1;
489
490 }
491
video_enable(struct vdIn * vd)492 int video_enable(struct vdIn *vd)
493 {
494 int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
495 int ret;
496
497 ret = xioctl(vd->fd, VIDIOC_STREAMON, &type);
498 if(ret < 0) {
499 perror("Unable to start capture");
500 return ret;
501 }
502 vd->streamingState = STREAMING_ON;
503 return 0;
504 }
505
video_disable(struct vdIn * vd,streaming_state disabledState)506 static int video_disable(struct vdIn *vd, streaming_state disabledState)
507 {
508 int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
509 int ret;
510 DBG("STopping capture\n");
511 ret = xioctl(vd->fd, VIDIOC_STREAMOFF, &type);
512 if(ret != 0) {
513 perror("Unable to stop capture");
514 return ret;
515 }
516 DBG("STopping capture done\n");
517 vd->streamingState = disabledState;
518 return 0;
519 }
520
video_set_dv_timings(struct vdIn * vd)521 int video_set_dv_timings(struct vdIn *vd)
522 {
523 struct v4l2_dv_timings timings;
524 v4l2_std_id std;
525
526 memset(&timings, 0, sizeof(timings));
527 if (xioctl(vd->fd, VIDIOC_QUERY_DV_TIMINGS, &timings) >= 0) {
528 IPRINT("QUERY_DV_TIMINGS returned %ux%u pixclk %llu\n", timings.bt.width, timings.bt.height, timings.bt.pixelclock);
529 // Can read DV timings, so set them.
530 if (xioctl(vd->fd, VIDIOC_S_DV_TIMINGS, &timings) < 0) {
531 perror("Failed to set DV timings");
532 return -1;
533 } else {
534 vd->width = timings.bt.width;
535 vd->height = timings.bt.height;
536 }
537 } else {
538 memset(&std, 0, sizeof(std));
539 if (xioctl(vd->fd, VIDIOC_QUERYSTD, &std) >= 0) {
540 // Can read standard, so set it.
541 if (xioctl(vd->fd, VIDIOC_S_STD, &std) < 0) {
542 perror("Failed to set standard");
543 return -1;
544 }
545 }
546 }
547 return 0;
548 }
549
video_handle_event(struct vdIn * vd)550 int video_handle_event(struct vdIn *vd)
551 {
552 struct v4l2_event ev;
553 if (!ioctl(vd->fd, VIDIOC_DQEVENT, &ev)) {
554 switch (ev.type) {
555 case V4L2_EVENT_SOURCE_CHANGE:
556 IPRINT("V4L2_EVENT_SOURCE_CHANGE: Source changed\n");
557 if (setResolution(vd, vd->width, vd->height) < 0) {
558 return -1;
559 }
560 break;
561 case V4L2_EVENT_EOS:
562 IPRINT("V4L2_EVENT_EOS\n");
563 break;
564 }
565 }
566 return 0;
567 }
568
569 /******************************************************************************
570 Description.:
571 Input Value.:
572 Return Value:
573 ******************************************************************************/
is_huffman(unsigned char * buf)574 int is_huffman(unsigned char *buf)
575 {
576 unsigned char *ptbuf;
577 int i = 0;
578 ptbuf = buf;
579 while(((ptbuf[0] << 8) | ptbuf[1]) != 0xffda) {
580 if(i++ > 2048)
581 return 0;
582 if(((ptbuf[0] << 8) | ptbuf[1]) == 0xffc4)
583 return 1;
584 ptbuf++;
585 }
586 return 0;
587 }
588
589 /******************************************************************************
590 Description.:
591 Input Value.:
592 Return Value:
593 ******************************************************************************/
memcpy_picture(unsigned char * out,unsigned char * buf,int size)594 int memcpy_picture(unsigned char *out, unsigned char *buf, int size)
595 {
596 unsigned char *ptdeb, *ptlimit, *ptcur = buf;
597 int sizein, pos = 0;
598
599 if(!is_huffman(buf)) {
600 ptdeb = ptcur = buf;
601 ptlimit = buf + size;
602 while((((ptcur[0] << 8) | ptcur[1]) != 0xffc0) && (ptcur < ptlimit))
603 ptcur++;
604 if(ptcur >= ptlimit)
605 return pos;
606 sizein = ptcur - ptdeb;
607
608 memcpy(out + pos, buf, sizein); pos += sizein;
609 memcpy(out + pos, dht_data, sizeof(dht_data)); pos += sizeof(dht_data);
610 memcpy(out + pos, ptcur, size - sizein); pos += size - sizein;
611 } else {
612 memcpy(out + pos, ptcur, size); pos += size;
613 }
614 return pos;
615 }
616
uvcGrab(struct vdIn * vd)617 int uvcGrab(struct vdIn *vd)
618 {
619 #define HEADERFRAME1 0xaf
620 int ret;
621
622 if(vd->streamingState == STREAMING_OFF) {
623 if(video_enable(vd))
624 goto err;
625 }
626 memset(&vd->buf, 0, sizeof(struct v4l2_buffer));
627 vd->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
628 vd->buf.memory = V4L2_MEMORY_MMAP;
629
630 ret = xioctl(vd->fd, VIDIOC_DQBUF, &vd->buf);
631 if(ret < 0) {
632 perror("Unable to dequeue buffer");
633 goto err;
634 }
635
636 switch(vd->formatIn) {
637 case V4L2_PIX_FMT_JPEG:
638 // Fall-through intentional
639 case V4L2_PIX_FMT_MJPEG:
640 if(vd->buf.bytesused <= HEADERFRAME1) {
641 /* Prevent crash
642 * on empty image */
643 fprintf(stderr, "Ignoring empty buffer ...\n");
644 break;
645 }
646
647 /* memcpy(vd->tmpbuffer, vd->mem[vd->buf.index], vd->buf.bytesused);
648
649 memcpy (vd->tmpbuffer, vd->mem[vd->buf.index], HEADERFRAME1);
650 memcpy (vd->tmpbuffer + HEADERFRAME1, dht_data, sizeof(dht_data));
651 memcpy (vd->tmpbuffer + HEADERFRAME1 + sizeof(dht_data), vd->mem[vd->buf.index] + HEADERFRAME1, (vd->buf.bytesused - HEADERFRAME1));
652 */
653
654 memcpy(vd->tmpbuffer, vd->mem[vd->buf.index], vd->buf.bytesused);
655 vd->tmpbytesused = vd->buf.bytesused;
656 vd->tmptimestamp = vd->buf.timestamp;
657
658 if(debug) {
659 fprintf(stderr, "bytes in used %d \n", vd->buf.bytesused);
660 }
661 break;
662 case V4L2_PIX_FMT_RGB24:
663 case V4L2_PIX_FMT_RGB565:
664 case V4L2_PIX_FMT_YUYV:
665 case V4L2_PIX_FMT_UYVY:
666 if(vd->buf.bytesused > vd->framesizeIn) {
667 memcpy(vd->framebuffer, vd->mem[vd->buf.index], (size_t) vd->framesizeIn);
668 } else {
669 memcpy(vd->framebuffer, vd->mem[vd->buf.index], (size_t) vd->buf.bytesused);
670 }
671 vd->tmpbytesused = vd->buf.bytesused;
672 vd->tmptimestamp = vd->buf.timestamp;
673 break;
674 default:
675 goto err;
676 break;
677 }
678
679 ret = xioctl(vd->fd, VIDIOC_QBUF, &vd->buf);
680 if(ret < 0) {
681 perror("Unable to requeue buffer");
682 goto err;
683 }
684
685 return 0;
686
687 err:
688 vd->signalquit = 0;
689 return -1;
690 }
691
close_v4l2(struct vdIn * vd)692 int close_v4l2(struct vdIn *vd)
693 {
694 if(vd->streamingState == STREAMING_ON)
695 video_disable(vd, STREAMING_OFF);
696 free_framebuffer(vd);
697 free(vd->videodevice);
698 free(vd->status);
699 free(vd->pictName);
700 vd->videodevice = NULL;
701 vd->status = NULL;
702 vd->pictName = NULL;
703
704 return 0;
705 }
706
707 /* return >= 0 ok otherwhise -1 */
isv4l2Control(struct vdIn * vd,int control,struct v4l2_queryctrl * queryctrl)708 static int isv4l2Control(struct vdIn *vd, int control, struct v4l2_queryctrl *queryctrl)
709 {
710 int err = 0;
711
712 queryctrl->id = control;
713 if((err = xioctl(vd->fd, VIDIOC_QUERYCTRL, queryctrl)) < 0) {
714 //fprintf(stderr, "ioctl querycontrol error %d \n",errno);
715 return -1;
716 }
717
718 if(queryctrl->flags & V4L2_CTRL_FLAG_DISABLED) {
719 //fprintf(stderr, "control %s disabled \n", (char *) queryctrl->name);
720 return -1;
721 }
722
723 if(queryctrl->type & V4L2_CTRL_TYPE_BOOLEAN) {
724 return 1;
725 }
726
727 if(queryctrl->type & V4L2_CTRL_TYPE_INTEGER) {
728 return 0;
729 }
730
731 fprintf(stderr, "contol %s unsupported \n", (char *) queryctrl->name);
732 return -1;
733 }
734
v4l2GetControl(struct vdIn * vd,int control)735 int v4l2GetControl(struct vdIn *vd, int control)
736 {
737 struct v4l2_queryctrl queryctrl;
738 struct v4l2_control control_s;
739 int err;
740
741 if((err = isv4l2Control(vd, control, &queryctrl)) < 0) {
742 return -1;
743 }
744
745 control_s.id = control;
746 if((err = xioctl(vd->fd, VIDIOC_G_CTRL, &control_s)) < 0) {
747 return -1;
748 }
749
750 return control_s.value;
751 }
752
v4l2SetControl(struct vdIn * vd,int control_id,int value,int plugin_number,globals * pglobal)753 int v4l2SetControl(struct vdIn *vd, int control_id, int value, int plugin_number, globals *pglobal)
754 {
755 struct v4l2_control control_s;
756 int min, max;
757 int ret = 0;
758 int err;
759 int i;
760 int got = -1;
761 DBG("Looking for the 0x%08x V4L2 control\n", control_id);
762 for (i = 0; i<pglobal->in[plugin_number].parametercount; i++) {
763 if (pglobal->in[plugin_number].in_parameters[i].ctrl.id == control_id) {
764 got = 0;
765 break;
766 }
767 }
768
769 if (got == 0) { // we have found the control with the specified id
770 DBG("V4L2 ctrl 0x%08x found\n", control_id);
771 if (pglobal->in[plugin_number].in_parameters[i].class_id == V4L2_CTRL_CLASS_USER) {
772 DBG("Control type: USER\n");
773 min = pglobal->in[plugin_number].in_parameters[i].ctrl.minimum;
774 max = pglobal->in[plugin_number].in_parameters[i].ctrl.maximum;
775
776 if((value >= min) && (value <= max)) {
777 control_s.id = control_id;
778 control_s.value = value;
779 if((err = xioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
780 DBG("VIDIOC_S_CTRL failed\n");
781 return -1;
782 } else {
783 DBG("V4L2 ctrl 0x%08x new value: %d\n", control_id, value);
784 pglobal->in[plugin_number].in_parameters[i].value = value;
785 }
786 } else {
787 LOG("Value (%d) out of range (%d .. %d)\n", value, min, max);
788 }
789 return 0;
790 } else { // not user class controls
791 DBG("Control type: EXTENDED\n");
792 struct v4l2_ext_controls ext_ctrls = {0};
793 struct v4l2_ext_control ext_ctrl = {0};
794 ext_ctrl.id = pglobal->in[plugin_number].in_parameters[i].ctrl.id;
795
796 switch(pglobal->in[plugin_number].in_parameters[i].ctrl.type) {
797 #ifdef V4L2_CTRL_TYPE_STRING
798 case V4L2_CTRL_TYPE_STRING:
799 //string gets set on VIDIOC_G_EXT_CTRLS
800 //add the maximum size to value
801 ext_ctrl.size = value;
802 DBG("STRING extended controls are currently broken\n");
803 //ext_ctrl.string = control->string; // FIXMEE
804 break;
805 #endif
806 case V4L2_CTRL_TYPE_INTEGER64:
807 ext_ctrl.value64 = value;
808 break;
809 default:
810 ext_ctrl.value = value;
811 break;
812 }
813
814 ext_ctrls.count = 1;
815 ext_ctrls.controls = &ext_ctrl;
816 ret = xioctl(vd->fd, VIDIOC_S_EXT_CTRLS, &ext_ctrls);
817 if(ret) {
818 LOG("control id: 0x%08x failed to set value (error %i)\n", ext_ctrl.id, ret);
819 return -1;
820 } else {
821 DBG("control id: 0x%08x new value: %d\n", ext_ctrl.id, ext_ctrl.value);
822 }
823 return 0;
824 }
825 } else {
826 LOG("Invalid V4L2_set_control request for the id: 0x%08x. Control cannot be found in the list\n", control_id);
827 return -1;
828 }
829 }
830
v4l2ResetControl(struct vdIn * vd,int control)831 int v4l2ResetControl(struct vdIn *vd, int control)
832 {
833 struct v4l2_control control_s;
834 struct v4l2_queryctrl queryctrl;
835 int val_def;
836 int err;
837
838 if(isv4l2Control(vd, control, &queryctrl) < 0)
839 return -1;
840
841 val_def = queryctrl.default_value;
842 control_s.id = control;
843 control_s.value = val_def;
844
845 if((err = xioctl(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
846 return -1;
847 }
848
849 return 0;
850 }
851
control_readed(struct vdIn * vd,struct v4l2_queryctrl * ctrl,globals * pglobal,int id)852 void control_readed(struct vdIn *vd, struct v4l2_queryctrl *ctrl, globals *pglobal, int id)
853 {
854 struct v4l2_control c;
855 memset(&c, 0, sizeof(struct v4l2_control));
856 c.id = ctrl->id;
857
858 if (pglobal->in[id].in_parameters == NULL) {
859 pglobal->in[id].in_parameters = (control*)calloc(1, sizeof(control));
860 } else {
861 pglobal->in[id].in_parameters =
862 (control*)realloc(pglobal->in[id].in_parameters,(pglobal->in[id].parametercount + 1) * sizeof(control));
863 }
864
865 if (pglobal->in[id].in_parameters == NULL) {
866 DBG("Calloc failed\n");
867 return;
868 }
869
870 memcpy(&pglobal->in[id].in_parameters[pglobal->in[id].parametercount].ctrl, ctrl, sizeof(struct v4l2_queryctrl));
871 pglobal->in[id].in_parameters[pglobal->in[id].parametercount].group = IN_CMD_V4L2;
872 pglobal->in[id].in_parameters[pglobal->in[id].parametercount].value = c.value;
873 if(ctrl->type == V4L2_CTRL_TYPE_MENU) {
874 pglobal->in[id].in_parameters[pglobal->in[id].parametercount].menuitems =
875 (struct v4l2_querymenu*)malloc((ctrl->maximum + 1) * sizeof(struct v4l2_querymenu));
876 int i;
877 for(i = ctrl->minimum; i <= ctrl->maximum; i++) {
878 struct v4l2_querymenu qm;
879 memset(&qm, 0 , sizeof(struct v4l2_querymenu));
880 qm.id = ctrl->id;
881 qm.index = i;
882 if(xioctl(vd->fd, VIDIOC_QUERYMENU, &qm) == 0) {
883 memcpy(&pglobal->in[id].in_parameters[pglobal->in[id].parametercount].menuitems[i], &qm, sizeof(struct v4l2_querymenu));
884 DBG("Menu item %d: %s\n", qm.index, qm.name);
885 } else {
886 DBG("Unable to get menu item for %s, index=%d\n", ctrl->name, qm.index);
887 }
888 }
889 } else {
890 pglobal->in[id].in_parameters[pglobal->in[id].parametercount].menuitems = NULL;
891 }
892
893 pglobal->in[id].in_parameters[pglobal->in[id].parametercount].value = 0;
894 pglobal->in[id].in_parameters[pglobal->in[id].parametercount].class_id = (ctrl->id & 0xFFFF0000);
895 #ifndef V4L2_CTRL_FLAG_NEXT_CTRL
896 pglobal->in[id].in_parameters[pglobal->in[id].parametercount].class_id = V4L2_CTRL_CLASS_USER;
897 #endif
898
899 int ret = -1;
900 if (pglobal->in[id].in_parameters[pglobal->in[id].parametercount].class_id == V4L2_CTRL_CLASS_USER) {
901 DBG("V4L2 parameter found: %s value %d Class: USER \n", ctrl->name, c.value);
902 ret = xioctl(vd->fd, VIDIOC_G_CTRL, &c);
903 if(ret == 0) {
904 pglobal->in[id].in_parameters[pglobal->in[id].parametercount].value = c.value;
905 } else {
906 DBG("Unable to get the value of %s retcode: %d %s\n", ctrl->name, ret, strerror(errno));
907 }
908 } else {
909 DBG("V4L2 parameter found: %s value %d Class: EXTENDED \n", ctrl->name, c.value);
910 struct v4l2_ext_controls ext_ctrls = {0};
911 struct v4l2_ext_control ext_ctrl = {0};
912 ext_ctrl.id = ctrl->id;
913 #ifdef V4L2_CTRL_TYPE_STRING
914 ext_ctrl.size = 0;
915 if(ctrl.type == V4L2_CTRL_TYPE_STRING) {
916 ext_ctrl.size = ctrl->maximum + 1;
917 // FIXMEEEEext_ctrl.string = control->string;
918 }
919 #endif
920 ext_ctrls.count = 1;
921 ext_ctrls.controls = &ext_ctrl;
922 ret = xioctl(vd->fd, VIDIOC_G_EXT_CTRLS, &ext_ctrls);
923 if(ret) {
924 switch (ext_ctrl.id) {
925 case V4L2_CID_PAN_RESET:
926 pglobal->in[id].in_parameters[pglobal->in[id].parametercount].value = 1;
927 DBG("Setting PAN reset value to 1\n");
928 break;
929 case V4L2_CID_TILT_RESET:
930 pglobal->in[id].in_parameters[pglobal->in[id].parametercount].value = 1;
931 DBG("Setting the Tilt reset value to 2\n");
932 break;
933 case V4L2_CID_PANTILT_RESET_LOGITECH:
934 pglobal->in[id].in_parameters[pglobal->in[id].parametercount].value = 3;
935 DBG("Setting the PAN/TILT reset value to 3\n");
936 break;
937 default:
938 DBG("control id: 0x%08x failed to get value (error %i)\n", ext_ctrl.id, ret);
939 }
940 } else {
941 switch(ctrl->type)
942 {
943 #ifdef V4L2_CTRL_TYPE_STRING
944 case V4L2_CTRL_TYPE_STRING:
945 //string gets set on VIDIOC_G_EXT_CTRLS
946 //add the maximum size to value
947 pglobal->in[id].in_parameters[pglobal->in[id].parametercount].value = ext_ctrl.size;
948 break;
949 #endif
950 case V4L2_CTRL_TYPE_INTEGER64:
951 pglobal->in[id].in_parameters[pglobal->in[id].parametercount].value = ext_ctrl.value64;
952 break;
953 default:
954 pglobal->in[id].in_parameters[pglobal->in[id].parametercount].value = ext_ctrl.value;
955 break;
956 }
957 }
958 }
959
960 pglobal->in[id].parametercount++;
961 }
962
963 /* It should set the capture resolution
964 Cheated from the openCV cap_libv4l.cpp the method is the following:
965 Turn off the stream (video_disable)
966 Unmap buffers
967 Close the filedescriptor
968 Initialize the camera again with the new resolution
969 */
setResolution(struct vdIn * vd,int width,int height)970 int setResolution(struct vdIn *vd, int width, int height)
971 {
972 vd->streamingState = STREAMING_PAUSED;
973 if (video_disable(vd, STREAMING_PAUSED) < 0) {
974 IPRINT("Unable to disable streaming\n");
975 return -1;
976 }
977
978 DBG("Unmap buffers\n");
979 int i;
980 for (i = 0; i < NB_BUFFER; i++) {
981 munmap(vd->mem[i], vd->buf.length);
982 }
983
984 if (CLOSE_VIDEO(vd->fd) == 0) {
985 DBG("Device closed successfully\n");
986 }
987
988 vd->width = width;
989 vd->height = height;
990 if (init_v4l2(vd) < 0) {
991 return -1;
992 }
993
994 free_framebuffer(vd);
995 if (init_framebuffer(vd) < 0) {
996 IPRINT("Can\'t reallocate framebuffer\n");
997 return -1;
998 }
999
1000 DBG("Resolution changed to %dx%d , enabling the video...\n", width, height);
1001 if (video_enable(vd) < 0) {
1002 IPRINT("Can\'t RE-enable the video after setResolution(%dx%d)", width, height);
1003 return -1;
1004 }
1005
1006 return 0;
1007 }
1008
1009 /*
1010 *
1011 * Enumarates all V4L2 controls using various methods.
1012 * It places them to the
1013 *
1014 */
1015
enumerateControls(struct vdIn * vd,globals * pglobal,int id)1016 void enumerateControls(struct vdIn *vd, globals *pglobal, int id)
1017 {
1018 // enumerating v4l2 controls
1019 struct v4l2_queryctrl ctrl;
1020 memset(&ctrl, 0, sizeof(struct v4l2_queryctrl));
1021 pglobal->in[id].parametercount = 0;
1022 pglobal->in[id].in_parameters = malloc(0 * sizeof(control));
1023 /* Enumerate the v4l2 controls
1024 Try the extended control API first */
1025 #ifdef V4L2_CTRL_FLAG_NEXT_CTRL
1026 DBG("V4L2 API's V4L2_CTRL_FLAG_NEXT_CTRL is supported\n");
1027 ctrl.id = V4L2_CTRL_FLAG_NEXT_CTRL;
1028 if(0 == IOCTL_VIDEO(vd->fd, VIDIOC_QUERYCTRL, &ctrl)) {
1029 do {
1030 control_readed(vd, &ctrl, pglobal, id);
1031 ctrl.id |= V4L2_CTRL_FLAG_NEXT_CTRL;
1032 } while(0 == IOCTL_VIDEO(vd->fd, VIDIOC_QUERYCTRL, &ctrl));
1033 // note: use simple ioctl or v4l2_ioctl instead of the xioctl
1034 } else
1035 #endif
1036 {
1037 DBG("V4L2 API's V4L2_CTRL_FLAG_NEXT_CTRL is NOT supported\n");
1038 /* Fall back on the standard API */
1039 /* Check all the standard controls */
1040 int i;
1041 for(i = V4L2_CID_BASE; i < V4L2_CID_LASTP1; i++) {
1042 ctrl.id = i;
1043 if(IOCTL_VIDEO(vd->fd, VIDIOC_QUERYCTRL, &ctrl) == 0) {
1044 control_readed(vd, &ctrl, pglobal, id);
1045 }
1046 }
1047
1048 /* Check any custom controls */
1049 for(i = V4L2_CID_PRIVATE_BASE; ; i++) {
1050 ctrl.id = i;
1051 if(IOCTL_VIDEO(vd->fd, VIDIOC_QUERYCTRL, &ctrl) == 0) {
1052 control_readed(vd, &ctrl, pglobal, id);
1053 } else {
1054 break;
1055 }
1056 }
1057 }
1058
1059 memset(&pglobal->in[id].jpegcomp, 0, sizeof(struct v4l2_jpegcompression));
1060 if(xioctl(vd->fd, VIDIOC_G_JPEGCOMP, &pglobal->in[id].jpegcomp) != EINVAL) {
1061 DBG("JPEG compression details:\n");
1062 DBG("Quality: %d\n", pglobal->in[id].jpegcomp.quality);
1063 DBG("APPn: %d\n", pglobal->in[id].jpegcomp.APPn);
1064 DBG("APP length: %d\n", pglobal->in[id].jpegcomp.APP_len);
1065 DBG("APP data: %s\n", pglobal->in[id].jpegcomp.APP_data);
1066 DBG("COM length: %d\n", pglobal->in[id].jpegcomp.COM_len);
1067 DBG("COM data: %s\n", pglobal->in[id].jpegcomp.COM_data);
1068 struct v4l2_queryctrl ctrl_jpeg;
1069 ctrl_jpeg.id = 1;
1070 sprintf((char*)&ctrl_jpeg.name, "JPEG quality");
1071 ctrl_jpeg.minimum = 0;
1072 ctrl_jpeg.maximum = 100;
1073 ctrl_jpeg.step = 1;
1074 ctrl_jpeg.default_value = 50;
1075 ctrl_jpeg.flags = 0;
1076 ctrl_jpeg.type = V4L2_CTRL_TYPE_INTEGER;
1077 if (pglobal->in[id].in_parameters == NULL) {
1078 pglobal->in[id].in_parameters = (control*)calloc(1, sizeof(control));
1079 } else {
1080 pglobal->in[id].in_parameters = (control*)realloc(pglobal->in[id].in_parameters,(pglobal->in[id].parametercount + 1) * sizeof(control));
1081 }
1082
1083 if (pglobal->in[id].in_parameters == NULL) {
1084 DBG("Calloc/realloc failed\n");
1085 return;
1086 }
1087
1088 memcpy(&pglobal->in[id].in_parameters[pglobal->in[id].parametercount].ctrl, &ctrl_jpeg, sizeof(struct v4l2_queryctrl));
1089 pglobal->in[id].in_parameters[pglobal->in[id].parametercount].group = IN_CMD_JPEG_QUALITY;
1090 pglobal->in[id].in_parameters[pglobal->in[id].parametercount].value = pglobal->in[id].jpegcomp.quality;
1091 pglobal->in[id].parametercount++;
1092 } else {
1093 DBG("Modifying the setting of the JPEG compression is not supported\n");
1094 pglobal->in[id].jpegcomp.quality = -1;
1095 }
1096 }
1097