[linux-2.6-block.git] / drivers / usb / gadget / function / uvc_video.c

// SPDX-License-Identifier: GPL-2.0+
/*
 *	uvc_video.c  --  USB Video Class Gadget driver
 *
 *	Copyright (C) 2009-2010
 *	    Laurent Pinchart (laurent.pinchart@ideasonboard.com)
 */

#include <linux/kernel.h>
#include <linux/device.h>
#include <linux/errno.h>
#include <linux/usb/ch9.h>
#include <linux/usb/gadget.h>
#include <linux/usb/video.h>
#include <asm/unaligned.h>

#include <media/v4l2-dev.h>

#include "uvc.h"
#include "uvc_queue.h"
#include "uvc_video.h"

/* --------------------------------------------------------------------------
 * Video codecs
 */

static int
uvc_video_encode_header(struct uvc_video *video, struct uvc_buffer *buf,
		u8 *data, int len)
{
	struct uvc_device *uvc = container_of(video, struct uvc_device, video);
	struct usb_composite_dev *cdev = uvc->func.config->cdev;
	struct timespec64 ts = ns_to_timespec64(buf->buf.vb2_buf.timestamp);
	int pos = 2;

	data[1] = UVC_STREAM_EOH | video->fid;

	if (video->queue.buf_used == 0 && ts.tv_sec) {
		/* dwClockFrequency is 48 MHz */
		u32 pts = ((u64)ts.tv_sec * USEC_PER_SEC + ts.tv_nsec / NSEC_PER_USEC) * 48;

		data[1] |= UVC_STREAM_PTS;
		put_unaligned_le32(pts, &data[pos]);
		pos += 4;
	}

	if (cdev->gadget->ops->get_frame) {
		u32 sof, stc;

		sof = usb_gadget_frame_number(cdev->gadget);
		ktime_get_ts64(&ts);
		stc = ((u64)ts.tv_sec * USEC_PER_SEC + ts.tv_nsec / NSEC_PER_USEC) * 48;

		data[1] |= UVC_STREAM_SCR;
		put_unaligned_le32(stc, &data[pos]);
		put_unaligned_le16(sof, &data[pos+4]);
		pos += 6;
	}

	data[0] = pos;

	if (buf->bytesused - video->queue.buf_used <= len - pos)
		data[1] |= UVC_STREAM_EOF;

	return pos;
}

static int
uvc_video_encode_data(struct uvc_video *video, struct uvc_buffer *buf,
		u8 *data, int len)
{
	struct uvc_video_queue *queue = &video->queue;
	unsigned int nbytes;
	void *mem;

	/* Copy video data to the USB buffer. */
	mem = buf->mem + queue->buf_used;
	nbytes = min((unsigned int)len, buf->bytesused - queue->buf_used);

	memcpy(data, mem, nbytes);
	queue->buf_used += nbytes;

	return nbytes;
}

static void
uvc_video_encode_bulk(struct usb_request *req, struct uvc_video *video,
		struct uvc_buffer *buf)
{
	void *mem = req->buf;
	struct uvc_request *ureq = req->context;
	int len = video->req_size;
	int ret;

	/* Add a header at the beginning of the payload. */
	if (video->payload_size == 0) {
		ret = uvc_video_encode_header(video, buf, mem, len);
		video->payload_size += ret;
		mem += ret;
		len -= ret;
	}

	/* Process video data. */
	len = min((int)(video->max_payload_size - video->payload_size), len);
	ret = uvc_video_encode_data(video, buf, mem, len);

	video->payload_size += ret;
	len -= ret;

	req->length = video->req_size - len;
	req->zero = video->payload_size == video->max_payload_size;

	if (buf->bytesused == video->queue.buf_used) {
		video->queue.buf_used = 0;
		buf->state = UVC_BUF_STATE_DONE;
		list_del(&buf->queue);
		video->fid ^= UVC_STREAM_FID;
		ureq->last_buf = buf;

		video->payload_size = 0;
	}

	if (video->payload_size == video->max_payload_size ||
	    video->queue.flags & UVC_QUEUE_DROP_INCOMPLETE ||
	    buf->bytesused == video->queue.buf_used)
		video->payload_size = 0;
}

static void
uvc_video_encode_isoc_sg(struct usb_request *req, struct uvc_video *video,
		struct uvc_buffer *buf)
{
	unsigned int pending = buf->bytesused - video->queue.buf_used;
	struct uvc_request *ureq = req->context;
	struct scatterlist *sg, *iter;
	unsigned int len = video->req_size;
	unsigned int sg_left, part = 0;
	unsigned int i;
	int header_len;

	sg = ureq->sgt.sgl;
	sg_init_table(sg, ureq->sgt.nents);

	/* Init the header. */
	header_len = uvc_video_encode_header(video, buf, ureq->header,
				      video->req_size);
	sg_set_buf(sg, ureq->header, header_len);
	len -= header_len;

	if (pending <= len)
		len = pending;

	req->length = (len == pending) ?
		len + header_len : video->req_size;

	/* Init the pending sgs with payload */
	sg = sg_next(sg);

	for_each_sg(sg, iter, ureq->sgt.nents - 1, i) {
		if (!len || !buf->sg || !buf->sg->length)
			break;

		sg_left = buf->sg->length - buf->offset;
		part = min_t(unsigned int, len, sg_left);

		sg_set_page(iter, sg_page(buf->sg), part, buf->offset);

		if (part == sg_left) {
			buf->offset = 0;
			buf->sg = sg_next(buf->sg);
		} else {
			buf->offset += part;
		}
		len -= part;
	}

	/* Assign the video data with header. */
	req->buf = NULL;
	req->sg	= ureq->sgt.sgl;
	req->num_sgs = i + 1;

	req->length -= len;
	video->queue.buf_used += req->length - header_len;

	if (buf->bytesused == video->queue.buf_used || !buf->sg ||
			video->queue.flags & UVC_QUEUE_DROP_INCOMPLETE) {
		video->queue.buf_used = 0;
		buf->state = UVC_BUF_STATE_DONE;
		buf->offset = 0;
		list_del(&buf->queue);
		video->fid ^= UVC_STREAM_FID;
		ureq->last_buf = buf;
	}
}

static void
uvc_video_encode_isoc(struct usb_request *req, struct uvc_video *video,
		struct uvc_buffer *buf)
{
	void *mem = req->buf;
	struct uvc_request *ureq = req->context;
	int len = video->req_size;
	int ret;

	/* Add the header. */
	ret = uvc_video_encode_header(video, buf, mem, len);
	mem += ret;
	len -= ret;

	/* Process video data. */
	ret = uvc_video_encode_data(video, buf, mem, len);
	len -= ret;

	req->length = video->req_size - len;

	if (buf->bytesused == video->queue.buf_used ||
			video->queue.flags & UVC_QUEUE_DROP_INCOMPLETE) {
		video->queue.buf_used = 0;
		buf->state = UVC_BUF_STATE_DONE;
		list_del(&buf->queue);
		video->fid ^= UVC_STREAM_FID;
		ureq->last_buf = buf;
	}
}

/* --------------------------------------------------------------------------
 * Request handling
 */

static int uvcg_video_ep_queue(struct uvc_video *video, struct usb_request *req)
{
	int ret;

	ret = usb_ep_queue(video->ep, req, GFP_ATOMIC);
	if (ret < 0) {
		uvcg_err(&video->uvc->func, "Failed to queue request (%d).\n",
			 ret);

		/* If the endpoint is disabled the descriptor may be NULL. */
		if (video->ep->desc) {
			/* Isochronous endpoints can't be halted. */
			if (usb_endpoint_xfer_bulk(video->ep->desc))
				usb_ep_set_halt(video->ep);
		}
	}

	return ret;
}

static void
uvc_video_complete(struct usb_ep *ep, struct usb_request *req)
{
	struct uvc_request *ureq = req->context;
	struct uvc_video *video = ureq->video;
	struct uvc_video_queue *queue = &video->queue;
	struct uvc_device *uvc = video->uvc;
	unsigned long flags;

	switch (req->status) {
	case 0:
		break;

	case -EXDEV:
		uvcg_dbg(&video->uvc->func, "VS request missed xfer.\n");
		queue->flags |= UVC_QUEUE_DROP_INCOMPLETE;
		break;

	case -ESHUTDOWN:	/* disconnect from host. */
		uvcg_dbg(&video->uvc->func, "VS request cancelled.\n");
		uvcg_queue_cancel(queue, 1);
		break;

	default:
		uvcg_warn(&video->uvc->func,
			  "VS request completed with status %d.\n",
			  req->status);
		uvcg_queue_cancel(queue, 0);
	}

	if (ureq->last_buf) {
		uvcg_complete_buffer(&video->queue, ureq->last_buf);
		ureq->last_buf = NULL;
	}

	spin_lock_irqsave(&video->req_lock, flags);
	list_add_tail(&req->list, &video->req_free);
	spin_unlock_irqrestore(&video->req_lock, flags);

	if (uvc->state == UVC_STATE_STREAMING)
		queue_work(video->async_wq, &video->pump);
}

static int
uvc_video_free_requests(struct uvc_video *video)
{
	unsigned int i;

	if (video->ureq) {
		for (i = 0; i < video->uvc_num_requests; ++i) {
			sg_free_table(&video->ureq[i].sgt);

			if (video->ureq[i].req) {
				usb_ep_free_request(video->ep, video->ureq[i].req);
				video->ureq[i].req = NULL;
			}

			if (video->ureq[i].req_buffer) {
				kfree(video->ureq[i].req_buffer);
				video->ureq[i].req_buffer = NULL;
			}
		}

		kfree(video->ureq);
		video->ureq = NULL;
	}

	INIT_LIST_HEAD(&video->req_free);
	video->req_size = 0;
	return 0;
}

static int
uvc_video_alloc_requests(struct uvc_video *video)
{
	unsigned int req_size;
	unsigned int i;
	int ret = -ENOMEM;

	BUG_ON(video->req_size);

	req_size = video->ep->maxpacket
		 * max_t(unsigned int, video->ep->maxburst, 1)
		 * (video->ep->mult);

	video->ureq = kcalloc(video->uvc_num_requests, sizeof(struct uvc_request), GFP_KERNEL);
	if (video->ureq == NULL)
		return -ENOMEM;

	for (i = 0; i < video->uvc_num_requests; ++i) {
		video->ureq[i].req_buffer = kmalloc(req_size, GFP_KERNEL);
		if (video->ureq[i].req_buffer == NULL)
			goto error;

		video->ureq[i].req = usb_ep_alloc_request(video->ep, GFP_KERNEL);
		if (video->ureq[i].req == NULL)
			goto error;

		video->ureq[i].req->buf = video->ureq[i].req_buffer;
		video->ureq[i].req->length = 0;
		video->ureq[i].req->complete = uvc_video_complete;
		video->ureq[i].req->context = &video->ureq[i];
		video->ureq[i].video = video;
		video->ureq[i].last_buf = NULL;

		list_add_tail(&video->ureq[i].req->list, &video->req_free);
		/* req_size/PAGE_SIZE + 1 for overruns and + 1 for header */
		sg_alloc_table(&video->ureq[i].sgt,
			       DIV_ROUND_UP(req_size - UVCG_REQUEST_HEADER_LEN,
					    PAGE_SIZE) + 2, GFP_KERNEL);
	}

	video->req_size = req_size;

	return 0;

error:
	uvc_video_free_requests(video);
	return ret;
}

/* --------------------------------------------------------------------------
 * Video streaming
 */

/*
 * uvcg_video_pump - Pump video data into the USB requests
 *
 * This function fills the available USB requests (listed in req_free) with
 * video data from the queued buffers.
 */
static void uvcg_video_pump(struct work_struct *work)
{
	struct uvc_video *video = container_of(work, struct uvc_video, pump);
	struct uvc_video_queue *queue = &video->queue;
	/* video->max_payload_size is only set when using bulk transfer */
	bool is_bulk = video->max_payload_size;
	struct uvc_device *uvc = video->uvc;
	struct usb_request *req = NULL;
	struct uvc_buffer *buf;
	unsigned long flags;
	bool buf_done;
	int ret;

	while (uvc->state == UVC_STATE_STREAMING && video->ep->enabled) {
		/*
		 * Retrieve the first available USB request, protected by the
		 * request lock.
		 */
		spin_lock_irqsave(&video->req_lock, flags);
		if (list_empty(&video->req_free)) {
			spin_unlock_irqrestore(&video->req_lock, flags);
			return;
		}
		req = list_first_entry(&video->req_free, struct usb_request,
					list);
		list_del(&req->list);
		spin_unlock_irqrestore(&video->req_lock, flags);

		/*
		 * Retrieve the first available video buffer and fill the
		 * request, protected by the video queue irqlock.
		 */
		spin_lock_irqsave(&queue->irqlock, flags);
		buf = uvcg_queue_head(queue);

		if (buf != NULL) {
			video->encode(req, video, buf);
			buf_done = buf->state == UVC_BUF_STATE_DONE;
		} else if (!(queue->flags & UVC_QUEUE_DISCONNECTED) && !is_bulk) {
			/*
			 * No video buffer available; the queue is still connected and
			 * we're transferring over ISOC. Queue a 0 length request to
			 * prevent missed ISOC transfers.
			 */
			req->length = 0;
			buf_done = false;
		} else {
			/*
			 * Either the queue has been disconnected or no video buffer
			 * available for bulk transfer. Either way, stop processing
			 * further.
			 */
			spin_unlock_irqrestore(&queue->irqlock, flags);
			break;
		}

		/*
		 * With USB3 handling more requests at a higher speed, we can't
		 * afford to generate an interrupt for every request. Decide to
		 * interrupt:
		 *
		 * - When no more requests are available in the free queue, as
		 *   this may be our last chance to refill the endpoint's
		 *   request queue.
		 *
		 * - When this is request is the last request for the video
		 *   buffer, as we want to start sending the next video buffer
		 *   ASAP in case it doesn't get started already in the next
		 *   iteration of this loop.
		 *
		 * - Four times over the length of the requests queue (as
		 *   indicated by video->uvc_num_requests), as a trade-off
		 *   between latency and interrupt load.
		 */
		if (list_empty(&video->req_free) || buf_done ||
		    !(video->req_int_count %
		       DIV_ROUND_UP(video->uvc_num_requests, 4))) {
			video->req_int_count = 0;
			req->no_interrupt = 0;
		} else {
			req->no_interrupt = 1;
		}

		/* Queue the USB request */
		ret = uvcg_video_ep_queue(video, req);
		spin_unlock_irqrestore(&queue->irqlock, flags);

		if (ret < 0) {
			uvcg_queue_cancel(queue, 0);
			break;
		}

		/* Endpoint now owns the request */
		req = NULL;
		video->req_int_count++;
	}

	if (!req)
		return;

	spin_lock_irqsave(&video->req_lock, flags);
	list_add_tail(&req->list, &video->req_free);
	spin_unlock_irqrestore(&video->req_lock, flags);
	return;
}

/*
 * Enable or disable the video stream.
 */
int uvcg_video_enable(struct uvc_video *video, int enable)
{
	unsigned int i;
	int ret;

	if (video->ep == NULL) {
		uvcg_info(&video->uvc->func,
			  "Video enable failed, device is uninitialized.\n");
		return -ENODEV;
	}

	if (!enable) {
		cancel_work_sync(&video->pump);
		uvcg_queue_cancel(&video->queue, 0);

		for (i = 0; i < video->uvc_num_requests; ++i)
			if (video->ureq && video->ureq[i].req)
				usb_ep_dequeue(video->ep, video->ureq[i].req);

		uvc_video_free_requests(video);
		uvcg_queue_enable(&video->queue, 0);
		return 0;
	}

	if ((ret = uvcg_queue_enable(&video->queue, 1)) < 0)
		return ret;

	if ((ret = uvc_video_alloc_requests(video)) < 0)
		return ret;

	if (video->max_payload_size) {
		video->encode = uvc_video_encode_bulk;
		video->payload_size = 0;
	} else
		video->encode = video->queue.use_sg ?
			uvc_video_encode_isoc_sg : uvc_video_encode_isoc;

	video->req_int_count = 0;

	queue_work(video->async_wq, &video->pump);

	return ret;
}

/*
 * Initialize the UVC video stream.
 */
int uvcg_video_init(struct uvc_video *video, struct uvc_device *uvc)
{
	INIT_LIST_HEAD(&video->req_free);
	spin_lock_init(&video->req_lock);
	INIT_WORK(&video->pump, uvcg_video_pump);

	/* Allocate a work queue for asynchronous video pump handler. */
	video->async_wq = alloc_workqueue("uvcgadget", WQ_UNBOUND | WQ_HIGHPRI, 0);
	if (!video->async_wq)
		return -EINVAL;

	video->uvc = uvc;
	video->fcc = V4L2_PIX_FMT_YUYV;
	video->bpp = 16;
	video->width = 320;
	video->height = 240;
	video->imagesize = 320 * 240 * 2;

	/* Initialize the video buffers queue. */
	uvcg_queue_init(&video->queue, uvc->v4l2_dev.dev->parent,
			V4L2_BUF_TYPE_VIDEO_OUTPUT, &video->mutex);
	return 0;
}
Commit	Line	Data
5fd54ace	1	// SPDX-License-Identifier: GPL-2.0+
cdda479f LP	2	/*
	3	* uvc_video.c -- USB Video Class Gadget driver
	4	*
	5	* Copyright (C) 2009-2010
	6	* Laurent Pinchart (laurent.pinchart@ideasonboard.com)
cdda479f LP	7	*/
	8
	9	#include <linux/kernel.h>
	10	#include <linux/device.h>
	11	#include <linux/errno.h>
	12	#include <linux/usb/ch9.h>
	13	#include <linux/usb/gadget.h>
3a83c16e	14	#include <linux/usb/video.h>
fd03af27	15	#include <asm/unaligned.h>
cdda479f LP	16
	17	#include <media/v4l2-dev.h>
	18
	19	#include "uvc.h"
	20	#include "uvc_queue.h"
70685711	21	#include "uvc_video.h"
cdda479f LP	22
	23	/* --------------------------------------------------------------------------
	24	* Video codecs
	25	*/
	26
	27	static int
	28	uvc_video_encode_header(struct uvc_video video, struct uvc_buffer buf,
	29	u8 *data, int len)
	30	{
fd03af27 MO	31	struct uvc_device *uvc = container_of(video, struct uvc_device, video);
	32	struct usb_composite_dev *cdev = uvc->func.config->cdev;
	33	struct timespec64 ts = ns_to_timespec64(buf->buf.vb2_buf.timestamp);
	34	int pos = 2;
	35
cdda479f LP	36	data[1] = UVC_STREAM_EOH \| video->fid;
cdda479f LP	37
fd03af27 MO	38	if (video->queue.buf_used == 0 && ts.tv_sec) {
	39	/* dwClockFrequency is 48 MHz */
	40	u32 pts = ((u64)ts.tv_sec * USEC_PER_SEC + ts.tv_nsec / NSEC_PER_USEC) * 48;
	41
	42	data[1] \|= UVC_STREAM_PTS;
	43	put_unaligned_le32(pts, &data[pos]);
	44	pos += 4;
	45	}
	46
	47	if (cdev->gadget->ops->get_frame) {
	48	u32 sof, stc;
	49
	50	sof = usb_gadget_frame_number(cdev->gadget);
	51	ktime_get_ts64(&ts);
	52	stc = ((u64)ts.tv_sec * USEC_PER_SEC + ts.tv_nsec / NSEC_PER_USEC) * 48;
	53
	54	data[1] \|= UVC_STREAM_SCR;
	55	put_unaligned_le32(stc, &data[pos]);
	56	put_unaligned_le16(sof, &data[pos+4]);
	57	pos += 6;
	58	}
	59
	60	data[0] = pos;
	61
	62	if (buf->bytesused - video->queue.buf_used <= len - pos)
cdda479f LP	63	data[1] \|= UVC_STREAM_EOF;
cdda479f LP	64
fd03af27	65	return pos;
cdda479f LP	66	}
	67
	68	static int
	69	uvc_video_encode_data(struct uvc_video video, struct uvc_buffer buf,
	70	u8 *data, int len)
	71	{
	72	struct uvc_video_queue *queue = &video->queue;
	73	unsigned int nbytes;
	74	void *mem;
	75
	76	/* Copy video data to the USB buffer. */
d6925225 BS	77	mem = buf->mem + queue->buf_used;
d6925225 BS	78	nbytes = min((unsigned int)len, buf->bytesused - queue->buf_used);
cdda479f LP	79
	80	memcpy(data, mem, nbytes);
	81	queue->buf_used += nbytes;
	82
	83	return nbytes;
	84	}
	85
	86	static void
	87	uvc_video_encode_bulk(struct usb_request req, struct uvc_video video,
	88	struct uvc_buffer *buf)
	89	{
	90	void *mem = req->buf;
0a0a2760	91	struct uvc_request *ureq = req->context;
cdda479f LP	92	int len = video->req_size;
	93	int ret;
	94
	95	/* Add a header at the beginning of the payload. */
	96	if (video->payload_size == 0) {
	97	ret = uvc_video_encode_header(video, buf, mem, len);
	98	video->payload_size += ret;
	99	mem += ret;
	100	len -= ret;
	101	}
	102
	103	/* Process video data. */
	104	len = min((int)(video->max_payload_size - video->payload_size), len);
	105	ret = uvc_video_encode_data(video, buf, mem, len);
	106
	107	video->payload_size += ret;
	108	len -= ret;
	109
	110	req->length = video->req_size - len;
	111	req->zero = video->payload_size == video->max_payload_size;
	112
d6925225	113	if (buf->bytesused == video->queue.buf_used) {
cdda479f LP	114	video->queue.buf_used = 0;
cdda479f LP	115	buf->state = UVC_BUF_STATE_DONE;
9b969f93	116	list_del(&buf->queue);
cdda479f	117	video->fid ^= UVC_STREAM_FID;
0a0a2760	118	ureq->last_buf = buf;
cdda479f LP	119
	120	video->payload_size = 0;
	121	}
	122
	123	if (video->payload_size == video->max_payload_size \|\|
0a0a2760	124	video->queue.flags & UVC_QUEUE_DROP_INCOMPLETE \|\|
d6925225	125	buf->bytesused == video->queue.buf_used)
cdda479f LP	126	video->payload_size = 0;
	127	}
	128
e81e7f9a MG	129	static void
	130	uvc_video_encode_isoc_sg(struct usb_request req, struct uvc_video video,
	131	struct uvc_buffer *buf)
	132	{
	133	unsigned int pending = buf->bytesused - video->queue.buf_used;
	134	struct uvc_request *ureq = req->context;
	135	struct scatterlist sg, iter;
	136	unsigned int len = video->req_size;
	137	unsigned int sg_left, part = 0;
	138	unsigned int i;
f262ce66	139	int header_len;
e81e7f9a MG	140
	141	sg = ureq->sgt.sgl;
	142	sg_init_table(sg, ureq->sgt.nents);
	143
	144	/* Init the header. */
f262ce66	145	header_len = uvc_video_encode_header(video, buf, ureq->header,
e81e7f9a	146	video->req_size);
f262ce66 MG	147	sg_set_buf(sg, ureq->header, header_len);
f262ce66 MG	148	len -= header_len;
e81e7f9a MG	149
	150	if (pending <= len)
	151	len = pending;
	152
	153	req->length = (len == pending) ?
f262ce66	154	len + header_len : video->req_size;
e81e7f9a MG	155
	156	/* Init the pending sgs with payload */
	157	sg = sg_next(sg);
	158
	159	for_each_sg(sg, iter, ureq->sgt.nents - 1, i) {
b57b08e6	160	if (!len \|\| !buf->sg \|\| !buf->sg->length)
e81e7f9a MG	161	break;
e81e7f9a MG	162
b57b08e6	163	sg_left = buf->sg->length - buf->offset;
e81e7f9a MG	164	part = min_t(unsigned int, len, sg_left);
	165
	166	sg_set_page(iter, sg_page(buf->sg), part, buf->offset);
	167
	168	if (part == sg_left) {
	169	buf->offset = 0;
	170	buf->sg = sg_next(buf->sg);
	171	} else {
	172	buf->offset += part;
	173	}
	174	len -= part;
	175	}
	176
	177	/* Assign the video data with header. */
	178	req->buf = NULL;
	179	req->sg = ureq->sgt.sgl;
	180	req->num_sgs = i + 1;
	181
	182	req->length -= len;
f262ce66	183	video->queue.buf_used += req->length - header_len;
e81e7f9a	184
0a0a2760 DV	185	if (buf->bytesused == video->queue.buf_used \|\| !buf->sg \|\|
0a0a2760 DV	186	video->queue.flags & UVC_QUEUE_DROP_INCOMPLETE) {
e81e7f9a MG	187	video->queue.buf_used = 0;
	188	buf->state = UVC_BUF_STATE_DONE;
	189	buf->offset = 0;
9b969f93	190	list_del(&buf->queue);
e81e7f9a	191	video->fid ^= UVC_STREAM_FID;
9b969f93	192	ureq->last_buf = buf;
e81e7f9a MG	193	}
	194	}
	195
cdda479f LP	196	static void
	197	uvc_video_encode_isoc(struct usb_request req, struct uvc_video video,
	198	struct uvc_buffer *buf)
	199	{
	200	void *mem = req->buf;
0a0a2760	201	struct uvc_request *ureq = req->context;
cdda479f LP	202	int len = video->req_size;
	203	int ret;
	204
	205	/* Add the header. */
	206	ret = uvc_video_encode_header(video, buf, mem, len);
	207	mem += ret;
	208	len -= ret;
	209
	210	/* Process video data. */
	211	ret = uvc_video_encode_data(video, buf, mem, len);
	212	len -= ret;
	213
	214	req->length = video->req_size - len;
	215
0a0a2760 DV	216	if (buf->bytesused == video->queue.buf_used \|\|
0a0a2760 DV	217	video->queue.flags & UVC_QUEUE_DROP_INCOMPLETE) {
cdda479f LP	218	video->queue.buf_used = 0;
cdda479f LP	219	buf->state = UVC_BUF_STATE_DONE;
9b969f93	220	list_del(&buf->queue);
cdda479f	221	video->fid ^= UVC_STREAM_FID;
0a0a2760	222	ureq->last_buf = buf;
cdda479f LP	223	}
	224	}
	225
	226	/* --------------------------------------------------------------------------
	227	* Request handling
	228	*/
	229
9d1ff5dc LP	230	static int uvcg_video_ep_queue(struct uvc_video video, struct usb_request req)
	231	{
	232	int ret;
	233
	234	ret = usb_ep_queue(video->ep, req, GFP_ATOMIC);
	235	if (ret < 0) {
dc0f755b LP	236	uvcg_err(&video->uvc->func, "Failed to queue request (%d).\n",
	237	ret);
	238
38db3716 MG	239	/* If the endpoint is disabled the descriptor may be NULL. */
	240	if (video->ep->desc) {
	241	/* Isochronous endpoints can't be halted. */
	242	if (usb_endpoint_xfer_bulk(video->ep->desc))
	243	usb_ep_set_halt(video->ep);
	244	}
9d1ff5dc LP	245	}
	246
	247	return ret;
	248	}
	249
cdda479f LP	250	static void
	251	uvc_video_complete(struct usb_ep ep, struct usb_request req)
	252	{
9973772d MG	253	struct uvc_request *ureq = req->context;
9973772d MG	254	struct uvc_video *video = ureq->video;
d6925225	255	struct uvc_video_queue *queue = &video->queue;
5fc49d8b	256	struct uvc_device *uvc = video->uvc;
cdda479f	257	unsigned long flags;
cdda479f LP	258
	259	switch (req->status) {
	260	case 0:
	261	break;
	262
0a0a2760 DV	263	case -EXDEV:
	264	uvcg_dbg(&video->uvc->func, "VS request missed xfer.\n");
	265	queue->flags \|= UVC_QUEUE_DROP_INCOMPLETE;
	266	break;
	267
d6925225	268	case -ESHUTDOWN: /* disconnect from host. */
dc0f755b	269	uvcg_dbg(&video->uvc->func, "VS request cancelled.\n");
7ea95b11	270	uvcg_queue_cancel(queue, 1);
43cd0023	271	break;
cdda479f LP	272
cdda479f LP	273	default:
a725d0f6	274	uvcg_warn(&video->uvc->func,
dc0f755b LP	275	"VS request completed with status %d.\n",
dc0f755b LP	276	req->status);
7ea95b11	277	uvcg_queue_cancel(queue, 0);
cdda479f LP	278	}
cdda479f LP	279
9b969f93 MG	280	if (ureq->last_buf) {
	281	uvcg_complete_buffer(&video->queue, ureq->last_buf);
	282	ureq->last_buf = NULL;
	283	}
	284
cdda479f LP	285	spin_lock_irqsave(&video->req_lock, flags);
	286	list_add_tail(&req->list, &video->req_free);
	287	spin_unlock_irqrestore(&video->req_lock, flags);
43cd0023	288
5fc49d8b	289	if (uvc->state == UVC_STATE_STREAMING)
9b91a652	290	queue_work(video->async_wq, &video->pump);
cdda479f LP	291	}
	292
	293	static int
	294	uvc_video_free_requests(struct uvc_video *video)
	295	{
	296	unsigned int i;
	297
9973772d MG	298	if (video->ureq) {
9973772d MG	299	for (i = 0; i < video->uvc_num_requests; ++i) {
e81e7f9a MG	300	sg_free_table(&video->ureq[i].sgt);
e81e7f9a MG	301
9973772d MG	302	if (video->ureq[i].req) {
	303	usb_ep_free_request(video->ep, video->ureq[i].req);
	304	video->ureq[i].req = NULL;
	305	}
	306
	307	if (video->ureq[i].req_buffer) {
	308	kfree(video->ureq[i].req_buffer);
	309	video->ureq[i].req_buffer = NULL;
	310	}
cdda479f LP	311	}
cdda479f LP	312
9973772d MG	313	kfree(video->ureq);
9973772d MG	314	video->ureq = NULL;
cdda479f LP	315	}
	316
	317	INIT_LIST_HEAD(&video->req_free);
	318	video->req_size = 0;
	319	return 0;
	320	}
	321
	322	static int
	323	uvc_video_alloc_requests(struct uvc_video *video)
	324	{
326b0e61	325	unsigned int req_size;
cdda479f LP	326	unsigned int i;
	327	int ret = -ENOMEM;
	328
	329	BUG_ON(video->req_size);
	330
326b0e61 BS	331	req_size = video->ep->maxpacket
326b0e61 BS	332	* max_t(unsigned int, video->ep->maxburst, 1)
eaa496ff	333	* (video->ep->mult);
326b0e61	334
9973772d MG	335	video->ureq = kcalloc(video->uvc_num_requests, sizeof(struct uvc_request), GFP_KERNEL);
	336	if (video->ureq == NULL)
	337	return -ENOMEM;
	338
	339	for (i = 0; i < video->uvc_num_requests; ++i) {
	340	video->ureq[i].req_buffer = kmalloc(req_size, GFP_KERNEL);
	341	if (video->ureq[i].req_buffer == NULL)
cdda479f LP	342	goto error;
cdda479f LP	343
9973772d MG	344	video->ureq[i].req = usb_ep_alloc_request(video->ep, GFP_KERNEL);
9973772d MG	345	if (video->ureq[i].req == NULL)
cdda479f LP	346	goto error;
cdda479f LP	347
9973772d MG	348	video->ureq[i].req->buf = video->ureq[i].req_buffer;
	349	video->ureq[i].req->length = 0;
	350	video->ureq[i].req->complete = uvc_video_complete;
	351	video->ureq[i].req->context = &video->ureq[i];
	352	video->ureq[i].video = video;
9b969f93	353	video->ureq[i].last_buf = NULL;
cdda479f	354
9973772d	355	list_add_tail(&video->ureq[i].req->list, &video->req_free);
e81e7f9a MG	356	/* req_size/PAGE_SIZE + 1 for overruns and + 1 for header */
e81e7f9a MG	357	sg_alloc_table(&video->ureq[i].sgt,
859c675d MG	358	DIV_ROUND_UP(req_size - UVCG_REQUEST_HEADER_LEN,
859c675d MG	359	PAGE_SIZE) + 2, GFP_KERNEL);
cdda479f LP	360	}
cdda479f LP	361
326b0e61 BS	362	video->req_size = req_size;
326b0e61 BS	363
cdda479f LP	364	return 0;
	365
	366	error:
	367	uvc_video_free_requests(video);
	368	return ret;
	369	}
	370
	371	/* --------------------------------------------------------------------------
	372	* Video streaming
	373	*/
	374
	375	/*
7ea95b11	376	* uvcg_video_pump - Pump video data into the USB requests
cdda479f LP	377	*
	378	* This function fills the available USB requests (listed in req_free) with
	379	* video data from the queued buffers.
	380	*/
43cd0023	381	static void uvcg_video_pump(struct work_struct *work)
cdda479f	382	{
43cd0023	383	struct uvc_video *video = container_of(work, struct uvc_video, pump);
bd52b813	384	struct uvc_video_queue *queue = &video->queue;
5ae8a354 AR	385	/* video->max_payload_size is only set when using bulk transfer */
5ae8a354 AR	386	bool is_bulk = video->max_payload_size;
991544dc	387	struct uvc_device *uvc = video->uvc;
f9897ec0	388	struct usb_request *req = NULL;
cdda479f LP	389	struct uvc_buffer *buf;
cdda479f LP	390	unsigned long flags;
5ae8a354	391	bool buf_done;
cdda479f LP	392	int ret;
cdda479f LP	393
991544dc	394	while (uvc->state == UVC_STATE_STREAMING && video->ep->enabled) {
c5d337a3 LP	395	/*
c5d337a3 LP	396	* Retrieve the first available USB request, protected by the
cdda479f LP	397	* request lock.
	398	*/
	399	spin_lock_irqsave(&video->req_lock, flags);
	400	if (list_empty(&video->req_free)) {
	401	spin_unlock_irqrestore(&video->req_lock, flags);
43cd0023	402	return;
cdda479f LP	403	}
	404	req = list_first_entry(&video->req_free, struct usb_request,
	405	list);
	406	list_del(&req->list);
	407	spin_unlock_irqrestore(&video->req_lock, flags);
	408
c5d337a3 LP	409	/*
c5d337a3 LP	410	* Retrieve the first available video buffer and fill the
cdda479f LP	411	* request, protected by the video queue irqlock.
cdda479f LP	412	*/
6dd5b021 LP	413	spin_lock_irqsave(&queue->irqlock, flags);
6dd5b021 LP	414	buf = uvcg_queue_head(queue);
c3ff12a9 AR	415
	416	if (buf != NULL) {
	417	video->encode(req, video, buf);
5ae8a354	418	buf_done = buf->state == UVC_BUF_STATE_DONE;
c3ff12a9 AR	419	} else if (!(queue->flags & UVC_QUEUE_DISCONNECTED) && !is_bulk) {
	420	/*
	421	* No video buffer available; the queue is still connected and
5ae8a354	422	* we're transferring over ISOC. Queue a 0 length request to
c3ff12a9 AR	423	* prevent missed ISOC transfers.
	424	*/
	425	req->length = 0;
5ae8a354	426	buf_done = false;
c3ff12a9 AR	427	} else {
c3ff12a9 AR	428	/*
5ae8a354 AR	429	* Either the queue has been disconnected or no video buffer
5ae8a354 AR	430	* available for bulk transfer. Either way, stop processing
c3ff12a9 AR	431	* further.
c3ff12a9 AR	432	*/
6dd5b021	433	spin_unlock_irqrestore(&queue->irqlock, flags);
cdda479f LP	434	break;
	435	}
	436
c5d337a3	437	/*
5ae8a354 AR	438	* With USB3 handling more requests at a higher speed, we can't
	439	* afford to generate an interrupt for every request. Decide to
	440	* interrupt:
	441	*
	442	* - When no more requests are available in the free queue, as
	443	* this may be our last chance to refill the endpoint's
	444	* request queue.
	445	*
	446	* - When this is request is the last request for the video
	447	* buffer, as we want to start sending the next video buffer
	448	* ASAP in case it doesn't get started already in the next
	449	* iteration of this loop.
	450	*
	451	* - Four times over the length of the requests queue (as
	452	* indicated by video->uvc_num_requests), as a trade-off
	453	* between latency and interrupt load.
c5d337a3	454	*/
5ae8a354	455	if (list_empty(&video->req_free) \|\| buf_done \|\|
fc78941d MG	456	!(video->req_int_count %
	457	DIV_ROUND_UP(video->uvc_num_requests, 4))) {
	458	video->req_int_count = 0;
	459	req->no_interrupt = 0;
	460	} else {
	461	req->no_interrupt = 1;
	462	}
	463
cdda479f	464	/* Queue the USB request */
9d1ff5dc LP	465	ret = uvcg_video_ep_queue(video, req);
	466	spin_unlock_irqrestore(&queue->irqlock, flags);
	467
6854bcdc	468	if (ret < 0) {
7ea95b11	469	uvcg_queue_cancel(queue, 0);
cdda479f LP	470	break;
cdda479f LP	471	}
96163f83 DV	472
	473	/* Endpoint now owns the request */
	474	req = NULL;
c3ff12a9	475	video->req_int_count++;
cdda479f LP	476	}
cdda479f LP	477
f9897ec0 MG	478	if (!req)
	479	return;
	480
cdda479f LP	481	spin_lock_irqsave(&video->req_lock, flags);
	482	list_add_tail(&req->list, &video->req_free);
	483	spin_unlock_irqrestore(&video->req_lock, flags);
43cd0023	484	return;
cdda479f LP	485	}
	486
	487	/*
	488	* Enable or disable the video stream.
	489	*/
3a83c16e	490	int uvcg_video_enable(struct uvc_video *video, int enable)
cdda479f LP	491	{
	492	unsigned int i;
	493	int ret;
	494
	495	if (video->ep == NULL) {
dc0f755b LP	496	uvcg_info(&video->uvc->func,
dc0f755b LP	497	"Video enable failed, device is uninitialized.\n");
cdda479f LP	498	return -ENODEV;
	499	}
	500
	501	if (!enable) {
43cd0023 MG	502	cancel_work_sync(&video->pump);
	503	uvcg_queue_cancel(&video->queue, 0);
	504
9973772d MG	505	for (i = 0; i < video->uvc_num_requests; ++i)
	506	if (video->ureq && video->ureq[i].req)
	507	usb_ep_dequeue(video->ep, video->ureq[i].req);
cdda479f LP	508
cdda479f LP	509	uvc_video_free_requests(video);
7ea95b11	510	uvcg_queue_enable(&video->queue, 0);
cdda479f LP	511	return 0;
	512	}
	513
7ea95b11	514	if ((ret = uvcg_queue_enable(&video->queue, 1)) < 0)
cdda479f LP	515	return ret;
	516
	517	if ((ret = uvc_video_alloc_requests(video)) < 0)
	518	return ret;
	519
	520	if (video->max_payload_size) {
	521	video->encode = uvc_video_encode_bulk;
	522	video->payload_size = 0;
88c8e05e GKH	523	} else
88c8e05e GKH	524	video->encode = video->queue.use_sg ?
e81e7f9a	525	uvc_video_encode_isoc_sg : uvc_video_encode_isoc;
cdda479f	526
fc78941d MG	527	video->req_int_count = 0;
fc78941d MG	528
9b91a652	529	queue_work(video->async_wq, &video->pump);
43cd0023 MG	530
43cd0023 MG	531	return ret;
cdda479f LP	532	}
	533
	534	/*
	535	* Initialize the UVC video stream.
	536	*/
dc0f755b	537	int uvcg_video_init(struct uvc_video video, struct uvc_device uvc)
cdda479f LP	538	{
	539	INIT_LIST_HEAD(&video->req_free);
	540	spin_lock_init(&video->req_lock);
43cd0023	541	INIT_WORK(&video->pump, uvcg_video_pump);
cdda479f	542
9b91a652 MG	543	/* Allocate a work queue for asynchronous video pump handler. */
	544	video->async_wq = alloc_workqueue("uvcgadget", WQ_UNBOUND \| WQ_HIGHPRI, 0);
	545	if (!video->async_wq)
	546	return -EINVAL;
	547
dc0f755b	548	video->uvc = uvc;
cdda479f LP	549	video->fcc = V4L2_PIX_FMT_YUYV;
	550	video->bpp = 16;
	551	video->width = 320;
	552	video->height = 240;
	553	video->imagesize = 320 * 240 * 2;
	554
	555	/* Initialize the video buffers queue. */
e81e7f9a MG	556	uvcg_queue_init(&video->queue, uvc->v4l2_dev.dev->parent,
e81e7f9a MG	557	V4L2_BUF_TYPE_VIDEO_OUTPUT, &video->mutex);
cdda479f LP	558	return 0;
cdda479f LP	559	}