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