uvc-gadget: Sort include headers
[uvc-gadget.git] / uvc-gadget.c
1 /*
2  * UVC gadget test application
3  *
4  * Copyright (C) 2010 Ideas on board SPRL <laurent.pinchart@ideasonboard.com>
5  *
6  * This program is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License along
17  * with this program; if not, write to the Free Software Foundation, Inc.,
18  */
19
20 #include <sys/ioctl.h>
21 #include <sys/mman.h>
22 #include <sys/select.h>
23 #include <sys/stat.h>
24 #include <sys/time.h>
25 #include <sys/types.h>
26
27 #include <errno.h>
28 #include <fcntl.h>
29 #include <signal.h>
30 #include <stdint.h>
31 #include <stdio.h>
32 #include <stdlib.h>
33 #include <string.h>
34 #include <unistd.h>
35
36 #include <linux/usb/ch9.h>
37 #include <linux/usb/g_uvc.h>
38 #include <linux/usb/video.h>
39 #include <linux/videodev2.h>
40
41 #include "events.h"
42 #include "tools.h"
43 #include "v4l2.h"
44
45 #define UVC_INTF_CONTROL        0
46 #define UVC_INTF_STREAMING      1
47
48 struct uvc_device
49 {
50         struct v4l2_device *vdev;
51
52         struct uvc_streaming_control probe;
53         struct uvc_streaming_control commit;
54
55         int control;
56
57         unsigned int fcc;
58         unsigned int width;
59         unsigned int height;
60
61         unsigned int bulk;
62         uint8_t color;
63         unsigned int imgsize;
64         void *imgdata;
65
66         struct events events;
67 };
68
69 static struct uvc_device *
70 uvc_open(const char *devname)
71 {
72         struct uvc_device *dev;
73
74         dev = malloc(sizeof *dev);
75         if (dev == NULL)
76                 return NULL;
77
78         memset(dev, 0, sizeof *dev);
79
80         dev->vdev = v4l2_open(devname);
81         if (dev->vdev == NULL) {
82                 free(dev);
83                 return NULL;
84         }
85
86         events_init(&dev->events);
87
88         return dev;
89 }
90
91 static void
92 uvc_close(struct uvc_device *dev)
93 {
94         v4l2_close(dev->vdev);
95         dev->vdev = NULL;
96
97         free(dev->imgdata);
98         free(dev);
99 }
100
101 /* ---------------------------------------------------------------------------
102  * Video streaming
103  */
104
105 static void
106 uvc_video_fill_buffer(struct uvc_device *dev, struct v4l2_video_buffer *buf)
107 {
108         unsigned int bpl;
109         unsigned int i;
110
111         switch (dev->fcc) {
112         case V4L2_PIX_FMT_YUYV:
113                 /* Fill the buffer with video data. */
114                 bpl = dev->width * 2;
115                 for (i = 0; i < dev->height; ++i)
116                         memset(buf->mem + i*bpl, dev->color++, bpl);
117
118                 buf->bytesused = bpl * dev->height;
119                 break;
120
121         case V4L2_PIX_FMT_MJPEG:
122                 memcpy(buf->mem, dev->imgdata, dev->imgsize);
123                 buf->bytesused = dev->imgsize;
124                 break;
125         }
126 }
127
128 static void
129 uvc_video_process(void *d)
130 {
131         struct uvc_device *dev = d;
132         struct v4l2_video_buffer buf;
133         int ret;
134
135         ret = v4l2_dequeue_buffer(dev->vdev, &buf);
136         if (ret < 0)
137                 return;
138
139         uvc_video_fill_buffer(dev, &buf);
140
141         v4l2_queue_buffer(dev->vdev, &buf);
142 }
143
144 static int
145 uvc_video_stream(struct uvc_device *dev, int enable)
146 {
147         unsigned int i;
148         int ret;
149
150         if (!enable) {
151                 printf("Stopping video stream.\n");
152                 events_unwatch_fd(&dev->events, dev->vdev->fd, EVENT_WRITE);
153                 v4l2_stream_off(dev->vdev);
154                 v4l2_free_buffers(dev->vdev);
155                 return 0;
156         }
157
158         printf("Starting video stream.\n");
159
160         ret = v4l2_alloc_buffers(dev->vdev, V4L2_MEMORY_MMAP, 4);
161         if (ret < 0) {
162                 printf("Failed to allocate buffers.\n");
163                 return ret;
164         }
165
166         ret = v4l2_mmap_buffers(dev->vdev);
167         if (ret < 0) {
168                 printf("Failed to mmap buffers.\n");
169                 goto error;
170         }
171
172         for (i = 0; i < dev->vdev->nbufs; ++i) {
173                 struct v4l2_video_buffer *buf = &dev->vdev->buffers[i];
174
175                 uvc_video_fill_buffer(dev, buf);
176
177                 ret = v4l2_queue_buffer(dev->vdev, buf);
178                 if (ret < 0)
179                         goto error;
180         }
181
182         v4l2_stream_on(dev->vdev);
183         events_watch_fd(&dev->events, dev->vdev->fd, EVENT_WRITE,
184                         uvc_video_process, dev);
185
186         return 0;
187
188 error:
189         v4l2_free_buffers(dev->vdev);
190         return ret;
191 }
192
193 static int
194 uvc_video_set_format(struct uvc_device *dev)
195 {
196         struct v4l2_pix_format fmt;
197
198         printf("Setting format to 0x%08x %ux%u\n",
199                 dev->fcc, dev->width, dev->height);
200
201         memset(&fmt, 0, sizeof fmt);
202         fmt.width = dev->width;
203         fmt.height = dev->height;
204         fmt.pixelformat = dev->fcc;
205         fmt.field = V4L2_FIELD_NONE;
206         if (dev->fcc == V4L2_PIX_FMT_MJPEG)
207                 fmt.sizeimage = dev->imgsize * 1.5;
208
209         return v4l2_set_format(dev->vdev, &fmt);
210 }
211
212 static int
213 uvc_video_init(struct uvc_device *dev __attribute__((__unused__)))
214 {
215         return 0;
216 }
217
218 /* ---------------------------------------------------------------------------
219  * Request processing
220  */
221
222 struct uvc_frame_info
223 {
224         unsigned int width;
225         unsigned int height;
226         unsigned int intervals[8];
227 };
228
229 struct uvc_format_info
230 {
231         unsigned int fcc;
232         const struct uvc_frame_info *frames;
233 };
234
235 static const struct uvc_frame_info uvc_frames_yuyv[] = {
236         {  640, 360, { 666666, 10000000, 50000000, 0 }, },
237         { 1280, 720, { 50000000, 0 }, },
238         { 0, 0, { 0, }, },
239 };
240
241 static const struct uvc_frame_info uvc_frames_mjpeg[] = {
242         {  640, 360, { 666666, 10000000, 50000000, 0 }, },
243         { 1280, 720, { 50000000, 0 }, },
244         { 0, 0, { 0, }, },
245 };
246
247 static const struct uvc_format_info uvc_formats[] = {
248         { V4L2_PIX_FMT_YUYV, uvc_frames_yuyv },
249         { V4L2_PIX_FMT_MJPEG, uvc_frames_mjpeg },
250 };
251
252 static void
253 uvc_fill_streaming_control(struct uvc_device *dev,
254                            struct uvc_streaming_control *ctrl,
255                            int iframe, int iformat)
256 {
257         const struct uvc_format_info *format;
258         const struct uvc_frame_info *frame;
259         unsigned int nframes;
260
261         if (iformat < 0)
262                 iformat = ARRAY_SIZE(uvc_formats) + iformat;
263         if (iformat < 0 || iformat >= (int)ARRAY_SIZE(uvc_formats))
264                 return;
265         format = &uvc_formats[iformat];
266
267         nframes = 0;
268         while (format->frames[nframes].width != 0)
269                 ++nframes;
270
271         if (iframe < 0)
272                 iframe = nframes + iframe;
273         if (iframe < 0 || iframe >= (int)nframes)
274                 return;
275         frame = &format->frames[iframe];
276
277         memset(ctrl, 0, sizeof *ctrl);
278
279         ctrl->bmHint = 1;
280         ctrl->bFormatIndex = iformat + 1;
281         ctrl->bFrameIndex = iframe + 1;
282         ctrl->dwFrameInterval = frame->intervals[0];
283         switch (format->fcc) {
284         case V4L2_PIX_FMT_YUYV:
285                 ctrl->dwMaxVideoFrameSize = frame->width * frame->height * 2;
286                 break;
287         case V4L2_PIX_FMT_MJPEG:
288                 ctrl->dwMaxVideoFrameSize = dev->imgsize;
289                 break;
290         }
291         ctrl->dwMaxPayloadTransferSize = 512;   /* TODO this should be filled by the driver. */
292         ctrl->bmFramingInfo = 3;
293         ctrl->bPreferedVersion = 1;
294         ctrl->bMaxVersion = 1;
295 }
296
297 static void
298 uvc_events_process_standard(struct uvc_device *dev, struct usb_ctrlrequest *ctrl,
299                             struct uvc_request_data *resp)
300 {
301         printf("standard request\n");
302         (void)dev;
303         (void)ctrl;
304         (void)resp;
305 }
306
307 static void
308 uvc_events_process_control(struct uvc_device *dev, uint8_t req, uint8_t cs,
309                            struct uvc_request_data *resp)
310 {
311         printf("control request (req %02x cs %02x)\n", req, cs);
312         (void)dev;
313         (void)resp;
314 }
315
316 static void
317 uvc_events_process_streaming(struct uvc_device *dev, uint8_t req, uint8_t cs,
318                              struct uvc_request_data *resp)
319 {
320         struct uvc_streaming_control *ctrl;
321
322         printf("streaming request (req %02x cs %02x)\n", req, cs);
323
324         if (cs != UVC_VS_PROBE_CONTROL && cs != UVC_VS_COMMIT_CONTROL)
325                 return;
326
327         ctrl = (struct uvc_streaming_control *)&resp->data;
328         resp->length = sizeof *ctrl;
329
330         switch (req) {
331         case UVC_SET_CUR:
332                 dev->control = cs;
333                 resp->length = 34;
334                 break;
335
336         case UVC_GET_CUR:
337                 if (cs == UVC_VS_PROBE_CONTROL)
338                         memcpy(ctrl, &dev->probe, sizeof *ctrl);
339                 else
340                         memcpy(ctrl, &dev->commit, sizeof *ctrl);
341                 break;
342
343         case UVC_GET_MIN:
344         case UVC_GET_MAX:
345         case UVC_GET_DEF:
346                 uvc_fill_streaming_control(dev, ctrl, req == UVC_GET_MAX ? -1 : 0,
347                                            req == UVC_GET_MAX ? -1 : 0);
348                 break;
349
350         case UVC_GET_RES:
351                 memset(ctrl, 0, sizeof *ctrl);
352                 break;
353
354         case UVC_GET_LEN:
355                 resp->data[0] = 0x00;
356                 resp->data[1] = 0x22;
357                 resp->length = 2;
358                 break;
359
360         case UVC_GET_INFO:
361                 resp->data[0] = 0x03;
362                 resp->length = 1;
363                 break;
364         }
365 }
366
367 static void
368 uvc_events_process_class(struct uvc_device *dev, struct usb_ctrlrequest *ctrl,
369                          struct uvc_request_data *resp)
370 {
371         if ((ctrl->bRequestType & USB_RECIP_MASK) != USB_RECIP_INTERFACE)
372                 return;
373
374         switch (ctrl->wIndex & 0xff) {
375         case UVC_INTF_CONTROL:
376                 uvc_events_process_control(dev, ctrl->bRequest, ctrl->wValue >> 8, resp);
377                 break;
378
379         case UVC_INTF_STREAMING:
380                 uvc_events_process_streaming(dev, ctrl->bRequest, ctrl->wValue >> 8, resp);
381                 break;
382
383         default:
384                 break;
385         }
386 }
387
388 static void
389 uvc_events_process_setup(struct uvc_device *dev, struct usb_ctrlrequest *ctrl,
390                          struct uvc_request_data *resp)
391 {
392         dev->control = 0;
393
394         printf("bRequestType %02x bRequest %02x wValue %04x wIndex %04x "
395                 "wLength %04x\n", ctrl->bRequestType, ctrl->bRequest,
396                 ctrl->wValue, ctrl->wIndex, ctrl->wLength);
397
398         switch (ctrl->bRequestType & USB_TYPE_MASK) {
399         case USB_TYPE_STANDARD:
400                 uvc_events_process_standard(dev, ctrl, resp);
401                 break;
402
403         case USB_TYPE_CLASS:
404                 uvc_events_process_class(dev, ctrl, resp);
405                 break;
406
407         default:
408                 break;
409         }
410 }
411
412 static void
413 uvc_events_process_data(struct uvc_device *dev, struct uvc_request_data *data)
414 {
415         struct uvc_streaming_control *target;
416         struct uvc_streaming_control *ctrl;
417         const struct uvc_format_info *format;
418         const struct uvc_frame_info *frame;
419         const unsigned int *interval;
420         unsigned int iformat, iframe;
421         unsigned int nframes;
422
423         switch (dev->control) {
424         case UVC_VS_PROBE_CONTROL:
425                 printf("setting probe control, length = %d\n", data->length);
426                 target = &dev->probe;
427                 break;
428
429         case UVC_VS_COMMIT_CONTROL:
430                 printf("setting commit control, length = %d\n", data->length);
431                 target = &dev->commit;
432                 break;
433
434         default:
435                 printf("setting unknown control, length = %d\n", data->length);
436                 return;
437         }
438
439         ctrl = (struct uvc_streaming_control *)&data->data;
440         iformat = clamp((unsigned int)ctrl->bFormatIndex, 1U,
441                         (unsigned int)ARRAY_SIZE(uvc_formats));
442         format = &uvc_formats[iformat-1];
443
444         nframes = 0;
445         while (format->frames[nframes].width != 0)
446                 ++nframes;
447
448         iframe = clamp((unsigned int)ctrl->bFrameIndex, 1U, nframes);
449         frame = &format->frames[iframe-1];
450         interval = frame->intervals;
451
452         while (interval[0] < ctrl->dwFrameInterval && interval[1])
453                 ++interval;
454
455         target->bFormatIndex = iformat;
456         target->bFrameIndex = iframe;
457         switch (format->fcc) {
458         case V4L2_PIX_FMT_YUYV:
459                 target->dwMaxVideoFrameSize = frame->width * frame->height * 2;
460                 break;
461         case V4L2_PIX_FMT_MJPEG:
462                 if (dev->imgsize == 0)
463                         printf("WARNING: MJPEG requested and no image loaded.\n");
464                 target->dwMaxVideoFrameSize = dev->imgsize;
465                 break;
466         }
467         target->dwFrameInterval = *interval;
468
469         if (dev->control == UVC_VS_COMMIT_CONTROL) {
470                 dev->fcc = format->fcc;
471                 dev->width = frame->width;
472                 dev->height = frame->height;
473
474                 uvc_video_set_format(dev);
475                 if (dev->bulk)
476                         uvc_video_stream(dev, 1);
477         }
478 }
479
480 static void
481 uvc_events_process(void *d)
482 {
483         struct uvc_device *dev = d;
484         struct v4l2_event v4l2_event;
485         struct uvc_event *uvc_event = (void *)&v4l2_event.u.data;
486         struct uvc_request_data resp;
487         int ret;
488
489         ret = ioctl(dev->vdev->fd, VIDIOC_DQEVENT, &v4l2_event);
490         if (ret < 0) {
491                 printf("VIDIOC_DQEVENT failed: %s (%d)\n", strerror(errno),
492                         errno);
493                 return;
494         }
495
496         memset(&resp, 0, sizeof resp);
497         resp.length = -EL2HLT;
498
499         switch (v4l2_event.type) {
500         case UVC_EVENT_CONNECT:
501         case UVC_EVENT_DISCONNECT:
502                 return;
503
504         case UVC_EVENT_SETUP:
505                 uvc_events_process_setup(dev, &uvc_event->req, &resp);
506                 break;
507
508         case UVC_EVENT_DATA:
509                 uvc_events_process_data(dev, &uvc_event->data);
510                 return;
511
512         case UVC_EVENT_STREAMON:
513                 uvc_video_stream(dev, 1);
514                 return;
515
516         case UVC_EVENT_STREAMOFF:
517                 uvc_video_stream(dev, 0);
518                 return;
519         }
520
521         ioctl(dev->vdev->fd, UVCIOC_SEND_RESPONSE, &resp);
522         if (ret < 0) {
523                 printf("UVCIOC_S_EVENT failed: %s (%d)\n", strerror(errno),
524                         errno);
525                 return;
526         }
527 }
528
529 static void
530 uvc_events_init(struct uvc_device *dev)
531 {
532         struct v4l2_event_subscription sub;
533
534         uvc_fill_streaming_control(dev, &dev->probe, 0, 0);
535         uvc_fill_streaming_control(dev, &dev->commit, 0, 0);
536
537         if (dev->bulk) {
538                 /* FIXME Crude hack, must be negotiated with the driver. */
539                 dev->probe.dwMaxPayloadTransferSize = 16 * 1024;
540                 dev->commit.dwMaxPayloadTransferSize = 16 * 1024;
541         }
542
543
544         memset(&sub, 0, sizeof sub);
545         sub.type = UVC_EVENT_SETUP;
546         ioctl(dev->vdev->fd, VIDIOC_SUBSCRIBE_EVENT, &sub);
547         sub.type = UVC_EVENT_DATA;
548         ioctl(dev->vdev->fd, VIDIOC_SUBSCRIBE_EVENT, &sub);
549         sub.type = UVC_EVENT_STREAMON;
550         ioctl(dev->vdev->fd, VIDIOC_SUBSCRIBE_EVENT, &sub);
551         sub.type = UVC_EVENT_STREAMOFF;
552         ioctl(dev->vdev->fd, VIDIOC_SUBSCRIBE_EVENT, &sub);
553 }
554
555 /* ---------------------------------------------------------------------------
556  * main
557  */
558
559 static void image_load(struct uvc_device *dev, const char *img)
560 {
561         int fd = -1;
562
563         if (img == NULL)
564                 return;
565
566         fd = open(img, O_RDONLY);
567         if (fd == -1) {
568                 printf("Unable to open MJPEG image '%s'\n", img);
569                 return;
570         }
571
572         dev->imgsize = lseek(fd, 0, SEEK_END);
573         lseek(fd, 0, SEEK_SET);
574         dev->imgdata = malloc(dev->imgsize);
575         if (dev->imgdata == NULL) {
576                 printf("Unable to allocate memory for MJPEG image\n");
577                 dev->imgsize = 0;
578                 return;
579         }
580
581         read(fd, dev->imgdata, dev->imgsize);
582         close(fd);
583 }
584
585 static void usage(const char *argv0)
586 {
587         fprintf(stderr, "Usage: %s [options]\n", argv0);
588         fprintf(stderr, "Available options are\n");
589         fprintf(stderr, " -b            Use bulk mode\n");
590         fprintf(stderr, " -d device     Video device\n");
591         fprintf(stderr, " -h            Print this help screen and exit\n");
592         fprintf(stderr, " -i image      MJPEG image\n");
593 }
594
595 /* Necesssary for and only used by signal handler. */
596 static struct uvc_device *uvc_device;
597
598 static void sigint_handler(int signal __attribute__((__unused__)))
599 {
600         /* Stop the main loop when the user presses CTRL-C */
601         events_stop(&uvc_device->events);
602 }
603
604 int main(int argc, char *argv[])
605 {
606         char *device = "/dev/video0";
607         struct uvc_device *dev;
608         int bulk_mode = 0;
609         char *mjpeg_image = NULL;
610         int opt;
611
612         while ((opt = getopt(argc, argv, "bd:hi:")) != -1) {
613                 switch (opt) {
614                 case 'b':
615                         bulk_mode = 1;
616                         break;
617
618                 case 'd':
619                         device = optarg;
620                         break;
621
622                 case 'h':
623                         usage(argv[0]);
624                         return 0;
625
626                 case 'i':
627                         mjpeg_image = optarg;
628                         break;
629
630                 default:
631                         fprintf(stderr, "Invalid option '-%c'\n", opt);
632                         usage(argv[0]);
633                         return 1;
634                 }
635         }
636
637         dev = uvc_open(device);
638         if (dev == NULL)
639                 return 1;
640
641         image_load(dev, mjpeg_image);
642
643         dev->bulk = bulk_mode;
644
645         uvc_events_init(dev);
646         uvc_video_init(dev);
647
648         /*
649          * Register a signal handler for SIGINT, received when the user presses
650          * CTRL-C. This will allow the main loop to be interrupted, and resources
651          * to be freed cleanly.
652          */
653         uvc_device = dev;
654         signal(SIGINT, sigint_handler);
655
656         events_watch_fd(&dev->events, dev->vdev->fd, EVENT_EXCEPTION,
657                         uvc_events_process, dev);
658
659         /* Main capture loop */
660         events_loop(&dev->events);
661
662         events_cleanup(&dev->events);
663         uvc_close(dev);
664         return 0;
665 }