--- linux-4.9.37/drivers/usb/gadget/function/uvc_video.c 2017-07-12 16:42:41.000000000 +0300 +++ linux-4.9.y/drivers/usb/gadget/function/uvc_video.c 2021-06-07 13:01:34.000000000 +0300 @@ -23,6 +23,8 @@ #include "uvc_queue.h" #include "uvc_video.h" +#include +#include /* -------------------------------------------------------------------------- * Video codecs */ @@ -102,9 +104,45 @@ uvc_video_encode_isoc(struct usb_request *req, struct uvc_video *video, struct uvc_buffer *buf) { + int ret; +#ifdef UVC_SG_REQ + int len; + int ttllen = 0; + unsigned int sg_idx; + u8 *mem = NULL; + + for (sg_idx = 0; sg_idx < video->num_sgs; sg_idx++) { + mem = sg_virt(&req->sg[sg_idx]); + len = video->req_size; + + /* 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; + + /* Sync sg buffer len , default is 1024 or 3072 */ + sg_set_buf(&req->sg[sg_idx], sg_virt(&req->sg[sg_idx]), + video->req_size - len); + ttllen += video->req_size - len; + + if (buf->bytesused == video->queue.buf_used) { + video->queue.buf_used = 0; + buf->state = UVC_BUF_STATE_DONE; + uvcg_queue_next_buffer(&video->queue, buf); + video->fid ^= UVC_STREAM_FID; + break; + } + } + req->num_sgs = sg_idx + 1; + sg_mark_end(&req->sg[sg_idx]); + req->length = ttllen; +#else void *mem = req->buf; int len = video->req_size; - int ret; /* Add the header. */ ret = uvc_video_encode_header(video, buf, mem, len); @@ -123,12 +161,33 @@ uvcg_queue_next_buffer(&video->queue, buf); video->fid ^= UVC_STREAM_FID; } +#endif } /* -------------------------------------------------------------------------- * Request handling */ +static int uvcg_video_ep_queue(struct uvc_video *video, struct usb_request *req) +{ + int ret; + + /* + * Fixme, this is just to workaround the warning by udc core when the ep + * is disabled, this may happens when the uvc application is still + * streaming new data while the uvc gadget driver has already recieved + * the streamoff but the streamoff event is not yet received by the app + */ + if (!video->ep->enabled) + return -EINVAL; + + ret = usb_ep_queue(video->ep, req, GFP_ATOMIC); + if (ret < 0) + printk(KERN_INFO "Failed to queue request (%d).\n", ret); + + return ret; +} + /* * I somehow feel that synchronisation won't be easy to achieve here. We have * three events that control USB requests submission: @@ -190,22 +249,26 @@ spin_unlock_irqrestore(&video->queue.irqlock, flags); goto requeue; } - +#ifdef UVC_SG_REQ + sg_unmark_end(&req->sg[req->num_sgs - 1]); +#endif video->encode(req, video, buf); - if ((ret = usb_ep_queue(ep, req, GFP_ATOMIC)) < 0) { - printk(KERN_INFO "Failed to queue request (%d).\n", ret); - usb_ep_set_halt(ep); - spin_unlock_irqrestore(&video->queue.irqlock, flags); + ret = uvcg_video_ep_queue(video, req); + spin_unlock_irqrestore(&video->queue.irqlock, flags); + + if (ret < 0) { uvcg_queue_cancel(queue, 0); goto requeue; } - spin_unlock_irqrestore(&video->queue.irqlock, flags); return; requeue: spin_lock_irqsave(&video->req_lock, flags); +#ifdef UVC_SG_REQ + sg_unmark_end(&req->sg[req->num_sgs - 1]); +#endif list_add_tail(&req->list, &video->req_free); spin_unlock_irqrestore(&video->req_lock, flags); } @@ -214,9 +277,22 @@ uvc_video_free_requests(struct uvc_video *video) { unsigned int i; +#ifdef UVC_SG_REQ + unsigned int sg_idx; +#endif for (i = 0; i < UVC_NUM_REQUESTS; ++i) { if (video->req[i]) { +#ifdef UVC_SG_REQ + for (sg_idx = 0; sg_idx < video->num_sgs; sg_idx++) + if (sg_page(&video->req[i]->sg[sg_idx])) + kfree(sg_virt(&video->req[i]->sg[sg_idx])); + + if (video->req[i]->sg) { + kfree(video->req[i]->sg); + video->req[i]->sg = NULL; + } +#endif usb_ep_free_request(video->ep, video->req[i]); video->req[i] = NULL; } @@ -238,6 +314,11 @@ unsigned int req_size; unsigned int i; int ret = -ENOMEM; +#ifdef UVC_SG_REQ + struct scatterlist *sg; + unsigned int num_sgs; + unsigned int sg_idx; +#endif BUG_ON(video->req_size); @@ -245,6 +326,35 @@ * max_t(unsigned int, video->ep->maxburst, 1) * (video->ep->mult); +#ifdef UVC_SG_REQ + num_sgs = ((video->imagesize / (req_size - 2)) + 1); + video->num_sgs = num_sgs; + + for (i = 0; i < UVC_NUM_REQUESTS; ++i) { + sg = kmalloc(num_sgs * sizeof(struct scatterlist), GFP_ATOMIC); + if (sg == NULL) + goto error; + sg_init_table(sg, num_sgs); + + video->req[i] = usb_ep_alloc_request(video->ep, GFP_KERNEL); + if (video->req[i] == NULL) + goto error; + + for (sg_idx = 0 ; sg_idx < num_sgs ; sg_idx++) { + video->sg_buf = kmalloc(req_size, GFP_KERNEL); + if (video->sg_buf == NULL) + goto error; + sg_set_buf(&sg[sg_idx], video->sg_buf, req_size); + } + video->req[i]->sg = sg; + video->req[i]->num_sgs = num_sgs; + video->req[i]->length = 0; + video->req[i]->complete = uvc_video_complete; + video->req[i]->context = video; + + list_add_tail(&video->req[i]->list, &video->req_free); + } +#else for (i = 0; i < UVC_NUM_REQUESTS; ++i) { video->req_buffer[i] = kmalloc(req_size, GFP_KERNEL); if (video->req_buffer[i] == NULL) @@ -261,7 +371,7 @@ list_add_tail(&video->req[i]->list, &video->req_free); } - +#endif video->req_size = req_size; return 0; @@ -320,15 +430,13 @@ video->encode(req, video, buf); /* Queue the USB request */ - ret = usb_ep_queue(video->ep, req, GFP_ATOMIC); + ret = uvcg_video_ep_queue(video, req); + spin_unlock_irqrestore(&queue->irqlock, flags); + if (ret < 0) { - printk(KERN_INFO "Failed to queue request (%d)\n", ret); - usb_ep_set_halt(video->ep); - spin_unlock_irqrestore(&queue->irqlock, flags); uvcg_queue_cancel(queue, 0); break; } - spin_unlock_irqrestore(&queue->irqlock, flags); } spin_lock_irqsave(&video->req_lock, flags); @@ -379,16 +487,22 @@ /* * Initialize the UVC video stream. */ -int uvcg_video_init(struct uvc_video *video) +int uvcg_video_init(struct uvc_video *video, struct uvc_device *uvc) { INIT_LIST_HEAD(&video->req_free); spin_lock_init(&video->req_lock); - +#ifndef CONFIG_GOKE_MC video->fcc = V4L2_PIX_FMT_YUYV; + video->imagesize = 320 * 240 * 2; video->bpp = 16; +#else + video->fcc = V4L2_PIX_FMT_NV21; + video->imagesize = 320 * 240 * 3 / 2; /* YUV420: w*h*1.5 */ + video->bpp = 12; +#endif video->width = 320; video->height = 240; - video->imagesize = 320 * 240 * 2; + video->uvc = uvc; /* Initialize the video buffers queue. */ uvcg_queue_init(&video->queue, V4L2_BUF_TYPE_VIDEO_OUTPUT,