drivers/media/video/mem2mem_testdev.c

   1 /*
   2  * A virtual v4l2-mem2mem example device.
   3  *
   4  * This is a virtual device driver for testing mem-to-mem videobuf framework.
   5  * It simulates a device that uses memory buffers for both source and
   6  * destination, processes the data and issues an "irq" (simulated by a timer).
   7  * The device is capable of multi-instance, multi-buffer-per-transaction
   8  * operation (via the mem2mem framework).
   9  *
  10  * Copyright (c) 2009-2010 Samsung Electronics Co., Ltd.
  11  * Pawel Osciak, <pawel@osciak.com>
  12  * Marek Szyprowski, <m.szyprowski@samsung.com>
  13  *
  14  * This program is free software; you can redistribute it and/or modify
  15  * it under the terms of the GNU General Public License as published by the
  16  * Free Software Foundation; either version 2 of the
  17  * License, or (at your option) any later version
  18  */
  19 #include <linux/module.h>
  20 #include <linux/delay.h>
  21 #include <linux/fs.h>
  22 #include <linux/timer.h>
  23 #include <linux/sched.h>
  24 #include <linux/slab.h>
  25
  26 #include <linux/platform_device.h>
  27 #include <media/v4l2-mem2mem.h>
  28 #include <media/v4l2-device.h>
  29 #include <media/v4l2-ioctl.h>
  30 #include <media/v4l2-ctrls.h>
  31 #include <media/v4l2-event.h>
  32 #include <media/videobuf2-vmalloc.h>
  33
  34 #define MEM2MEM_TEST_MODULE_NAME "mem2mem-testdev"
  35
  36 MODULE_DESCRIPTION("Virtual device for mem2mem framework testing");
  37 MODULE_AUTHOR("Pawel Osciak, <pawel@osciak.com>");
  38 MODULE_LICENSE("GPL");
  39 MODULE_VERSION("0.1.1");
  40
  41 #define MIN_W 32
  42 #define MIN_H 32
  43 #define MAX_W 640
  44 #define MAX_H 480
  45 #define DIM_ALIGN_MASK 7 /* 8-byte alignment for line length */
  46
  47 /* Flags that indicate a format can be used for capture/output */
  48 #define MEM2MEM_CAPTURE (1 << 0)
  49 #define MEM2MEM_OUTPUT  (1 << 1)
  50
  51 #define MEM2MEM_NAME            "m2m-testdev"
  52
  53 /* Per queue */
  54 #define MEM2MEM_DEF_NUM_BUFS    VIDEO_MAX_FRAME
  55 /* In bytes, per queue */
  56 #define MEM2MEM_VID_MEM_LIMIT   (16 * 1024 * 1024)
  57
  58 /* Default transaction time in msec */
  59 #define MEM2MEM_DEF_TRANSTIME   1000
  60 /* Default number of buffers per transaction */
  61 #define MEM2MEM_DEF_TRANSLEN    1
  62 #define MEM2MEM_COLOR_STEP      (0xff >> 4)
  63 #define MEM2MEM_NUM_TILES       8
  64
  65 /* Flags that indicate processing mode */
  66 #define MEM2MEM_HFLIP   (1 << 0)
  67 #define MEM2MEM_VFLIP   (1 << 1)
  68
  69 #define dprintk(dev, fmt, arg...) \
  70         v4l2_dbg(1, 1, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg)
  71
  72
  73 void m2mtest_dev_release(struct device *dev)
  74 {}
  75
  76 static struct platform_device m2mtest_pdev = {
  77         .name           = MEM2MEM_NAME,
  78         .dev.release    = m2mtest_dev_release,
  79 };
  80
  81 struct m2mtest_fmt {
  82         char    *name;
  83         u32     fourcc;
  84         int     depth;
  85         /* Types the format can be used for */
  86         u32     types;
  87 };
  88
  89 static struct m2mtest_fmt formats[] = {
  90         {
  91                 .name   = "RGB565 (BE)",
  92                 .fourcc = V4L2_PIX_FMT_RGB565X, /* rrrrrggg gggbbbbb */
  93                 .depth  = 16,
  94                 /* Both capture and output format */
  95                 .types  = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
  96         },
  97         {
  98                 .name   = "4:2:2, packed, YUYV",
  99                 .fourcc = V4L2_PIX_FMT_YUYV,
 100                 .depth  = 16,
 101                 /* Output-only format */
 102                 .types  = MEM2MEM_OUTPUT,
 103         },
 104 };
 105
 106 #define NUM_FORMATS ARRAY_SIZE(formats)
 107
 108 /* Per-queue, driver-specific private data */
 109 struct m2mtest_q_data {
 110         unsigned int            width;
 111         unsigned int            height;
 112         unsigned int            sizeimage;
 113         struct m2mtest_fmt      *fmt;
 114 };
 115
 116 enum {
 117         V4L2_M2M_SRC = 0,
 118         V4L2_M2M_DST = 1,
 119 };
 120
 121 #define V4L2_CID_TRANS_TIME_MSEC        (V4L2_CID_USER_BASE + 0x1000)
 122 #define V4L2_CID_TRANS_NUM_BUFS         (V4L2_CID_USER_BASE + 0x1001)
 123
 124 static struct m2mtest_fmt *find_format(struct v4l2_format *f)
 125 {
 126         struct m2mtest_fmt *fmt;
 127         unsigned int k;
 128
 129         for (k = 0; k < NUM_FORMATS; k++) {
 130                 fmt = &formats[k];
 131                 if (fmt->fourcc == f->fmt.pix.pixelformat)
 132                         break;
 133         }
 134
 135         if (k == NUM_FORMATS)
 136                 return NULL;
 137
 138         return &formats[k];
 139 }
 140
 141 struct m2mtest_dev {
 142         struct v4l2_device      v4l2_dev;
 143         struct video_device     *vfd;
 144
 145         atomic_t                num_inst;
 146         struct mutex            dev_mutex;
 147         spinlock_t              irqlock;
 148
 149         struct timer_list       timer;
 150
 151         struct v4l2_m2m_dev     *m2m_dev;
 152 };
 153
 154 struct m2mtest_ctx {
 155         struct v4l2_fh          fh;
 156         struct m2mtest_dev      *dev;
 157
 158         struct v4l2_ctrl_handler hdl;
 159
 160         /* Processed buffers in this transaction */
 161         u8                      num_processed;
 162
 163         /* Transaction length (i.e. how many buffers per transaction) */
 164         u32                     translen;
 165         /* Transaction time (i.e. simulated processing time) in milliseconds */
 166         u32                     transtime;
 167
 168         /* Abort requested by m2m */
 169         int                     aborting;
 170
 171         /* Processing mode */
 172         int                     mode;
 173
 174         struct v4l2_m2m_ctx     *m2m_ctx;
 175
 176         /* Source and destination queue data */
 177         struct m2mtest_q_data   q_data[2];
 178 };
 179
 180 static inline struct m2mtest_ctx *file2ctx(struct file *file)
 181 {
 182         return container_of(file->private_data, struct m2mtest_ctx, fh);
 183 }
 184
 185 static struct m2mtest_q_data *get_q_data(struct m2mtest_ctx *ctx,
 186                                          enum v4l2_buf_type type)
 187 {
 188         switch (type) {
 189         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 190                 return &ctx->q_data[V4L2_M2M_SRC];
 191         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 192                 return &ctx->q_data[V4L2_M2M_DST];
 193         default:
 194                 BUG();
 195         }
 196         return NULL;
 197 }
 198
 199
 200 static int device_process(struct m2mtest_ctx *ctx,
 201                           struct vb2_buffer *in_vb,
 202                           struct vb2_buffer *out_vb)
 203 {
 204         struct m2mtest_dev *dev = ctx->dev;
 205         struct m2mtest_q_data *q_data;
 206         u8 *p_in, *p_out;
 207         int x, y, t, w;
 208         int tile_w, bytes_left;
 209         int width, height, bytesperline;
 210
 211         q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
 212
 213         width   = q_data->width;
 214         height  = q_data->height;
 215         bytesperline    = (q_data->width * q_data->fmt->depth) >> 3;
 216
 217         p_in = vb2_plane_vaddr(in_vb, 0);
 218         p_out = vb2_plane_vaddr(out_vb, 0);
 219         if (!p_in || !p_out) {
 220                 v4l2_err(&dev->v4l2_dev,
 221                          "Acquiring kernel pointers to buffers failed\n");
 222                 return -EFAULT;
 223         }
 224
 225         if (vb2_plane_size(in_vb, 0) > vb2_plane_size(out_vb, 0)) {
 226                 v4l2_err(&dev->v4l2_dev, "Output buffer is too small\n");
 227                 return -EINVAL;
 228         }
 229
 230         tile_w = (width * (q_data[V4L2_M2M_DST].fmt->depth >> 3))
 231                 / MEM2MEM_NUM_TILES;
 232         bytes_left = bytesperline - tile_w * MEM2MEM_NUM_TILES;
 233         w = 0;
 234
 235         switch (ctx->mode) {
 236         case MEM2MEM_HFLIP | MEM2MEM_VFLIP:
 237                 p_out += bytesperline * height - bytes_left;
 238                 for (y = 0; y < height; ++y) {
 239                         for (t = 0; t < MEM2MEM_NUM_TILES; ++t) {
 240                                 if (w & 0x1) {
 241                                         for (x = 0; x < tile_w; ++x)
 242                                                 *--p_out = *p_in++ +
 243                                                         MEM2MEM_COLOR_STEP;
 244                                 } else {
 245                                         for (x = 0; x < tile_w; ++x)
 246                                                 *--p_out = *p_in++ -
 247                                                         MEM2MEM_COLOR_STEP;
 248                                 }
 249                                 ++w;
 250                         }
 251                         p_in += bytes_left;
 252                         p_out -= bytes_left;
 253                 }
 254                 break;
 255
 256         case MEM2MEM_HFLIP:
 257                 for (y = 0; y < height; ++y) {
 258                         p_out += MEM2MEM_NUM_TILES * tile_w;
 259                         for (t = 0; t < MEM2MEM_NUM_TILES; ++t) {
 260                                 if (w & 0x01) {
 261                                         for (x = 0; x < tile_w; ++x)
 262                                                 *--p_out = *p_in++ +
 263                                                         MEM2MEM_COLOR_STEP;
 264                                 } else {
 265                                         for (x = 0; x < tile_w; ++x)
 266                                                 *--p_out = *p_in++ -
 267                                                         MEM2MEM_COLOR_STEP;
 268                                 }
 269                                 ++w;
 270                         }
 271                         p_in += bytes_left;
 272                         p_out += bytesperline;
 273                 }
 274                 break;
 275
 276         case MEM2MEM_VFLIP:
 277                 p_out += bytesperline * (height - 1);
 278                 for (y = 0; y < height; ++y) {
 279                         for (t = 0; t < MEM2MEM_NUM_TILES; ++t) {
 280                                 if (w & 0x1) {
 281                                         for (x = 0; x < tile_w; ++x)
 282                                                 *p_out++ = *p_in++ +
 283                                                         MEM2MEM_COLOR_STEP;
 284                                 } else {
 285                                         for (x = 0; x < tile_w; ++x)
 286                                                 *p_out++ = *p_in++ -
 287                                                         MEM2MEM_COLOR_STEP;
 288                                 }
 289                                 ++w;
 290                         }
 291                         p_in += bytes_left;
 292                         p_out += bytes_left - 2 * bytesperline;
 293                 }
 294                 break;
 295
 296         default:
 297                 for (y = 0; y < height; ++y) {
 298                         for (t = 0; t < MEM2MEM_NUM_TILES; ++t) {
 299                                 if (w & 0x1) {
 300                                         for (x = 0; x < tile_w; ++x)
 301                                                 *p_out++ = *p_in++ +
 302                                                         MEM2MEM_COLOR_STEP;
 303                                 } else {
 304                                         for (x = 0; x < tile_w; ++x)
 305                                                 *p_out++ = *p_in++ -
 306                                                         MEM2MEM_COLOR_STEP;
 307                                 }
 308                                 ++w;
 309                         }
 310                         p_in += bytes_left;
 311                         p_out += bytes_left;
 312                 }
 313         }
 314
 315         return 0;
 316 }
 317
 318 static void schedule_irq(struct m2mtest_dev *dev, int msec_timeout)
 319 {
 320         dprintk(dev, "Scheduling a simulated irq\n");
 321         mod_timer(&dev->timer, jiffies + msecs_to_jiffies(msec_timeout));
 322 }
 323
 324 /*
 325  * mem2mem callbacks
 326  */
 327
 328 /**
 329  * job_ready() - check whether an instance is ready to be scheduled to run
 330  */
 331 static int job_ready(void *priv)
 332 {
 333         struct m2mtest_ctx *ctx = priv;
 334
 335         if (v4l2_m2m_num_src_bufs_ready(ctx->m2m_ctx) < ctx->translen
 336             || v4l2_m2m_num_dst_bufs_ready(ctx->m2m_ctx) < ctx->translen) {
 337                 dprintk(ctx->dev, "Not enough buffers available\n");
 338                 return 0;
 339         }
 340
 341         return 1;
 342 }
 343
 344 static void job_abort(void *priv)
 345 {
 346         struct m2mtest_ctx *ctx = priv;
 347
 348         /* Will cancel the transaction in the next interrupt handler */
 349         ctx->aborting = 1;
 350 }
 351
 352 static void m2mtest_lock(void *priv)
 353 {
 354         struct m2mtest_ctx *ctx = priv;
 355         struct m2mtest_dev *dev = ctx->dev;
 356         mutex_lock(&dev->dev_mutex);
 357 }
 358
 359 static void m2mtest_unlock(void *priv)
 360 {
 361         struct m2mtest_ctx *ctx = priv;
 362         struct m2mtest_dev *dev = ctx->dev;
 363         mutex_unlock(&dev->dev_mutex);
 364 }
 365
 366
 367 /* device_run() - prepares and starts the device
 368  *
 369  * This simulates all the immediate preparations required before starting
 370  * a device. This will be called by the framework when it decides to schedule
 371  * a particular instance.
 372  */
 373 static void device_run(void *priv)
 374 {
 375         struct m2mtest_ctx *ctx = priv;
 376         struct m2mtest_dev *dev = ctx->dev;
 377         struct vb2_buffer *src_buf, *dst_buf;
 378
 379         src_buf = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
 380         dst_buf = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
 381
 382         device_process(ctx, src_buf, dst_buf);
 383
 384         /* Run a timer, which simulates a hardware irq  */
 385         schedule_irq(dev, ctx->transtime);
 386 }
 387
 388 static void device_isr(unsigned long priv)
 389 {
 390         struct m2mtest_dev *m2mtest_dev = (struct m2mtest_dev *)priv;
 391         struct m2mtest_ctx *curr_ctx;
 392         struct vb2_buffer *src_vb, *dst_vb;
 393         unsigned long flags;
 394
 395         curr_ctx = v4l2_m2m_get_curr_priv(m2mtest_dev->m2m_dev);
 396
 397         if (NULL == curr_ctx) {
 398                 printk(KERN_ERR
 399                         "Instance released before the end of transaction\n");
 400                 return;
 401         }
 402
 403         src_vb = v4l2_m2m_src_buf_remove(curr_ctx->m2m_ctx);
 404         dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->m2m_ctx);
 405
 406         curr_ctx->num_processed++;
 407
 408         spin_lock_irqsave(&m2mtest_dev->irqlock, flags);
 409         v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
 410         v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
 411         spin_unlock_irqrestore(&m2mtest_dev->irqlock, flags);
 412
 413         if (curr_ctx->num_processed == curr_ctx->translen
 414             || curr_ctx->aborting) {
 415                 dprintk(curr_ctx->dev, "Finishing transaction\n");
 416                 curr_ctx->num_processed = 0;
 417                 v4l2_m2m_job_finish(m2mtest_dev->m2m_dev, curr_ctx->m2m_ctx);
 418         } else {
 419                 device_run(curr_ctx);
 420         }
 421 }
 422
 423 /*
 424  * video ioctls
 425  */
 426 static int vidioc_querycap(struct file *file, void *priv,
 427                            struct v4l2_capability *cap)
 428 {
 429         strncpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver) - 1);
 430         strncpy(cap->card, MEM2MEM_NAME, sizeof(cap->card) - 1);
 431         strlcpy(cap->bus_info, MEM2MEM_NAME, sizeof(cap->bus_info));
 432         cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT
 433                           | V4L2_CAP_STREAMING;
 434         cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 435         return 0;
 436 }
 437
 438 static int enum_fmt(struct v4l2_fmtdesc *f, u32 type)
 439 {
 440         int i, num;
 441         struct m2mtest_fmt *fmt;
 442
 443         num = 0;
 444
 445         for (i = 0; i < NUM_FORMATS; ++i) {
 446                 if (formats[i].types & type) {
 447                         /* index-th format of type type found ? */
 448                         if (num == f->index)
 449                                 break;
 450                         /* Correct type but haven't reached our index yet,
 451                          * just increment per-type index */
 452                         ++num;
 453                 }
 454         }
 455
 456         if (i < NUM_FORMATS) {
 457                 /* Format found */
 458                 fmt = &formats[i];
 459                 strncpy(f->description, fmt->name, sizeof(f->description) - 1);
 460                 f->pixelformat = fmt->fourcc;
 461                 return 0;
 462         }
 463
 464         /* Format not found */
 465         return -EINVAL;
 466 }
 467
 468 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
 469                                    struct v4l2_fmtdesc *f)
 470 {
 471         return enum_fmt(f, MEM2MEM_CAPTURE);
 472 }
 473
 474 static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
 475                                    struct v4l2_fmtdesc *f)
 476 {
 477         return enum_fmt(f, MEM2MEM_OUTPUT);
 478 }
 479
 480 static int vidioc_g_fmt(struct m2mtest_ctx *ctx, struct v4l2_format *f)
 481 {
 482         struct vb2_queue *vq;
 483         struct m2mtest_q_data *q_data;
 484
 485         vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
 486         if (!vq)
 487                 return -EINVAL;
 488
 489         q_data = get_q_data(ctx, f->type);
 490
 491         f->fmt.pix.width        = q_data->width;
 492         f->fmt.pix.height       = q_data->height;
 493         f->fmt.pix.field        = V4L2_FIELD_NONE;
 494         f->fmt.pix.pixelformat  = q_data->fmt->fourcc;
 495         f->fmt.pix.bytesperline = (q_data->width * q_data->fmt->depth) >> 3;
 496         f->fmt.pix.sizeimage    = q_data->sizeimage;
 497
 498         return 0;
 499 }
 500
 501 static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
 502                                 struct v4l2_format *f)
 503 {
 504         return vidioc_g_fmt(file2ctx(file), f);
 505 }
 506
 507 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 508                                 struct v4l2_format *f)
 509 {
 510         return vidioc_g_fmt(file2ctx(file), f);
 511 }
 512
 513 static int vidioc_try_fmt(struct v4l2_format *f, struct m2mtest_fmt *fmt)
 514 {
 515         enum v4l2_field field;
 516
 517         field = f->fmt.pix.field;
 518
 519         if (field == V4L2_FIELD_ANY)
 520                 field = V4L2_FIELD_NONE;
 521         else if (V4L2_FIELD_NONE != field)
 522                 return -EINVAL;
 523
 524         /* V4L2 specification suggests the driver corrects the format struct
 525          * if any of the dimensions is unsupported */
 526         f->fmt.pix.field = field;
 527
 528         if (f->fmt.pix.height < MIN_H)
 529                 f->fmt.pix.height = MIN_H;
 530         else if (f->fmt.pix.height > MAX_H)
 531                 f->fmt.pix.height = MAX_H;
 532
 533         if (f->fmt.pix.width < MIN_W)
 534                 f->fmt.pix.width = MIN_W;
 535         else if (f->fmt.pix.width > MAX_W)
 536                 f->fmt.pix.width = MAX_W;
 537
 538         f->fmt.pix.width &= ~DIM_ALIGN_MASK;
 539         f->fmt.pix.bytesperline = (f->fmt.pix.width * fmt->depth) >> 3;
 540         f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
 541
 542         return 0;
 543 }
 544
 545 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 546                                   struct v4l2_format *f)
 547 {
 548         struct m2mtest_fmt *fmt;
 549         struct m2mtest_ctx *ctx = file2ctx(file);
 550
 551         fmt = find_format(f);
 552         if (!fmt || !(fmt->types & MEM2MEM_CAPTURE)) {
 553                 v4l2_err(&ctx->dev->v4l2_dev,
 554                          "Fourcc format (0x%08x) invalid.\n",
 555                          f->fmt.pix.pixelformat);
 556                 return -EINVAL;
 557         }
 558
 559         return vidioc_try_fmt(f, fmt);
 560 }
 561
 562 static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
 563                                   struct v4l2_format *f)
 564 {
 565         struct m2mtest_fmt *fmt;
 566         struct m2mtest_ctx *ctx = file2ctx(file);
 567
 568         fmt = find_format(f);
 569         if (!fmt || !(fmt->types & MEM2MEM_OUTPUT)) {
 570                 v4l2_err(&ctx->dev->v4l2_dev,
 571                          "Fourcc format (0x%08x) invalid.\n",
 572                          f->fmt.pix.pixelformat);
 573                 return -EINVAL;
 574         }
 575
 576         return vidioc_try_fmt(f, fmt);
 577 }
 578
 579 static int vidioc_s_fmt(struct m2mtest_ctx *ctx, struct v4l2_format *f)
 580 {
 581         struct m2mtest_q_data *q_data;
 582         struct vb2_queue *vq;
 583
 584         vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
 585         if (!vq)
 586                 return -EINVAL;
 587
 588         q_data = get_q_data(ctx, f->type);
 589         if (!q_data)
 590                 return -EINVAL;
 591
 592         if (vb2_is_busy(vq)) {
 593                 v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
 594                 return -EBUSY;
 595         }
 596
 597         q_data->fmt             = find_format(f);
 598         q_data->width           = f->fmt.pix.width;
 599         q_data->height          = f->fmt.pix.height;
 600         q_data->sizeimage       = q_data->width * q_data->height
 601                                 * q_data->fmt->depth >> 3;
 602
 603         dprintk(ctx->dev,
 604                 "Setting format for type %d, wxh: %dx%d, fmt: %d\n",
 605                 f->type, q_data->width, q_data->height, q_data->fmt->fourcc);
 606
 607         return 0;
 608 }
 609
 610 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 611                                 struct v4l2_format *f)
 612 {
 613         int ret;
 614
 615         ret = vidioc_try_fmt_vid_cap(file, priv, f);
 616         if (ret)
 617                 return ret;
 618
 619         return vidioc_s_fmt(file2ctx(file), f);
 620 }
 621
 622 static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
 623                                 struct v4l2_format *f)
 624 {
 625         int ret;
 626
 627         ret = vidioc_try_fmt_vid_out(file, priv, f);
 628         if (ret)
 629                 return ret;
 630
 631         return vidioc_s_fmt(file2ctx(file), f);
 632 }
 633
 634 static int vidioc_reqbufs(struct file *file, void *priv,
 635                           struct v4l2_requestbuffers *reqbufs)
 636 {
 637         struct m2mtest_ctx *ctx = file2ctx(file);
 638
 639         return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
 640 }
 641
 642 static int vidioc_querybuf(struct file *file, void *priv,
 643                            struct v4l2_buffer *buf)
 644 {
 645         struct m2mtest_ctx *ctx = file2ctx(file);
 646
 647         return v4l2_m2m_querybuf(file, ctx->m2m_ctx, buf);
 648 }
 649
 650 static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
 651 {
 652         struct m2mtest_ctx *ctx = file2ctx(file);
 653
 654         return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
 655 }
 656
 657 static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
 658 {
 659         struct m2mtest_ctx *ctx = file2ctx(file);
 660
 661         return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
 662 }
 663
 664 static int vidioc_streamon(struct file *file, void *priv,
 665                            enum v4l2_buf_type type)
 666 {
 667         struct m2mtest_ctx *ctx = file2ctx(file);
 668
 669         return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
 670 }
 671
 672 static int vidioc_streamoff(struct file *file, void *priv,
 673                             enum v4l2_buf_type type)
 674 {
 675         struct m2mtest_ctx *ctx = file2ctx(file);
 676
 677         return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
 678 }
 679
 680 static int m2mtest_s_ctrl(struct v4l2_ctrl *ctrl)
 681 {
 682         struct m2mtest_ctx *ctx =
 683                 container_of(ctrl->handler, struct m2mtest_ctx, hdl);
 684
 685         switch (ctrl->id) {
 686         case V4L2_CID_HFLIP:
 687                 if (ctrl->val)
 688                         ctx->mode |= MEM2MEM_HFLIP;
 689                 else
 690                         ctx->mode &= ~MEM2MEM_HFLIP;
 691                 break;
 692
 693         case V4L2_CID_VFLIP:
 694                 if (ctrl->val)
 695                         ctx->mode |= MEM2MEM_VFLIP;
 696                 else
 697                         ctx->mode &= ~MEM2MEM_VFLIP;
 698                 break;
 699
 700         case V4L2_CID_TRANS_TIME_MSEC:
 701                 ctx->transtime = ctrl->val;
 702                 break;
 703
 704         case V4L2_CID_TRANS_NUM_BUFS:
 705                 ctx->translen = ctrl->val;
 706                 break;
 707
 708         default:
 709                 v4l2_err(&ctx->dev->v4l2_dev, "Invalid control\n");
 710                 return -EINVAL;
 711         }
 712
 713         return 0;
 714 }
 715
 716 static const struct v4l2_ctrl_ops m2mtest_ctrl_ops = {
 717         .s_ctrl = m2mtest_s_ctrl,
 718 };
 719
 720
 721 static const struct v4l2_ioctl_ops m2mtest_ioctl_ops = {
 722         .vidioc_querycap        = vidioc_querycap,
 723
 724         .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
 725         .vidioc_g_fmt_vid_cap   = vidioc_g_fmt_vid_cap,
 726         .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
 727         .vidioc_s_fmt_vid_cap   = vidioc_s_fmt_vid_cap,
 728
 729         .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
 730         .vidioc_g_fmt_vid_out   = vidioc_g_fmt_vid_out,
 731         .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out,
 732         .vidioc_s_fmt_vid_out   = vidioc_s_fmt_vid_out,
 733
 734         .vidioc_reqbufs         = vidioc_reqbufs,
 735         .vidioc_querybuf        = vidioc_querybuf,
 736
 737         .vidioc_qbuf            = vidioc_qbuf,
 738         .vidioc_dqbuf           = vidioc_dqbuf,
 739
 740         .vidioc_streamon        = vidioc_streamon,
 741         .vidioc_streamoff       = vidioc_streamoff,
 742         .vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
 743         .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 744 };
 745
 746
 747 /*
 748  * Queue operations
 749  */
 750
 751 static int m2mtest_queue_setup(struct vb2_queue *vq,
 752                                 const struct v4l2_format *fmt,
 753                                 unsigned int *nbuffers, unsigned int *nplanes,
 754                                 unsigned int sizes[], void *alloc_ctxs[])
 755 {
 756         struct m2mtest_ctx *ctx = vb2_get_drv_priv(vq);
 757         struct m2mtest_q_data *q_data;
 758         unsigned int size, count = *nbuffers;
 759
 760         q_data = get_q_data(ctx, vq->type);
 761
 762         size = q_data->width * q_data->height * q_data->fmt->depth >> 3;
 763
 764         while (size * count > MEM2MEM_VID_MEM_LIMIT)
 765                 (count)--;
 766
 767         *nplanes = 1;
 768         *nbuffers = count;
 769         sizes[0] = size;
 770
 771         /*
 772          * videobuf2-vmalloc allocator is context-less so no need to set
 773          * alloc_ctxs array.
 774          */
 775
 776         dprintk(ctx->dev, "get %d buffer(s) of size %d each.\n", count, size);
 777
 778         return 0;
 779 }
 780
 781 static int m2mtest_buf_prepare(struct vb2_buffer *vb)
 782 {
 783         struct m2mtest_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 784         struct m2mtest_q_data *q_data;
 785
 786         dprintk(ctx->dev, "type: %d\n", vb->vb2_queue->type);
 787
 788         q_data = get_q_data(ctx, vb->vb2_queue->type);
 789
 790         if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
 791                 dprintk(ctx->dev, "%s data will not fit into plane (%lu < %lu)\n",
 792                                 __func__, vb2_plane_size(vb, 0), (long)q_data->sizeimage);
 793                 return -EINVAL;
 794         }
 795
 796         vb2_set_plane_payload(vb, 0, q_data->sizeimage);
 797
 798         return 0;
 799 }
 800
 801 static void m2mtest_buf_queue(struct vb2_buffer *vb)
 802 {
 803         struct m2mtest_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 804         v4l2_m2m_buf_queue(ctx->m2m_ctx, vb);
 805 }
 806
 807 static void m2mtest_wait_prepare(struct vb2_queue *q)
 808 {
 809         struct m2mtest_ctx *ctx = vb2_get_drv_priv(q);
 810         m2mtest_unlock(ctx);
 811 }
 812
 813 static void m2mtest_wait_finish(struct vb2_queue *q)
 814 {
 815         struct m2mtest_ctx *ctx = vb2_get_drv_priv(q);
 816         m2mtest_lock(ctx);
 817 }
 818
 819 static struct vb2_ops m2mtest_qops = {
 820         .queue_setup     = m2mtest_queue_setup,
 821         .buf_prepare     = m2mtest_buf_prepare,
 822         .buf_queue       = m2mtest_buf_queue,
 823         .wait_prepare    = m2mtest_wait_prepare,
 824         .wait_finish     = m2mtest_wait_finish,
 825 };
 826
 827 static int queue_init(void *priv, struct vb2_queue *src_vq, struct vb2_queue *dst_vq)
 828 {
 829         struct m2mtest_ctx *ctx = priv;
 830         int ret;
 831
 832         memset(src_vq, 0, sizeof(*src_vq));
 833         src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
 834         src_vq->io_modes = VB2_MMAP;
 835         src_vq->drv_priv = ctx;
 836         src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 837         src_vq->ops = &m2mtest_qops;
 838         src_vq->mem_ops = &vb2_vmalloc_memops;
 839
 840         ret = vb2_queue_init(src_vq);
 841         if (ret)
 842                 return ret;
 843
 844         memset(dst_vq, 0, sizeof(*dst_vq));
 845         dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 846         dst_vq->io_modes = VB2_MMAP;
 847         dst_vq->drv_priv = ctx;
 848         dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 849         dst_vq->ops = &m2mtest_qops;
 850         dst_vq->mem_ops = &vb2_vmalloc_memops;
 851
 852         return vb2_queue_init(dst_vq);
 853 }
 854
 855 static const struct v4l2_ctrl_config m2mtest_ctrl_trans_time_msec = {
 856         .ops = &m2mtest_ctrl_ops,
 857         .id = V4L2_CID_TRANS_TIME_MSEC,
 858         .name = "Transaction Time (msec)",
 859         .type = V4L2_CTRL_TYPE_INTEGER,
 860         .def = 1001,
 861         .min = 1,
 862         .max = 10001,
 863         .step = 100,
 864 };
 865
 866 static const struct v4l2_ctrl_config m2mtest_ctrl_trans_num_bufs = {
 867         .ops = &m2mtest_ctrl_ops,
 868         .id = V4L2_CID_TRANS_NUM_BUFS,
 869         .name = "Buffers Per Transaction",
 870         .type = V4L2_CTRL_TYPE_INTEGER,
 871         .def = 1,
 872         .min = 1,
 873         .max = MEM2MEM_DEF_NUM_BUFS,
 874         .step = 1,
 875 };
 876
 877 /*
 878  * File operations
 879  */
 880 static int m2mtest_open(struct file *file)
 881 {
 882         struct m2mtest_dev *dev = video_drvdata(file);
 883         struct m2mtest_ctx *ctx = NULL;
 884         struct v4l2_ctrl_handler *hdl;
 885
 886         ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
 887         if (!ctx)
 888                 return -ENOMEM;
 889
 890         v4l2_fh_init(&ctx->fh, video_devdata(file));
 891         file->private_data = &ctx->fh;
 892         ctx->dev = dev;
 893         hdl = &ctx->hdl;
 894         v4l2_ctrl_handler_init(hdl, 4);
 895         v4l2_ctrl_new_std(hdl, &m2mtest_ctrl_ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
 896         v4l2_ctrl_new_std(hdl, &m2mtest_ctrl_ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
 897         v4l2_ctrl_new_custom(hdl, &m2mtest_ctrl_trans_time_msec, NULL);
 898         v4l2_ctrl_new_custom(hdl, &m2mtest_ctrl_trans_num_bufs, NULL);
 899         if (hdl->error) {
 900                 int err = hdl->error;
 901
 902                 v4l2_ctrl_handler_free(hdl);
 903                 return err;
 904         }
 905         ctx->fh.ctrl_handler = hdl;
 906         v4l2_ctrl_handler_setup(hdl);
 907
 908         ctx->q_data[V4L2_M2M_SRC].fmt = &formats[0];
 909         ctx->q_data[V4L2_M2M_DST].fmt = &formats[0];
 910
 911         ctx->m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx, &queue_init);
 912
 913         if (IS_ERR(ctx->m2m_ctx)) {
 914                 int ret = PTR_ERR(ctx->m2m_ctx);
 915
 916                 v4l2_ctrl_handler_free(hdl);
 917                 kfree(ctx);
 918                 return ret;
 919         }
 920
 921         v4l2_fh_add(&ctx->fh);
 922         atomic_inc(&dev->num_inst);
 923
 924         dprintk(dev, "Created instance %p, m2m_ctx: %p\n", ctx, ctx->m2m_ctx);
 925
 926         return 0;
 927 }
 928
 929 static int m2mtest_release(struct file *file)
 930 {
 931         struct m2mtest_dev *dev = video_drvdata(file);
 932         struct m2mtest_ctx *ctx = file2ctx(file);
 933
 934         dprintk(dev, "Releasing instance %p\n", ctx);
 935
 936         v4l2_fh_del(&ctx->fh);
 937         v4l2_fh_exit(&ctx->fh);
 938         v4l2_ctrl_handler_free(&ctx->hdl);
 939         v4l2_m2m_ctx_release(ctx->m2m_ctx);
 940         kfree(ctx);
 941
 942         atomic_dec(&dev->num_inst);
 943
 944         return 0;
 945 }
 946
 947 static unsigned int m2mtest_poll(struct file *file,
 948                                  struct poll_table_struct *wait)
 949 {
 950         struct m2mtest_ctx *ctx = file2ctx(file);
 951
 952         return v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
 953 }
 954
 955 static int m2mtest_mmap(struct file *file, struct vm_area_struct *vma)
 956 {
 957         struct m2mtest_ctx *ctx = file2ctx(file);
 958
 959         return v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
 960 }
 961
 962 static const struct v4l2_file_operations m2mtest_fops = {
 963         .owner          = THIS_MODULE,
 964         .open           = m2mtest_open,
 965         .release        = m2mtest_release,
 966         .poll           = m2mtest_poll,
 967         .unlocked_ioctl = video_ioctl2,
 968         .mmap           = m2mtest_mmap,
 969 };
 970
 971 static struct video_device m2mtest_videodev = {
 972         .name           = MEM2MEM_NAME,
 973         .fops           = &m2mtest_fops,
 974         .ioctl_ops      = &m2mtest_ioctl_ops,
 975         .minor          = -1,
 976         .release        = video_device_release,
 977 };
 978
 979 static struct v4l2_m2m_ops m2m_ops = {
 980         .device_run     = device_run,
 981         .job_ready      = job_ready,
 982         .job_abort      = job_abort,
 983         .lock           = m2mtest_lock,
 984         .unlock         = m2mtest_unlock,
 985 };
 986
 987 static int m2mtest_probe(struct platform_device *pdev)
 988 {
 989         struct m2mtest_dev *dev;
 990         struct video_device *vfd;
 991         int ret;
 992
 993         dev = kzalloc(sizeof *dev, GFP_KERNEL);
 994         if (!dev)
 995                 return -ENOMEM;
 996
 997         spin_lock_init(&dev->irqlock);
 998
 999         ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
1000         if (ret)
1001                 goto free_dev;
1002
1003         atomic_set(&dev->num_inst, 0);
1004         mutex_init(&dev->dev_mutex);
1005
1006         vfd = video_device_alloc();
1007         if (!vfd) {
1008                 v4l2_err(&dev->v4l2_dev, "Failed to allocate video device\n");
1009                 ret = -ENOMEM;
1010                 goto unreg_dev;
1011         }
1012
1013         *vfd = m2mtest_videodev;
1014         /* Locking in file operations other than ioctl should be done
1015            by the driver, not the V4L2 core.
1016            This driver needs auditing so that this flag can be removed. */
1017         set_bit(V4L2_FL_LOCK_ALL_FOPS, &vfd->flags);
1018         vfd->lock = &dev->dev_mutex;
1019
1020         ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
1021         if (ret) {
1022                 v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
1023                 goto rel_vdev;
1024         }
1025
1026         video_set_drvdata(vfd, dev);
1027         snprintf(vfd->name, sizeof(vfd->name), "%s", m2mtest_videodev.name);
1028         dev->vfd = vfd;
1029         v4l2_info(&dev->v4l2_dev, MEM2MEM_TEST_MODULE_NAME
1030                         "Device registered as /dev/video%d\n", vfd->num);
1031
1032         setup_timer(&dev->timer, device_isr, (long)dev);
1033         platform_set_drvdata(pdev, dev);
1034
1035         dev->m2m_dev = v4l2_m2m_init(&m2m_ops);
1036         if (IS_ERR(dev->m2m_dev)) {
1037                 v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem device\n");
1038                 ret = PTR_ERR(dev->m2m_dev);
1039                 goto err_m2m;
1040         }
1041
1042         return 0;
1043
1044         v4l2_m2m_release(dev->m2m_dev);
1045 err_m2m:
1046         video_unregister_device(dev->vfd);
1047 rel_vdev:
1048         video_device_release(vfd);
1049 unreg_dev:
1050         v4l2_device_unregister(&dev->v4l2_dev);
1051 free_dev:
1052         kfree(dev);
1053
1054         return ret;
1055 }
1056
1057 static int m2mtest_remove(struct platform_device *pdev)
1058 {
1059         struct m2mtest_dev *dev =
1060                 (struct m2mtest_dev *)platform_get_drvdata(pdev);
1061
1062         v4l2_info(&dev->v4l2_dev, "Removing " MEM2MEM_TEST_MODULE_NAME);
1063         v4l2_m2m_release(dev->m2m_dev);
1064         del_timer_sync(&dev->timer);
1065         video_unregister_device(dev->vfd);
1066         v4l2_device_unregister(&dev->v4l2_dev);
1067         kfree(dev);
1068
1069         return 0;
1070 }
1071
1072 static struct platform_driver m2mtest_pdrv = {
1073         .probe          = m2mtest_probe,
1074         .remove         = m2mtest_remove,
1075         .driver         = {
1076                 .name   = MEM2MEM_NAME,
1077                 .owner  = THIS_MODULE,
1078         },
1079 };
1080
1081 static void __exit m2mtest_exit(void)
1082 {
1083         platform_driver_unregister(&m2mtest_pdrv);
1084         platform_device_unregister(&m2mtest_pdev);
1085 }
1086
1087 static int __init m2mtest_init(void)
1088 {
1089         int ret;
1090
1091         ret = platform_device_register(&m2mtest_pdev);
1092         if (ret)
1093                 return ret;
1094
1095         ret = platform_driver_register(&m2mtest_pdrv);
1096         if (ret)
1097                 platform_device_unregister(&m2mtest_pdev);
1098
1099         return 0;
1100 }
1101
1102 module_init(m2mtest_init);
1103 module_exit(m2mtest_exit);
1104