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