LXR linux/drivers/usb/gadget/function/uvc

   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
  16#include <media/v4l2-dev.h>
  17
  18#include "uvc.h"
  19#include "uvc_queue.h"
  20#include "uvc_video.h"
  21
  22/* --------------------------------------------------------------------------
  23 * Video codecs
  24 */
  25
  26static int
  27uvc_video_encode_header(struct uvc_video *video, struct uvc_buffer *buf,
  28                u8 *data, int len)
  29{
  30        data[0] = UVCG_REQUEST_HEADER_LEN;
  31        data[1] = UVC_STREAM_EOH | video->fid;
  32
  33        if (buf->bytesused - video->queue.buf_used <= len - UVCG_REQUEST_HEADER_LEN)
  34                data[1] |= UVC_STREAM_EOF;
  35
  36        return 2;
  37}
  38
  39static int
  40uvc_video_encode_data(struct uvc_video *video, struct uvc_buffer *buf,
  41                u8 *data, int len)
  42{
  43        struct uvc_video_queue *queue = &video->queue;
  44        unsigned int nbytes;
  45        void *mem;
  46
  47        /* Copy video data to the USB buffer. */
  48        mem = buf->mem + queue->buf_used;
  49        nbytes = min((unsigned int)len, buf->bytesused - queue->buf_used);
  50
  51        memcpy(data, mem, nbytes);
  52        queue->buf_used += nbytes;
  53
  54        return nbytes;
  55}
  56
  57static void
  58uvc_video_encode_bulk(struct usb_request *req, struct uvc_video *video,
  59                struct uvc_buffer *buf)
  60{
  61        void *mem = req->buf;
  62        int len = video->req_size;
  63        int ret;
  64
  65        /* Add a header at the beginning of the payload. */
  66        if (video->payload_size == 0) {
  67                ret = uvc_video_encode_header(video, buf, mem, len);
  68                video->payload_size += ret;
  69                mem += ret;
  70                len -= ret;
  71        }
  72
  73        /* Process video data. */
  74        len = min((int)(video->max_payload_size - video->payload_size), len);
  75        ret = uvc_video_encode_data(video, buf, mem, len);
  76
  77        video->payload_size += ret;
  78        len -= ret;
  79
  80        req->length = video->req_size - len;
  81        req->zero = video->payload_size == video->max_payload_size;
  82
  83        if (buf->bytesused == video->queue.buf_used) {
  84                video->queue.buf_used = 0;
  85                buf->state = UVC_BUF_STATE_DONE;
  86                uvcg_queue_next_buffer(&video->queue, buf);
  87                video->fid ^= UVC_STREAM_FID;
  88
  89                video->payload_size = 0;
  90        }
  91
  92        if (video->payload_size == video->max_payload_size ||
  93            buf->bytesused == video->queue.buf_used)
  94                video->payload_size = 0;
  95}
  96
  97static void
  98uvc_video_encode_isoc_sg(struct usb_request *req, struct uvc_video *video,
  99                struct uvc_buffer *buf)
 100{
 101        unsigned int pending = buf->bytesused - video->queue.buf_used;
 102        struct uvc_request *ureq = req->context;
 103        struct scatterlist *sg, *iter;
 104        unsigned int len = video->req_size;
 105        unsigned int sg_left, part = 0;
 106        unsigned int i;
 107        int ret;
 108
 109        sg = ureq->sgt.sgl;
 110        sg_init_table(sg, ureq->sgt.nents);
 111
 112        /* Init the header. */
 113        ret = uvc_video_encode_header(video, buf, ureq->header,
 114                                      video->req_size);
 115        sg_set_buf(sg, ureq->header, UVCG_REQUEST_HEADER_LEN);
 116        len -= ret;
 117
 118        if (pending <= len)
 119                len = pending;
 120
 121        req->length = (len == pending) ?
 122                len + UVCG_REQUEST_HEADER_LEN : video->req_size;
 123
 124        /* Init the pending sgs with payload */
 125        sg = sg_next(sg);
 126
 127        for_each_sg(sg, iter, ureq->sgt.nents - 1, i) {
 128                if (!len || !buf->sg)
 129                        break;
 130
 131                sg_left = sg_dma_len(buf->sg) - buf->offset;
 132                part = min_t(unsigned int, len, sg_left);
 133
 134                sg_set_page(iter, sg_page(buf->sg), part, buf->offset);
 135
 136                if (part == sg_left) {
 137                        buf->offset = 0;
 138                        buf->sg = sg_next(buf->sg);
 139                } else {
 140                        buf->offset += part;
 141                }
 142                len -= part;
 143        }
 144
 145        /* Assign the video data with header. */
 146        req->buf = NULL;
 147        req->sg = ureq->sgt.sgl;
 148        req->num_sgs = i + 1;
 149
 150        req->length -= len;
 151        video->queue.buf_used += req->length - UVCG_REQUEST_HEADER_LEN;
 152
 153        if (buf->bytesused == video->queue.buf_used || !buf->sg) {
 154                video->queue.buf_used = 0;
 155                buf->state = UVC_BUF_STATE_DONE;
 156                buf->offset = 0;
 157                uvcg_queue_next_buffer(&video->queue, buf);
 158                video->fid ^= UVC_STREAM_FID;
 159        }
 160}
 161
 162static void
 163uvc_video_encode_isoc(struct usb_request *req, struct uvc_video *video,
 164                struct uvc_buffer *buf)
 165{
 166        void *mem = req->buf;
 167        int len = video->req_size;
 168        int ret;
 169
 170        /* Add the header. */
 171        ret = uvc_video_encode_header(video, buf, mem, len);
 172        mem += ret;
 173        len -= ret;
 174
 175        /* Process video data. */
 176        ret = uvc_video_encode_data(video, buf, mem, len);
 177        len -= ret;
 178
 179        req->length = video->req_size - len;
 180
 181        if (buf->bytesused == video->queue.buf_used) {
 182                video->queue.buf_used = 0;
 183                buf->state = UVC_BUF_STATE_DONE;
 184                uvcg_queue_next_buffer(&video->queue, buf);
 185                video->fid ^= UVC_STREAM_FID;
 186        }
 187}
 188
 189/* --------------------------------------------------------------------------
 190 * Request handling
 191 */
 192
 193static int uvcg_video_ep_queue(struct uvc_video *video, struct usb_request *req)
 194{
 195        int ret;
 196
 197        ret = usb_ep_queue(video->ep, req, GFP_ATOMIC);
 198        if (ret < 0) {
 199                uvcg_err(&video->uvc->func, "Failed to queue request (%d).\n",
 200                         ret);
 201
 202                /* Isochronous endpoints can't be halted. */
 203                if (usb_endpoint_xfer_bulk(video->ep->desc))
 204                        usb_ep_set_halt(video->ep);
 205        }
 206
 207        return ret;
 208}
 209
 210static void
 211uvc_video_complete(struct usb_ep *ep, struct usb_request *req)
 212{
 213        struct uvc_request *ureq = req->context;
 214        struct uvc_video *video = ureq->video;
 215        struct uvc_video_queue *queue = &video->queue;
 216        unsigned long flags;
 217
 218        switch (req->status) {
 219        case 0:
 220                break;
 221
 222        case -ESHUTDOWN:        /* disconnect from host. */
 223                uvcg_dbg(&video->uvc->func, "VS request cancelled.\n");
 224                uvcg_queue_cancel(queue, 1);
 225                break;
 226
 227        default:
 228                uvcg_info(&video->uvc->func,
 229                          "VS request completed with status %d.\n",
 230                          req->status);
 231                uvcg_queue_cancel(queue, 0);
 232        }
 233
 234        spin_lock_irqsave(&video->req_lock, flags);
 235        list_add_tail(&req->list, &video->req_free);
 236        spin_unlock_irqrestore(&video->req_lock, flags);
 237
 238        schedule_work(&video->pump);
 239}
 240
 241static int
 242uvc_video_free_requests(struct uvc_video *video)
 243{
 244        unsigned int i;
 245
 246        if (video->ureq) {
 247                for (i = 0; i < video->uvc_num_requests; ++i) {
 248                        sg_free_table(&video->ureq[i].sgt);
 249
 250                        if (video->ureq[i].req) {
 251                                usb_ep_free_request(video->ep, video->ureq[i].req);
 252                                video->ureq[i].req = NULL;
 253                        }
 254
 255                        if (video->ureq[i].req_buffer) {
 256                                kfree(video->ureq[i].req_buffer);
 257                                video->ureq[i].req_buffer = NULL;
 258                        }
 259                }
 260
 261                kfree(video->ureq);
 262                video->ureq = NULL;
 263        }
 264
 265        INIT_LIST_HEAD(&video->req_free);
 266        video->req_size = 0;
 267        return 0;
 268}
 269
 270static int
 271uvc_video_alloc_requests(struct uvc_video *video)
 272{
 273        unsigned int req_size;
 274        unsigned int i;
 275        int ret = -ENOMEM;
 276
 277        BUG_ON(video->req_size);
 278
 279        req_size = video->ep->maxpacket
 280                 * max_t(unsigned int, video->ep->maxburst, 1)
 281                 * (video->ep->mult);
 282
 283        video->ureq = kcalloc(video->uvc_num_requests, sizeof(struct uvc_request), GFP_KERNEL);
 284        if (video->ureq == NULL)
 285                return -ENOMEM;
 286
 287        for (i = 0; i < video->uvc_num_requests; ++i) {
 288                video->ureq[i].req_buffer = kmalloc(req_size, GFP_KERNEL);
 289                if (video->ureq[i].req_buffer == NULL)
 290                        goto error;
 291
 292                video->ureq[i].req = usb_ep_alloc_request(video->ep, GFP_KERNEL);
 293                if (video->ureq[i].req == NULL)
 294                        goto error;
 295
 296                video->ureq[i].req->buf = video->ureq[i].req_buffer;
 297                video->ureq[i].req->length = 0;
 298                video->ureq[i].req->complete = uvc_video_complete;
 299                video->ureq[i].req->context = &video->ureq[i];
 300                video->ureq[i].video = video;
 301
 302                list_add_tail(&video->ureq[i].req->list, &video->req_free);
 303                /* req_size/PAGE_SIZE + 1 for overruns and + 1 for header */
 304                sg_alloc_table(&video->ureq[i].sgt,
 305                               DIV_ROUND_UP(req_size - 2, PAGE_SIZE) + 2,
 306                               GFP_KERNEL);
 307        }
 308
 309        video->req_size = req_size;
 310
 311        return 0;
 312
 313error:
 314        uvc_video_free_requests(video);
 315        return ret;
 316}
 317
 318/* --------------------------------------------------------------------------
 319 * Video streaming
 320 */
 321
 322/*
 323 * uvcg_video_pump - Pump video data into the USB requests
 324 *
 325 * This function fills the available USB requests (listed in req_free) with
 326 * video data from the queued buffers.
 327 */
 328static void uvcg_video_pump(struct work_struct *work)
 329{
 330        struct uvc_video *video = container_of(work, struct uvc_video, pump);
 331        struct uvc_video_queue *queue = &video->queue;
 332        struct usb_request *req;
 333        struct uvc_buffer *buf;
 334        unsigned long flags;
 335        int ret;
 336
 337        while (1) {
 338                /* Retrieve the first available USB request, protected by the
 339                 * request lock.
 340                 */
 341                spin_lock_irqsave(&video->req_lock, flags);
 342                if (list_empty(&video->req_free)) {
 343                        spin_unlock_irqrestore(&video->req_lock, flags);
 344                        return;
 345                }
 346                req = list_first_entry(&video->req_free, struct usb_request,
 347                                        list);
 348                list_del(&req->list);
 349                spin_unlock_irqrestore(&video->req_lock, flags);
 350
 351                /* Retrieve the first available video buffer and fill the
 352                 * request, protected by the video queue irqlock.
 353                 */
 354                spin_lock_irqsave(&queue->irqlock, flags);
 355                buf = uvcg_queue_head(queue);
 356                if (buf == NULL) {
 357                        spin_unlock_irqrestore(&queue->irqlock, flags);
 358                        break;
 359                }
 360
 361                video->encode(req, video, buf);
 362
 363                /* With usb3 we have more requests. This will decrease the
 364                 * interrupt load to a quarter but also catches the corner
 365                 * cases, which needs to be handled */
 366                if (list_empty(&video->req_free) ||
 367                    buf->state == UVC_BUF_STATE_DONE ||
 368                    !(video->req_int_count %
 369                       DIV_ROUND_UP(video->uvc_num_requests, 4))) {
 370                        video->req_int_count = 0;
 371                        req->no_interrupt = 0;
 372                } else {
 373                        req->no_interrupt = 1;
 374                }
 375
 376                /* Queue the USB request */
 377                ret = uvcg_video_ep_queue(video, req);
 378                spin_unlock_irqrestore(&queue->irqlock, flags);
 379
 380                if (ret < 0) {
 381                        uvcg_queue_cancel(queue, 0);
 382                        break;
 383                }
 384                video->req_int_count++;
 385        }
 386
 387        spin_lock_irqsave(&video->req_lock, flags);
 388        list_add_tail(&req->list, &video->req_free);
 389        spin_unlock_irqrestore(&video->req_lock, flags);
 390        return;
 391}
 392
 393/*
 394 * Enable or disable the video stream.
 395 */
 396int uvcg_video_enable(struct uvc_video *video, int enable)
 397{
 398        unsigned int i;
 399        int ret;
 400
 401        if (video->ep == NULL) {
 402                uvcg_info(&video->uvc->func,
 403                          "Video enable failed, device is uninitialized.\n");
 404                return -ENODEV;
 405        }
 406
 407        if (!enable) {
 408                cancel_work_sync(&video->pump);
 409                uvcg_queue_cancel(&video->queue, 0);
 410
 411                for (i = 0; i < video->uvc_num_requests; ++i)
 412                        if (video->ureq && video->ureq[i].req)
 413                                usb_ep_dequeue(video->ep, video->ureq[i].req);
 414
 415                uvc_video_free_requests(video);
 416                uvcg_queue_enable(&video->queue, 0);
 417                return 0;
 418        }
 419
 420        if ((ret = uvcg_queue_enable(&video->queue, 1)) < 0)
 421                return ret;
 422
 423        if ((ret = uvc_video_alloc_requests(video)) < 0)
 424                return ret;
 425
 426        if (video->max_payload_size) {
 427                video->encode = uvc_video_encode_bulk;
 428                video->payload_size = 0;
 429        } else
 430                video->encode = video->queue.use_sg ?
 431                        uvc_video_encode_isoc_sg : uvc_video_encode_isoc;
 432
 433        video->req_int_count = 0;
 434
 435        schedule_work(&video->pump);
 436
 437        return ret;
 438}
 439
 440/*
 441 * Initialize the UVC video stream.
 442 */
 443int uvcg_video_init(struct uvc_video *video, struct uvc_device *uvc)
 444{
 445        INIT_LIST_HEAD(&video->req_free);
 446        spin_lock_init(&video->req_lock);
 447        INIT_WORK(&video->pump, uvcg_video_pump);
 448
 449        video->uvc = uvc;
 450        video->fcc = V4L2_PIX_FMT_YUYV;
 451        video->bpp = 16;
 452        video->width = 320;
 453        video->height = 240;
 454        video->imagesize = 320 * 240 * 2;
 455
 456        /* Initialize the video buffers queue. */
 457        uvcg_queue_init(&video->queue, uvc->v4l2_dev.dev->parent,
 458                        V4L2_BUF_TYPE_VIDEO_OUTPUT, &video->mutex);
 459        return 0;
 460}
 461
 462