drivers/media/platform/mx2_emmaprp.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * Support eMMa-PrP through mem2mem framework.
   4  *
   5  * eMMa-PrP is a piece of HW that allows fetching buffers
   6  * from one memory location and do several operations on
   7  * them such as scaling or format conversion giving, as a result
   8  * a new processed buffer in another memory location.
   9  *
  10  * Based on mem2mem_testdev.c by Pawel Osciak.
  11  *
  12  * Copyright (c) 2011 Vista Silicon S.L.
  13  * Javier Martin <javier.martin@vista-silicon.com>
  14  */
  15 #include <linux/module.h>
  16 #include <linux/clk.h>
  17 #include <linux/slab.h>
  18 #include <linux/interrupt.h>
  19 #include <linux/io.h>
  20
  21 #include <linux/platform_device.h>
  22 #include <media/v4l2-mem2mem.h>
  23 #include <media/v4l2-device.h>
  24 #include <media/v4l2-ioctl.h>
  25 #include <media/videobuf2-dma-contig.h>
  26 #include <linux/sizes.h>
  27
  28 #define EMMAPRP_MODULE_NAME "mem2mem-emmaprp"
  29
  30 MODULE_DESCRIPTION("Mem-to-mem device which supports eMMa-PrP present in mx2 SoCs");
  31 MODULE_AUTHOR("Javier Martin <javier.martin@vista-silicon.com");
  32 MODULE_LICENSE("GPL");
  33 MODULE_VERSION("0.0.1");
  34
  35 static bool debug;
  36 module_param(debug, bool, 0644);
  37
  38 #define MIN_W 32
  39 #define MIN_H 32
  40 #define MAX_W 2040
  41 #define MAX_H 2046
  42
  43 #define S_ALIGN         1 /* multiple of 2 */
  44 #define W_ALIGN_YUV420  3 /* multiple of 8 */
  45 #define W_ALIGN_OTHERS  2 /* multiple of 4 */
  46 #define H_ALIGN         1 /* multiple of 2 */
  47
  48 /* Flags that indicate a format can be used for capture/output */
  49 #define MEM2MEM_CAPTURE (1 << 0)
  50 #define MEM2MEM_OUTPUT  (1 << 1)
  51
  52 #define MEM2MEM_NAME            "m2m-emmaprp"
  53
  54 /* In bytes, per queue */
  55 #define MEM2MEM_VID_MEM_LIMIT   SZ_16M
  56
  57 #define dprintk(dev, fmt, arg...) \
  58         v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg)
  59
  60 /* EMMA PrP */
  61 #define PRP_CNTL                        0x00
  62 #define PRP_INTR_CNTL                   0x04
  63 #define PRP_INTRSTATUS                  0x08
  64 #define PRP_SOURCE_Y_PTR                0x0c
  65 #define PRP_SOURCE_CB_PTR               0x10
  66 #define PRP_SOURCE_CR_PTR               0x14
  67 #define PRP_DEST_RGB1_PTR               0x18
  68 #define PRP_DEST_RGB2_PTR               0x1c
  69 #define PRP_DEST_Y_PTR                  0x20
  70 #define PRP_DEST_CB_PTR                 0x24
  71 #define PRP_DEST_CR_PTR                 0x28
  72 #define PRP_SRC_FRAME_SIZE              0x2c
  73 #define PRP_DEST_CH1_LINE_STRIDE        0x30
  74 #define PRP_SRC_PIXEL_FORMAT_CNTL       0x34
  75 #define PRP_CH1_PIXEL_FORMAT_CNTL       0x38
  76 #define PRP_CH1_OUT_IMAGE_SIZE          0x3c
  77 #define PRP_CH2_OUT_IMAGE_SIZE          0x40
  78 #define PRP_SRC_LINE_STRIDE             0x44
  79 #define PRP_CSC_COEF_012                0x48
  80 #define PRP_CSC_COEF_345                0x4c
  81 #define PRP_CSC_COEF_678                0x50
  82 #define PRP_CH1_RZ_HORI_COEF1           0x54
  83 #define PRP_CH1_RZ_HORI_COEF2           0x58
  84 #define PRP_CH1_RZ_HORI_VALID           0x5c
  85 #define PRP_CH1_RZ_VERT_COEF1           0x60
  86 #define PRP_CH1_RZ_VERT_COEF2           0x64
  87 #define PRP_CH1_RZ_VERT_VALID           0x68
  88 #define PRP_CH2_RZ_HORI_COEF1           0x6c
  89 #define PRP_CH2_RZ_HORI_COEF2           0x70
  90 #define PRP_CH2_RZ_HORI_VALID           0x74
  91 #define PRP_CH2_RZ_VERT_COEF1           0x78
  92 #define PRP_CH2_RZ_VERT_COEF2           0x7c
  93 #define PRP_CH2_RZ_VERT_VALID           0x80
  94
  95 #define PRP_CNTL_CH1EN          (1 << 0)
  96 #define PRP_CNTL_CH2EN          (1 << 1)
  97 #define PRP_CNTL_CSIEN          (1 << 2)
  98 #define PRP_CNTL_DATA_IN_YUV420 (0 << 3)
  99 #define PRP_CNTL_DATA_IN_YUV422 (1 << 3)
 100 #define PRP_CNTL_DATA_IN_RGB16  (2 << 3)
 101 #define PRP_CNTL_DATA_IN_RGB32  (3 << 3)
 102 #define PRP_CNTL_CH1_OUT_RGB8   (0 << 5)
 103 #define PRP_CNTL_CH1_OUT_RGB16  (1 << 5)
 104 #define PRP_CNTL_CH1_OUT_RGB32  (2 << 5)
 105 #define PRP_CNTL_CH1_OUT_YUV422 (3 << 5)
 106 #define PRP_CNTL_CH2_OUT_YUV420 (0 << 7)
 107 #define PRP_CNTL_CH2_OUT_YUV422 (1 << 7)
 108 #define PRP_CNTL_CH2_OUT_YUV444 (2 << 7)
 109 #define PRP_CNTL_CH1_LEN        (1 << 9)
 110 #define PRP_CNTL_CH2_LEN        (1 << 10)
 111 #define PRP_CNTL_SKIP_FRAME     (1 << 11)
 112 #define PRP_CNTL_SWRST          (1 << 12)
 113 #define PRP_CNTL_CLKEN          (1 << 13)
 114 #define PRP_CNTL_WEN            (1 << 14)
 115 #define PRP_CNTL_CH1BYP         (1 << 15)
 116 #define PRP_CNTL_IN_TSKIP(x)    ((x) << 16)
 117 #define PRP_CNTL_CH1_TSKIP(x)   ((x) << 19)
 118 #define PRP_CNTL_CH2_TSKIP(x)   ((x) << 22)
 119 #define PRP_CNTL_INPUT_FIFO_LEVEL(x)    ((x) << 25)
 120 #define PRP_CNTL_RZ_FIFO_LEVEL(x)       ((x) << 27)
 121 #define PRP_CNTL_CH2B1EN        (1 << 29)
 122 #define PRP_CNTL_CH2B2EN        (1 << 30)
 123 #define PRP_CNTL_CH2FEN         (1 << 31)
 124
 125 #define PRP_SIZE_HEIGHT(x)      (x)
 126 #define PRP_SIZE_WIDTH(x)       ((x) << 16)
 127
 128 /* IRQ Enable and status register */
 129 #define PRP_INTR_RDERR          (1 << 0)
 130 #define PRP_INTR_CH1WERR        (1 << 1)
 131 #define PRP_INTR_CH2WERR        (1 << 2)
 132 #define PRP_INTR_CH1FC          (1 << 3)
 133 #define PRP_INTR_CH2FC          (1 << 5)
 134 #define PRP_INTR_LBOVF          (1 << 7)
 135 #define PRP_INTR_CH2OVF         (1 << 8)
 136
 137 #define PRP_INTR_ST_RDERR       (1 << 0)
 138 #define PRP_INTR_ST_CH1WERR     (1 << 1)
 139 #define PRP_INTR_ST_CH2WERR     (1 << 2)
 140 #define PRP_INTR_ST_CH2B2CI     (1 << 3)
 141 #define PRP_INTR_ST_CH2B1CI     (1 << 4)
 142 #define PRP_INTR_ST_CH1B2CI     (1 << 5)
 143 #define PRP_INTR_ST_CH1B1CI     (1 << 6)
 144 #define PRP_INTR_ST_LBOVF       (1 << 7)
 145 #define PRP_INTR_ST_CH2OVF      (1 << 8)
 146
 147 struct emmaprp_fmt {
 148         char    *name;
 149         u32     fourcc;
 150         /* Types the format can be used for */
 151         u32     types;
 152 };
 153
 154 static struct emmaprp_fmt formats[] = {
 155         {
 156                 .name   = "YUV 4:2:0 Planar",
 157                 .fourcc = V4L2_PIX_FMT_YUV420,
 158                 .types  = MEM2MEM_CAPTURE,
 159         },
 160         {
 161                 .name   = "4:2:2, packed, YUYV",
 162                 .fourcc = V4L2_PIX_FMT_YUYV,
 163                 .types  = MEM2MEM_OUTPUT,
 164         },
 165 };
 166
 167 /* Per-queue, driver-specific private data */
 168 struct emmaprp_q_data {
 169         unsigned int            width;
 170         unsigned int            height;
 171         unsigned int            sizeimage;
 172         struct emmaprp_fmt      *fmt;
 173 };
 174
 175 enum {
 176         V4L2_M2M_SRC = 0,
 177         V4L2_M2M_DST = 1,
 178 };
 179
 180 #define NUM_FORMATS ARRAY_SIZE(formats)
 181
 182 static struct emmaprp_fmt *find_format(struct v4l2_format *f)
 183 {
 184         struct emmaprp_fmt *fmt;
 185         unsigned int k;
 186
 187         for (k = 0; k < NUM_FORMATS; k++) {
 188                 fmt = &formats[k];
 189                 if (fmt->fourcc == f->fmt.pix.pixelformat)
 190                         break;
 191         }
 192
 193         if (k == NUM_FORMATS)
 194                 return NULL;
 195
 196         return &formats[k];
 197 }
 198
 199 struct emmaprp_dev {
 200         struct v4l2_device      v4l2_dev;
 201         struct video_device     *vfd;
 202
 203         struct mutex            dev_mutex;
 204         spinlock_t              irqlock;
 205
 206         void __iomem            *base_emma;
 207         struct clk              *clk_emma_ahb, *clk_emma_ipg;
 208
 209         struct v4l2_m2m_dev     *m2m_dev;
 210 };
 211
 212 struct emmaprp_ctx {
 213         struct emmaprp_dev      *dev;
 214         /* Abort requested by m2m */
 215         int                     aborting;
 216         struct emmaprp_q_data   q_data[2];
 217         struct v4l2_m2m_ctx     *m2m_ctx;
 218 };
 219
 220 static struct emmaprp_q_data *get_q_data(struct emmaprp_ctx *ctx,
 221                                          enum v4l2_buf_type type)
 222 {
 223         switch (type) {
 224         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 225                 return &(ctx->q_data[V4L2_M2M_SRC]);
 226         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 227                 return &(ctx->q_data[V4L2_M2M_DST]);
 228         default:
 229                 BUG();
 230         }
 231         return NULL;
 232 }
 233
 234 /*
 235  * mem2mem callbacks
 236  */
 237 static void emmaprp_job_abort(void *priv)
 238 {
 239         struct emmaprp_ctx *ctx = priv;
 240         struct emmaprp_dev *pcdev = ctx->dev;
 241
 242         ctx->aborting = 1;
 243
 244         dprintk(pcdev, "Aborting task\n");
 245
 246         v4l2_m2m_job_finish(pcdev->m2m_dev, ctx->m2m_ctx);
 247 }
 248
 249 static inline void emmaprp_dump_regs(struct emmaprp_dev *pcdev)
 250 {
 251         dprintk(pcdev,
 252                 "eMMa-PrP Registers:\n"
 253                 "  SOURCE_Y_PTR = 0x%08X\n"
 254                 "  SRC_FRAME_SIZE = 0x%08X\n"
 255                 "  DEST_Y_PTR = 0x%08X\n"
 256                 "  DEST_CR_PTR = 0x%08X\n"
 257                 "  DEST_CB_PTR = 0x%08X\n"
 258                 "  CH2_OUT_IMAGE_SIZE = 0x%08X\n"
 259                 "  CNTL = 0x%08X\n",
 260                 readl(pcdev->base_emma + PRP_SOURCE_Y_PTR),
 261                 readl(pcdev->base_emma + PRP_SRC_FRAME_SIZE),
 262                 readl(pcdev->base_emma + PRP_DEST_Y_PTR),
 263                 readl(pcdev->base_emma + PRP_DEST_CR_PTR),
 264                 readl(pcdev->base_emma + PRP_DEST_CB_PTR),
 265                 readl(pcdev->base_emma + PRP_CH2_OUT_IMAGE_SIZE),
 266                 readl(pcdev->base_emma + PRP_CNTL));
 267 }
 268
 269 static void emmaprp_device_run(void *priv)
 270 {
 271         struct emmaprp_ctx *ctx = priv;
 272         struct emmaprp_q_data *s_q_data, *d_q_data;
 273         struct vb2_v4l2_buffer *src_buf, *dst_buf;
 274         struct emmaprp_dev *pcdev = ctx->dev;
 275         unsigned int s_width, s_height;
 276         unsigned int d_width, d_height;
 277         unsigned int d_size;
 278         dma_addr_t p_in, p_out;
 279         u32 tmp;
 280
 281         src_buf = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
 282         dst_buf = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
 283
 284         s_q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
 285         s_width = s_q_data->width;
 286         s_height = s_q_data->height;
 287
 288         d_q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
 289         d_width = d_q_data->width;
 290         d_height = d_q_data->height;
 291         d_size = d_width * d_height;
 292
 293         p_in = vb2_dma_contig_plane_dma_addr(&src_buf->vb2_buf, 0);
 294         p_out = vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf, 0);
 295         if (!p_in || !p_out) {
 296                 v4l2_err(&pcdev->v4l2_dev,
 297                          "Acquiring kernel pointers to buffers failed\n");
 298                 return;
 299         }
 300
 301         /* Input frame parameters */
 302         writel(p_in, pcdev->base_emma + PRP_SOURCE_Y_PTR);
 303         writel(PRP_SIZE_WIDTH(s_width) | PRP_SIZE_HEIGHT(s_height),
 304                pcdev->base_emma + PRP_SRC_FRAME_SIZE);
 305
 306         /* Output frame parameters */
 307         writel(p_out, pcdev->base_emma + PRP_DEST_Y_PTR);
 308         writel(p_out + d_size, pcdev->base_emma + PRP_DEST_CB_PTR);
 309         writel(p_out + d_size + (d_size >> 2),
 310                pcdev->base_emma + PRP_DEST_CR_PTR);
 311         writel(PRP_SIZE_WIDTH(d_width) | PRP_SIZE_HEIGHT(d_height),
 312                pcdev->base_emma + PRP_CH2_OUT_IMAGE_SIZE);
 313
 314         /* IRQ configuration */
 315         tmp = readl(pcdev->base_emma + PRP_INTR_CNTL);
 316         writel(tmp | PRP_INTR_RDERR |
 317                 PRP_INTR_CH2WERR |
 318                 PRP_INTR_CH2FC,
 319                 pcdev->base_emma + PRP_INTR_CNTL);
 320
 321         emmaprp_dump_regs(pcdev);
 322
 323         /* Enable transfer */
 324         tmp = readl(pcdev->base_emma + PRP_CNTL);
 325         writel(tmp | PRP_CNTL_CH2_OUT_YUV420 |
 326                 PRP_CNTL_DATA_IN_YUV422 |
 327                 PRP_CNTL_CH2EN,
 328                 pcdev->base_emma + PRP_CNTL);
 329 }
 330
 331 static irqreturn_t emmaprp_irq(int irq_emma, void *data)
 332 {
 333         struct emmaprp_dev *pcdev = data;
 334         struct emmaprp_ctx *curr_ctx;
 335         struct vb2_v4l2_buffer *src_vb, *dst_vb;
 336         unsigned long flags;
 337         u32 irqst;
 338
 339         /* Check irq flags and clear irq */
 340         irqst = readl(pcdev->base_emma + PRP_INTRSTATUS);
 341         writel(irqst, pcdev->base_emma + PRP_INTRSTATUS);
 342         dprintk(pcdev, "irqst = 0x%08x\n", irqst);
 343
 344         curr_ctx = v4l2_m2m_get_curr_priv(pcdev->m2m_dev);
 345         if (curr_ctx == NULL) {
 346                 pr_err("Instance released before the end of transaction\n");
 347                 return IRQ_HANDLED;
 348         }
 349
 350         if (!curr_ctx->aborting) {
 351                 if ((irqst & PRP_INTR_ST_RDERR) ||
 352                 (irqst & PRP_INTR_ST_CH2WERR)) {
 353                         pr_err("PrP bus error occurred, this transfer is probably corrupted\n");
 354                         writel(PRP_CNTL_SWRST, pcdev->base_emma + PRP_CNTL);
 355                 } else if (irqst & PRP_INTR_ST_CH2B1CI) { /* buffer ready */
 356                         src_vb = v4l2_m2m_src_buf_remove(curr_ctx->m2m_ctx);
 357                         dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->m2m_ctx);
 358
 359                         dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp;
 360                         dst_vb->flags &=
 361                                 ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 362                         dst_vb->flags |=
 363                                 src_vb->flags
 364                                 & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 365                         dst_vb->timecode = src_vb->timecode;
 366
 367                         spin_lock_irqsave(&pcdev->irqlock, flags);
 368                         v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
 369                         v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
 370                         spin_unlock_irqrestore(&pcdev->irqlock, flags);
 371                 }
 372         }
 373
 374         v4l2_m2m_job_finish(pcdev->m2m_dev, curr_ctx->m2m_ctx);
 375         return IRQ_HANDLED;
 376 }
 377
 378 /*
 379  * video ioctls
 380  */
 381 static int vidioc_querycap(struct file *file, void *priv,
 382                            struct v4l2_capability *cap)
 383 {
 384         strscpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver));
 385         strscpy(cap->card, MEM2MEM_NAME, sizeof(cap->card));
 386         cap->device_caps = V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING;
 387         cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 388         return 0;
 389 }
 390
 391 static int enum_fmt(struct v4l2_fmtdesc *f, u32 type)
 392 {
 393         int i, num;
 394         struct emmaprp_fmt *fmt;
 395
 396         num = 0;
 397
 398         for (i = 0; i < NUM_FORMATS; ++i) {
 399                 if (formats[i].types & type) {
 400                         /* index-th format of type type found ? */
 401                         if (num == f->index)
 402                                 break;
 403                         /* Correct type but haven't reached our index yet,
 404                          * just increment per-type index */
 405                         ++num;
 406                 }
 407         }
 408
 409         if (i < NUM_FORMATS) {
 410                 /* Format found */
 411                 fmt = &formats[i];
 412                 strscpy(f->description, fmt->name, sizeof(f->description) - 1);
 413                 f->pixelformat = fmt->fourcc;
 414                 return 0;
 415         }
 416
 417         /* Format not found */
 418         return -EINVAL;
 419 }
 420
 421 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
 422                                    struct v4l2_fmtdesc *f)
 423 {
 424         return enum_fmt(f, MEM2MEM_CAPTURE);
 425 }
 426
 427 static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
 428                                    struct v4l2_fmtdesc *f)
 429 {
 430         return enum_fmt(f, MEM2MEM_OUTPUT);
 431 }
 432
 433 static int vidioc_g_fmt(struct emmaprp_ctx *ctx, struct v4l2_format *f)
 434 {
 435         struct vb2_queue *vq;
 436         struct emmaprp_q_data *q_data;
 437
 438         vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
 439         if (!vq)
 440                 return -EINVAL;
 441
 442         q_data = get_q_data(ctx, f->type);
 443
 444         f->fmt.pix.width        = q_data->width;
 445         f->fmt.pix.height       = q_data->height;
 446         f->fmt.pix.field        = V4L2_FIELD_NONE;
 447         f->fmt.pix.pixelformat  = q_data->fmt->fourcc;
 448         if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420)
 449                 f->fmt.pix.bytesperline = q_data->width * 3 / 2;
 450         else /* YUYV */
 451                 f->fmt.pix.bytesperline = q_data->width * 2;
 452         f->fmt.pix.sizeimage    = q_data->sizeimage;
 453
 454         return 0;
 455 }
 456
 457 static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
 458                                 struct v4l2_format *f)
 459 {
 460         return vidioc_g_fmt(priv, f);
 461 }
 462
 463 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 464                                 struct v4l2_format *f)
 465 {
 466         return vidioc_g_fmt(priv, f);
 467 }
 468
 469 static int vidioc_try_fmt(struct v4l2_format *f)
 470 {
 471         enum v4l2_field field;
 472
 473
 474         if (!find_format(f))
 475                 return -EINVAL;
 476
 477         field = f->fmt.pix.field;
 478         if (field == V4L2_FIELD_ANY)
 479                 field = V4L2_FIELD_NONE;
 480         else if (V4L2_FIELD_NONE != field)
 481                 return -EINVAL;
 482
 483         /* V4L2 specification suggests the driver corrects the format struct
 484          * if any of the dimensions is unsupported */
 485         f->fmt.pix.field = field;
 486
 487         if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420) {
 488                 v4l_bound_align_image(&f->fmt.pix.width, MIN_W, MAX_W,
 489                                       W_ALIGN_YUV420, &f->fmt.pix.height,
 490                                       MIN_H, MAX_H, H_ALIGN, S_ALIGN);
 491                 f->fmt.pix.bytesperline = f->fmt.pix.width * 3 / 2;
 492         } else {
 493                 v4l_bound_align_image(&f->fmt.pix.width, MIN_W, MAX_W,
 494                                       W_ALIGN_OTHERS, &f->fmt.pix.height,
 495                                       MIN_H, MAX_H, H_ALIGN, S_ALIGN);
 496                 f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
 497         }
 498         f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
 499
 500         return 0;
 501 }
 502
 503 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 504                                   struct v4l2_format *f)
 505 {
 506         struct emmaprp_fmt *fmt;
 507         struct emmaprp_ctx *ctx = priv;
 508
 509         fmt = find_format(f);
 510         if (!fmt || !(fmt->types & MEM2MEM_CAPTURE)) {
 511                 v4l2_err(&ctx->dev->v4l2_dev,
 512                          "Fourcc format (0x%08x) invalid.\n",
 513                          f->fmt.pix.pixelformat);
 514                 return -EINVAL;
 515         }
 516
 517         return vidioc_try_fmt(f);
 518 }
 519
 520 static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
 521                                   struct v4l2_format *f)
 522 {
 523         struct emmaprp_fmt *fmt;
 524         struct emmaprp_ctx *ctx = priv;
 525
 526         fmt = find_format(f);
 527         if (!fmt || !(fmt->types & MEM2MEM_OUTPUT)) {
 528                 v4l2_err(&ctx->dev->v4l2_dev,
 529                          "Fourcc format (0x%08x) invalid.\n",
 530                          f->fmt.pix.pixelformat);
 531                 return -EINVAL;
 532         }
 533
 534         return vidioc_try_fmt(f);
 535 }
 536
 537 static int vidioc_s_fmt(struct emmaprp_ctx *ctx, struct v4l2_format *f)
 538 {
 539         struct emmaprp_q_data *q_data;
 540         struct vb2_queue *vq;
 541         int ret;
 542
 543         vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
 544         if (!vq)
 545                 return -EINVAL;
 546
 547         q_data = get_q_data(ctx, f->type);
 548         if (!q_data)
 549                 return -EINVAL;
 550
 551         if (vb2_is_busy(vq)) {
 552                 v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
 553                 return -EBUSY;
 554         }
 555
 556         ret = vidioc_try_fmt(f);
 557         if (ret)
 558                 return ret;
 559
 560         q_data->fmt             = find_format(f);
 561         q_data->width           = f->fmt.pix.width;
 562         q_data->height          = f->fmt.pix.height;
 563         if (q_data->fmt->fourcc == V4L2_PIX_FMT_YUV420)
 564                 q_data->sizeimage = q_data->width * q_data->height * 3 / 2;
 565         else /* YUYV */
 566                 q_data->sizeimage = q_data->width * q_data->height * 2;
 567
 568         dprintk(ctx->dev,
 569                 "Setting format for type %d, wxh: %dx%d, fmt: %d\n",
 570                 f->type, q_data->width, q_data->height, q_data->fmt->fourcc);
 571
 572         return 0;
 573 }
 574
 575 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 576                                 struct v4l2_format *f)
 577 {
 578         int ret;
 579
 580         ret = vidioc_try_fmt_vid_cap(file, priv, f);
 581         if (ret)
 582                 return ret;
 583
 584         return vidioc_s_fmt(priv, f);
 585 }
 586
 587 static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
 588                                 struct v4l2_format *f)
 589 {
 590         int ret;
 591
 592         ret = vidioc_try_fmt_vid_out(file, priv, f);
 593         if (ret)
 594                 return ret;
 595
 596         return vidioc_s_fmt(priv, f);
 597 }
 598
 599 static int vidioc_reqbufs(struct file *file, void *priv,
 600                           struct v4l2_requestbuffers *reqbufs)
 601 {
 602         struct emmaprp_ctx *ctx = priv;
 603
 604         return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
 605 }
 606
 607 static int vidioc_querybuf(struct file *file, void *priv,
 608                            struct v4l2_buffer *buf)
 609 {
 610         struct emmaprp_ctx *ctx = priv;
 611
 612         return v4l2_m2m_querybuf(file, ctx->m2m_ctx, buf);
 613 }
 614
 615 static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
 616 {
 617         struct emmaprp_ctx *ctx = priv;
 618
 619         return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
 620 }
 621
 622 static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
 623 {
 624         struct emmaprp_ctx *ctx = priv;
 625
 626         return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
 627 }
 628
 629 static int vidioc_streamon(struct file *file, void *priv,
 630                            enum v4l2_buf_type type)
 631 {
 632         struct emmaprp_ctx *ctx = priv;
 633
 634         return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
 635 }
 636
 637 static int vidioc_streamoff(struct file *file, void *priv,
 638                             enum v4l2_buf_type type)
 639 {
 640         struct emmaprp_ctx *ctx = priv;
 641
 642         return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
 643 }
 644
 645 static const struct v4l2_ioctl_ops emmaprp_ioctl_ops = {
 646         .vidioc_querycap        = vidioc_querycap,
 647
 648         .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
 649         .vidioc_g_fmt_vid_cap   = vidioc_g_fmt_vid_cap,
 650         .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
 651         .vidioc_s_fmt_vid_cap   = vidioc_s_fmt_vid_cap,
 652
 653         .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
 654         .vidioc_g_fmt_vid_out   = vidioc_g_fmt_vid_out,
 655         .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out,
 656         .vidioc_s_fmt_vid_out   = vidioc_s_fmt_vid_out,
 657
 658         .vidioc_reqbufs         = vidioc_reqbufs,
 659         .vidioc_querybuf        = vidioc_querybuf,
 660
 661         .vidioc_qbuf            = vidioc_qbuf,
 662         .vidioc_dqbuf           = vidioc_dqbuf,
 663
 664         .vidioc_streamon        = vidioc_streamon,
 665         .vidioc_streamoff       = vidioc_streamoff,
 666 };
 667
 668
 669 /*
 670  * Queue operations
 671  */
 672 static int emmaprp_queue_setup(struct vb2_queue *vq,
 673                                 unsigned int *nbuffers, unsigned int *nplanes,
 674                                 unsigned int sizes[], struct device *alloc_devs[])
 675 {
 676         struct emmaprp_ctx *ctx = vb2_get_drv_priv(vq);
 677         struct emmaprp_q_data *q_data;
 678         unsigned int size, count = *nbuffers;
 679
 680         q_data = get_q_data(ctx, vq->type);
 681
 682         if (q_data->fmt->fourcc == V4L2_PIX_FMT_YUV420)
 683                 size = q_data->width * q_data->height * 3 / 2;
 684         else
 685                 size = q_data->width * q_data->height * 2;
 686
 687         while (size * count > MEM2MEM_VID_MEM_LIMIT)
 688                 (count)--;
 689
 690         *nplanes = 1;
 691         *nbuffers = count;
 692         sizes[0] = size;
 693
 694         dprintk(ctx->dev, "get %d buffer(s) of size %d each.\n", count, size);
 695
 696         return 0;
 697 }
 698
 699 static int emmaprp_buf_prepare(struct vb2_buffer *vb)
 700 {
 701         struct emmaprp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 702         struct emmaprp_q_data *q_data;
 703
 704         dprintk(ctx->dev, "type: %d\n", vb->vb2_queue->type);
 705
 706         q_data = get_q_data(ctx, vb->vb2_queue->type);
 707
 708         if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
 709                 dprintk(ctx->dev,
 710                         "%s data will not fit into plane(%lu < %lu)\n",
 711                         __func__, vb2_plane_size(vb, 0),
 712                         (long)q_data->sizeimage);
 713                 return -EINVAL;
 714         }
 715
 716         vb2_set_plane_payload(vb, 0, q_data->sizeimage);
 717
 718         return 0;
 719 }
 720
 721 static void emmaprp_buf_queue(struct vb2_buffer *vb)
 722 {
 723         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 724         struct emmaprp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 725         v4l2_m2m_buf_queue(ctx->m2m_ctx, vbuf);
 726 }
 727
 728 static const struct vb2_ops emmaprp_qops = {
 729         .queue_setup     = emmaprp_queue_setup,
 730         .buf_prepare     = emmaprp_buf_prepare,
 731         .buf_queue       = emmaprp_buf_queue,
 732         .wait_prepare    = vb2_ops_wait_prepare,
 733         .wait_finish     = vb2_ops_wait_finish,
 734 };
 735
 736 static int queue_init(void *priv, struct vb2_queue *src_vq,
 737                       struct vb2_queue *dst_vq)
 738 {
 739         struct emmaprp_ctx *ctx = priv;
 740         int ret;
 741
 742         src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
 743         src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
 744         src_vq->drv_priv = ctx;
 745         src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 746         src_vq->ops = &emmaprp_qops;
 747         src_vq->mem_ops = &vb2_dma_contig_memops;
 748         src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 749         src_vq->dev = ctx->dev->v4l2_dev.dev;
 750         src_vq->lock = &ctx->dev->dev_mutex;
 751
 752         ret = vb2_queue_init(src_vq);
 753         if (ret)
 754                 return ret;
 755
 756         dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 757         dst_vq->io_modes = VB2_MMAP | VB2_USERPTR;
 758         dst_vq->drv_priv = ctx;
 759         dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 760         dst_vq->ops = &emmaprp_qops;
 761         dst_vq->mem_ops = &vb2_dma_contig_memops;
 762         dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 763         dst_vq->dev = ctx->dev->v4l2_dev.dev;
 764         dst_vq->lock = &ctx->dev->dev_mutex;
 765
 766         return vb2_queue_init(dst_vq);
 767 }
 768
 769 /*
 770  * File operations
 771  */
 772 static int emmaprp_open(struct file *file)
 773 {
 774         struct emmaprp_dev *pcdev = video_drvdata(file);
 775         struct emmaprp_ctx *ctx;
 776
 777         ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
 778         if (!ctx)
 779                 return -ENOMEM;
 780
 781         file->private_data = ctx;
 782         ctx->dev = pcdev;
 783
 784         if (mutex_lock_interruptible(&pcdev->dev_mutex)) {
 785                 kfree(ctx);
 786                 return -ERESTARTSYS;
 787         }
 788
 789         ctx->m2m_ctx = v4l2_m2m_ctx_init(pcdev->m2m_dev, ctx, &queue_init);
 790
 791         if (IS_ERR(ctx->m2m_ctx)) {
 792                 int ret = PTR_ERR(ctx->m2m_ctx);
 793
 794                 mutex_unlock(&pcdev->dev_mutex);
 795                 kfree(ctx);
 796                 return ret;
 797         }
 798
 799         clk_prepare_enable(pcdev->clk_emma_ipg);
 800         clk_prepare_enable(pcdev->clk_emma_ahb);
 801         ctx->q_data[V4L2_M2M_SRC].fmt = &formats[1];
 802         ctx->q_data[V4L2_M2M_DST].fmt = &formats[0];
 803         mutex_unlock(&pcdev->dev_mutex);
 804
 805         dprintk(pcdev, "Created instance %p, m2m_ctx: %p\n", ctx, ctx->m2m_ctx);
 806
 807         return 0;
 808 }
 809
 810 static int emmaprp_release(struct file *file)
 811 {
 812         struct emmaprp_dev *pcdev = video_drvdata(file);
 813         struct emmaprp_ctx *ctx = file->private_data;
 814
 815         dprintk(pcdev, "Releasing instance %p\n", ctx);
 816
 817         mutex_lock(&pcdev->dev_mutex);
 818         clk_disable_unprepare(pcdev->clk_emma_ahb);
 819         clk_disable_unprepare(pcdev->clk_emma_ipg);
 820         v4l2_m2m_ctx_release(ctx->m2m_ctx);
 821         mutex_unlock(&pcdev->dev_mutex);
 822         kfree(ctx);
 823
 824         return 0;
 825 }
 826
 827 static __poll_t emmaprp_poll(struct file *file,
 828                                  struct poll_table_struct *wait)
 829 {
 830         struct emmaprp_dev *pcdev = video_drvdata(file);
 831         struct emmaprp_ctx *ctx = file->private_data;
 832         __poll_t res;
 833
 834         mutex_lock(&pcdev->dev_mutex);
 835         res = v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
 836         mutex_unlock(&pcdev->dev_mutex);
 837         return res;
 838 }
 839
 840 static int emmaprp_mmap(struct file *file, struct vm_area_struct *vma)
 841 {
 842         struct emmaprp_dev *pcdev = video_drvdata(file);
 843         struct emmaprp_ctx *ctx = file->private_data;
 844         int ret;
 845
 846         if (mutex_lock_interruptible(&pcdev->dev_mutex))
 847                 return -ERESTARTSYS;
 848         ret = v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
 849         mutex_unlock(&pcdev->dev_mutex);
 850         return ret;
 851 }
 852
 853 static const struct v4l2_file_operations emmaprp_fops = {
 854         .owner          = THIS_MODULE,
 855         .open           = emmaprp_open,
 856         .release        = emmaprp_release,
 857         .poll           = emmaprp_poll,
 858         .unlocked_ioctl = video_ioctl2,
 859         .mmap           = emmaprp_mmap,
 860 };
 861
 862 static const struct video_device emmaprp_videodev = {
 863         .name           = MEM2MEM_NAME,
 864         .fops           = &emmaprp_fops,
 865         .ioctl_ops      = &emmaprp_ioctl_ops,
 866         .minor          = -1,
 867         .release        = video_device_release,
 868         .vfl_dir        = VFL_DIR_M2M,
 869 };
 870
 871 static const struct v4l2_m2m_ops m2m_ops = {
 872         .device_run     = emmaprp_device_run,
 873         .job_abort      = emmaprp_job_abort,
 874 };
 875
 876 static int emmaprp_probe(struct platform_device *pdev)
 877 {
 878         struct emmaprp_dev *pcdev;
 879         struct video_device *vfd;
 880         struct resource *res;
 881         int irq, ret;
 882
 883         pcdev = devm_kzalloc(&pdev->dev, sizeof(*pcdev), GFP_KERNEL);
 884         if (!pcdev)
 885                 return -ENOMEM;
 886
 887         spin_lock_init(&pcdev->irqlock);
 888
 889         pcdev->clk_emma_ipg = devm_clk_get(&pdev->dev, "ipg");
 890         if (IS_ERR(pcdev->clk_emma_ipg)) {
 891                 return PTR_ERR(pcdev->clk_emma_ipg);
 892         }
 893
 894         pcdev->clk_emma_ahb = devm_clk_get(&pdev->dev, "ahb");
 895         if (IS_ERR(pcdev->clk_emma_ahb))
 896                 return PTR_ERR(pcdev->clk_emma_ahb);
 897
 898         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 899         pcdev->base_emma = devm_ioremap_resource(&pdev->dev, res);
 900         if (IS_ERR(pcdev->base_emma))
 901                 return PTR_ERR(pcdev->base_emma);
 902
 903         ret = v4l2_device_register(&pdev->dev, &pcdev->v4l2_dev);
 904         if (ret)
 905                 return ret;
 906
 907         mutex_init(&pcdev->dev_mutex);
 908
 909         vfd = video_device_alloc();
 910         if (!vfd) {
 911                 v4l2_err(&pcdev->v4l2_dev, "Failed to allocate video device\n");
 912                 ret = -ENOMEM;
 913                 goto unreg_dev;
 914         }
 915
 916         *vfd = emmaprp_videodev;
 917         vfd->lock = &pcdev->dev_mutex;
 918         vfd->v4l2_dev = &pcdev->v4l2_dev;
 919
 920         video_set_drvdata(vfd, pcdev);
 921         pcdev->vfd = vfd;
 922         v4l2_info(&pcdev->v4l2_dev, EMMAPRP_MODULE_NAME
 923                   " Device registered as /dev/video%d\n", vfd->num);
 924
 925         platform_set_drvdata(pdev, pcdev);
 926
 927         irq = platform_get_irq(pdev, 0);
 928         if (irq < 0)
 929                 return irq;
 930         ret = devm_request_irq(&pdev->dev, irq, emmaprp_irq, 0,
 931                                dev_name(&pdev->dev), pcdev);
 932         if (ret)
 933                 goto rel_vdev;
 934
 935         pcdev->m2m_dev = v4l2_m2m_init(&m2m_ops);
 936         if (IS_ERR(pcdev->m2m_dev)) {
 937                 v4l2_err(&pcdev->v4l2_dev, "Failed to init mem2mem device\n");
 938                 ret = PTR_ERR(pcdev->m2m_dev);
 939                 goto rel_vdev;
 940         }
 941
 942         ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
 943         if (ret) {
 944                 v4l2_err(&pcdev->v4l2_dev, "Failed to register video device\n");
 945                 goto rel_m2m;
 946         }
 947
 948         return 0;
 949
 950
 951 rel_m2m:
 952         v4l2_m2m_release(pcdev->m2m_dev);
 953 rel_vdev:
 954         video_device_release(vfd);
 955 unreg_dev:
 956         v4l2_device_unregister(&pcdev->v4l2_dev);
 957
 958         mutex_destroy(&pcdev->dev_mutex);
 959
 960         return ret;
 961 }
 962
 963 static int emmaprp_remove(struct platform_device *pdev)
 964 {
 965         struct emmaprp_dev *pcdev = platform_get_drvdata(pdev);
 966
 967         v4l2_info(&pcdev->v4l2_dev, "Removing " EMMAPRP_MODULE_NAME);
 968
 969         video_unregister_device(pcdev->vfd);
 970         v4l2_m2m_release(pcdev->m2m_dev);
 971         v4l2_device_unregister(&pcdev->v4l2_dev);
 972         mutex_destroy(&pcdev->dev_mutex);
 973
 974         return 0;
 975 }
 976
 977 static struct platform_driver emmaprp_pdrv = {
 978         .probe          = emmaprp_probe,
 979         .remove         = emmaprp_remove,
 980         .driver         = {
 981                 .name   = MEM2MEM_NAME,
 982         },
 983 };
 984 module_platform_driver(emmaprp_pdrv);