drivers/media/test-drivers/vim2m.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * A virtual v4l2-mem2mem example device.
   4  *
   5  * This is a virtual device driver for testing mem-to-mem vb2 framework.
   6  * It simulates a device that uses memory buffers for both source and
   7  * destination, processes the data and issues an "irq" (simulated by a delayed
   8  * workqueue).
   9  * The device is capable of multi-instance, multi-buffer-per-transaction
  10  * operation (via the mem2mem framework).
  11  *
  12  * Copyright (c) 2009-2010 Samsung Electronics Co., Ltd.
  13  * Pawel Osciak, <pawel@osciak.com>
  14  * Marek Szyprowski, <m.szyprowski@samsung.com>
  15  */
  16 #include <linux/module.h>
  17 #include <linux/delay.h>
  18 #include <linux/fs.h>
  19 #include <linux/sched.h>
  20 #include <linux/slab.h>
  21
  22 #include <linux/platform_device.h>
  23 #include <media/v4l2-mem2mem.h>
  24 #include <media/v4l2-device.h>
  25 #include <media/v4l2-ioctl.h>
  26 #include <media/v4l2-ctrls.h>
  27 #include <media/v4l2-event.h>
  28 #include <media/videobuf2-vmalloc.h>
  29
  30 MODULE_DESCRIPTION("Virtual device for mem2mem framework testing");
  31 MODULE_AUTHOR("Pawel Osciak, <pawel@osciak.com>");
  32 MODULE_LICENSE("GPL");
  33 MODULE_VERSION("0.2");
  34 MODULE_ALIAS("mem2mem_testdev");
  35
  36 static unsigned int debug;
  37 module_param(debug, uint, 0644);
  38 MODULE_PARM_DESC(debug, "debug level");
  39
  40 /* Default transaction time in msec */
  41 static unsigned int default_transtime = 40; /* Max 25 fps */
  42 module_param(default_transtime, uint, 0644);
  43 MODULE_PARM_DESC(default_transtime, "default transaction time in ms");
  44
  45 #define MIN_W 32
  46 #define MIN_H 32
  47 #define MAX_W 640
  48 #define MAX_H 480
  49
  50 /* Pixel alignment for non-bayer formats */
  51 #define WIDTH_ALIGN 2
  52 #define HEIGHT_ALIGN 1
  53
  54 /* Pixel alignment for bayer formats */
  55 #define BAYER_WIDTH_ALIGN  2
  56 #define BAYER_HEIGHT_ALIGN 2
  57
  58 /* Flags that indicate a format can be used for capture/output */
  59 #define MEM2MEM_CAPTURE BIT(0)
  60 #define MEM2MEM_OUTPUT  BIT(1)
  61
  62 #define MEM2MEM_NAME            "vim2m"
  63
  64 /* Per queue */
  65 #define MEM2MEM_DEF_NUM_BUFS    VIDEO_MAX_FRAME
  66 /* In bytes, per queue */
  67 #define MEM2MEM_VID_MEM_LIMIT   (16 * 1024 * 1024)
  68
  69 /* Flags that indicate processing mode */
  70 #define MEM2MEM_HFLIP   BIT(0)
  71 #define MEM2MEM_VFLIP   BIT(1)
  72
  73 #define dprintk(dev, lvl, fmt, arg...) \
  74         v4l2_dbg(lvl, debug, &(dev)->v4l2_dev, "%s: " fmt, __func__, ## arg)
  75
  76 static void vim2m_dev_release(struct device *dev)
  77 {}
  78
  79 static struct platform_device vim2m_pdev = {
  80         .name           = MEM2MEM_NAME,
  81         .dev.release    = vim2m_dev_release,
  82 };
  83
  84 struct vim2m_fmt {
  85         u32     fourcc;
  86         int     depth;
  87         /* Types the format can be used for */
  88         u32     types;
  89 };
  90
  91 static struct vim2m_fmt formats[] = {
  92         {
  93                 .fourcc = V4L2_PIX_FMT_RGB565,  /* rrrrrggg gggbbbbb */
  94                 .depth  = 16,
  95                 .types  = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
  96         }, {
  97                 .fourcc = V4L2_PIX_FMT_RGB565X, /* gggbbbbb rrrrrggg */
  98                 .depth  = 16,
  99                 .types  = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
 100         }, {
 101                 .fourcc = V4L2_PIX_FMT_RGB24,
 102                 .depth  = 24,
 103                 .types  = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
 104         }, {
 105                 .fourcc = V4L2_PIX_FMT_BGR24,
 106                 .depth  = 24,
 107                 .types  = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
 108         }, {
 109                 .fourcc = V4L2_PIX_FMT_YUYV,
 110                 .depth  = 16,
 111                 .types  = MEM2MEM_CAPTURE,
 112         }, {
 113                 .fourcc = V4L2_PIX_FMT_SBGGR8,
 114                 .depth  = 8,
 115                 .types  = MEM2MEM_CAPTURE,
 116         }, {
 117                 .fourcc = V4L2_PIX_FMT_SGBRG8,
 118                 .depth  = 8,
 119                 .types  = MEM2MEM_CAPTURE,
 120         }, {
 121                 .fourcc = V4L2_PIX_FMT_SGRBG8,
 122                 .depth  = 8,
 123                 .types  = MEM2MEM_CAPTURE,
 124         }, {
 125                 .fourcc = V4L2_PIX_FMT_SRGGB8,
 126                 .depth  = 8,
 127                 .types  = MEM2MEM_CAPTURE,
 128         },
 129 };
 130
 131 #define NUM_FORMATS ARRAY_SIZE(formats)
 132
 133 /* Per-queue, driver-specific private data */
 134 struct vim2m_q_data {
 135         unsigned int            width;
 136         unsigned int            height;
 137         unsigned int            sizeimage;
 138         unsigned int            sequence;
 139         struct vim2m_fmt        *fmt;
 140 };
 141
 142 enum {
 143         V4L2_M2M_SRC = 0,
 144         V4L2_M2M_DST = 1,
 145 };
 146
 147 #define V4L2_CID_TRANS_TIME_MSEC        (V4L2_CID_USER_BASE + 0x1000)
 148 #define V4L2_CID_TRANS_NUM_BUFS         (V4L2_CID_USER_BASE + 0x1001)
 149
 150 static struct vim2m_fmt *find_format(u32 fourcc)
 151 {
 152         struct vim2m_fmt *fmt;
 153         unsigned int k;
 154
 155         for (k = 0; k < NUM_FORMATS; k++) {
 156                 fmt = &formats[k];
 157                 if (fmt->fourcc == fourcc)
 158                         break;
 159         }
 160
 161         if (k == NUM_FORMATS)
 162                 return NULL;
 163
 164         return &formats[k];
 165 }
 166
 167 static void get_alignment(u32 fourcc,
 168                           unsigned int *walign, unsigned int *halign)
 169 {
 170         switch (fourcc) {
 171         case V4L2_PIX_FMT_SBGGR8:
 172         case V4L2_PIX_FMT_SGBRG8:
 173         case V4L2_PIX_FMT_SGRBG8:
 174         case V4L2_PIX_FMT_SRGGB8:
 175                 *walign = BAYER_WIDTH_ALIGN;
 176                 *halign = BAYER_HEIGHT_ALIGN;
 177                 return;
 178         default:
 179                 *walign = WIDTH_ALIGN;
 180                 *halign = HEIGHT_ALIGN;
 181                 return;
 182         }
 183 }
 184
 185 struct vim2m_dev {
 186         struct v4l2_device      v4l2_dev;
 187         struct video_device     vfd;
 188 #ifdef CONFIG_MEDIA_CONTROLLER
 189         struct media_device     mdev;
 190 #endif
 191
 192         atomic_t                num_inst;
 193         struct mutex            dev_mutex;
 194
 195         struct v4l2_m2m_dev     *m2m_dev;
 196 };
 197
 198 struct vim2m_ctx {
 199         struct v4l2_fh          fh;
 200         struct vim2m_dev        *dev;
 201
 202         struct v4l2_ctrl_handler hdl;
 203
 204         /* Processed buffers in this transaction */
 205         u8                      num_processed;
 206
 207         /* Transaction length (i.e. how many buffers per transaction) */
 208         u32                     translen;
 209         /* Transaction time (i.e. simulated processing time) in milliseconds */
 210         u32                     transtime;
 211
 212         struct mutex            vb_mutex;
 213         struct delayed_work     work_run;
 214
 215         /* Abort requested by m2m */
 216         int                     aborting;
 217
 218         /* Processing mode */
 219         int                     mode;
 220
 221         enum v4l2_colorspace    colorspace;
 222         enum v4l2_ycbcr_encoding ycbcr_enc;
 223         enum v4l2_xfer_func     xfer_func;
 224         enum v4l2_quantization  quant;
 225
 226         /* Source and destination queue data */
 227         struct vim2m_q_data   q_data[2];
 228 };
 229
 230 static inline struct vim2m_ctx *file2ctx(struct file *file)
 231 {
 232         return container_of(file->private_data, struct vim2m_ctx, fh);
 233 }
 234
 235 static struct vim2m_q_data *get_q_data(struct vim2m_ctx *ctx,
 236                                        enum v4l2_buf_type type)
 237 {
 238         switch (type) {
 239         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 240                 return &ctx->q_data[V4L2_M2M_SRC];
 241         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 242                 return &ctx->q_data[V4L2_M2M_DST];
 243         default:
 244                 return NULL;
 245         }
 246 }
 247
 248 static const char *type_name(enum v4l2_buf_type type)
 249 {
 250         switch (type) {
 251         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 252                 return "Output";
 253         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 254                 return "Capture";
 255         default:
 256                 return "Invalid";
 257         }
 258 }
 259
 260 #define CLIP(__color) \
 261         (u8)(((__color) > 0xff) ? 0xff : (((__color) < 0) ? 0 : (__color)))
 262
 263 static void copy_line(struct vim2m_q_data *q_data_out,
 264                       u8 *src, u8 *dst, bool reverse)
 265 {
 266         int x, depth = q_data_out->fmt->depth >> 3;
 267
 268         if (!reverse) {
 269                 memcpy(dst, src, q_data_out->width * depth);
 270         } else {
 271                 for (x = 0; x < q_data_out->width >> 1; x++) {
 272                         memcpy(dst, src, depth);
 273                         memcpy(dst + depth, src - depth, depth);
 274                         src -= depth << 1;
 275                         dst += depth << 1;
 276                 }
 277                 return;
 278         }
 279 }
 280
 281 static void copy_two_pixels(struct vim2m_q_data *q_data_in,
 282                             struct vim2m_q_data *q_data_out,
 283                             u8 *src[2], u8 **dst, int ypos, bool reverse)
 284 {
 285         struct vim2m_fmt *out = q_data_out->fmt;
 286         struct vim2m_fmt *in = q_data_in->fmt;
 287         u8 _r[2], _g[2], _b[2], *r, *g, *b;
 288         int i;
 289
 290         /* Step 1: read two consecutive pixels from src pointer */
 291
 292         r = _r;
 293         g = _g;
 294         b = _b;
 295
 296         switch (in->fourcc) {
 297         case V4L2_PIX_FMT_RGB565: /* rrrrrggg gggbbbbb */
 298                 for (i = 0; i < 2; i++) {
 299                         u16 pix = le16_to_cpu(*(__le16 *)(src[i]));
 300
 301                         *r++ = (u8)(((pix & 0xf800) >> 11) << 3) | 0x07;
 302                         *g++ = (u8)((((pix & 0x07e0) >> 5)) << 2) | 0x03;
 303                         *b++ = (u8)((pix & 0x1f) << 3) | 0x07;
 304                 }
 305                 break;
 306         case V4L2_PIX_FMT_RGB565X: /* gggbbbbb rrrrrggg */
 307                 for (i = 0; i < 2; i++) {
 308                         u16 pix = be16_to_cpu(*(__be16 *)(src[i]));
 309
 310                         *r++ = (u8)(((pix & 0xf800) >> 11) << 3) | 0x07;
 311                         *g++ = (u8)((((pix & 0x07e0) >> 5)) << 2) | 0x03;
 312                         *b++ = (u8)((pix & 0x1f) << 3) | 0x07;
 313                 }
 314                 break;
 315         default:
 316         case V4L2_PIX_FMT_RGB24:
 317                 for (i = 0; i < 2; i++) {
 318                         *r++ = src[i][0];
 319                         *g++ = src[i][1];
 320                         *b++ = src[i][2];
 321                 }
 322                 break;
 323         case V4L2_PIX_FMT_BGR24:
 324                 for (i = 0; i < 2; i++) {
 325                         *b++ = src[i][0];
 326                         *g++ = src[i][1];
 327                         *r++ = src[i][2];
 328                 }
 329                 break;
 330         }
 331
 332         /* Step 2: store two consecutive points, reversing them if needed */
 333
 334         r = _r;
 335         g = _g;
 336         b = _b;
 337
 338         switch (out->fourcc) {
 339         case V4L2_PIX_FMT_RGB565: /* rrrrrggg gggbbbbb */
 340                 for (i = 0; i < 2; i++) {
 341                         u16 pix;
 342                         __le16 *dst_pix = (__le16 *)*dst;
 343
 344                         pix = ((*r << 8) & 0xf800) | ((*g << 3) & 0x07e0) |
 345                               (*b >> 3);
 346
 347                         *dst_pix = cpu_to_le16(pix);
 348
 349                         *dst += 2;
 350                 }
 351                 return;
 352         case V4L2_PIX_FMT_RGB565X: /* gggbbbbb rrrrrggg */
 353                 for (i = 0; i < 2; i++) {
 354                         u16 pix;
 355                         __be16 *dst_pix = (__be16 *)*dst;
 356
 357                         pix = ((*r << 8) & 0xf800) | ((*g << 3) & 0x07e0) |
 358                               (*b >> 3);
 359
 360                         *dst_pix = cpu_to_be16(pix);
 361
 362                         *dst += 2;
 363                 }
 364                 return;
 365         case V4L2_PIX_FMT_RGB24:
 366                 for (i = 0; i < 2; i++) {
 367                         *(*dst)++ = *r++;
 368                         *(*dst)++ = *g++;
 369                         *(*dst)++ = *b++;
 370                 }
 371                 return;
 372         case V4L2_PIX_FMT_BGR24:
 373                 for (i = 0; i < 2; i++) {
 374                         *(*dst)++ = *b++;
 375                         *(*dst)++ = *g++;
 376                         *(*dst)++ = *r++;
 377                 }
 378                 return;
 379         case V4L2_PIX_FMT_YUYV:
 380         default:
 381         {
 382                 u8 y, y1, u, v;
 383
 384                 y = ((8453  * (*r) + 16594 * (*g) +  3223 * (*b)
 385                      + 524288) >> 15);
 386                 u = ((-4878 * (*r) - 9578  * (*g) + 14456 * (*b)
 387                      + 4210688) >> 15);
 388                 v = ((14456 * (*r++) - 12105 * (*g++) - 2351 * (*b++)
 389                      + 4210688) >> 15);
 390                 y1 = ((8453 * (*r) + 16594 * (*g) +  3223 * (*b)
 391                      + 524288) >> 15);
 392
 393                 *(*dst)++ = y;
 394                 *(*dst)++ = u;
 395
 396                 *(*dst)++ = y1;
 397                 *(*dst)++ = v;
 398                 return;
 399         }
 400         case V4L2_PIX_FMT_SBGGR8:
 401                 if (!(ypos & 1)) {
 402                         *(*dst)++ = *b;
 403                         *(*dst)++ = *++g;
 404                 } else {
 405                         *(*dst)++ = *g;
 406                         *(*dst)++ = *++r;
 407                 }
 408                 return;
 409         case V4L2_PIX_FMT_SGBRG8:
 410                 if (!(ypos & 1)) {
 411                         *(*dst)++ = *g;
 412                         *(*dst)++ = *++b;
 413                 } else {
 414                         *(*dst)++ = *r;
 415                         *(*dst)++ = *++g;
 416                 }
 417                 return;
 418         case V4L2_PIX_FMT_SGRBG8:
 419                 if (!(ypos & 1)) {
 420                         *(*dst)++ = *g;
 421                         *(*dst)++ = *++r;
 422                 } else {
 423                         *(*dst)++ = *b;
 424                         *(*dst)++ = *++g;
 425                 }
 426                 return;
 427         case V4L2_PIX_FMT_SRGGB8:
 428                 if (!(ypos & 1)) {
 429                         *(*dst)++ = *r;
 430                         *(*dst)++ = *++g;
 431                 } else {
 432                         *(*dst)++ = *g;
 433                         *(*dst)++ = *++b;
 434                 }
 435                 return;
 436         }
 437 }
 438
 439 static int device_process(struct vim2m_ctx *ctx,
 440                           struct vb2_v4l2_buffer *in_vb,
 441                           struct vb2_v4l2_buffer *out_vb)
 442 {
 443         struct vim2m_dev *dev = ctx->dev;
 444         struct vim2m_q_data *q_data_in, *q_data_out;
 445         u8 *p_in, *p_line, *p_in_x[2], *p, *p_out;
 446         unsigned int width, height, bytesperline, bytes_per_pixel;
 447         unsigned int x, y, y_in, y_out, x_int, x_fract, x_err, x_offset;
 448         int start, end, step;
 449
 450         q_data_in = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
 451         if (!q_data_in)
 452                 return 0;
 453         bytesperline = (q_data_in->width * q_data_in->fmt->depth) >> 3;
 454         bytes_per_pixel = q_data_in->fmt->depth >> 3;
 455
 456         q_data_out = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
 457         if (!q_data_out)
 458                 return 0;
 459
 460         /* As we're doing scaling, use the output dimensions here */
 461         height = q_data_out->height;
 462         width = q_data_out->width;
 463
 464         p_in = vb2_plane_vaddr(&in_vb->vb2_buf, 0);
 465         p_out = vb2_plane_vaddr(&out_vb->vb2_buf, 0);
 466         if (!p_in || !p_out) {
 467                 v4l2_err(&dev->v4l2_dev,
 468                          "Acquiring kernel pointers to buffers failed\n");
 469                 return -EFAULT;
 470         }
 471
 472         out_vb->sequence = q_data_out->sequence++;
 473         in_vb->sequence = q_data_in->sequence++;
 474         v4l2_m2m_buf_copy_metadata(in_vb, out_vb, true);
 475
 476         if (ctx->mode & MEM2MEM_VFLIP) {
 477                 start = height - 1;
 478                 end = -1;
 479                 step = -1;
 480         } else {
 481                 start = 0;
 482                 end = height;
 483                 step = 1;
 484         }
 485         y_out = 0;
 486
 487         /*
 488          * When format and resolution are identical,
 489          * we can use a faster copy logic
 490          */
 491         if (q_data_in->fmt->fourcc == q_data_out->fmt->fourcc &&
 492             q_data_in->width == q_data_out->width &&
 493             q_data_in->height == q_data_out->height) {
 494                 for (y = start; y != end; y += step, y_out++) {
 495                         p = p_in + (y * bytesperline);
 496                         if (ctx->mode & MEM2MEM_HFLIP)
 497                                 p += bytesperline - (q_data_in->fmt->depth >> 3);
 498
 499                         copy_line(q_data_out, p, p_out,
 500                                   ctx->mode & MEM2MEM_HFLIP);
 501
 502                         p_out += bytesperline;
 503                 }
 504                 return 0;
 505         }
 506
 507         /* Slower algorithm with format conversion, hflip, vflip and scaler */
 508
 509         /* To speed scaler up, use Bresenham for X dimension */
 510         x_int = q_data_in->width / q_data_out->width;
 511         x_fract = q_data_in->width % q_data_out->width;
 512
 513         for (y = start; y != end; y += step, y_out++) {
 514                 y_in = (y * q_data_in->height) / q_data_out->height;
 515                 x_offset = 0;
 516                 x_err = 0;
 517
 518                 p_line = p_in + (y_in * bytesperline);
 519                 if (ctx->mode & MEM2MEM_HFLIP)
 520                         p_line += bytesperline - (q_data_in->fmt->depth >> 3);
 521                 p_in_x[0] = p_line;
 522
 523                 for (x = 0; x < width >> 1; x++) {
 524                         x_offset += x_int;
 525                         x_err += x_fract;
 526                         if (x_err > width) {
 527                                 x_offset++;
 528                                 x_err -= width;
 529                         }
 530
 531                         if (ctx->mode & MEM2MEM_HFLIP)
 532                                 p_in_x[1] = p_line - x_offset * bytes_per_pixel;
 533                         else
 534                                 p_in_x[1] = p_line + x_offset * bytes_per_pixel;
 535
 536                         copy_two_pixels(q_data_in, q_data_out,
 537                                         p_in_x, &p_out, y_out,
 538                                         ctx->mode & MEM2MEM_HFLIP);
 539
 540                         /* Calculate the next p_in_x0 */
 541                         x_offset += x_int;
 542                         x_err += x_fract;
 543                         if (x_err > width) {
 544                                 x_offset++;
 545                                 x_err -= width;
 546                         }
 547
 548                         if (ctx->mode & MEM2MEM_HFLIP)
 549                                 p_in_x[0] = p_line - x_offset * bytes_per_pixel;
 550                         else
 551                                 p_in_x[0] = p_line + x_offset * bytes_per_pixel;
 552                 }
 553         }
 554
 555         return 0;
 556 }
 557
 558 /*
 559  * mem2mem callbacks
 560  */
 561
 562 /*
 563  * job_ready() - check whether an instance is ready to be scheduled to run
 564  */
 565 static int job_ready(void *priv)
 566 {
 567         struct vim2m_ctx *ctx = priv;
 568
 569         if (v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx) < ctx->translen
 570             || v4l2_m2m_num_dst_bufs_ready(ctx->fh.m2m_ctx) < ctx->translen) {
 571                 dprintk(ctx->dev, 1, "Not enough buffers available\n");
 572                 return 0;
 573         }
 574
 575         return 1;
 576 }
 577
 578 static void job_abort(void *priv)
 579 {
 580         struct vim2m_ctx *ctx = priv;
 581
 582         /* Will cancel the transaction in the next interrupt handler */
 583         ctx->aborting = 1;
 584 }
 585
 586 /* device_run() - prepares and starts the device
 587  *
 588  * This simulates all the immediate preparations required before starting
 589  * a device. This will be called by the framework when it decides to schedule
 590  * a particular instance.
 591  */
 592 static void device_run(void *priv)
 593 {
 594         struct vim2m_ctx *ctx = priv;
 595         struct vb2_v4l2_buffer *src_buf, *dst_buf;
 596
 597         src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
 598         dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
 599
 600         /* Apply request controls if any */
 601         v4l2_ctrl_request_setup(src_buf->vb2_buf.req_obj.req,
 602                                 &ctx->hdl);
 603
 604         device_process(ctx, src_buf, dst_buf);
 605
 606         /* Complete request controls if any */
 607         v4l2_ctrl_request_complete(src_buf->vb2_buf.req_obj.req,
 608                                    &ctx->hdl);
 609
 610         /* Run delayed work, which simulates a hardware irq  */
 611         schedule_delayed_work(&ctx->work_run, msecs_to_jiffies(ctx->transtime));
 612 }
 613
 614 static void device_work(struct work_struct *w)
 615 {
 616         struct vim2m_ctx *curr_ctx;
 617         struct vim2m_dev *vim2m_dev;
 618         struct vb2_v4l2_buffer *src_vb, *dst_vb;
 619
 620         curr_ctx = container_of(w, struct vim2m_ctx, work_run.work);
 621
 622         vim2m_dev = curr_ctx->dev;
 623
 624         src_vb = v4l2_m2m_src_buf_remove(curr_ctx->fh.m2m_ctx);
 625         dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->fh.m2m_ctx);
 626
 627         curr_ctx->num_processed++;
 628
 629         v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
 630         v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
 631
 632         if (curr_ctx->num_processed == curr_ctx->translen
 633             || curr_ctx->aborting) {
 634                 dprintk(curr_ctx->dev, 2, "Finishing capture buffer fill\n");
 635                 curr_ctx->num_processed = 0;
 636                 v4l2_m2m_job_finish(vim2m_dev->m2m_dev, curr_ctx->fh.m2m_ctx);
 637         } else {
 638                 device_run(curr_ctx);
 639         }
 640 }
 641
 642 /*
 643  * video ioctls
 644  */
 645 static int vidioc_querycap(struct file *file, void *priv,
 646                            struct v4l2_capability *cap)
 647 {
 648         strscpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver));
 649         strscpy(cap->card, MEM2MEM_NAME, sizeof(cap->card));
 650         snprintf(cap->bus_info, sizeof(cap->bus_info),
 651                  "platform:%s", MEM2MEM_NAME);
 652         return 0;
 653 }
 654
 655 static int enum_fmt(struct v4l2_fmtdesc *f, u32 type)
 656 {
 657         int i, num;
 658         struct vim2m_fmt *fmt;
 659
 660         num = 0;
 661
 662         for (i = 0; i < NUM_FORMATS; ++i) {
 663                 if (formats[i].types & type) {
 664                         /* index-th format of type type found ? */
 665                         if (num == f->index)
 666                                 break;
 667                         /*
 668                          * Correct type but haven't reached our index yet,
 669                          * just increment per-type index
 670                          */
 671                         ++num;
 672                 }
 673         }
 674
 675         if (i < NUM_FORMATS) {
 676                 /* Format found */
 677                 fmt = &formats[i];
 678                 f->pixelformat = fmt->fourcc;
 679                 return 0;
 680         }
 681
 682         /* Format not found */
 683         return -EINVAL;
 684 }
 685
 686 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
 687                                    struct v4l2_fmtdesc *f)
 688 {
 689         return enum_fmt(f, MEM2MEM_CAPTURE);
 690 }
 691
 692 static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
 693                                    struct v4l2_fmtdesc *f)
 694 {
 695         return enum_fmt(f, MEM2MEM_OUTPUT);
 696 }
 697
 698 static int vidioc_enum_framesizes(struct file *file, void *priv,
 699                                   struct v4l2_frmsizeenum *fsize)
 700 {
 701         if (fsize->index != 0)
 702                 return -EINVAL;
 703
 704         if (!find_format(fsize->pixel_format))
 705                 return -EINVAL;
 706
 707         fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
 708         fsize->stepwise.min_width = MIN_W;
 709         fsize->stepwise.min_height = MIN_H;
 710         fsize->stepwise.max_width = MAX_W;
 711         fsize->stepwise.max_height = MAX_H;
 712
 713         get_alignment(fsize->pixel_format,
 714                       &fsize->stepwise.step_width,
 715                       &fsize->stepwise.step_height);
 716         return 0;
 717 }
 718
 719 static int vidioc_g_fmt(struct vim2m_ctx *ctx, struct v4l2_format *f)
 720 {
 721         struct vb2_queue *vq;
 722         struct vim2m_q_data *q_data;
 723
 724         vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 725         if (!vq)
 726                 return -EINVAL;
 727
 728         q_data = get_q_data(ctx, f->type);
 729         if (!q_data)
 730                 return -EINVAL;
 731
 732         f->fmt.pix.width        = q_data->width;
 733         f->fmt.pix.height       = q_data->height;
 734         f->fmt.pix.field        = V4L2_FIELD_NONE;
 735         f->fmt.pix.pixelformat  = q_data->fmt->fourcc;
 736         f->fmt.pix.bytesperline = (q_data->width * q_data->fmt->depth) >> 3;
 737         f->fmt.pix.sizeimage    = q_data->sizeimage;
 738         f->fmt.pix.colorspace   = ctx->colorspace;
 739         f->fmt.pix.xfer_func    = ctx->xfer_func;
 740         f->fmt.pix.ycbcr_enc    = ctx->ycbcr_enc;
 741         f->fmt.pix.quantization = ctx->quant;
 742
 743         return 0;
 744 }
 745
 746 static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
 747                                 struct v4l2_format *f)
 748 {
 749         return vidioc_g_fmt(file2ctx(file), f);
 750 }
 751
 752 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 753                                 struct v4l2_format *f)
 754 {
 755         return vidioc_g_fmt(file2ctx(file), f);
 756 }
 757
 758 static int vidioc_try_fmt(struct v4l2_format *f, struct vim2m_fmt *fmt)
 759 {
 760         int walign, halign;
 761         /*
 762          * V4L2 specification specifies the driver corrects the
 763          * format struct if any of the dimensions is unsupported
 764          */
 765         if (f->fmt.pix.height < MIN_H)
 766                 f->fmt.pix.height = MIN_H;
 767         else if (f->fmt.pix.height > MAX_H)
 768                 f->fmt.pix.height = MAX_H;
 769
 770         if (f->fmt.pix.width < MIN_W)
 771                 f->fmt.pix.width = MIN_W;
 772         else if (f->fmt.pix.width > MAX_W)
 773                 f->fmt.pix.width = MAX_W;
 774
 775         get_alignment(f->fmt.pix.pixelformat, &walign, &halign);
 776         f->fmt.pix.width &= ~(walign - 1);
 777         f->fmt.pix.height &= ~(halign - 1);
 778         f->fmt.pix.bytesperline = (f->fmt.pix.width * fmt->depth) >> 3;
 779         f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
 780         f->fmt.pix.field = V4L2_FIELD_NONE;
 781
 782         return 0;
 783 }
 784
 785 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 786                                   struct v4l2_format *f)
 787 {
 788         struct vim2m_fmt *fmt;
 789         struct vim2m_ctx *ctx = file2ctx(file);
 790
 791         fmt = find_format(f->fmt.pix.pixelformat);
 792         if (!fmt) {
 793                 f->fmt.pix.pixelformat = formats[0].fourcc;
 794                 fmt = find_format(f->fmt.pix.pixelformat);
 795         }
 796         if (!(fmt->types & MEM2MEM_CAPTURE)) {
 797                 v4l2_err(&ctx->dev->v4l2_dev,
 798                          "Fourcc format (0x%08x) invalid.\n",
 799                          f->fmt.pix.pixelformat);
 800                 return -EINVAL;
 801         }
 802         f->fmt.pix.colorspace = ctx->colorspace;
 803         f->fmt.pix.xfer_func = ctx->xfer_func;
 804         f->fmt.pix.ycbcr_enc = ctx->ycbcr_enc;
 805         f->fmt.pix.quantization = ctx->quant;
 806
 807         return vidioc_try_fmt(f, fmt);
 808 }
 809
 810 static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
 811                                   struct v4l2_format *f)
 812 {
 813         struct vim2m_fmt *fmt;
 814         struct vim2m_ctx *ctx = file2ctx(file);
 815
 816         fmt = find_format(f->fmt.pix.pixelformat);
 817         if (!fmt) {
 818                 f->fmt.pix.pixelformat = formats[0].fourcc;
 819                 fmt = find_format(f->fmt.pix.pixelformat);
 820         }
 821         if (!(fmt->types & MEM2MEM_OUTPUT)) {
 822                 v4l2_err(&ctx->dev->v4l2_dev,
 823                          "Fourcc format (0x%08x) invalid.\n",
 824                          f->fmt.pix.pixelformat);
 825                 return -EINVAL;
 826         }
 827         if (!f->fmt.pix.colorspace)
 828                 f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
 829
 830         return vidioc_try_fmt(f, fmt);
 831 }
 832
 833 static int vidioc_s_fmt(struct vim2m_ctx *ctx, struct v4l2_format *f)
 834 {
 835         struct vim2m_q_data *q_data;
 836         struct vb2_queue *vq;
 837
 838         vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 839         if (!vq)
 840                 return -EINVAL;
 841
 842         q_data = get_q_data(ctx, f->type);
 843         if (!q_data)
 844                 return -EINVAL;
 845
 846         if (vb2_is_busy(vq)) {
 847                 v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
 848                 return -EBUSY;
 849         }
 850
 851         q_data->fmt             = find_format(f->fmt.pix.pixelformat);
 852         q_data->width           = f->fmt.pix.width;
 853         q_data->height          = f->fmt.pix.height;
 854         q_data->sizeimage       = q_data->width * q_data->height
 855                                 * q_data->fmt->depth >> 3;
 856
 857         dprintk(ctx->dev, 1,
 858                 "Format for type %s: %dx%d (%d bpp), fmt: %c%c%c%c\n",
 859                 type_name(f->type), q_data->width, q_data->height,
 860                 q_data->fmt->depth,
 861                 (q_data->fmt->fourcc & 0xff),
 862                 (q_data->fmt->fourcc >>  8) & 0xff,
 863                 (q_data->fmt->fourcc >> 16) & 0xff,
 864                 (q_data->fmt->fourcc >> 24) & 0xff);
 865
 866         return 0;
 867 }
 868
 869 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 870                                 struct v4l2_format *f)
 871 {
 872         int ret;
 873
 874         ret = vidioc_try_fmt_vid_cap(file, priv, f);
 875         if (ret)
 876                 return ret;
 877
 878         return vidioc_s_fmt(file2ctx(file), f);
 879 }
 880
 881 static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
 882                                 struct v4l2_format *f)
 883 {
 884         struct vim2m_ctx *ctx = file2ctx(file);
 885         int ret;
 886
 887         ret = vidioc_try_fmt_vid_out(file, priv, f);
 888         if (ret)
 889                 return ret;
 890
 891         ret = vidioc_s_fmt(file2ctx(file), f);
 892         if (!ret) {
 893                 ctx->colorspace = f->fmt.pix.colorspace;
 894                 ctx->xfer_func = f->fmt.pix.xfer_func;
 895                 ctx->ycbcr_enc = f->fmt.pix.ycbcr_enc;
 896                 ctx->quant = f->fmt.pix.quantization;
 897         }
 898         return ret;
 899 }
 900
 901 static int vim2m_s_ctrl(struct v4l2_ctrl *ctrl)
 902 {
 903         struct vim2m_ctx *ctx =
 904                 container_of(ctrl->handler, struct vim2m_ctx, hdl);
 905
 906         switch (ctrl->id) {
 907         case V4L2_CID_HFLIP:
 908                 if (ctrl->val)
 909                         ctx->mode |= MEM2MEM_HFLIP;
 910                 else
 911                         ctx->mode &= ~MEM2MEM_HFLIP;
 912                 break;
 913
 914         case V4L2_CID_VFLIP:
 915                 if (ctrl->val)
 916                         ctx->mode |= MEM2MEM_VFLIP;
 917                 else
 918                         ctx->mode &= ~MEM2MEM_VFLIP;
 919                 break;
 920
 921         case V4L2_CID_TRANS_TIME_MSEC:
 922                 ctx->transtime = ctrl->val;
 923                 if (ctx->transtime < 1)
 924                         ctx->transtime = 1;
 925                 break;
 926
 927         case V4L2_CID_TRANS_NUM_BUFS:
 928                 ctx->translen = ctrl->val;
 929                 break;
 930
 931         default:
 932                 v4l2_err(&ctx->dev->v4l2_dev, "Invalid control\n");
 933                 return -EINVAL;
 934         }
 935
 936         return 0;
 937 }
 938
 939 static const struct v4l2_ctrl_ops vim2m_ctrl_ops = {
 940         .s_ctrl = vim2m_s_ctrl,
 941 };
 942
 943 static const struct v4l2_ioctl_ops vim2m_ioctl_ops = {
 944         .vidioc_querycap        = vidioc_querycap,
 945
 946         .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
 947         .vidioc_enum_framesizes = vidioc_enum_framesizes,
 948         .vidioc_g_fmt_vid_cap   = vidioc_g_fmt_vid_cap,
 949         .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
 950         .vidioc_s_fmt_vid_cap   = vidioc_s_fmt_vid_cap,
 951
 952         .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
 953         .vidioc_g_fmt_vid_out   = vidioc_g_fmt_vid_out,
 954         .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out,
 955         .vidioc_s_fmt_vid_out   = vidioc_s_fmt_vid_out,
 956
 957         .vidioc_reqbufs         = v4l2_m2m_ioctl_reqbufs,
 958         .vidioc_querybuf        = v4l2_m2m_ioctl_querybuf,
 959         .vidioc_qbuf            = v4l2_m2m_ioctl_qbuf,
 960         .vidioc_dqbuf           = v4l2_m2m_ioctl_dqbuf,
 961         .vidioc_prepare_buf     = v4l2_m2m_ioctl_prepare_buf,
 962         .vidioc_create_bufs     = v4l2_m2m_ioctl_create_bufs,
 963         .vidioc_expbuf          = v4l2_m2m_ioctl_expbuf,
 964
 965         .vidioc_streamon        = v4l2_m2m_ioctl_streamon,
 966         .vidioc_streamoff       = v4l2_m2m_ioctl_streamoff,
 967
 968         .vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
 969         .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 970 };
 971
 972 /*
 973  * Queue operations
 974  */
 975
 976 static int vim2m_queue_setup(struct vb2_queue *vq,
 977                              unsigned int *nbuffers,
 978                              unsigned int *nplanes,
 979                              unsigned int sizes[],
 980                              struct device *alloc_devs[])
 981 {
 982         struct vim2m_ctx *ctx = vb2_get_drv_priv(vq);
 983         struct vim2m_q_data *q_data;
 984         unsigned int size, count = *nbuffers;
 985
 986         q_data = get_q_data(ctx, vq->type);
 987         if (!q_data)
 988                 return -EINVAL;
 989
 990         size = q_data->width * q_data->height * q_data->fmt->depth >> 3;
 991
 992         while (size * count > MEM2MEM_VID_MEM_LIMIT)
 993                 (count)--;
 994         *nbuffers = count;
 995
 996         if (*nplanes)
 997                 return sizes[0] < size ? -EINVAL : 0;
 998
 999         *nplanes = 1;
1000         sizes[0] = size;
1001
1002         dprintk(ctx->dev, 1, "%s: get %d buffer(s) of size %d each.\n",
1003                 type_name(vq->type), count, size);
1004
1005         return 0;
1006 }
1007
1008 static int vim2m_buf_out_validate(struct vb2_buffer *vb)
1009 {
1010         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
1011         struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1012
1013         if (vbuf->field == V4L2_FIELD_ANY)
1014                 vbuf->field = V4L2_FIELD_NONE;
1015         if (vbuf->field != V4L2_FIELD_NONE) {
1016                 dprintk(ctx->dev, 1, "%s field isn't supported\n", __func__);
1017                 return -EINVAL;
1018         }
1019
1020         return 0;
1021 }
1022
1023 static int vim2m_buf_prepare(struct vb2_buffer *vb)
1024 {
1025         struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1026         struct vim2m_q_data *q_data;
1027
1028         dprintk(ctx->dev, 2, "type: %s\n", type_name(vb->vb2_queue->type));
1029
1030         q_data = get_q_data(ctx, vb->vb2_queue->type);
1031         if (!q_data)
1032                 return -EINVAL;
1033         if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
1034                 dprintk(ctx->dev, 1,
1035                         "%s data will not fit into plane (%lu < %lu)\n",
1036                         __func__, vb2_plane_size(vb, 0),
1037                         (long)q_data->sizeimage);
1038                 return -EINVAL;
1039         }
1040
1041         vb2_set_plane_payload(vb, 0, q_data->sizeimage);
1042
1043         return 0;
1044 }
1045
1046 static void vim2m_buf_queue(struct vb2_buffer *vb)
1047 {
1048         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
1049         struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1050
1051         v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
1052 }
1053
1054 static int vim2m_start_streaming(struct vb2_queue *q, unsigned int count)
1055 {
1056         struct vim2m_ctx *ctx = vb2_get_drv_priv(q);
1057         struct vim2m_q_data *q_data = get_q_data(ctx, q->type);
1058
1059         if (!q_data)
1060                 return -EINVAL;
1061
1062         if (V4L2_TYPE_IS_OUTPUT(q->type))
1063                 ctx->aborting = 0;
1064
1065         q_data->sequence = 0;
1066         return 0;
1067 }
1068
1069 static void vim2m_stop_streaming(struct vb2_queue *q)
1070 {
1071         struct vim2m_ctx *ctx = vb2_get_drv_priv(q);
1072         struct vb2_v4l2_buffer *vbuf;
1073
1074         cancel_delayed_work_sync(&ctx->work_run);
1075
1076         for (;;) {
1077                 if (V4L2_TYPE_IS_OUTPUT(q->type))
1078                         vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1079                 else
1080                         vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1081                 if (!vbuf)
1082                         return;
1083                 v4l2_ctrl_request_complete(vbuf->vb2_buf.req_obj.req,
1084                                            &ctx->hdl);
1085                 v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
1086         }
1087 }
1088
1089 static void vim2m_buf_request_complete(struct vb2_buffer *vb)
1090 {
1091         struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1092
1093         v4l2_ctrl_request_complete(vb->req_obj.req, &ctx->hdl);
1094 }
1095
1096 static const struct vb2_ops vim2m_qops = {
1097         .queue_setup     = vim2m_queue_setup,
1098         .buf_out_validate        = vim2m_buf_out_validate,
1099         .buf_prepare     = vim2m_buf_prepare,
1100         .buf_queue       = vim2m_buf_queue,
1101         .start_streaming = vim2m_start_streaming,
1102         .stop_streaming  = vim2m_stop_streaming,
1103         .wait_prepare    = vb2_ops_wait_prepare,
1104         .wait_finish     = vb2_ops_wait_finish,
1105         .buf_request_complete = vim2m_buf_request_complete,
1106 };
1107
1108 static int queue_init(void *priv, struct vb2_queue *src_vq,
1109                       struct vb2_queue *dst_vq)
1110 {
1111         struct vim2m_ctx *ctx = priv;
1112         int ret;
1113
1114         src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
1115         src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
1116         src_vq->drv_priv = ctx;
1117         src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1118         src_vq->ops = &vim2m_qops;
1119         src_vq->mem_ops = &vb2_vmalloc_memops;
1120         src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1121         src_vq->lock = &ctx->vb_mutex;
1122         src_vq->supports_requests = true;
1123
1124         ret = vb2_queue_init(src_vq);
1125         if (ret)
1126                 return ret;
1127
1128         dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1129         dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
1130         dst_vq->drv_priv = ctx;
1131         dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1132         dst_vq->ops = &vim2m_qops;
1133         dst_vq->mem_ops = &vb2_vmalloc_memops;
1134         dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1135         dst_vq->lock = &ctx->vb_mutex;
1136
1137         return vb2_queue_init(dst_vq);
1138 }
1139
1140 static struct v4l2_ctrl_config vim2m_ctrl_trans_time_msec = {
1141         .ops = &vim2m_ctrl_ops,
1142         .id = V4L2_CID_TRANS_TIME_MSEC,
1143         .name = "Transaction Time (msec)",
1144         .type = V4L2_CTRL_TYPE_INTEGER,
1145         .min = 1,
1146         .max = 10001,
1147         .step = 1,
1148 };
1149
1150 static const struct v4l2_ctrl_config vim2m_ctrl_trans_num_bufs = {
1151         .ops = &vim2m_ctrl_ops,
1152         .id = V4L2_CID_TRANS_NUM_BUFS,
1153         .name = "Buffers Per Transaction",
1154         .type = V4L2_CTRL_TYPE_INTEGER,
1155         .def = 1,
1156         .min = 1,
1157         .max = MEM2MEM_DEF_NUM_BUFS,
1158         .step = 1,
1159 };
1160
1161 /*
1162  * File operations
1163  */
1164 static int vim2m_open(struct file *file)
1165 {
1166         struct vim2m_dev *dev = video_drvdata(file);
1167         struct vim2m_ctx *ctx = NULL;
1168         struct v4l2_ctrl_handler *hdl;
1169         int rc = 0;
1170
1171         if (mutex_lock_interruptible(&dev->dev_mutex))
1172                 return -ERESTARTSYS;
1173         ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
1174         if (!ctx) {
1175                 rc = -ENOMEM;
1176                 goto open_unlock;
1177         }
1178
1179         v4l2_fh_init(&ctx->fh, video_devdata(file));
1180         file->private_data = &ctx->fh;
1181         ctx->dev = dev;
1182         hdl = &ctx->hdl;
1183         v4l2_ctrl_handler_init(hdl, 4);
1184         v4l2_ctrl_new_std(hdl, &vim2m_ctrl_ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
1185         v4l2_ctrl_new_std(hdl, &vim2m_ctrl_ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
1186
1187         vim2m_ctrl_trans_time_msec.def = default_transtime;
1188         v4l2_ctrl_new_custom(hdl, &vim2m_ctrl_trans_time_msec, NULL);
1189         v4l2_ctrl_new_custom(hdl, &vim2m_ctrl_trans_num_bufs, NULL);
1190         if (hdl->error) {
1191                 rc = hdl->error;
1192                 v4l2_ctrl_handler_free(hdl);
1193                 kfree(ctx);
1194                 goto open_unlock;
1195         }
1196         ctx->fh.ctrl_handler = hdl;
1197         v4l2_ctrl_handler_setup(hdl);
1198
1199         ctx->q_data[V4L2_M2M_SRC].fmt = &formats[0];
1200         ctx->q_data[V4L2_M2M_SRC].width = 640;
1201         ctx->q_data[V4L2_M2M_SRC].height = 480;
1202         ctx->q_data[V4L2_M2M_SRC].sizeimage =
1203                 ctx->q_data[V4L2_M2M_SRC].width *
1204                 ctx->q_data[V4L2_M2M_SRC].height *
1205                 (ctx->q_data[V4L2_M2M_SRC].fmt->depth >> 3);
1206         ctx->q_data[V4L2_M2M_DST] = ctx->q_data[V4L2_M2M_SRC];
1207         ctx->colorspace = V4L2_COLORSPACE_REC709;
1208
1209         ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx, &queue_init);
1210
1211         mutex_init(&ctx->vb_mutex);
1212         INIT_DELAYED_WORK(&ctx->work_run, device_work);
1213
1214         if (IS_ERR(ctx->fh.m2m_ctx)) {
1215                 rc = PTR_ERR(ctx->fh.m2m_ctx);
1216
1217                 v4l2_ctrl_handler_free(hdl);
1218                 v4l2_fh_exit(&ctx->fh);
1219                 kfree(ctx);
1220                 goto open_unlock;
1221         }
1222
1223         v4l2_fh_add(&ctx->fh);
1224         atomic_inc(&dev->num_inst);
1225
1226         dprintk(dev, 1, "Created instance: %p, m2m_ctx: %p\n",
1227                 ctx, ctx->fh.m2m_ctx);
1228
1229 open_unlock:
1230         mutex_unlock(&dev->dev_mutex);
1231         return rc;
1232 }
1233
1234 static int vim2m_release(struct file *file)
1235 {
1236         struct vim2m_dev *dev = video_drvdata(file);
1237         struct vim2m_ctx *ctx = file2ctx(file);
1238
1239         dprintk(dev, 1, "Releasing instance %p\n", ctx);
1240
1241         v4l2_fh_del(&ctx->fh);
1242         v4l2_fh_exit(&ctx->fh);
1243         v4l2_ctrl_handler_free(&ctx->hdl);
1244         mutex_lock(&dev->dev_mutex);
1245         v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
1246         mutex_unlock(&dev->dev_mutex);
1247         kfree(ctx);
1248
1249         atomic_dec(&dev->num_inst);
1250
1251         return 0;
1252 }
1253
1254 static void vim2m_device_release(struct video_device *vdev)
1255 {
1256         struct vim2m_dev *dev = container_of(vdev, struct vim2m_dev, vfd);
1257
1258         v4l2_device_unregister(&dev->v4l2_dev);
1259         v4l2_m2m_release(dev->m2m_dev);
1260 #ifdef CONFIG_MEDIA_CONTROLLER
1261         media_device_cleanup(&dev->mdev);
1262 #endif
1263         kfree(dev);
1264 }
1265
1266 static const struct v4l2_file_operations vim2m_fops = {
1267         .owner          = THIS_MODULE,
1268         .open           = vim2m_open,
1269         .release        = vim2m_release,
1270         .poll           = v4l2_m2m_fop_poll,
1271         .unlocked_ioctl = video_ioctl2,
1272         .mmap           = v4l2_m2m_fop_mmap,
1273 };
1274
1275 static const struct video_device vim2m_videodev = {
1276         .name           = MEM2MEM_NAME,
1277         .vfl_dir        = VFL_DIR_M2M,
1278         .fops           = &vim2m_fops,
1279         .ioctl_ops      = &vim2m_ioctl_ops,
1280         .minor          = -1,
1281         .release        = vim2m_device_release,
1282         .device_caps    = V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING,
1283 };
1284
1285 static const struct v4l2_m2m_ops m2m_ops = {
1286         .device_run     = device_run,
1287         .job_ready      = job_ready,
1288         .job_abort      = job_abort,
1289 };
1290
1291 static const struct media_device_ops m2m_media_ops = {
1292         .req_validate = vb2_request_validate,
1293         .req_queue = v4l2_m2m_request_queue,
1294 };
1295
1296 static int vim2m_probe(struct platform_device *pdev)
1297 {
1298         struct vim2m_dev *dev;
1299         struct video_device *vfd;
1300         int ret;
1301
1302         dev = kzalloc(sizeof(*dev), GFP_KERNEL);
1303         if (!dev)
1304                 return -ENOMEM;
1305
1306         ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
1307         if (ret)
1308                 goto error_free;
1309
1310         atomic_set(&dev->num_inst, 0);
1311         mutex_init(&dev->dev_mutex);
1312
1313         dev->vfd = vim2m_videodev;
1314         vfd = &dev->vfd;
1315         vfd->lock = &dev->dev_mutex;
1316         vfd->v4l2_dev = &dev->v4l2_dev;
1317
1318         video_set_drvdata(vfd, dev);
1319         v4l2_info(&dev->v4l2_dev,
1320                   "Device registered as /dev/video%d\n", vfd->num);
1321
1322         platform_set_drvdata(pdev, dev);
1323
1324         dev->m2m_dev = v4l2_m2m_init(&m2m_ops);
1325         if (IS_ERR(dev->m2m_dev)) {
1326                 v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem device\n");
1327                 ret = PTR_ERR(dev->m2m_dev);
1328                 dev->m2m_dev = NULL;
1329                 goto error_dev;
1330         }
1331
1332 #ifdef CONFIG_MEDIA_CONTROLLER
1333         dev->mdev.dev = &pdev->dev;
1334         strscpy(dev->mdev.model, "vim2m", sizeof(dev->mdev.model));
1335         strscpy(dev->mdev.bus_info, "platform:vim2m",
1336                 sizeof(dev->mdev.bus_info));
1337         media_device_init(&dev->mdev);
1338         dev->mdev.ops = &m2m_media_ops;
1339         dev->v4l2_dev.mdev = &dev->mdev;
1340 #endif
1341
1342         ret = video_register_device(vfd, VFL_TYPE_VIDEO, 0);
1343         if (ret) {
1344                 v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
1345                 goto error_m2m;
1346         }
1347
1348 #ifdef CONFIG_MEDIA_CONTROLLER
1349         ret = v4l2_m2m_register_media_controller(dev->m2m_dev, vfd,
1350                                                  MEDIA_ENT_F_PROC_VIDEO_SCALER);
1351         if (ret) {
1352                 v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem media controller\n");
1353                 goto error_v4l2;
1354         }
1355
1356         ret = media_device_register(&dev->mdev);
1357         if (ret) {
1358                 v4l2_err(&dev->v4l2_dev, "Failed to register mem2mem media device\n");
1359                 goto error_m2m_mc;
1360         }
1361 #endif
1362         return 0;
1363
1364 #ifdef CONFIG_MEDIA_CONTROLLER
1365 error_m2m_mc:
1366         v4l2_m2m_unregister_media_controller(dev->m2m_dev);
1367 #endif
1368 error_v4l2:
1369         video_unregister_device(&dev->vfd);
1370         /* vim2m_device_release called by video_unregister_device to release various objects */
1371         return ret;
1372 error_m2m:
1373         v4l2_m2m_release(dev->m2m_dev);
1374 error_dev:
1375         v4l2_device_unregister(&dev->v4l2_dev);
1376 error_free:
1377         kfree(dev);
1378
1379         return ret;
1380 }
1381
1382 static void vim2m_remove(struct platform_device *pdev)
1383 {
1384         struct vim2m_dev *dev = platform_get_drvdata(pdev);
1385
1386         v4l2_info(&dev->v4l2_dev, "Removing " MEM2MEM_NAME);
1387
1388 #ifdef CONFIG_MEDIA_CONTROLLER
1389         media_device_unregister(&dev->mdev);
1390         v4l2_m2m_unregister_media_controller(dev->m2m_dev);
1391 #endif
1392         video_unregister_device(&dev->vfd);
1393 }
1394
1395 static struct platform_driver vim2m_pdrv = {
1396         .probe          = vim2m_probe,
1397         .remove_new     = vim2m_remove,
1398         .driver         = {
1399                 .name   = MEM2MEM_NAME,
1400         },
1401 };
1402
1403 static void __exit vim2m_exit(void)
1404 {
1405         platform_driver_unregister(&vim2m_pdrv);
1406         platform_device_unregister(&vim2m_pdev);
1407 }
1408
1409 static int __init vim2m_init(void)
1410 {
1411         int ret;
1412
1413         ret = platform_device_register(&vim2m_pdev);
1414         if (ret)
1415                 return ret;
1416
1417         ret = platform_driver_register(&vim2m_pdrv);
1418         if (ret)
1419                 platform_device_unregister(&vim2m_pdev);
1420
1421         return ret;
1422 }
1423
1424 module_init(vim2m_init);
1425 module_exit(vim2m_exit);