uvc-gadget: Add struct uvc_stream
[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         unsigned int maxsize;
61 };
62
63 struct uvc_stream
64 {
65         struct uvc_device *uvc;
66
67         uint8_t color;
68         unsigned int imgsize;
69         void *imgdata;
70
71         struct events *events;
72 };
73
74 static struct uvc_device *
75 uvc_open(const char *devname)
76 {
77         struct uvc_device *dev;
78
79         dev = malloc(sizeof *dev);
80         if (dev == NULL)
81                 return NULL;
82
83         memset(dev, 0, sizeof *dev);
84
85         dev->vdev = v4l2_open(devname);
86         if (dev->vdev == NULL) {
87                 free(dev);
88                 return NULL;
89         }
90
91         return dev;
92 }
93
94 static void
95 uvc_close(struct uvc_device *dev)
96 {
97         v4l2_close(dev->vdev);
98         dev->vdev = NULL;
99
100         free(dev);
101 }
102
103 /* ---------------------------------------------------------------------------
104  * Video streaming
105  */
106
107 static void
108 uvc_video_fill_buffer(struct uvc_stream *stream, struct v4l2_video_buffer *buf)
109 {
110         struct uvc_device *dev = stream->uvc;
111         unsigned int bpl;
112         unsigned int i;
113
114         switch (dev->fcc) {
115         case V4L2_PIX_FMT_YUYV:
116                 /* Fill the buffer with video data. */
117                 bpl = dev->width * 2;
118                 for (i = 0; i < dev->height; ++i)
119                         memset(buf->mem + i*bpl, stream->color++, bpl);
120
121                 buf->bytesused = bpl * dev->height;
122                 break;
123
124         case V4L2_PIX_FMT_MJPEG:
125                 memcpy(buf->mem, stream->imgdata, stream->imgsize);
126                 buf->bytesused = stream->imgsize;
127                 break;
128         }
129 }
130
131 static void
132 uvc_video_process(void *d)
133 {
134         struct uvc_stream *stream = d;
135         struct v4l2_video_buffer buf;
136         int ret;
137
138         ret = v4l2_dequeue_buffer(stream->uvc->vdev, &buf);
139         if (ret < 0)
140                 return;
141
142         uvc_video_fill_buffer(stream, &buf);
143
144         v4l2_queue_buffer(stream->uvc->vdev, &buf);
145 }
146
147 static int
148 uvc_video_stream(struct uvc_stream *stream, int enable)
149 {
150         struct uvc_device *dev = stream->uvc;
151         unsigned int i;
152         int ret;
153
154         if (!enable) {
155                 printf("Stopping video stream.\n");
156                 events_unwatch_fd(stream->events, dev->vdev->fd, EVENT_WRITE);
157                 v4l2_stream_off(dev->vdev);
158                 v4l2_free_buffers(dev->vdev);
159                 return 0;
160         }
161
162         printf("Starting video stream.\n");
163
164         ret = v4l2_alloc_buffers(dev->vdev, V4L2_MEMORY_MMAP, 4);
165         if (ret < 0) {
166                 printf("Failed to allocate buffers.\n");
167                 return ret;
168         }
169
170         ret = v4l2_mmap_buffers(dev->vdev);
171         if (ret < 0) {
172                 printf("Failed to mmap buffers.\n");
173                 goto error;
174         }
175
176         for (i = 0; i < dev->vdev->nbufs; ++i) {
177                 struct v4l2_video_buffer *buf = &dev->vdev->buffers[i];
178
179                 uvc_video_fill_buffer(stream, buf);
180
181                 ret = v4l2_queue_buffer(dev->vdev, buf);
182                 if (ret < 0)
183                         goto error;
184         }
185
186         v4l2_stream_on(dev->vdev);
187         events_watch_fd(stream->events, dev->vdev->fd, EVENT_WRITE,
188                         uvc_video_process, stream);
189
190         return 0;
191
192 error:
193         v4l2_free_buffers(dev->vdev);
194         return ret;
195 }
196
197 static int
198 uvc_video_set_format(struct uvc_device *dev)
199 {
200         struct v4l2_pix_format fmt;
201
202         printf("Setting format to 0x%08x %ux%u\n",
203                 dev->fcc, dev->width, dev->height);
204
205         memset(&fmt, 0, sizeof fmt);
206         fmt.width = dev->width;
207         fmt.height = dev->height;
208         fmt.pixelformat = dev->fcc;
209         fmt.field = V4L2_FIELD_NONE;
210         if (dev->fcc == V4L2_PIX_FMT_MJPEG)
211                 fmt.sizeimage = dev->maxsize * 1.5;
212
213         return v4l2_set_format(dev->vdev, &fmt);
214 }
215
216 static int
217 uvc_video_init(struct uvc_device *dev __attribute__((__unused__)))
218 {
219         return 0;
220 }
221
222 /* ---------------------------------------------------------------------------
223  * Request processing
224  */
225
226 struct uvc_frame_info
227 {
228         unsigned int width;
229         unsigned int height;
230         unsigned int intervals[8];
231 };
232
233 struct uvc_format_info
234 {
235         unsigned int fcc;
236         const struct uvc_frame_info *frames;
237 };
238
239 static const struct uvc_frame_info uvc_frames_yuyv[] = {
240         {  640, 360, { 666666, 10000000, 50000000, 0 }, },
241         { 1280, 720, { 50000000, 0 }, },
242         { 0, 0, { 0, }, },
243 };
244
245 static const struct uvc_frame_info uvc_frames_mjpeg[] = {
246         {  640, 360, { 666666, 10000000, 50000000, 0 }, },
247         { 1280, 720, { 50000000, 0 }, },
248         { 0, 0, { 0, }, },
249 };
250
251 static const struct uvc_format_info uvc_formats[] = {
252         { V4L2_PIX_FMT_YUYV, uvc_frames_yuyv },
253         { V4L2_PIX_FMT_MJPEG, uvc_frames_mjpeg },
254 };
255
256 static void
257 uvc_fill_streaming_control(struct uvc_device *dev,
258                            struct uvc_streaming_control *ctrl,
259                            int iformat, int iframe, unsigned int ival)
260 {
261         const struct uvc_format_info *format;
262         const struct uvc_frame_info *frame;
263         const unsigned int *interval;
264         unsigned int nframes;
265
266         /*
267          * Restrict the iformat, iframe and ival to valid values. Negative
268          * values for iformat or iframe will result in the maximum valid value
269          * being selected.
270          */
271         iformat = clamp((unsigned int)iformat, 1U,
272                         (unsigned int)ARRAY_SIZE(uvc_formats));
273         format = &uvc_formats[iformat-1];
274
275         nframes = 0;
276         while (format->frames[nframes].width != 0)
277                 ++nframes;
278
279         iframe = clamp((unsigned int)iframe, 1U, nframes);
280         frame = &format->frames[iframe-1];
281
282         interval = frame->intervals;
283         while (interval[0] < ival && interval[1])
284                 ++interval;
285
286         memset(ctrl, 0, sizeof *ctrl);
287
288         ctrl->bmHint = 1;
289         ctrl->bFormatIndex = iformat;
290         ctrl->bFrameIndex = iframe ;
291         ctrl->dwFrameInterval = *interval;
292
293         switch (format->fcc) {
294         case V4L2_PIX_FMT_YUYV:
295                 ctrl->dwMaxVideoFrameSize = frame->width * frame->height * 2;
296                 break;
297         case V4L2_PIX_FMT_MJPEG:
298                 ctrl->dwMaxVideoFrameSize = dev->maxsize;
299                 break;
300         }
301
302         ctrl->dwMaxPayloadTransferSize = 512;   /* TODO this should be filled by the driver. */
303         ctrl->bmFramingInfo = 3;
304         ctrl->bPreferedVersion = 1;
305         ctrl->bMaxVersion = 1;
306 }
307
308 static void
309 uvc_events_process_standard(struct uvc_device *dev,
310                             const struct usb_ctrlrequest *ctrl,
311                             struct uvc_request_data *resp)
312 {
313         printf("standard request\n");
314         (void)dev;
315         (void)ctrl;
316         (void)resp;
317 }
318
319 static void
320 uvc_events_process_control(struct uvc_device *dev, uint8_t req, uint8_t cs,
321                            struct uvc_request_data *resp)
322 {
323         printf("control request (req %02x cs %02x)\n", req, cs);
324         (void)dev;
325         (void)resp;
326 }
327
328 static void
329 uvc_events_process_streaming(struct uvc_device *dev, uint8_t req, uint8_t cs,
330                              struct uvc_request_data *resp)
331 {
332         struct uvc_streaming_control *ctrl;
333
334         printf("streaming request (req %02x cs %02x)\n", req, cs);
335
336         if (cs != UVC_VS_PROBE_CONTROL && cs != UVC_VS_COMMIT_CONTROL)
337                 return;
338
339         ctrl = (struct uvc_streaming_control *)&resp->data;
340         resp->length = sizeof *ctrl;
341
342         switch (req) {
343         case UVC_SET_CUR:
344                 dev->control = cs;
345                 resp->length = 34;
346                 break;
347
348         case UVC_GET_CUR:
349                 if (cs == UVC_VS_PROBE_CONTROL)
350                         memcpy(ctrl, &dev->probe, sizeof *ctrl);
351                 else
352                         memcpy(ctrl, &dev->commit, sizeof *ctrl);
353                 break;
354
355         case UVC_GET_MIN:
356         case UVC_GET_MAX:
357         case UVC_GET_DEF:
358                 uvc_fill_streaming_control(dev, ctrl, req == UVC_GET_MAX ? -1 : 1,
359                                            req == UVC_GET_MAX ? -1 : 1, 0);
360                 break;
361
362         case UVC_GET_RES:
363                 memset(ctrl, 0, sizeof *ctrl);
364                 break;
365
366         case UVC_GET_LEN:
367                 resp->data[0] = 0x00;
368                 resp->data[1] = 0x22;
369                 resp->length = 2;
370                 break;
371
372         case UVC_GET_INFO:
373                 resp->data[0] = 0x03;
374                 resp->length = 1;
375                 break;
376         }
377 }
378
379 static void
380 uvc_events_process_class(struct uvc_device *dev,
381                          const struct usb_ctrlrequest *ctrl,
382                          struct uvc_request_data *resp)
383 {
384         if ((ctrl->bRequestType & USB_RECIP_MASK) != USB_RECIP_INTERFACE)
385                 return;
386
387         switch (ctrl->wIndex & 0xff) {
388         case UVC_INTF_CONTROL:
389                 uvc_events_process_control(dev, ctrl->bRequest, ctrl->wValue >> 8, resp);
390                 break;
391
392         case UVC_INTF_STREAMING:
393                 uvc_events_process_streaming(dev, ctrl->bRequest, ctrl->wValue >> 8, resp);
394                 break;
395
396         default:
397                 break;
398         }
399 }
400
401 static void
402 uvc_events_process_setup(struct uvc_device *dev,
403                          const struct usb_ctrlrequest *ctrl,
404                          struct uvc_request_data *resp)
405 {
406         dev->control = 0;
407
408         printf("bRequestType %02x bRequest %02x wValue %04x wIndex %04x "
409                 "wLength %04x\n", ctrl->bRequestType, ctrl->bRequest,
410                 ctrl->wValue, ctrl->wIndex, ctrl->wLength);
411
412         switch (ctrl->bRequestType & USB_TYPE_MASK) {
413         case USB_TYPE_STANDARD:
414                 uvc_events_process_standard(dev, ctrl, resp);
415                 break;
416
417         case USB_TYPE_CLASS:
418                 uvc_events_process_class(dev, ctrl, resp);
419                 break;
420
421         default:
422                 break;
423         }
424 }
425
426 static void
427 uvc_events_process_data(struct uvc_device *dev,
428                         const struct uvc_request_data *data)
429 {
430         const struct uvc_streaming_control *ctrl =
431                 (const struct uvc_streaming_control *)&data->data;
432         struct uvc_streaming_control *target;
433
434         switch (dev->control) {
435         case UVC_VS_PROBE_CONTROL:
436                 printf("setting probe control, length = %d\n", data->length);
437                 target = &dev->probe;
438                 break;
439
440         case UVC_VS_COMMIT_CONTROL:
441                 printf("setting commit control, length = %d\n", data->length);
442                 target = &dev->commit;
443                 break;
444
445         default:
446                 printf("setting unknown control, length = %d\n", data->length);
447                 return;
448         }
449
450         uvc_fill_streaming_control(dev, target, ctrl->bFormatIndex,
451                                    ctrl->bFrameIndex, ctrl->dwFrameInterval);
452
453         if (dev->control == UVC_VS_COMMIT_CONTROL) {
454                 const struct uvc_format_info *format;
455                 const struct uvc_frame_info *frame;
456
457                 format = &uvc_formats[target->bFormatIndex-1];
458                 frame = &format->frames[target->bFrameIndex-1];
459
460                 dev->fcc = format->fcc;
461                 dev->width = frame->width;
462                 dev->height = frame->height;
463
464                 uvc_video_set_format(dev);
465         }
466 }
467
468 static void
469 uvc_events_process(void *d)
470 {
471         struct uvc_stream *stream = d;
472         struct uvc_device *dev = stream->uvc;
473         struct v4l2_event v4l2_event;
474         const struct uvc_event *uvc_event = (void *)&v4l2_event.u.data;
475         struct uvc_request_data resp;
476         int ret;
477
478         ret = ioctl(dev->vdev->fd, VIDIOC_DQEVENT, &v4l2_event);
479         if (ret < 0) {
480                 printf("VIDIOC_DQEVENT failed: %s (%d)\n", strerror(errno),
481                         errno);
482                 return;
483         }
484
485         memset(&resp, 0, sizeof resp);
486         resp.length = -EL2HLT;
487
488         switch (v4l2_event.type) {
489         case UVC_EVENT_CONNECT:
490         case UVC_EVENT_DISCONNECT:
491                 return;
492
493         case UVC_EVENT_SETUP:
494                 uvc_events_process_setup(dev, &uvc_event->req, &resp);
495                 break;
496
497         case UVC_EVENT_DATA:
498                 uvc_events_process_data(dev, &uvc_event->data);
499                 return;
500
501         case UVC_EVENT_STREAMON:
502                 uvc_video_stream(stream, 1);
503                 return;
504
505         case UVC_EVENT_STREAMOFF:
506                 uvc_video_stream(stream, 0);
507                 return;
508         }
509
510         ioctl(dev->vdev->fd, UVCIOC_SEND_RESPONSE, &resp);
511         if (ret < 0) {
512                 printf("UVCIOC_S_EVENT failed: %s (%d)\n", strerror(errno),
513                         errno);
514                 return;
515         }
516 }
517
518 static void
519 uvc_events_init(struct uvc_device *dev)
520 {
521         struct v4l2_event_subscription sub;
522
523         /* Default to the minimum values. */
524         uvc_fill_streaming_control(dev, &dev->probe, 1, 1, 0);
525         uvc_fill_streaming_control(dev, &dev->commit, 1, 1, 0);
526
527         memset(&sub, 0, sizeof sub);
528         sub.type = UVC_EVENT_SETUP;
529         ioctl(dev->vdev->fd, VIDIOC_SUBSCRIBE_EVENT, &sub);
530         sub.type = UVC_EVENT_DATA;
531         ioctl(dev->vdev->fd, VIDIOC_SUBSCRIBE_EVENT, &sub);
532         sub.type = UVC_EVENT_STREAMON;
533         ioctl(dev->vdev->fd, VIDIOC_SUBSCRIBE_EVENT, &sub);
534         sub.type = UVC_EVENT_STREAMOFF;
535         ioctl(dev->vdev->fd, VIDIOC_SUBSCRIBE_EVENT, &sub);
536 }
537
538 /* ---------------------------------------------------------------------------
539  * Stream handling
540  */
541
542 static struct uvc_stream *uvc_stream_new(const char *device)
543 {
544         struct uvc_stream *stream;
545
546         stream = malloc(sizeof(*stream));
547         if (stream == NULL)
548                 return NULL;
549
550         memset(stream, 0, sizeof(*stream));
551
552         stream->uvc = uvc_open(device);
553         if (stream->uvc == NULL) {
554                 free(stream);
555                 return NULL;
556         }
557
558         return stream;
559 }
560
561 static void uvc_stream_delete(struct uvc_stream *stream)
562 {
563         if (stream == NULL)
564                 return;
565
566         uvc_close(stream->uvc);
567
568         free(stream->imgdata);
569         free(stream);
570 }
571
572 static void uvc_stream_load_image(struct uvc_stream *stream, const char *img)
573 {
574         int fd = -1;
575
576         if (img == NULL)
577                 return;
578
579         fd = open(img, O_RDONLY);
580         if (fd == -1) {
581                 printf("Unable to open MJPEG image '%s'\n", img);
582                 return;
583         }
584
585         stream->imgsize = lseek(fd, 0, SEEK_END);
586         lseek(fd, 0, SEEK_SET);
587         stream->imgdata = malloc(stream->imgsize);
588         if (stream->imgdata == NULL) {
589                 printf("Unable to allocate memory for MJPEG image\n");
590                 stream->imgsize = 0;
591                 return;
592         }
593
594         read(fd, stream->imgdata, stream->imgsize);
595         close(fd);
596 }
597
598 static void uvc_stream_init_uvc(struct uvc_stream *stream)
599 {
600         /*
601          * FIXME: The maximum size should be specified per format and frame.
602          * For now just hardcode it to support MJPEG.
603          */
604         stream->uvc->maxsize = stream->imgsize;
605
606         uvc_events_init(stream->uvc);
607         uvc_video_init(stream->uvc);
608 }
609
610 static void uvc_stream_set_event_handler(struct uvc_stream *stream,
611                                          struct events *events)
612 {
613         stream->events = events;
614
615         events_watch_fd(stream->events, stream->uvc->vdev->fd, EVENT_EXCEPTION,
616                         uvc_events_process, stream);
617 }
618
619 /* ---------------------------------------------------------------------------
620  * main
621  */
622
623 static void usage(const char *argv0)
624 {
625         fprintf(stderr, "Usage: %s [options]\n", argv0);
626         fprintf(stderr, "Available options are\n");
627         fprintf(stderr, " -d device     Video device\n");
628         fprintf(stderr, " -h            Print this help screen and exit\n");
629         fprintf(stderr, " -i image      MJPEG image\n");
630 }
631
632 /* Necessary for and only used by signal handler. */
633 static struct events *sigint_events;
634
635 static void sigint_handler(int signal __attribute__((__unused__)))
636 {
637         /* Stop the main loop when the user presses CTRL-C */
638         events_stop(sigint_events);
639 }
640
641 int main(int argc, char *argv[])
642 {
643         char *device = "/dev/video0";
644         struct uvc_stream *stream;
645         struct events events;
646         char *mjpeg_image = NULL;
647         int ret = 0;
648         int opt;
649
650         while ((opt = getopt(argc, argv, "d:hi:")) != -1) {
651                 switch (opt) {
652                 case 'd':
653                         device = optarg;
654                         break;
655
656                 case 'h':
657                         usage(argv[0]);
658                         return 0;
659
660                 case 'i':
661                         mjpeg_image = optarg;
662                         break;
663
664                 default:
665                         fprintf(stderr, "Invalid option '-%c'\n", opt);
666                         usage(argv[0]);
667                         return 1;
668                 }
669         }
670
671         /*
672          * Create the events handler. Register a signal handler for SIGINT,
673          * received when the user presses CTRL-C. This will allow the main loop
674          * to be interrupted, and resources to be freed cleanly.
675          */
676         events_init(&events);
677
678         sigint_events = &events;
679         signal(SIGINT, sigint_handler);
680
681         /* Create and initialise the stream. */
682         stream = uvc_stream_new(device);
683         if (stream == NULL) {
684                 ret = 1;
685                 goto done;
686         }
687
688         uvc_stream_load_image(stream, mjpeg_image);
689         uvc_stream_init_uvc(stream);
690         uvc_stream_set_event_handler(stream, &events);
691
692         /* Main capture loop */
693         events_loop(&events);
694
695 done:
696         /* Cleanup */
697         uvc_stream_delete(stream);
698         events_cleanup(&events);
699
700         return ret;
701 }