drivers/media/video/videobuf2-core.c

   1 /*
   2  * videobuf2-core.c - V4L2 driver helper framework
   3  *
   4  * Copyright (C) 2010 Samsung Electronics
   5  *
   6  * Author: Pawel Osciak <pawel@osciak.com>
   7  *         Marek Szyprowski <m.szyprowski@samsung.com>
   8  *
   9  * This program is free software; you can redistribute it and/or modify
  10  * it under the terms of the GNU General Public License as published by
  11  * the Free Software Foundation.
  12  */
  13
  14 #include <linux/err.h>
  15 #include <linux/kernel.h>
  16 #include <linux/module.h>
  17 #include <linux/mm.h>
  18 #include <linux/poll.h>
  19 #include <linux/slab.h>
  20 #include <linux/sched.h>
  21
  22 #include <media/videobuf2-core.h>
  23
  24 static int debug;
  25 module_param(debug, int, 0644);
  26
  27 #define dprintk(level, fmt, arg...)                                     \
  28         do {                                                            \
  29                 if (debug >= level)                                     \
  30                         printk(KERN_DEBUG "vb2: " fmt, ## arg);         \
  31         } while (0)
  32
  33 #define call_memop(q, op, args...)                                      \
  34         (((q)->mem_ops->op) ?                                           \
  35                 ((q)->mem_ops->op(args)) : 0)
  36
  37 #define call_qop(q, op, args...)                                        \
  38         (((q)->ops->op) ? ((q)->ops->op(args)) : 0)
  39
  40 #define V4L2_BUFFER_STATE_FLAGS (V4L2_BUF_FLAG_MAPPED | V4L2_BUF_FLAG_QUEUED | \
  41                                  V4L2_BUF_FLAG_DONE | V4L2_BUF_FLAG_ERROR | \
  42                                  V4L2_BUF_FLAG_PREPARED)
  43
  44 /**
  45  * __vb2_buf_mem_alloc() - allocate video memory for the given buffer
  46  */
  47 static int __vb2_buf_mem_alloc(struct vb2_buffer *vb)
  48 {
  49         struct vb2_queue *q = vb->vb2_queue;
  50         void *mem_priv;
  51         int plane;
  52
  53         /* Allocate memory for all planes in this buffer */
  54         for (plane = 0; plane < vb->num_planes; ++plane) {
  55                 mem_priv = call_memop(q, alloc, q->alloc_ctx[plane],
  56                                       q->plane_sizes[plane]);
  57                 if (IS_ERR_OR_NULL(mem_priv))
  58                         goto free;
  59
  60                 /* Associate allocator private data with this plane */
  61                 vb->planes[plane].mem_priv = mem_priv;
  62                 vb->v4l2_planes[plane].length = q->plane_sizes[plane];
  63         }
  64
  65         return 0;
  66 free:
  67         /* Free already allocated memory if one of the allocations failed */
  68         for (; plane > 0; --plane) {
  69                 call_memop(q, put, vb->planes[plane - 1].mem_priv);
  70                 vb->planes[plane - 1].mem_priv = NULL;
  71         }
  72
  73         return -ENOMEM;
  74 }
  75
  76 /**
  77  * __vb2_buf_mem_free() - free memory of the given buffer
  78  */
  79 static void __vb2_buf_mem_free(struct vb2_buffer *vb)
  80 {
  81         struct vb2_queue *q = vb->vb2_queue;
  82         unsigned int plane;
  83
  84         for (plane = 0; plane < vb->num_planes; ++plane) {
  85                 call_memop(q, put, vb->planes[plane].mem_priv);
  86                 vb->planes[plane].mem_priv = NULL;
  87                 dprintk(3, "Freed plane %d of buffer %d\n", plane,
  88                         vb->v4l2_buf.index);
  89         }
  90 }
  91
  92 /**
  93  * __vb2_buf_userptr_put() - release userspace memory associated with
  94  * a USERPTR buffer
  95  */
  96 static void __vb2_buf_userptr_put(struct vb2_buffer *vb)
  97 {
  98         struct vb2_queue *q = vb->vb2_queue;
  99         unsigned int plane;
 100
 101         for (plane = 0; plane < vb->num_planes; ++plane) {
 102                 if (vb->planes[plane].mem_priv)
 103                         call_memop(q, put_userptr, vb->planes[plane].mem_priv);
 104                 vb->planes[plane].mem_priv = NULL;
 105         }
 106 }
 107
 108 /**
 109  * __vb2_plane_dmabuf_put() - release memory associated with
 110  * a DMABUF shared plane
 111  */
 112 static void __vb2_plane_dmabuf_put(struct vb2_queue *q, struct vb2_plane *p)
 113 {
 114         if (!p->mem_priv)
 115                 return;
 116
 117         if (p->dbuf_mapped)
 118                 call_memop(q, unmap_dmabuf, p->mem_priv);
 119
 120         call_memop(q, detach_dmabuf, p->mem_priv);
 121         dma_buf_put(p->dbuf);
 122         memset(p, 0, sizeof *p);
 123 }
 124
 125 /**
 126  * __vb2_buf_dmabuf_put() - release memory associated with
 127  * a DMABUF shared buffer
 128  */
 129 static void __vb2_buf_dmabuf_put(struct vb2_buffer *vb)
 130 {
 131         struct vb2_queue *q = vb->vb2_queue;
 132         unsigned int plane;
 133
 134         for (plane = 0; plane < vb->num_planes; ++plane)
 135                 __vb2_plane_dmabuf_put(q, &vb->planes[plane]);
 136 }
 137
 138 /**
 139  * __setup_offsets() - setup unique offsets ("cookies") for every plane in
 140  * every buffer on the queue
 141  */
 142 static void __setup_offsets(struct vb2_queue *q, unsigned int n)
 143 {
 144         unsigned int buffer, plane;
 145         struct vb2_buffer *vb;
 146         unsigned long off;
 147
 148         if (q->num_buffers) {
 149                 struct v4l2_plane *p;
 150                 vb = q->bufs[q->num_buffers - 1];
 151                 p = &vb->v4l2_planes[vb->num_planes - 1];
 152                 off = PAGE_ALIGN(p->m.mem_offset + p->length);
 153         } else {
 154                 off = 0;
 155         }
 156
 157         for (buffer = q->num_buffers; buffer < q->num_buffers + n; ++buffer) {
 158                 vb = q->bufs[buffer];
 159                 if (!vb)
 160                         continue;
 161
 162                 for (plane = 0; plane < vb->num_planes; ++plane) {
 163                         vb->v4l2_planes[plane].length = q->plane_sizes[plane];
 164                         vb->v4l2_planes[plane].m.mem_offset = off;
 165
 166                         dprintk(3, "Buffer %d, plane %d offset 0x%08lx\n",
 167                                         buffer, plane, off);
 168
 169                         off += vb->v4l2_planes[plane].length;
 170                         off = PAGE_ALIGN(off);
 171                 }
 172         }
 173 }
 174
 175 /**
 176  * __vb2_queue_alloc() - allocate videobuf buffer structures and (for MMAP type)
 177  * video buffer memory for all buffers/planes on the queue and initializes the
 178  * queue
 179  *
 180  * Returns the number of buffers successfully allocated.
 181  */
 182 static int __vb2_queue_alloc(struct vb2_queue *q, enum v4l2_memory memory,
 183                              unsigned int num_buffers, unsigned int num_planes)
 184 {
 185         unsigned int buffer;
 186         struct vb2_buffer *vb;
 187         int ret;
 188
 189         for (buffer = 0; buffer < num_buffers; ++buffer) {
 190                 /* Allocate videobuf buffer structures */
 191                 vb = kzalloc(q->buf_struct_size, GFP_KERNEL);
 192                 if (!vb) {
 193                         dprintk(1, "Memory alloc for buffer struct failed\n");
 194                         break;
 195                 }
 196
 197                 /* Length stores number of planes for multiplanar buffers */
 198                 if (V4L2_TYPE_IS_MULTIPLANAR(q->type))
 199                         vb->v4l2_buf.length = num_planes;
 200
 201                 vb->state = VB2_BUF_STATE_DEQUEUED;
 202                 vb->vb2_queue = q;
 203                 vb->num_planes = num_planes;
 204                 vb->v4l2_buf.index = q->num_buffers + buffer;
 205                 vb->v4l2_buf.type = q->type;
 206                 vb->v4l2_buf.memory = memory;
 207
 208                 /* Allocate video buffer memory for the MMAP type */
 209                 if (memory == V4L2_MEMORY_MMAP) {
 210                         ret = __vb2_buf_mem_alloc(vb);
 211                         if (ret) {
 212                                 dprintk(1, "Failed allocating memory for "
 213                                                 "buffer %d\n", buffer);
 214                                 kfree(vb);
 215                                 break;
 216                         }
 217                         /*
 218                          * Call the driver-provided buffer initialization
 219                          * callback, if given. An error in initialization
 220                          * results in queue setup failure.
 221                          */
 222                         ret = call_qop(q, buf_init, vb);
 223                         if (ret) {
 224                                 dprintk(1, "Buffer %d %p initialization"
 225                                         " failed\n", buffer, vb);
 226                                 __vb2_buf_mem_free(vb);
 227                                 kfree(vb);
 228                                 break;
 229                         }
 230                 }
 231
 232                 q->bufs[q->num_buffers + buffer] = vb;
 233         }
 234
 235         __setup_offsets(q, buffer);
 236
 237         dprintk(1, "Allocated %d buffers, %d plane(s) each\n",
 238                         buffer, num_planes);
 239
 240         return buffer;
 241 }
 242
 243 /**
 244  * __vb2_free_mem() - release all video buffer memory for a given queue
 245  */
 246 static void __vb2_free_mem(struct vb2_queue *q, unsigned int buffers)
 247 {
 248         unsigned int buffer;
 249         struct vb2_buffer *vb;
 250
 251         for (buffer = q->num_buffers - buffers; buffer < q->num_buffers;
 252              ++buffer) {
 253                 vb = q->bufs[buffer];
 254                 if (!vb)
 255                         continue;
 256
 257                 /* Free MMAP buffers or release USERPTR buffers */
 258                 if (q->memory == V4L2_MEMORY_MMAP)
 259                         __vb2_buf_mem_free(vb);
 260                 else if (q->memory == V4L2_MEMORY_DMABUF)
 261                         __vb2_buf_dmabuf_put(vb);
 262                 else
 263                         __vb2_buf_userptr_put(vb);
 264         }
 265 }
 266
 267 /**
 268  * __vb2_queue_free() - free buffers at the end of the queue - video memory and
 269  * related information, if no buffers are left return the queue to an
 270  * uninitialized state. Might be called even if the queue has already been freed.
 271  */
 272 static void __vb2_queue_free(struct vb2_queue *q, unsigned int buffers)
 273 {
 274         unsigned int buffer;
 275
 276         /* Call driver-provided cleanup function for each buffer, if provided */
 277         if (q->ops->buf_cleanup) {
 278                 for (buffer = q->num_buffers - buffers; buffer < q->num_buffers;
 279                      ++buffer) {
 280                         if (NULL == q->bufs[buffer])
 281                                 continue;
 282                         q->ops->buf_cleanup(q->bufs[buffer]);
 283                 }
 284         }
 285
 286         /* Release video buffer memory */
 287         __vb2_free_mem(q, buffers);
 288
 289         /* Free videobuf buffers */
 290         for (buffer = q->num_buffers - buffers; buffer < q->num_buffers;
 291              ++buffer) {
 292                 kfree(q->bufs[buffer]);
 293                 q->bufs[buffer] = NULL;
 294         }
 295
 296         q->num_buffers -= buffers;
 297         if (!q->num_buffers)
 298                 q->memory = 0;
 299         INIT_LIST_HEAD(&q->queued_list);
 300 }
 301
 302 /**
 303  * __verify_planes_array() - verify that the planes array passed in struct
 304  * v4l2_buffer from userspace can be safely used
 305  */
 306 static int __verify_planes_array(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 307 {
 308         /* Is memory for copying plane information present? */
 309         if (NULL == b->m.planes) {
 310                 dprintk(1, "Multi-planar buffer passed but "
 311                            "planes array not provided\n");
 312                 return -EINVAL;
 313         }
 314
 315         if (b->length < vb->num_planes || b->length > VIDEO_MAX_PLANES) {
 316                 dprintk(1, "Incorrect planes array length, "
 317                            "expected %d, got %d\n", vb->num_planes, b->length);
 318                 return -EINVAL;
 319         }
 320
 321         return 0;
 322 }
 323
 324 /**
 325  * __buffer_in_use() - return true if the buffer is in use and
 326  * the queue cannot be freed (by the means of REQBUFS(0)) call
 327  */
 328 static bool __buffer_in_use(struct vb2_queue *q, struct vb2_buffer *vb)
 329 {
 330         unsigned int plane;
 331         for (plane = 0; plane < vb->num_planes; ++plane) {
 332                 void *mem_priv = vb->planes[plane].mem_priv;
 333                 /*
 334                  * If num_users() has not been provided, call_memop
 335                  * will return 0, apparently nobody cares about this
 336                  * case anyway. If num_users() returns more than 1,
 337                  * we are not the only user of the plane's memory.
 338                  */
 339                 if (mem_priv && call_memop(q, num_users, mem_priv) > 1)
 340                         return true;
 341         }
 342         return false;
 343 }
 344
 345 /**
 346  * __buffers_in_use() - return true if any buffers on the queue are in use and
 347  * the queue cannot be freed (by the means of REQBUFS(0)) call
 348  */
 349 static bool __buffers_in_use(struct vb2_queue *q)
 350 {
 351         unsigned int buffer;
 352         for (buffer = 0; buffer < q->num_buffers; ++buffer) {
 353                 if (__buffer_in_use(q, q->bufs[buffer]))
 354                         return true;
 355         }
 356         return false;
 357 }
 358
 359 /**
 360  * __fill_v4l2_buffer() - fill in a struct v4l2_buffer with information to be
 361  * returned to userspace
 362  */
 363 static int __fill_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b)
 364 {
 365         struct vb2_queue *q = vb->vb2_queue;
 366         int ret;
 367
 368         /* Copy back data such as timestamp, flags, input, etc. */
 369         memcpy(b, &vb->v4l2_buf, offsetof(struct v4l2_buffer, m));
 370         b->input = vb->v4l2_buf.input;
 371         b->reserved = vb->v4l2_buf.reserved;
 372
 373         if (V4L2_TYPE_IS_MULTIPLANAR(q->type)) {
 374                 ret = __verify_planes_array(vb, b);
 375                 if (ret)
 376                         return ret;
 377
 378                 /*
 379                  * Fill in plane-related data if userspace provided an array
 380                  * for it. The memory and size is verified above.
 381                  */
 382                 memcpy(b->m.planes, vb->v4l2_planes,
 383                         b->length * sizeof(struct v4l2_plane));
 384
 385                 if (q->memory == V4L2_MEMORY_DMABUF) {
 386                         unsigned int plane;
 387                         for (plane = 0; plane < vb->num_planes; ++plane)
 388                                 b->m.planes[plane].m.fd = 0;
 389                 }
 390         } else {
 391                 /*
 392                  * We use length and offset in v4l2_planes array even for
 393                  * single-planar buffers, but userspace does not.
 394                  */
 395                 b->length = vb->v4l2_planes[0].length;
 396                 b->bytesused = vb->v4l2_planes[0].bytesused;
 397                 if (q->memory == V4L2_MEMORY_MMAP)
 398                         b->m.offset = vb->v4l2_planes[0].m.mem_offset;
 399                 else if (q->memory == V4L2_MEMORY_USERPTR)
 400                         b->m.userptr = vb->v4l2_planes[0].m.userptr;
 401                 else if (q->memory == V4L2_MEMORY_DMABUF)
 402                         b->m.fd = 0;
 403         }
 404
 405         /*
 406          * Clear any buffer state related flags.
 407          */
 408         b->flags &= ~V4L2_BUFFER_STATE_FLAGS;
 409
 410         switch (vb->state) {
 411         case VB2_BUF_STATE_QUEUED:
 412         case VB2_BUF_STATE_ACTIVE:
 413                 b->flags |= V4L2_BUF_FLAG_QUEUED;
 414                 break;
 415         case VB2_BUF_STATE_ERROR:
 416                 b->flags |= V4L2_BUF_FLAG_ERROR;
 417                 /* fall through */
 418         case VB2_BUF_STATE_DONE:
 419                 b->flags |= V4L2_BUF_FLAG_DONE;
 420                 break;
 421         case VB2_BUF_STATE_PREPARED:
 422                 b->flags |= V4L2_BUF_FLAG_PREPARED;
 423                 break;
 424         case VB2_BUF_STATE_DEQUEUED:
 425                 /* nothing */
 426                 break;
 427         }
 428
 429         if (__buffer_in_use(q, vb))
 430                 b->flags |= V4L2_BUF_FLAG_MAPPED;
 431
 432         return 0;
 433 }
 434
 435 /**
 436  * vb2_querybuf() - query video buffer information
 437  * @q:          videobuf queue
 438  * @b:          buffer struct passed from userspace to vidioc_querybuf handler
 439  *              in driver
 440  *
 441  * Should be called from vidioc_querybuf ioctl handler in driver.
 442  * This function will verify the passed v4l2_buffer structure and fill the
 443  * relevant information for the userspace.
 444  *
 445  * The return values from this function are intended to be directly returned
 446  * from vidioc_querybuf handler in driver.
 447  */
 448 int vb2_querybuf(struct vb2_queue *q, struct v4l2_buffer *b)
 449 {
 450         struct vb2_buffer *vb;
 451
 452         if (b->type != q->type) {
 453                 dprintk(1, "querybuf: wrong buffer type\n");
 454                 return -EINVAL;
 455         }
 456
 457         if (b->index >= q->num_buffers) {
 458                 dprintk(1, "querybuf: buffer index out of range\n");
 459                 return -EINVAL;
 460         }
 461         vb = q->bufs[b->index];
 462
 463         return __fill_v4l2_buffer(vb, b);
 464 }
 465 EXPORT_SYMBOL(vb2_querybuf);
 466
 467 /**
 468  * __verify_userptr_ops() - verify that all memory operations required for
 469  * USERPTR queue type have been provided
 470  */
 471 static int __verify_userptr_ops(struct vb2_queue *q)
 472 {
 473         if (!(q->io_modes & VB2_USERPTR) || !q->mem_ops->get_userptr ||
 474             !q->mem_ops->put_userptr)
 475                 return -EINVAL;
 476
 477         return 0;
 478 }
 479
 480 /**
 481  * __verify_mmap_ops() - verify that all memory operations required for
 482  * MMAP queue type have been provided
 483  */
 484 static int __verify_mmap_ops(struct vb2_queue *q)
 485 {
 486         if (!(q->io_modes & VB2_MMAP) || !q->mem_ops->alloc ||
 487             !q->mem_ops->put || !q->mem_ops->mmap)
 488                 return -EINVAL;
 489
 490         return 0;
 491 }
 492
 493 /**
 494  * __verify_dmabuf_ops() - verify that all memory operations required for
 495  * DMABUF queue type have been provided
 496  */
 497 static int __verify_dmabuf_ops(struct vb2_queue *q)
 498 {
 499         if (!(q->io_modes & VB2_DMABUF) || !q->mem_ops->attach_dmabuf ||
 500             !q->mem_ops->detach_dmabuf  || !q->mem_ops->map_dmabuf ||
 501             !q->mem_ops->unmap_dmabuf)
 502                 return -EINVAL;
 503
 504         return 0;
 505 }
 506
 507 /**
 508  * vb2_reqbufs() - Initiate streaming
 509  * @q:          videobuf2 queue
 510  * @req:        struct passed from userspace to vidioc_reqbufs handler in driver
 511  *
 512  * Should be called from vidioc_reqbufs ioctl handler of a driver.
 513  * This function:
 514  * 1) verifies streaming parameters passed from the userspace,
 515  * 2) sets up the queue,
 516  * 3) negotiates number of buffers and planes per buffer with the driver
 517  *    to be used during streaming,
 518  * 4) allocates internal buffer structures (struct vb2_buffer), according to
 519  *    the agreed parameters,
 520  * 5) for MMAP memory type, allocates actual video memory, using the
 521  *    memory handling/allocation routines provided during queue initialization
 522  *
 523  * If req->count is 0, all the memory will be freed instead.
 524  * If the queue has been allocated previously (by a previous vb2_reqbufs) call
 525  * and the queue is not busy, memory will be reallocated.
 526  *
 527  * The return values from this function are intended to be directly returned
 528  * from vidioc_reqbufs handler in driver.
 529  */
 530 int vb2_reqbufs(struct vb2_queue *q, struct v4l2_requestbuffers *req)
 531 {
 532         unsigned int num_buffers, allocated_buffers, num_planes = 0;
 533         int ret = 0;
 534
 535         if (q->fileio) {
 536                 dprintk(1, "reqbufs: file io in progress\n");
 537                 return -EBUSY;
 538         }
 539
 540         if (req->memory != V4L2_MEMORY_MMAP &&
 541             req->memory != V4L2_MEMORY_DMABUF &&
 542             req->memory != V4L2_MEMORY_USERPTR) {
 543                 dprintk(1, "reqbufs: unsupported memory type\n");
 544                 return -EINVAL;
 545         }
 546
 547         if (req->type != q->type) {
 548                 dprintk(1, "reqbufs: requested type is incorrect\n");
 549                 return -EINVAL;
 550         }
 551
 552         if (q->streaming) {
 553                 dprintk(1, "reqbufs: streaming active\n");
 554                 return -EBUSY;
 555         }
 556
 557         /*
 558          * Make sure all the required memory ops for given memory type
 559          * are available.
 560          */
 561         if (req->memory == V4L2_MEMORY_MMAP && __verify_mmap_ops(q)) {
 562                 dprintk(1, "reqbufs: MMAP for current setup unsupported\n");
 563                 return -EINVAL;
 564         }
 565
 566         if (req->memory == V4L2_MEMORY_USERPTR && __verify_userptr_ops(q)) {
 567                 dprintk(1, "reqbufs: USERPTR for current setup unsupported\n");
 568                 return -EINVAL;
 569         }
 570
 571         if (req->memory == V4L2_MEMORY_DMABUF && __verify_dmabuf_ops(q)) {
 572                 dprintk(1, "reqbufs: DMABUF for current setup unsupported\n");
 573                 return -EINVAL;
 574         }
 575
 576         if (req->count == 0 || q->num_buffers != 0 || q->memory != req->memory) {
 577                 /*
 578                  * We already have buffers allocated, so first check if they
 579                  * are not in use and can be freed.
 580                  */
 581                 if (q->memory == V4L2_MEMORY_MMAP && __buffers_in_use(q)) {
 582                         dprintk(1, "reqbufs: memory in use, cannot free\n");
 583                         return -EBUSY;
 584                 }
 585
 586                 __vb2_queue_free(q, q->num_buffers);
 587
 588                 /*
 589                  * In case of REQBUFS(0) return immediately without calling
 590                  * driver's queue_setup() callback and allocating resources.
 591                  */
 592                 if (req->count == 0)
 593                         return 0;
 594         }
 595
 596         /*
 597          * Make sure the requested values and current defaults are sane.
 598          */
 599         num_buffers = min_t(unsigned int, req->count, VIDEO_MAX_FRAME);
 600         memset(q->plane_sizes, 0, sizeof(q->plane_sizes));
 601         memset(q->alloc_ctx, 0, sizeof(q->alloc_ctx));
 602         q->memory = req->memory;
 603
 604         /*
 605          * Ask the driver how many buffers and planes per buffer it requires.
 606          * Driver also sets the size and allocator context for each plane.
 607          */
 608         ret = call_qop(q, queue_setup, q, NULL, &num_buffers, &num_planes,
 609                        q->plane_sizes, q->alloc_ctx);
 610         if (ret)
 611                 return ret;
 612
 613         /* Finally, allocate buffers and video memory */
 614         ret = __vb2_queue_alloc(q, req->memory, num_buffers, num_planes);
 615         if (ret == 0) {
 616                 dprintk(1, "Memory allocation failed\n");
 617                 return -ENOMEM;
 618         }
 619
 620         allocated_buffers = ret;
 621
 622         /*
 623          * Check if driver can handle the allocated number of buffers.
 624          */
 625         if (allocated_buffers < num_buffers) {
 626                 num_buffers = allocated_buffers;
 627
 628                 ret = call_qop(q, queue_setup, q, NULL, &num_buffers,
 629                                &num_planes, q->plane_sizes, q->alloc_ctx);
 630
 631                 if (!ret && allocated_buffers < num_buffers)
 632                         ret = -ENOMEM;
 633
 634                 /*
 635                  * Either the driver has accepted a smaller number of buffers,
 636                  * or .queue_setup() returned an error
 637                  */
 638         }
 639
 640         q->num_buffers = allocated_buffers;
 641
 642         if (ret < 0) {
 643                 __vb2_queue_free(q, allocated_buffers);
 644                 return ret;
 645         }
 646
 647         /*
 648          * Return the number of successfully allocated buffers
 649          * to the userspace.
 650          */
 651         req->count = allocated_buffers;
 652
 653         return 0;
 654 }
 655 EXPORT_SYMBOL_GPL(vb2_reqbufs);
 656
 657 /**
 658  * vb2_create_bufs() - Allocate buffers and any required auxiliary structs
 659  * @q:          videobuf2 queue
 660  * @create:     creation parameters, passed from userspace to vidioc_create_bufs
 661  *              handler in driver
 662  *
 663  * Should be called from vidioc_create_bufs ioctl handler of a driver.
 664  * This function:
 665  * 1) verifies parameter sanity
 666  * 2) calls the .queue_setup() queue operation
 667  * 3) performs any necessary memory allocations
 668  *
 669  * The return values from this function are intended to be directly returned
 670  * from vidioc_create_bufs handler in driver.
 671  */
 672 int vb2_create_bufs(struct vb2_queue *q, struct v4l2_create_buffers *create)
 673 {
 674         unsigned int num_planes = 0, num_buffers, allocated_buffers;
 675         int ret = 0;
 676
 677         if (q->fileio) {
 678                 dprintk(1, "%s(): file io in progress\n", __func__);
 679                 return -EBUSY;
 680         }
 681
 682         if (create->memory != V4L2_MEMORY_MMAP &&
 683             create->memory != V4L2_MEMORY_USERPTR &&
 684             create->memory != V4L2_MEMORY_DMABUF) {
 685                 dprintk(1, "%s(): unsupported memory type\n", __func__);
 686                 return -EINVAL;
 687         }
 688
 689         if (create->format.type != q->type) {
 690                 dprintk(1, "%s(): requested type is incorrect\n", __func__);
 691                 return -EINVAL;
 692         }
 693
 694         /*
 695          * Make sure all the required memory ops for given memory type
 696          * are available.
 697          */
 698         if (create->memory == V4L2_MEMORY_MMAP && __verify_mmap_ops(q)) {
 699                 dprintk(1, "%s(): MMAP for current setup unsupported\n", __func__);
 700                 return -EINVAL;
 701         }
 702
 703         if (create->memory == V4L2_MEMORY_USERPTR && __verify_userptr_ops(q)) {
 704                 dprintk(1, "%s(): USERPTR for current setup unsupported\n", __func__);
 705                 return -EINVAL;
 706         }
 707
 708         if (create->memory == V4L2_MEMORY_DMABUF && __verify_dmabuf_ops(q)) {
 709                 dprintk(1, "%s(): DMABUF for current setup unsupported\n", __func__);
 710                 return -EINVAL;
 711         }
 712
 713         if (q->num_buffers == VIDEO_MAX_FRAME) {
 714                 dprintk(1, "%s(): maximum number of buffers already allocated\n",
 715                         __func__);
 716                 return -ENOBUFS;
 717         }
 718
 719         create->index = q->num_buffers;
 720
 721         if (!q->num_buffers) {
 722                 memset(q->plane_sizes, 0, sizeof(q->plane_sizes));
 723                 memset(q->alloc_ctx, 0, sizeof(q->alloc_ctx));
 724                 q->memory = create->memory;
 725         }
 726
 727         num_buffers = min(create->count, VIDEO_MAX_FRAME - q->num_buffers);
 728
 729         /*
 730          * Ask the driver, whether the requested number of buffers, planes per
 731          * buffer and their sizes are acceptable
 732          */
 733         ret = call_qop(q, queue_setup, q, &create->format, &num_buffers,
 734                        &num_planes, q->plane_sizes, q->alloc_ctx);
 735         if (ret)
 736                 return ret;
 737
 738         /* Finally, allocate buffers and video memory */
 739         ret = __vb2_queue_alloc(q, create->memory, num_buffers,
 740                                 num_planes);
 741         if (ret < 0) {
 742                 dprintk(1, "Memory allocation failed with error: %d\n", ret);
 743                 return ret;
 744         }
 745
 746         allocated_buffers = ret;
 747
 748         /*
 749          * Check if driver can handle the so far allocated number of buffers.
 750          */
 751         if (ret < num_buffers) {
 752                 num_buffers = ret;
 753
 754                 /*
 755                  * q->num_buffers contains the total number of buffers, that the
 756                  * queue driver has set up
 757                  */
 758                 ret = call_qop(q, queue_setup, q, &create->format, &num_buffers,
 759                                &num_planes, q->plane_sizes, q->alloc_ctx);
 760
 761                 if (!ret && allocated_buffers < num_buffers)
 762                         ret = -ENOMEM;
 763
 764                 /*
 765                  * Either the driver has accepted a smaller number of buffers,
 766                  * or .queue_setup() returned an error
 767                  */
 768         }
 769
 770         q->num_buffers += allocated_buffers;
 771
 772         if (ret < 0) {
 773                 __vb2_queue_free(q, allocated_buffers);
 774                 return ret;
 775         }
 776
 777         /*
 778          * Return the number of successfully allocated buffers
 779          * to the userspace.
 780          */
 781         create->count = allocated_buffers;
 782
 783         return 0;
 784 }
 785 EXPORT_SYMBOL_GPL(vb2_create_bufs);
 786
 787 /**
 788  * vb2_plane_vaddr() - Return a kernel virtual address of a given plane
 789  * @vb:         vb2_buffer to which the plane in question belongs to
 790  * @plane_no:   plane number for which the address is to be returned
 791  *
 792  * This function returns a kernel virtual address of a given plane if
 793  * such a mapping exist, NULL otherwise.
 794  */
 795 void *vb2_plane_vaddr(struct vb2_buffer *vb, unsigned int plane_no)
 796 {
 797         struct vb2_queue *q = vb->vb2_queue;
 798
 799         if (plane_no > vb->num_planes || !vb->planes[plane_no].mem_priv)
 800                 return NULL;
 801
 802         return call_memop(q, vaddr, vb->planes[plane_no].mem_priv);
 803
 804 }
 805 EXPORT_SYMBOL_GPL(vb2_plane_vaddr);
 806
 807 /**
 808  * vb2_plane_cookie() - Return allocator specific cookie for the given plane
 809  * @vb:         vb2_buffer to which the plane in question belongs to
 810  * @plane_no:   plane number for which the cookie is to be returned
 811  *
 812  * This function returns an allocator specific cookie for a given plane if
 813  * available, NULL otherwise. The allocator should provide some simple static
 814  * inline function, which would convert this cookie to the allocator specific
 815  * type that can be used directly by the driver to access the buffer. This can
 816  * be for example physical address, pointer to scatter list or IOMMU mapping.
 817  */
 818 void *vb2_plane_cookie(struct vb2_buffer *vb, unsigned int plane_no)
 819 {
 820         struct vb2_queue *q = vb->vb2_queue;
 821
 822         if (plane_no > vb->num_planes || !vb->planes[plane_no].mem_priv)
 823                 return NULL;
 824
 825         return call_memop(q, cookie, vb->planes[plane_no].mem_priv);
 826 }
 827 EXPORT_SYMBOL_GPL(vb2_plane_cookie);
 828
 829 /**
 830  * vb2_buffer_done() - inform videobuf that an operation on a buffer is finished
 831  * @vb:         vb2_buffer returned from the driver
 832  * @state:      either VB2_BUF_STATE_DONE if the operation finished successfully
 833  *              or VB2_BUF_STATE_ERROR if the operation finished with an error
 834  *
 835  * This function should be called by the driver after a hardware operation on
 836  * a buffer is finished and the buffer may be returned to userspace. The driver
 837  * cannot use this buffer anymore until it is queued back to it by videobuf
 838  * by the means of buf_queue callback. Only buffers previously queued to the
 839  * driver by buf_queue can be passed to this function.
 840  */
 841 void vb2_buffer_done(struct vb2_buffer *vb, enum vb2_buffer_state state)
 842 {
 843         struct vb2_queue *q = vb->vb2_queue;
 844         unsigned long flags;
 845
 846         if (vb->state != VB2_BUF_STATE_ACTIVE)
 847                 return;
 848
 849         if (state != VB2_BUF_STATE_DONE && state != VB2_BUF_STATE_ERROR)
 850                 return;
 851
 852         dprintk(4, "Done processing on buffer %d, state: %d\n",
 853                         vb->v4l2_buf.index, vb->state);
 854
 855         /* Add the buffer to the done buffers list */
 856         spin_lock_irqsave(&q->done_lock, flags);
 857         vb->state = state;
 858         list_add_tail(&vb->done_entry, &q->done_list);
 859         atomic_dec(&q->queued_count);
 860         spin_unlock_irqrestore(&q->done_lock, flags);
 861
 862         /* Inform any processes that may be waiting for buffers */
 863         wake_up(&q->done_wq);
 864 }
 865 EXPORT_SYMBOL_GPL(vb2_buffer_done);
 866
 867 /**
 868  * __fill_vb2_buffer() - fill a vb2_buffer with information provided in
 869  * a v4l2_buffer by the userspace
 870  */
 871 static int __fill_vb2_buffer(struct vb2_buffer *vb, const struct v4l2_buffer *b,
 872                                 struct v4l2_plane *v4l2_planes)
 873 {
 874         unsigned int plane;
 875         int ret;
 876
 877         if (V4L2_TYPE_IS_MULTIPLANAR(b->type)) {
 878                 /*
 879                  * Verify that the userspace gave us a valid array for
 880                  * plane information.
 881                  */
 882                 ret = __verify_planes_array(vb, b);
 883                 if (ret)
 884                         return ret;
 885
 886                 /* Fill in driver-provided information for OUTPUT types */
 887                 if (V4L2_TYPE_IS_OUTPUT(b->type)) {
 888                         /*
 889                          * Will have to go up to b->length when API starts
 890                          * accepting variable number of planes.
 891                          */
 892                         for (plane = 0; plane < vb->num_planes; ++plane) {
 893                                 v4l2_planes[plane].bytesused =
 894                                         b->m.planes[plane].bytesused;
 895                                 v4l2_planes[plane].data_offset =
 896                                         b->m.planes[plane].data_offset;
 897                         }
 898                 }
 899
 900                 if (b->memory == V4L2_MEMORY_USERPTR) {
 901                         for (plane = 0; plane < vb->num_planes; ++plane) {
 902                                 v4l2_planes[plane].m.userptr =
 903                                         b->m.planes[plane].m.userptr;
 904                                 v4l2_planes[plane].length =
 905                                         b->m.planes[plane].length;
 906                         }
 907                 }
 908                 if (b->memory == V4L2_MEMORY_DMABUF) {
 909                         for (plane = 0; plane < vb->num_planes; ++plane) {
 910                                 v4l2_planes[plane].bytesused =
 911                                         b->m.planes[plane].bytesused;
 912                                 v4l2_planes[plane].m.fd =
 913                                         b->m.planes[plane].m.fd;
 914                         }
 915                 }
 916         } else {
 917                 /*
 918                  * Single-planar buffers do not use planes array,
 919                  * so fill in relevant v4l2_buffer struct fields instead.
 920                  * In videobuf we use our internal V4l2_planes struct for
 921                  * single-planar buffers as well, for simplicity.
 922                  */
 923                 if (V4L2_TYPE_IS_OUTPUT(b->type))
 924                         v4l2_planes[0].bytesused = b->bytesused;
 925
 926                 if (b->memory == V4L2_MEMORY_USERPTR) {
 927                         v4l2_planes[0].m.userptr = b->m.userptr;
 928                         v4l2_planes[0].length = b->length;
 929                 }
 930
 931                 if (b->memory == V4L2_MEMORY_DMABUF)
 932                         v4l2_planes[0].m.fd = b->m.fd;
 933
 934         }
 935
 936         vb->v4l2_buf.field = b->field;
 937         vb->v4l2_buf.timestamp = b->timestamp;
 938         vb->v4l2_buf.input = b->input;
 939         vb->v4l2_buf.flags = b->flags & ~V4L2_BUFFER_STATE_FLAGS;
 940
 941         return 0;
 942 }
 943
 944 /**
 945  * __qbuf_userptr() - handle qbuf of a USERPTR buffer
 946  */
 947 static int __qbuf_userptr(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 948 {
 949         struct v4l2_plane planes[VIDEO_MAX_PLANES];
 950         struct vb2_queue *q = vb->vb2_queue;
 951         void *mem_priv;
 952         unsigned int plane;
 953         int ret;
 954         int write = !V4L2_TYPE_IS_OUTPUT(q->type);
 955
 956         /* Verify and copy relevant information provided by the userspace */
 957         ret = __fill_vb2_buffer(vb, b, planes);
 958         if (ret)
 959                 return ret;
 960
 961         for (plane = 0; plane < vb->num_planes; ++plane) {
 962                 /* Skip the plane if already verified */
 963                 if (vb->v4l2_planes[plane].m.userptr &&
 964                     vb->v4l2_planes[plane].m.userptr == planes[plane].m.userptr
 965                     && vb->v4l2_planes[plane].length == planes[plane].length)
 966                         continue;
 967
 968                 dprintk(3, "qbuf: userspace address for plane %d changed, "
 969                                 "reacquiring memory\n", plane);
 970
 971                 /* Check if the provided plane buffer is large enough */
 972                 if (planes[plane].length < q->plane_sizes[plane]) {
 973                         ret = -EINVAL;
 974                         goto err;
 975                 }
 976
 977                 /* Release previously acquired memory if present */
 978                 if (vb->planes[plane].mem_priv)
 979                         call_memop(q, put_userptr, vb->planes[plane].mem_priv);
 980
 981                 vb->planes[plane].mem_priv = NULL;
 982                 vb->v4l2_planes[plane].m.userptr = 0;
 983                 vb->v4l2_planes[plane].length = 0;
 984
 985                 /* Acquire each plane's memory */
 986                 mem_priv = call_memop(q, get_userptr, q->alloc_ctx[plane],
 987                                       planes[plane].m.userptr,
 988                                       planes[plane].length, write);
 989                 if (IS_ERR_OR_NULL(mem_priv)) {
 990                         dprintk(1, "qbuf: failed acquiring userspace "
 991                                                 "memory for plane %d\n", plane);
 992                         ret = mem_priv ? PTR_ERR(mem_priv) : -EINVAL;
 993                         goto err;
 994                 }
 995                 vb->planes[plane].mem_priv = mem_priv;
 996         }
 997
 998         /*
 999          * Call driver-specific initialization on the newly acquired buffer,
1000          * if provided.
1001          */
1002         ret = call_qop(q, buf_init, vb);
1003         if (ret) {
1004                 dprintk(1, "qbuf: buffer initialization failed\n");
1005                 goto err;
1006         }
1007
1008         /*
1009          * Now that everything is in order, copy relevant information
1010          * provided by userspace.
1011          */
1012         for (plane = 0; plane < vb->num_planes; ++plane)
1013                 vb->v4l2_planes[plane] = planes[plane];
1014
1015         return 0;
1016 err:
1017         /* In case of errors, release planes that were already acquired */
1018         for (plane = 0; plane < vb->num_planes; ++plane) {
1019                 if (vb->planes[plane].mem_priv)
1020                         call_memop(q, put_userptr, vb->planes[plane].mem_priv);
1021                 vb->planes[plane].mem_priv = NULL;
1022                 vb->v4l2_planes[plane].m.userptr = 0;
1023                 vb->v4l2_planes[plane].length = 0;
1024         }
1025
1026         return ret;
1027 }
1028
1029 /**
1030  * __qbuf_mmap() - handle qbuf of an MMAP buffer
1031  */
1032 static int __qbuf_mmap(struct vb2_buffer *vb, const struct v4l2_buffer *b)
1033 {
1034         return __fill_vb2_buffer(vb, b, vb->v4l2_planes);
1035 }
1036
1037 /**
1038  * __qbuf_dmabuf() - handle qbuf of a DMABUF buffer
1039  */
1040 static int __qbuf_dmabuf(struct vb2_buffer *vb, const struct v4l2_buffer *b)
1041 {
1042         struct v4l2_plane planes[VIDEO_MAX_PLANES];
1043         struct vb2_queue *q = vb->vb2_queue;
1044         void *mem_priv;
1045         unsigned int plane;
1046         int ret;
1047         int write = !V4L2_TYPE_IS_OUTPUT(q->type);
1048
1049         /* Verify and copy relevant information provided by the userspace */
1050         ret = __fill_vb2_buffer(vb, b, planes);
1051         if (ret)
1052                 return ret;
1053
1054         for (plane = 0; plane < vb->num_planes; ++plane) {
1055                 struct dma_buf *dbuf = dma_buf_get(planes[plane].m.fd);
1056
1057                 if (IS_ERR_OR_NULL(dbuf)) {
1058                         dprintk(1, "qbuf: invalid dmabuf fd for "
1059                                 "plane %d\n", plane);
1060                         ret = -EINVAL;
1061                         goto err;
1062                 }
1063
1064                 /* Skip the plane if already verified */
1065                 if (dbuf == vb->planes[plane].dbuf) {
1066                         planes[plane].length = dbuf->size;
1067                         dma_buf_put(dbuf);
1068                         continue;
1069                 }
1070
1071                 dprintk(3, "qbuf: buffer description for plane %d changed, "
1072                         "reattaching dma buf\n", plane);
1073
1074                 /* Release previously acquired memory if present */
1075                 __vb2_plane_dmabuf_put(q, &vb->planes[plane]);
1076
1077                 /* Acquire each plane's memory */
1078                 mem_priv = call_memop(q, attach_dmabuf, q->alloc_ctx[plane],
1079                         dbuf, q->plane_sizes[plane], write);
1080                 if (IS_ERR(mem_priv)) {
1081                         dprintk(1, "qbuf: failed acquiring dmabuf "
1082                                 "memory for plane %d\n", plane);
1083                         ret = PTR_ERR(mem_priv);
1084                         goto err;
1085                 }
1086
1087                 planes[plane].length = dbuf->size;
1088                 vb->planes[plane].dbuf = dbuf;
1089                 vb->planes[plane].mem_priv = mem_priv;
1090         }
1091
1092         /* TODO: This pins the buffer(s) with  dma_buf_map_attachment()).. but
1093          * really we want to do this just before the DMA, not while queueing
1094          * the buffer(s)..
1095          */
1096         for (plane = 0; plane < vb->num_planes; ++plane) {
1097                 ret = call_memop(q, map_dmabuf, vb->planes[plane].mem_priv);
1098                 if (ret) {
1099                         dprintk(1, "qbuf: failed mapping dmabuf "
1100                                 "memory for plane %d\n", plane);
1101                         goto err;
1102                 }
1103                 vb->planes[plane].dbuf_mapped = 1;
1104         }
1105
1106         /*
1107          * Call driver-specific initialization on the newly acquired buffer,
1108          * if provided.
1109          */
1110         ret = call_qop(q, buf_init, vb);
1111         if (ret) {
1112                 dprintk(1, "qbuf: buffer initialization failed\n");
1113                 goto err;
1114         }
1115
1116         /*
1117          * Now that everything is in order, copy relevant information
1118          * provided by userspace.
1119          */
1120         for (plane = 0; plane < vb->num_planes; ++plane)
1121                 vb->v4l2_planes[plane] = planes[plane];
1122
1123         return 0;
1124 err:
1125         /* In case of errors, release planes that were already acquired */
1126         __vb2_buf_dmabuf_put(vb);
1127
1128         return ret;
1129 }
1130
1131 /**
1132  * __enqueue_in_driver() - enqueue a vb2_buffer in driver for processing
1133  */
1134 static void __enqueue_in_driver(struct vb2_buffer *vb)
1135 {
1136         struct vb2_queue *q = vb->vb2_queue;
1137
1138         vb->state = VB2_BUF_STATE_ACTIVE;
1139         atomic_inc(&q->queued_count);
1140         q->ops->buf_queue(vb);
1141 }
1142
1143 static int __buf_prepare(struct vb2_buffer *vb, const struct v4l2_buffer *b)
1144 {
1145         struct vb2_queue *q = vb->vb2_queue;
1146         int ret;
1147
1148         switch (q->memory) {
1149         case V4L2_MEMORY_MMAP:
1150                 ret = __qbuf_mmap(vb, b);
1151                 break;
1152         case V4L2_MEMORY_USERPTR:
1153                 ret = __qbuf_userptr(vb, b);
1154                 break;
1155         case V4L2_MEMORY_DMABUF:
1156                 ret = __qbuf_dmabuf(vb, b);
1157                 break;
1158         default:
1159                 WARN(1, "Invalid queue type\n");
1160                 ret = -EINVAL;
1161         }
1162
1163         if (!ret)
1164                 ret = call_qop(q, buf_prepare, vb);
1165         if (ret)
1166                 dprintk(1, "qbuf: buffer preparation failed: %d\n", ret);
1167         else
1168                 vb->state = VB2_BUF_STATE_PREPARED;
1169
1170         return ret;
1171 }
1172
1173 /**
1174  * vb2_prepare_buf() - Pass ownership of a buffer from userspace to the kernel
1175  * @q:          videobuf2 queue
1176  * @b:          buffer structure passed from userspace to vidioc_prepare_buf
1177  *              handler in driver
1178  *
1179  * Should be called from vidioc_prepare_buf ioctl handler of a driver.
1180  * This function:
1181  * 1) verifies the passed buffer,
1182  * 2) calls buf_prepare callback in the driver (if provided), in which
1183  *    driver-specific buffer initialization can be performed,
1184  *
1185  * The return values from this function are intended to be directly returned
1186  * from vidioc_prepare_buf handler in driver.
1187  */
1188 int vb2_prepare_buf(struct vb2_queue *q, struct v4l2_buffer *b)
1189 {
1190         struct vb2_buffer *vb;
1191         int ret;
1192
1193         if (q->fileio) {
1194                 dprintk(1, "%s(): file io in progress\n", __func__);
1195                 return -EBUSY;
1196         }
1197
1198         if (b->type != q->type) {
1199                 dprintk(1, "%s(): invalid buffer type\n", __func__);
1200                 return -EINVAL;
1201         }
1202
1203         if (b->index >= q->num_buffers) {
1204                 dprintk(1, "%s(): buffer index out of range\n", __func__);
1205                 return -EINVAL;
1206         }
1207
1208         vb = q->bufs[b->index];
1209         if (NULL == vb) {
1210                 /* Should never happen */
1211                 dprintk(1, "%s(): buffer is NULL\n", __func__);
1212                 return -EINVAL;
1213         }
1214
1215         if (b->memory != q->memory) {
1216                 dprintk(1, "%s(): invalid memory type\n", __func__);
1217                 return -EINVAL;
1218         }
1219
1220         if (vb->state != VB2_BUF_STATE_DEQUEUED) {
1221                 dprintk(1, "%s(): invalid buffer state %d\n", __func__, vb->state);
1222                 return -EINVAL;
1223         }
1224
1225         ret = __buf_prepare(vb, b);
1226         if (ret < 0)
1227                 return ret;
1228
1229         __fill_v4l2_buffer(vb, b);
1230
1231         return 0;
1232 }
1233 EXPORT_SYMBOL_GPL(vb2_prepare_buf);
1234
1235 /**
1236  * vb2_qbuf() - Queue a buffer from userspace
1237  * @q:          videobuf2 queue
1238  * @b:          buffer structure passed from userspace to vidioc_qbuf handler
1239  *              in driver
1240  *
1241  * Should be called from vidioc_qbuf ioctl handler of a driver.
1242  * This function:
1243  * 1) verifies the passed buffer,
1244  * 2) if necessary, calls buf_prepare callback in the driver (if provided), in
1245  *    which driver-specific buffer initialization can be performed,
1246  * 3) if streaming is on, queues the buffer in driver by the means of buf_queue
1247  *    callback for processing.
1248  *
1249  * The return values from this function are intended to be directly returned
1250  * from vidioc_qbuf handler in driver.
1251  */
1252 int vb2_qbuf(struct vb2_queue *q, struct v4l2_buffer *b)
1253 {
1254         struct rw_semaphore *mmap_sem = NULL;
1255         struct vb2_buffer *vb;
1256         int ret = 0;
1257
1258         /*
1259          * In case of user pointer buffers vb2 allocator needs to get direct
1260          * access to userspace pages. This requires getting read access on
1261          * mmap semaphore in the current process structure. The same
1262          * semaphore is taken before calling mmap operation, while both mmap
1263          * and qbuf are called by the driver or v4l2 core with driver's lock
1264          * held. To avoid a AB-BA deadlock (mmap_sem then driver's lock in
1265          * mmap and driver's lock then mmap_sem in qbuf) the videobuf2 core
1266          * release driver's lock, takes mmap_sem and then takes again driver's
1267          * lock.
1268          *
1269          * To avoid race with other vb2 calls, which might be called after
1270          * releasing driver's lock, this operation is performed at the
1271          * beggining of qbuf processing. This way the queue status is
1272          * consistent after getting driver's lock back.
1273          */
1274         if (q->memory == V4L2_MEMORY_USERPTR) {
1275                 mmap_sem = &current->mm->mmap_sem;
1276                 call_qop(q, wait_prepare, q);
1277                 down_read(mmap_sem);
1278                 call_qop(q, wait_finish, q);
1279         }
1280
1281         if (q->fileio) {
1282                 dprintk(1, "qbuf: file io in progress\n");
1283                 ret = -EBUSY;
1284                 goto unlock;
1285         }
1286
1287         if (b->type != q->type) {
1288                 dprintk(1, "qbuf: invalid buffer type\n");
1289                 ret = -EINVAL;
1290                 goto unlock;
1291         }
1292
1293         if (b->index >= q->num_buffers) {
1294                 dprintk(1, "qbuf: buffer index out of range\n");
1295                 ret = -EINVAL;
1296                 goto unlock;
1297         }
1298
1299         vb = q->bufs[b->index];
1300         if (NULL == vb) {
1301                 /* Should never happen */
1302                 dprintk(1, "qbuf: buffer is NULL\n");
1303                 ret = -EINVAL;
1304                 goto unlock;
1305         }
1306
1307         if (b->memory != q->memory) {
1308                 dprintk(1, "qbuf: invalid memory type\n");
1309                 ret = -EINVAL;
1310                 goto unlock;
1311         }
1312
1313         switch (vb->state) {
1314         case VB2_BUF_STATE_DEQUEUED:
1315                 ret = __buf_prepare(vb, b);
1316                 if (ret)
1317                         goto unlock;
1318         case VB2_BUF_STATE_PREPARED:
1319                 break;
1320         default:
1321                 dprintk(1, "qbuf: buffer already in use\n");
1322                 ret = -EINVAL;
1323                 goto unlock;
1324         }
1325
1326         /*
1327          * Add to the queued buffers list, a buffer will stay on it until
1328          * dequeued in dqbuf.
1329          */
1330         list_add_tail(&vb->queued_entry, &q->queued_list);
1331         vb->state = VB2_BUF_STATE_QUEUED;
1332
1333         /*
1334          * If already streaming, give the buffer to driver for processing.
1335          * If not, the buffer will be given to driver on next streamon.
1336          */
1337         if (q->streaming)
1338                 __enqueue_in_driver(vb);
1339
1340         /* Fill buffer information for the userspace */
1341         __fill_v4l2_buffer(vb, b);
1342
1343         dprintk(1, "qbuf of buffer %d succeeded\n", vb->v4l2_buf.index);
1344 unlock:
1345         if (mmap_sem)
1346                 up_read(mmap_sem);
1347         return ret;
1348 }
1349 EXPORT_SYMBOL_GPL(vb2_qbuf);
1350
1351 /**
1352  * __vb2_wait_for_done_vb() - wait for a buffer to become available
1353  * for dequeuing
1354  *
1355  * Will sleep if required for nonblocking == false.
1356  */
1357 static int __vb2_wait_for_done_vb(struct vb2_queue *q, int nonblocking)
1358 {
1359         /*
1360          * All operations on vb_done_list are performed under done_lock
1361          * spinlock protection. However, buffers may be removed from
1362          * it and returned to userspace only while holding both driver's
1363          * lock and the done_lock spinlock. Thus we can be sure that as
1364          * long as we hold the driver's lock, the list will remain not
1365          * empty if list_empty() check succeeds.
1366          */
1367
1368         for (;;) {
1369                 int ret;
1370
1371                 if (!q->streaming) {
1372                         dprintk(1, "Streaming off, will not wait for buffers\n");
1373                         return -EINVAL;
1374                 }
1375
1376                 if (!list_empty(&q->done_list)) {
1377                         /*
1378                          * Found a buffer that we were waiting for.
1379                          */
1380                         break;
1381                 }
1382
1383                 if (nonblocking) {
1384                         dprintk(1, "Nonblocking and no buffers to dequeue, "
1385                                                                 "will not wait\n");
1386                         return -EAGAIN;
1387                 }
1388
1389                 /*
1390                  * We are streaming and blocking, wait for another buffer to
1391                  * become ready or for streamoff. Driver's lock is released to
1392                  * allow streamoff or qbuf to be called while waiting.
1393                  */
1394                 call_qop(q, wait_prepare, q);
1395
1396                 /*
1397                  * All locks have been released, it is safe to sleep now.
1398                  */
1399                 dprintk(3, "Will sleep waiting for buffers\n");
1400                 ret = wait_event_interruptible(q->done_wq,
1401                                 !list_empty(&q->done_list) || !q->streaming);
1402
1403                 /*
1404                  * We need to reevaluate both conditions again after reacquiring
1405                  * the locks or return an error if one occurred.
1406                  */
1407                 call_qop(q, wait_finish, q);
1408                 if (ret)
1409                         return ret;
1410         }
1411         return 0;
1412 }
1413
1414 /**
1415  * __vb2_get_done_vb() - get a buffer ready for dequeuing
1416  *
1417  * Will sleep if required for nonblocking == false.
1418  */
1419 static int __vb2_get_done_vb(struct vb2_queue *q, struct vb2_buffer **vb,
1420                                 int nonblocking)
1421 {
1422         unsigned long flags;
1423         int ret;
1424
1425         /*
1426          * Wait for at least one buffer to become available on the done_list.
1427          */
1428         ret = __vb2_wait_for_done_vb(q, nonblocking);
1429         if (ret)
1430                 return ret;
1431
1432         /*
1433          * Driver's lock has been held since we last verified that done_list
1434          * is not empty, so no need for another list_empty(done_list) check.
1435          */
1436         spin_lock_irqsave(&q->done_lock, flags);
1437         *vb = list_first_entry(&q->done_list, struct vb2_buffer, done_entry);
1438         list_del(&(*vb)->done_entry);
1439         spin_unlock_irqrestore(&q->done_lock, flags);
1440
1441         return 0;
1442 }
1443
1444 /**
1445  * vb2_wait_for_all_buffers() - wait until all buffers are given back to vb2
1446  * @q:          videobuf2 queue
1447  *
1448  * This function will wait until all buffers that have been given to the driver
1449  * by buf_queue() are given back to vb2 with vb2_buffer_done(). It doesn't call
1450  * wait_prepare, wait_finish pair. It is intended to be called with all locks
1451  * taken, for example from stop_streaming() callback.
1452  */
1453 int vb2_wait_for_all_buffers(struct vb2_queue *q)
1454 {
1455         if (!q->streaming) {
1456                 dprintk(1, "Streaming off, will not wait for buffers\n");
1457                 return -EINVAL;
1458         }
1459
1460         wait_event(q->done_wq, !atomic_read(&q->queued_count));
1461         return 0;
1462 }
1463 EXPORT_SYMBOL_GPL(vb2_wait_for_all_buffers);
1464
1465 /**
1466  * vb2_dqbuf() - Dequeue a buffer to the userspace
1467  * @q:          videobuf2 queue
1468  * @b:          buffer structure passed from userspace to vidioc_dqbuf handler
1469  *              in driver
1470  * @nonblocking: if true, this call will not sleep waiting for a buffer if no
1471  *               buffers ready for dequeuing are present. Normally the driver
1472  *               would be passing (file->f_flags & O_NONBLOCK) here
1473  *
1474  * Should be called from vidioc_dqbuf ioctl handler of a driver.
1475  * This function:
1476  * 1) verifies the passed buffer,
1477  * 2) calls buf_finish callback in the driver (if provided), in which
1478  *    driver can perform any additional operations that may be required before
1479  *    returning the buffer to userspace, such as cache sync,
1480  * 3) the buffer struct members are filled with relevant information for
1481  *    the userspace.
1482  *
1483  * The return values from this function are intended to be directly returned
1484  * from vidioc_dqbuf handler in driver.
1485  */
1486 int vb2_dqbuf(struct vb2_queue *q, struct v4l2_buffer *b, bool nonblocking)
1487 {
1488         struct vb2_buffer *vb = NULL;
1489         int ret;
1490
1491         if (q->fileio) {
1492                 dprintk(1, "dqbuf: file io in progress\n");
1493                 return -EBUSY;
1494         }
1495
1496         if (b->type != q->type) {
1497                 dprintk(1, "dqbuf: invalid buffer type\n");
1498                 return -EINVAL;
1499         }
1500
1501         ret = __vb2_get_done_vb(q, &vb, nonblocking);
1502         if (ret < 0) {
1503                 dprintk(1, "dqbuf: error getting next done buffer\n");
1504                 return ret;
1505         }
1506
1507         ret = call_qop(q, buf_finish, vb);
1508         if (ret) {
1509                 dprintk(1, "dqbuf: buffer finish failed\n");
1510                 return ret;
1511         }
1512
1513         /* TODO: this unpins the buffer(dma_buf_unmap_attachment()).. but
1514          * really we want to do this just after DMA, not when the
1515          * buffer is dequeued..
1516          */
1517         if (q->memory == V4L2_MEMORY_DMABUF) {
1518                 unsigned int i;
1519
1520                 for (i = 0; i < vb->num_planes; ++i) {
1521                         call_memop(q, unmap_dmabuf, vb->planes[i].mem_priv);
1522                         vb->planes[i].dbuf_mapped = 0;
1523                 }
1524         }
1525
1526         switch (vb->state) {
1527         case VB2_BUF_STATE_DONE:
1528                 dprintk(3, "dqbuf: Returning done buffer\n");
1529                 break;
1530         case VB2_BUF_STATE_ERROR:
1531                 dprintk(3, "dqbuf: Returning done buffer with errors\n");
1532                 break;
1533         default:
1534                 dprintk(1, "dqbuf: Invalid buffer state\n");
1535                 return -EINVAL;
1536         }
1537
1538         /* Fill buffer information for the userspace */
1539         __fill_v4l2_buffer(vb, b);
1540         /* Remove from videobuf queue */
1541         list_del(&vb->queued_entry);
1542
1543         dprintk(1, "dqbuf of buffer %d, with state %d\n",
1544                         vb->v4l2_buf.index, vb->state);
1545
1546         vb->state = VB2_BUF_STATE_DEQUEUED;
1547         return 0;
1548 }
1549 EXPORT_SYMBOL_GPL(vb2_dqbuf);
1550
1551 /**
1552  * __vb2_queue_cancel() - cancel and stop (pause) streaming
1553  *
1554  * Removes all queued buffers from driver's queue and all buffers queued by
1555  * userspace from videobuf's queue. Returns to state after reqbufs.
1556  */
1557 static void __vb2_queue_cancel(struct vb2_queue *q)
1558 {
1559         unsigned int i;
1560
1561         /*
1562          * Tell driver to stop all transactions and release all queued
1563          * buffers.
1564          */
1565         if (q->streaming)
1566                 call_qop(q, stop_streaming, q);
1567         q->streaming = 0;
1568
1569         /*
1570          * Remove all buffers from videobuf's list...
1571          */
1572         INIT_LIST_HEAD(&q->queued_list);
1573         /*
1574          * ...and done list; userspace will not receive any buffers it
1575          * has not already dequeued before initiating cancel.
1576          */
1577         INIT_LIST_HEAD(&q->done_list);
1578         atomic_set(&q->queued_count, 0);
1579         wake_up_all(&q->done_wq);
1580
1581         /*
1582          * Reinitialize all buffers for next use.
1583          */
1584         for (i = 0; i < q->num_buffers; ++i)
1585                 q->bufs[i]->state = VB2_BUF_STATE_DEQUEUED;
1586 }
1587
1588 /**
1589  * vb2_streamon - start streaming
1590  * @q:          videobuf2 queue
1591  * @type:       type argument passed from userspace to vidioc_streamon handler
1592  *
1593  * Should be called from vidioc_streamon handler of a driver.
1594  * This function:
1595  * 1) verifies current state
1596  * 2) passes any previously queued buffers to the driver and starts streaming
1597  *
1598  * The return values from this function are intended to be directly returned
1599  * from vidioc_streamon handler in the driver.
1600  */
1601 int vb2_streamon(struct vb2_queue *q, enum v4l2_buf_type type)
1602 {
1603         struct vb2_buffer *vb;
1604         int ret;
1605
1606         if (q->fileio) {
1607                 dprintk(1, "streamon: file io in progress\n");
1608                 return -EBUSY;
1609         }
1610
1611         if (type != q->type) {
1612                 dprintk(1, "streamon: invalid stream type\n");
1613                 return -EINVAL;
1614         }
1615
1616         if (q->streaming) {
1617                 dprintk(1, "streamon: already streaming\n");
1618                 return -EBUSY;
1619         }
1620
1621         /*
1622          * If any buffers were queued before streamon,
1623          * we can now pass them to driver for processing.
1624          */
1625         list_for_each_entry(vb, &q->queued_list, queued_entry)
1626                 __enqueue_in_driver(vb);
1627
1628         /*
1629          * Let driver notice that streaming state has been enabled.
1630          */
1631         ret = call_qop(q, start_streaming, q, atomic_read(&q->queued_count));
1632         if (ret) {
1633                 dprintk(1, "streamon: driver refused to start streaming\n");
1634                 __vb2_queue_cancel(q);
1635                 return ret;
1636         }
1637
1638         q->streaming = 1;
1639
1640         dprintk(3, "Streamon successful\n");
1641         return 0;
1642 }
1643 EXPORT_SYMBOL_GPL(vb2_streamon);
1644
1645
1646 /**
1647  * vb2_streamoff - stop streaming
1648  * @q:          videobuf2 queue
1649  * @type:       type argument passed from userspace to vidioc_streamoff handler
1650  *
1651  * Should be called from vidioc_streamoff handler of a driver.
1652  * This function:
1653  * 1) verifies current state,
1654  * 2) stop streaming and dequeues any queued buffers, including those previously
1655  *    passed to the driver (after waiting for the driver to finish).
1656  *
1657  * This call can be used for pausing playback.
1658  * The return values from this function are intended to be directly returned
1659  * from vidioc_streamoff handler in the driver
1660  */
1661 int vb2_streamoff(struct vb2_queue *q, enum v4l2_buf_type type)
1662 {
1663         if (q->fileio) {
1664                 dprintk(1, "streamoff: file io in progress\n");
1665                 return -EBUSY;
1666         }
1667
1668         if (type != q->type) {
1669                 dprintk(1, "streamoff: invalid stream type\n");
1670                 return -EINVAL;
1671         }
1672
1673         if (!q->streaming) {
1674                 dprintk(1, "streamoff: not streaming\n");
1675                 return -EINVAL;
1676         }
1677
1678         /*
1679          * Cancel will pause streaming and remove all buffers from the driver
1680          * and videobuf, effectively returning control over them to userspace.
1681          */
1682         __vb2_queue_cancel(q);
1683
1684         dprintk(3, "Streamoff successful\n");
1685         return 0;
1686 }
1687 EXPORT_SYMBOL_GPL(vb2_streamoff);
1688
1689 /**
1690  * __find_plane_by_offset() - find plane associated with the given offset off
1691  */
1692 static int __find_plane_by_offset(struct vb2_queue *q, unsigned long off,
1693                         unsigned int *_buffer, unsigned int *_plane)
1694 {
1695         struct vb2_buffer *vb;
1696         unsigned int buffer, plane;
1697
1698         /*
1699          * Go over all buffers and their planes, comparing the given offset
1700          * with an offset assigned to each plane. If a match is found,
1701          * return its buffer and plane numbers.
1702          */
1703         for (buffer = 0; buffer < q->num_buffers; ++buffer) {
1704                 vb = q->bufs[buffer];
1705
1706                 for (plane = 0; plane < vb->num_planes; ++plane) {
1707                         if (vb->v4l2_planes[plane].m.mem_offset == off) {
1708                                 *_buffer = buffer;
1709                                 *_plane = plane;
1710                                 return 0;
1711                         }
1712                 }
1713         }
1714
1715         return -EINVAL;
1716 }
1717
1718 /**
1719  * vb2_mmap() - map video buffers into application address space
1720  * @q:          videobuf2 queue
1721  * @vma:        vma passed to the mmap file operation handler in the driver
1722  *
1723  * Should be called from mmap file operation handler of a driver.
1724  * This function maps one plane of one of the available video buffers to
1725  * userspace. To map whole video memory allocated on reqbufs, this function
1726  * has to be called once per each plane per each buffer previously allocated.
1727  *
1728  * When the userspace application calls mmap, it passes to it an offset returned
1729  * to it earlier by the means of vidioc_querybuf handler. That offset acts as
1730  * a "cookie", which is then used to identify the plane to be mapped.
1731  * This function finds a plane with a matching offset and a mapping is performed
1732  * by the means of a provided memory operation.
1733  *
1734  * The return values from this function are intended to be directly returned
1735  * from the mmap handler in driver.
1736  */
1737 int vb2_mmap(struct vb2_queue *q, struct vm_area_struct *vma)
1738 {
1739         unsigned long off = vma->vm_pgoff << PAGE_SHIFT;
1740         struct vb2_buffer *vb;
1741         unsigned int buffer, plane;
1742         int ret;
1743
1744         if (q->memory != V4L2_MEMORY_MMAP) {
1745                 dprintk(1, "Queue is not currently set up for mmap\n");
1746                 return -EINVAL;
1747         }
1748
1749         /*
1750          * Check memory area access mode.
1751          */
1752         if (!(vma->vm_flags & VM_SHARED)) {
1753                 dprintk(1, "Invalid vma flags, VM_SHARED needed\n");
1754                 return -EINVAL;
1755         }
1756         if (V4L2_TYPE_IS_OUTPUT(q->type)) {
1757                 if (!(vma->vm_flags & VM_WRITE)) {
1758                         dprintk(1, "Invalid vma flags, VM_WRITE needed\n");
1759                         return -EINVAL;
1760                 }
1761         } else {
1762                 if (!(vma->vm_flags & VM_READ)) {
1763                         dprintk(1, "Invalid vma flags, VM_READ needed\n");
1764                         return -EINVAL;
1765                 }
1766         }
1767
1768         /*
1769          * Find the plane corresponding to the offset passed by userspace.
1770          */
1771         ret = __find_plane_by_offset(q, off, &buffer, &plane);
1772         if (ret)
1773                 return ret;
1774
1775         vb = q->bufs[buffer];
1776
1777         ret = call_memop(q, mmap, vb->planes[plane].mem_priv, vma);
1778         if (ret)
1779                 return ret;
1780
1781         dprintk(3, "Buffer %d, plane %d successfully mapped\n", buffer, plane);
1782         return 0;
1783 }
1784 EXPORT_SYMBOL_GPL(vb2_mmap);
1785
1786 #ifndef CONFIG_MMU
1787 unsigned long vb2_get_unmapped_area(struct vb2_queue *q,
1788                                     unsigned long addr,
1789                                     unsigned long len,
1790                                     unsigned long pgoff,
1791                                     unsigned long flags)
1792 {
1793         unsigned long off = pgoff << PAGE_SHIFT;
1794         struct vb2_buffer *vb;
1795         unsigned int buffer, plane;
1796         int ret;
1797
1798         if (q->memory != V4L2_MEMORY_MMAP) {
1799                 dprintk(1, "Queue is not currently set up for mmap\n");
1800                 return -EINVAL;
1801         }
1802
1803         /*
1804          * Find the plane corresponding to the offset passed by userspace.
1805          */
1806         ret = __find_plane_by_offset(q, off, &buffer, &plane);
1807         if (ret)
1808                 return ret;
1809
1810         vb = q->bufs[buffer];
1811
1812         return (unsigned long)vb2_plane_vaddr(vb, plane);
1813 }
1814 EXPORT_SYMBOL_GPL(vb2_get_unmapped_area);
1815 #endif
1816
1817 static int __vb2_init_fileio(struct vb2_queue *q, int read);
1818 static int __vb2_cleanup_fileio(struct vb2_queue *q);
1819
1820 /**
1821  * vb2_poll() - implements poll userspace operation
1822  * @q:          videobuf2 queue
1823  * @file:       file argument passed to the poll file operation handler
1824  * @wait:       wait argument passed to the poll file operation handler
1825  *
1826  * This function implements poll file operation handler for a driver.
1827  * For CAPTURE queues, if a buffer is ready to be dequeued, the userspace will
1828  * be informed that the file descriptor of a video device is available for
1829  * reading.
1830  * For OUTPUT queues, if a buffer is ready to be dequeued, the file descriptor
1831  * will be reported as available for writing.
1832  *
1833  * The return values from this function are intended to be directly returned
1834  * from poll handler in driver.
1835  */
1836 unsigned int vb2_poll(struct vb2_queue *q, struct file *file, poll_table *wait)
1837 {
1838         unsigned long flags;
1839         unsigned int ret;
1840         struct vb2_buffer *vb = NULL;
1841
1842         /*
1843          * Start file I/O emulator only if streaming API has not been used yet.
1844          */
1845         if (q->num_buffers == 0 && q->fileio == NULL) {
1846                 if (!V4L2_TYPE_IS_OUTPUT(q->type) && (q->io_modes & VB2_READ)) {
1847                         ret = __vb2_init_fileio(q, 1);
1848                         if (ret)
1849                                 return POLLERR;
1850                 }
1851                 if (V4L2_TYPE_IS_OUTPUT(q->type) && (q->io_modes & VB2_WRITE)) {
1852                         ret = __vb2_init_fileio(q, 0);
1853                         if (ret)
1854                                 return POLLERR;
1855                         /*
1856                          * Write to OUTPUT queue can be done immediately.
1857                          */
1858                         return POLLOUT | POLLWRNORM;
1859                 }
1860         }
1861
1862         /*
1863          * There is nothing to wait for if no buffers have already been queued.
1864          */
1865         if (list_empty(&q->queued_list))
1866                 return POLLERR;
1867
1868         poll_wait(file, &q->done_wq, wait);
1869
1870         /*
1871          * Take first buffer available for dequeuing.
1872          */
1873         spin_lock_irqsave(&q->done_lock, flags);
1874         if (!list_empty(&q->done_list))
1875                 vb = list_first_entry(&q->done_list, struct vb2_buffer,
1876                                         done_entry);
1877         spin_unlock_irqrestore(&q->done_lock, flags);
1878
1879         if (vb && (vb->state == VB2_BUF_STATE_DONE
1880                         || vb->state == VB2_BUF_STATE_ERROR)) {
1881                 return (V4L2_TYPE_IS_OUTPUT(q->type)) ? POLLOUT | POLLWRNORM :
1882                         POLLIN | POLLRDNORM;
1883         }
1884         return 0;
1885 }
1886 EXPORT_SYMBOL_GPL(vb2_poll);
1887
1888 /**
1889  * vb2_queue_init() - initialize a videobuf2 queue
1890  * @q:          videobuf2 queue; this structure should be allocated in driver
1891  *
1892  * The vb2_queue structure should be allocated by the driver. The driver is
1893  * responsible of clearing it's content and setting initial values for some
1894  * required entries before calling this function.
1895  * q->ops, q->mem_ops, q->type and q->io_modes are mandatory. Please refer
1896  * to the struct vb2_queue description in include/media/videobuf2-core.h
1897  * for more information.
1898  */
1899 int vb2_queue_init(struct vb2_queue *q)
1900 {
1901         BUG_ON(!q);
1902         BUG_ON(!q->ops);
1903         BUG_ON(!q->mem_ops);
1904         BUG_ON(!q->type);
1905         BUG_ON(!q->io_modes);
1906
1907         BUG_ON(!q->ops->queue_setup);
1908         BUG_ON(!q->ops->buf_queue);
1909
1910         INIT_LIST_HEAD(&q->queued_list);
1911         INIT_LIST_HEAD(&q->done_list);
1912         spin_lock_init(&q->done_lock);
1913         init_waitqueue_head(&q->done_wq);
1914
1915         if (q->buf_struct_size == 0)
1916                 q->buf_struct_size = sizeof(struct vb2_buffer);
1917
1918         return 0;
1919 }
1920 EXPORT_SYMBOL_GPL(vb2_queue_init);
1921
1922 /**
1923  * vb2_queue_release() - stop streaming, release the queue and free memory
1924  * @q:          videobuf2 queue
1925  *
1926  * This function stops streaming and performs necessary clean ups, including
1927  * freeing video buffer memory. The driver is responsible for freeing
1928  * the vb2_queue structure itself.
1929  */
1930 void vb2_queue_release(struct vb2_queue *q)
1931 {
1932         __vb2_cleanup_fileio(q);
1933         __vb2_queue_cancel(q);
1934         __vb2_queue_free(q, q->num_buffers);
1935 }
1936 EXPORT_SYMBOL_GPL(vb2_queue_release);
1937
1938 /**
1939  * struct vb2_fileio_buf - buffer context used by file io emulator
1940  *
1941  * vb2 provides a compatibility layer and emulator of file io (read and
1942  * write) calls on top of streaming API. This structure is used for
1943  * tracking context related to the buffers.
1944  */
1945 struct vb2_fileio_buf {
1946         void *vaddr;
1947         unsigned int size;
1948         unsigned int pos;
1949         unsigned int queued:1;
1950 };
1951
1952 /**
1953  * struct vb2_fileio_data - queue context used by file io emulator
1954  *
1955  * vb2 provides a compatibility layer and emulator of file io (read and
1956  * write) calls on top of streaming API. For proper operation it required
1957  * this structure to save the driver state between each call of the read
1958  * or write function.
1959  */
1960 struct vb2_fileio_data {
1961         struct v4l2_requestbuffers req;
1962         struct v4l2_buffer b;
1963         struct vb2_fileio_buf bufs[VIDEO_MAX_FRAME];
1964         unsigned int index;
1965         unsigned int q_count;
1966         unsigned int dq_count;
1967         unsigned int flags;
1968 };
1969
1970 /**
1971  * __vb2_init_fileio() - initialize file io emulator
1972  * @q:          videobuf2 queue
1973  * @read:       mode selector (1 means read, 0 means write)
1974  */
1975 static int __vb2_init_fileio(struct vb2_queue *q, int read)
1976 {
1977         struct vb2_fileio_data *fileio;
1978         int i, ret;
1979         unsigned int count = 0;
1980
1981         /*
1982          * Sanity check
1983          */
1984         if ((read && !(q->io_modes & VB2_READ)) ||
1985            (!read && !(q->io_modes & VB2_WRITE)))
1986                 BUG();
1987
1988         /*
1989          * Check if device supports mapping buffers to kernel virtual space.
1990          */
1991         if (!q->mem_ops->vaddr)
1992                 return -EBUSY;
1993
1994         /*
1995          * Check if streaming api has not been already activated.
1996          */
1997         if (q->streaming || q->num_buffers > 0)
1998                 return -EBUSY;
1999
2000         /*
2001          * Start with count 1, driver can increase it in queue_setup()
2002          */
2003         count = 1;
2004
2005         dprintk(3, "setting up file io: mode %s, count %d, flags %08x\n",
2006                 (read) ? "read" : "write", count, q->io_flags);
2007
2008         fileio = kzalloc(sizeof(struct vb2_fileio_data), GFP_KERNEL);
2009         if (fileio == NULL)
2010                 return -ENOMEM;
2011
2012         fileio->flags = q->io_flags;
2013
2014         /*
2015          * Request buffers and use MMAP type to force driver
2016          * to allocate buffers by itself.
2017          */
2018         fileio->req.count = count;
2019         fileio->req.memory = V4L2_MEMORY_MMAP;
2020         fileio->req.type = q->type;
2021         ret = vb2_reqbufs(q, &fileio->req);
2022         if (ret)
2023                 goto err_kfree;
2024
2025         /*
2026          * Check if plane_count is correct
2027          * (multiplane buffers are not supported).
2028          */
2029         if (q->bufs[0]->num_planes != 1) {
2030                 fileio->req.count = 0;
2031                 ret = -EBUSY;
2032                 goto err_reqbufs;
2033         }
2034
2035         /*
2036          * Get kernel address of each buffer.
2037          */
2038         for (i = 0; i < q->num_buffers; i++) {
2039                 fileio->bufs[i].vaddr = vb2_plane_vaddr(q->bufs[i], 0);
2040                 if (fileio->bufs[i].vaddr == NULL)
2041                         goto err_reqbufs;
2042                 fileio->bufs[i].size = vb2_plane_size(q->bufs[i], 0);
2043         }
2044
2045         /*
2046          * Read mode requires pre queuing of all buffers.
2047          */
2048         if (read) {
2049                 /*
2050                  * Queue all buffers.
2051                  */
2052                 for (i = 0; i < q->num_buffers; i++) {
2053                         struct v4l2_buffer *b = &fileio->b;
2054                         memset(b, 0, sizeof(*b));
2055                         b->type = q->type;
2056                         b->memory = q->memory;
2057                         b->index = i;
2058                         ret = vb2_qbuf(q, b);
2059                         if (ret)
2060                                 goto err_reqbufs;
2061                         fileio->bufs[i].queued = 1;
2062                 }
2063
2064                 /*
2065                  * Start streaming.
2066                  */
2067                 ret = vb2_streamon(q, q->type);
2068                 if (ret)
2069                         goto err_reqbufs;
2070         }
2071
2072         q->fileio = fileio;
2073
2074         return ret;
2075
2076 err_reqbufs:
2077         vb2_reqbufs(q, &fileio->req);
2078
2079 err_kfree:
2080         kfree(fileio);
2081         return ret;
2082 }
2083
2084 /**
2085  * __vb2_cleanup_fileio() - free resourced used by file io emulator
2086  * @q:          videobuf2 queue
2087  */
2088 static int __vb2_cleanup_fileio(struct vb2_queue *q)
2089 {
2090         struct vb2_fileio_data *fileio = q->fileio;
2091
2092         if (fileio) {
2093                 /*
2094                  * Hack fileio context to enable direct calls to vb2 ioctl
2095                  * interface.
2096                  */
2097                 q->fileio = NULL;
2098
2099                 vb2_streamoff(q, q->type);
2100                 fileio->req.count = 0;
2101                 vb2_reqbufs(q, &fileio->req);
2102                 kfree(fileio);
2103                 dprintk(3, "file io emulator closed\n");
2104         }
2105         return 0;
2106 }
2107
2108 /**
2109  * __vb2_perform_fileio() - perform a single file io (read or write) operation
2110  * @q:          videobuf2 queue
2111  * @data:       pointed to target userspace buffer
2112  * @count:      number of bytes to read or write
2113  * @ppos:       file handle position tracking pointer
2114  * @nonblock:   mode selector (1 means blocking calls, 0 means nonblocking)
2115  * @read:       access mode selector (1 means read, 0 means write)
2116  */
2117 static size_t __vb2_perform_fileio(struct vb2_queue *q, char __user *data, size_t count,
2118                 loff_t *ppos, int nonblock, int read)
2119 {
2120         struct vb2_fileio_data *fileio;
2121         struct vb2_fileio_buf *buf;
2122         int ret, index;
2123
2124         dprintk(3, "file io: mode %s, offset %ld, count %zd, %sblocking\n",
2125                 read ? "read" : "write", (long)*ppos, count,
2126                 nonblock ? "non" : "");
2127
2128         if (!data)
2129                 return -EINVAL;
2130
2131         /*
2132          * Initialize emulator on first call.
2133          */
2134         if (!q->fileio) {
2135                 ret = __vb2_init_fileio(q, read);
2136                 dprintk(3, "file io: vb2_init_fileio result: %d\n", ret);
2137                 if (ret)
2138                         return ret;
2139         }
2140         fileio = q->fileio;
2141
2142         /*
2143          * Hack fileio context to enable direct calls to vb2 ioctl interface.
2144          * The pointer will be restored before returning from this function.
2145          */
2146         q->fileio = NULL;
2147
2148         index = fileio->index;
2149         buf = &fileio->bufs[index];
2150
2151         /*
2152          * Check if we need to dequeue the buffer.
2153          */
2154         if (buf->queued) {
2155                 struct vb2_buffer *vb;
2156
2157                 /*
2158                  * Call vb2_dqbuf to get buffer back.
2159                  */
2160                 memset(&fileio->b, 0, sizeof(fileio->b));
2161                 fileio->b.type = q->type;
2162                 fileio->b.memory = q->memory;
2163                 fileio->b.index = index;
2164                 ret = vb2_dqbuf(q, &fileio->b, nonblock);
2165                 dprintk(5, "file io: vb2_dqbuf result: %d\n", ret);
2166                 if (ret)
2167                         goto end;
2168                 fileio->dq_count += 1;
2169
2170                 /*
2171                  * Get number of bytes filled by the driver
2172                  */
2173                 vb = q->bufs[index];
2174                 buf->size = vb2_get_plane_payload(vb, 0);
2175                 buf->queued = 0;
2176         }
2177
2178         /*
2179          * Limit count on last few bytes of the buffer.
2180          */
2181         if (buf->pos + count > buf->size) {
2182                 count = buf->size - buf->pos;
2183                 dprintk(5, "reducing read count: %zd\n", count);
2184         }
2185
2186         /*
2187          * Transfer data to userspace.
2188          */
2189         dprintk(3, "file io: copying %zd bytes - buffer %d, offset %u\n",
2190                 count, index, buf->pos);
2191         if (read)
2192                 ret = copy_to_user(data, buf->vaddr + buf->pos, count);
2193         else
2194                 ret = copy_from_user(buf->vaddr + buf->pos, data, count);
2195         if (ret) {
2196                 dprintk(3, "file io: error copying data\n");
2197                 ret = -EFAULT;
2198                 goto end;
2199         }
2200
2201         /*
2202          * Update counters.
2203          */
2204         buf->pos += count;
2205         *ppos += count;
2206
2207         /*
2208          * Queue next buffer if required.
2209          */
2210         if (buf->pos == buf->size ||
2211            (!read && (fileio->flags & VB2_FILEIO_WRITE_IMMEDIATELY))) {
2212                 /*
2213                  * Check if this is the last buffer to read.
2214                  */
2215                 if (read && (fileio->flags & VB2_FILEIO_READ_ONCE) &&
2216                     fileio->dq_count == 1) {
2217                         dprintk(3, "file io: read limit reached\n");
2218                         /*
2219                          * Restore fileio pointer and release the context.
2220                          */
2221                         q->fileio = fileio;
2222                         return __vb2_cleanup_fileio(q);
2223                 }
2224
2225                 /*
2226                  * Call vb2_qbuf and give buffer to the driver.
2227                  */
2228                 memset(&fileio->b, 0, sizeof(fileio->b));
2229                 fileio->b.type = q->type;
2230                 fileio->b.memory = q->memory;
2231                 fileio->b.index = index;
2232                 fileio->b.bytesused = buf->pos;
2233                 ret = vb2_qbuf(q, &fileio->b);
2234                 dprintk(5, "file io: vb2_dbuf result: %d\n", ret);
2235                 if (ret)
2236                         goto end;
2237
2238                 /*
2239                  * Buffer has been queued, update the status
2240                  */
2241                 buf->pos = 0;
2242                 buf->queued = 1;
2243                 buf->size = q->bufs[0]->v4l2_planes[0].length;
2244                 fileio->q_count += 1;
2245
2246                 /*
2247                  * Switch to the next buffer
2248                  */
2249                 fileio->index = (index + 1) % q->num_buffers;
2250
2251                 /*
2252                  * Start streaming if required.
2253                  */
2254                 if (!read && !q->streaming) {
2255                         ret = vb2_streamon(q, q->type);
2256                         if (ret)
2257                                 goto end;
2258                 }
2259         }
2260
2261         /*
2262          * Return proper number of bytes processed.
2263          */
2264         if (ret == 0)
2265                 ret = count;
2266 end:
2267         /*
2268          * Restore the fileio context and block vb2 ioctl interface.
2269          */
2270         q->fileio = fileio;
2271         return ret;
2272 }
2273
2274 size_t vb2_read(struct vb2_queue *q, char __user *data, size_t count,
2275                 loff_t *ppos, int nonblocking)
2276 {
2277         return __vb2_perform_fileio(q, data, count, ppos, nonblocking, 1);
2278 }
2279 EXPORT_SYMBOL_GPL(vb2_read);
2280
2281 size_t vb2_write(struct vb2_queue *q, char __user *data, size_t count,
2282                 loff_t *ppos, int nonblocking)
2283 {
2284         return __vb2_perform_fileio(q, data, count, ppos, nonblocking, 0);
2285 }
2286 EXPORT_SYMBOL_GPL(vb2_write);
2287
2288 MODULE_DESCRIPTION("Driver helper framework for Video for Linux 2");
2289 MODULE_AUTHOR("Pawel Osciak <pawel@osciak.com>, Marek Szyprowski");
2290 MODULE_LICENSE("GPL");