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