usb: gadget: uvc: add scatter gather support

This patch adds support for scatter gather transfers. If the underlying
gadgets sg_supported == true, then the videeobuf2-dma-sg is used and the
encode routine maps all scatter entries to separate scatterlists for the
usb gadget.

When streaming 1080p with request size of 1024 times 3 bytes top shows a
difference of about 6.4% CPU load applying this patch:

 PID USER      PR  NI    VIRT    RES  %CPU  %MEM     TIME+ S COMMAND

  64 root       0 -20    0.0m   0.0m   7.7   0.0   0:01.25 I [kworker/u5:0-uvcvideo]
  83 root       0 -20    0.0m   0.0m   4.5   0.0   0:03.71 I [kworker/u5:3-uvcvideo]
 307 root     -51   0    0.0m   0.0m   3.8   0.0   0:01.05 S [irq/51-dwc3]

vs.

  64 root       0 -20    0.0m   0.0m   5.8   0.0   0:01.79 I [kworker/u5:0-uvcvideo]
 306 root     -51   0    0.0m   0.0m   3.2   0.0   0:01.97 S [irq/51-dwc3]
  82 root       0 -20    0.0m   0.0m   0.6   0.0   0:01.86 I [kworker/u5:1-uvcvideo]

Signed-off-by: Michael Grzeschik <m.grzeschik@pengutronix.de>
Link: https://lore.kernel.org/r/20210628155311.16762-5-m.grzeschik@pengutronix.de
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

1 files changed, 77 insertions, 5 deletions

diff --git a/drivers/usb/gadget/function/uvc_video.c b/drivers/usb/gadget/function/uvc_video.c
index 303cb427f687..2cefb8bd4f15 100644
--- a/drivers/usb/gadget/function/uvc_video.c
+++ b/drivers/usb/gadget/function/uvc_video.c
@@ -27,10 +27,10 @@ static int
 uvc_video_encode_header(struct uvc_video *video, struct uvc_buffer *buf,
 		u8 *data, int len)
 {
-	data[0] = 2;
+	data[0] = UVCG_REQUEST_HEADER_LEN;
 	data[1] = UVC_STREAM_EOH | video->fid;
 
-	if (buf->bytesused - video->queue.buf_used <= len - 2)
+	if (buf->bytesused - video->queue.buf_used <= len - UVCG_REQUEST_HEADER_LEN)
 		data[1] |= UVC_STREAM_EOF;
 
 	return 2;
@@ -95,6 +95,71 @@ uvc_video_encode_bulk(struct usb_request *req, struct uvc_video *video,
 }
 
 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 ret;
+
+	sg = ureq->sgt.sgl;
+	sg_init_table(sg, ureq->sgt.nents);
+
+	/* Init the header. */
+	ret = uvc_video_encode_header(video, buf, ureq->header,
+				      video->req_size);
+	sg_set_buf(sg, ureq->header, UVCG_REQUEST_HEADER_LEN);
+	len -= ret;
+
+	if (pending <= len)
+		len = pending;
+
+	req->length = (len == pending) ?
+		len + UVCG_REQUEST_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)
+			break;
+
+		sg_left = sg_dma_len(buf->sg) - 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 - UVCG_REQUEST_HEADER_LEN;
+
+	if (buf->bytesused == video->queue.buf_used || !buf->sg) {
+		video->queue.buf_used = 0;
+		buf->state = UVC_BUF_STATE_DONE;
+		buf->offset = 0;
+		uvcg_queue_next_buffer(&video->queue, buf);
+		video->fid ^= UVC_STREAM_FID;
+	}
+}
+
+static void
 uvc_video_encode_isoc(struct usb_request *req, struct uvc_video *video,
 		struct uvc_buffer *buf)
 {
@@ -180,6 +245,8 @@ uvc_video_free_requests(struct uvc_video *video)
 
 	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;
@@ -233,6 +300,10 @@ uvc_video_alloc_requests(struct uvc_video *video)
 		video->ureq[i].video = video;
 
 		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 - 2, PAGE_SIZE) + 2,
+			       GFP_KERNEL);
 	}
 
 	video->req_size = req_size;
@@ -342,7 +413,8 @@ int uvcg_video_enable(struct uvc_video *video, int enable)
 		video->encode = uvc_video_encode_bulk;
 		video->payload_size = 0;
 	} else
-		video->encode = uvc_video_encode_isoc;
+		video->encode = video->queue.use_sg ?
+			uvc_video_encode_isoc_sg : uvc_video_encode_isoc;
 
 	schedule_work(&video->pump);
 
@@ -366,8 +438,8 @@ int uvcg_video_init(struct uvc_video *video, struct uvc_device *uvc)
 	video->imagesize = 320 * 240 * 2;
 
 	/* Initialize the video buffers queue. */
-	uvcg_queue_init(&video->queue, V4L2_BUF_TYPE_VIDEO_OUTPUT,
-			&video->mutex);
+	uvcg_queue_init(&video->queue, uvc->v4l2_dev.dev->parent,
+			V4L2_BUF_TYPE_VIDEO_OUTPUT, &video->mutex);
 	return 0;
 }