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