uvc-gadget: factor out uvc protocol and stream handling code

uvc-gadget.c has been getting cluttered with functions related to
UVC protocol handling and stream handling. Additionally, it is forseen
that we might want stream handling to be modular for different system
models. Factor out code related to UVC protocol handling to uvc.c (and
uvc.h) and code related to stream handling to stream.c (and stream.h),
and update the Makefile accordingly.

Signed-off-by: Paul Elder <paul.elder@ideasonboard.com>
Signed-off-by: Laurent Pinchart <laurent.pinchart@ideasonboard.com>

1 files changed, 340 insertions, 0 deletions

diff --git a/uvc.c b/uvc.c
new file mode 100644
index 0000000..dc8c98e
--- /dev/null
+++ b/uvc.c
@@ -0,0 +1,340 @@
+/*
+ * UVC protocol handling
+ *
+ * Copyright (C) 2010 Ideas on board SPRL <laurent.pinchart@ideasonboard.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ */
+
+#include <errno.h>
+#include <linux/usb/ch9.h>
+#include <linux/usb/g_uvc.h>
+#include <linux/usb/video.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/ioctl.h>
+
+#include "configfs.h"
+#include "stream.h"
+#include "tools.h"
+#include "uvc.h"
+#include "v4l2.h"
+
+struct uvc_device *uvc_open(const char *devname)
+{
+	struct uvc_device *dev;
+
+	dev = malloc(sizeof *dev);
+	if (dev == NULL)
+		return NULL;
+
+	memset(dev, 0, sizeof *dev);
+
+	dev->vdev = v4l2_open(devname);
+	if (dev->vdev == NULL) {
+		free(dev);
+		return NULL;
+	}
+
+	return dev;
+}
+
+void uvc_close(struct uvc_device *dev)
+{
+	v4l2_close(dev->vdev);
+	dev->vdev = NULL;
+
+	free(dev);
+}
+
+/* ---------------------------------------------------------------------------
+ * Request processing
+ */
+
+static void
+uvc_fill_streaming_control(struct uvc_device *dev,
+			   struct uvc_streaming_control *ctrl,
+			   int iformat, int iframe, unsigned int ival)
+{
+	const struct uvc_function_config_format *format;
+	const struct uvc_function_config_frame *frame;
+	unsigned int i;
+
+	/*
+	 * Restrict the iformat, iframe and ival to valid values. Negative
+	 * values for iformat or iframe will result in the maximum valid value
+	 * being selected.
+	 */
+        iformat = clamp((unsigned int)iformat, 1U,
+                        dev->fc->streaming.num_formats);
+	format = &dev->fc->streaming.formats[iformat-1];
+
+	iframe = clamp((unsigned int)iframe, 1U, format->num_frames);
+	frame = &format->frames[iframe-1];
+
+	for (i = 0; i < frame->num_intervals; ++i) {
+		if (ival <= frame->intervals[i]) {
+			ival = frame->intervals[i];
+			break;
+		}
+	}
+
+	if (i == frame->num_intervals)
+		ival = frame->intervals[frame->num_intervals-1];
+
+	memset(ctrl, 0, sizeof *ctrl);
+
+	ctrl->bmHint = 1;
+	ctrl->bFormatIndex = iformat;
+	ctrl->bFrameIndex = iframe ;
+	ctrl->dwFrameInterval = ival;
+
+	switch (format->fcc) {
+	case V4L2_PIX_FMT_YUYV:
+		ctrl->dwMaxVideoFrameSize = frame->width * frame->height * 2;
+		break;
+	case V4L2_PIX_FMT_MJPEG:
+		ctrl->dwMaxVideoFrameSize = dev->maxsize;
+		break;
+	}
+
+	ctrl->dwMaxPayloadTransferSize = dev->fc->streaming.ep.wMaxPacketSize;
+	ctrl->bmFramingInfo = 3;
+	ctrl->bPreferedVersion = 1;
+	ctrl->bMaxVersion = 1;
+}
+
+static void
+uvc_events_process_standard(struct uvc_device *dev,
+			    const struct usb_ctrlrequest *ctrl,
+			    struct uvc_request_data *resp)
+{
+	printf("standard request\n");
+	(void)dev;
+	(void)ctrl;
+	(void)resp;
+}
+
+static void
+uvc_events_process_control(struct uvc_device *dev, uint8_t req, uint8_t cs,
+			   struct uvc_request_data *resp)
+{
+	printf("control request (req %02x cs %02x)\n", req, cs);
+	(void)dev;
+	(void)resp;
+}
+
+static void
+uvc_events_process_streaming(struct uvc_device *dev, uint8_t req, uint8_t cs,
+			     struct uvc_request_data *resp)
+{
+	struct uvc_streaming_control *ctrl;
+
+	printf("streaming request (req %02x cs %02x)\n", req, cs);
+
+	if (cs != UVC_VS_PROBE_CONTROL && cs != UVC_VS_COMMIT_CONTROL)
+		return;
+
+	ctrl = (struct uvc_streaming_control *)&resp->data;
+	resp->length = sizeof *ctrl;
+
+	switch (req) {
+	case UVC_SET_CUR:
+		dev->control = cs;
+		resp->length = 34;
+		break;
+
+	case UVC_GET_CUR:
+		if (cs == UVC_VS_PROBE_CONTROL)
+			memcpy(ctrl, &dev->probe, sizeof *ctrl);
+		else
+			memcpy(ctrl, &dev->commit, sizeof *ctrl);
+		break;
+
+	case UVC_GET_MIN:
+	case UVC_GET_MAX:
+	case UVC_GET_DEF:
+		uvc_fill_streaming_control(dev, ctrl, req == UVC_GET_MAX ? -1 : 1,
+					   req == UVC_GET_MAX ? -1 : 1, 0);
+		break;
+
+	case UVC_GET_RES:
+		memset(ctrl, 0, sizeof *ctrl);
+		break;
+
+	case UVC_GET_LEN:
+		resp->data[0] = 0x00;
+		resp->data[1] = 0x22;
+		resp->length = 2;
+		break;
+
+	case UVC_GET_INFO:
+		resp->data[0] = 0x03;
+		resp->length = 1;
+		break;
+	}
+}
+
+static void
+uvc_events_process_class(struct uvc_device *dev,
+			 const struct usb_ctrlrequest *ctrl,
+			 struct uvc_request_data *resp)
+{
+	unsigned int interface = ctrl->wIndex & 0xff;
+
+	if ((ctrl->bRequestType & USB_RECIP_MASK) != USB_RECIP_INTERFACE)
+		return;
+
+	if (interface == dev->fc->control.intf.bInterfaceNumber)
+		uvc_events_process_control(dev, ctrl->bRequest, ctrl->wValue >> 8, resp);
+	else if (interface == dev->fc->streaming.intf.bInterfaceNumber)
+		uvc_events_process_streaming(dev, ctrl->bRequest, ctrl->wValue >> 8, resp);
+}
+
+static void
+uvc_events_process_setup(struct uvc_device *dev,
+			 const struct usb_ctrlrequest *ctrl,
+			 struct uvc_request_data *resp)
+{
+	dev->control = 0;
+
+	printf("bRequestType %02x bRequest %02x wValue %04x wIndex %04x "
+		"wLength %04x\n", ctrl->bRequestType, ctrl->bRequest,
+		ctrl->wValue, ctrl->wIndex, ctrl->wLength);
+
+	switch (ctrl->bRequestType & USB_TYPE_MASK) {
+	case USB_TYPE_STANDARD:
+		uvc_events_process_standard(dev, ctrl, resp);
+		break;
+
+	case USB_TYPE_CLASS:
+		uvc_events_process_class(dev, ctrl, resp);
+		break;
+
+	default:
+		break;
+	}
+}
+
+static void
+uvc_events_process_data(struct uvc_stream *stream,
+			const struct uvc_request_data *data)
+{
+	struct uvc_device *dev = stream->uvc;
+	const struct uvc_streaming_control *ctrl =
+		(const struct uvc_streaming_control *)&data->data;
+	struct uvc_streaming_control *target;
+
+	switch (dev->control) {
+	case UVC_VS_PROBE_CONTROL:
+		printf("setting probe control, length = %d\n", data->length);
+		target = &dev->probe;
+		break;
+
+	case UVC_VS_COMMIT_CONTROL:
+		printf("setting commit control, length = %d\n", data->length);
+		target = &dev->commit;
+		break;
+
+	default:
+		printf("setting unknown control, length = %d\n", data->length);
+		return;
+	}
+
+	uvc_fill_streaming_control(dev, target, ctrl->bFormatIndex,
+				   ctrl->bFrameIndex, ctrl->dwFrameInterval);
+
+	if (dev->control == UVC_VS_COMMIT_CONTROL) {
+		const struct uvc_function_config_format *format;
+		const struct uvc_function_config_frame *frame;
+
+		format = &dev->fc->streaming.formats[target->bFormatIndex-1];
+		frame = &format->frames[target->bFrameIndex-1];
+
+		dev->fcc = format->fcc;
+		dev->width = frame->width;
+		dev->height = frame->height;
+
+		uvc_stream_set_format(stream);
+	}
+}
+
+void uvc_events_process(void *d)
+{
+	struct uvc_stream *stream = d;
+	struct uvc_device *dev = stream->uvc;
+	struct v4l2_event v4l2_event;
+	const struct uvc_event *uvc_event = (void *)&v4l2_event.u.data;
+	struct uvc_request_data resp;
+	int ret;
+
+	ret = ioctl(dev->vdev->fd, VIDIOC_DQEVENT, &v4l2_event);
+	if (ret < 0) {
+		printf("VIDIOC_DQEVENT failed: %s (%d)\n", strerror(errno),
+			errno);
+		return;
+	}
+
+	memset(&resp, 0, sizeof resp);
+	resp.length = -EL2HLT;
+
+	switch (v4l2_event.type) {
+	case UVC_EVENT_CONNECT:
+	case UVC_EVENT_DISCONNECT:
+		return;
+
+	case UVC_EVENT_SETUP:
+		uvc_events_process_setup(dev, &uvc_event->req, &resp);
+		break;
+
+	case UVC_EVENT_DATA:
+		uvc_events_process_data(stream, &uvc_event->data);
+		return;
+
+	case UVC_EVENT_STREAMON:
+		uvc_stream_enable(stream, 1);
+		return;
+
+	case UVC_EVENT_STREAMOFF:
+		uvc_stream_enable(stream, 0);
+		return;
+	}
+
+	ioctl(dev->vdev->fd, UVCIOC_SEND_RESPONSE, &resp);
+	if (ret < 0) {
+		printf("UVCIOC_S_EVENT failed: %s (%d)\n", strerror(errno),
+			errno);
+		return;
+	}
+}
+
+void uvc_events_init(struct uvc_device *dev)
+{
+	struct v4l2_event_subscription sub;
+
+	/* Default to the minimum values. */
+	uvc_fill_streaming_control(dev, &dev->probe, 1, 1, 0);
+	uvc_fill_streaming_control(dev, &dev->commit, 1, 1, 0);
+
+	memset(&sub, 0, sizeof sub);
+	sub.type = UVC_EVENT_SETUP;
+	ioctl(dev->vdev->fd, VIDIOC_SUBSCRIBE_EVENT, &sub);
+	sub.type = UVC_EVENT_DATA;
+	ioctl(dev->vdev->fd, VIDIOC_SUBSCRIBE_EVENT, &sub);
+	sub.type = UVC_EVENT_STREAMON;
+	ioctl(dev->vdev->fd, VIDIOC_SUBSCRIBE_EVENT, &sub);
+	sub.type = UVC_EVENT_STREAMOFF;
+	ioctl(dev->vdev->fd, VIDIOC_SUBSCRIBE_EVENT, &sub);
+}