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         (1UL << 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         u32     fourcc;
 149         /* Types the format can be used for */
 150         u32     types;
 151 };
 152
 153 static struct emmaprp_fmt formats[] = {
 154         {
 155                 .fourcc = V4L2_PIX_FMT_YUV420,
 156                 .types  = MEM2MEM_CAPTURE,
 157         },
 158         {
 159                 .fourcc = V4L2_PIX_FMT_YUYV,
 160                 .types  = MEM2MEM_OUTPUT,
 161         },
 162 };
 163
 164 /* Per-queue, driver-specific private data */
 165 struct emmaprp_q_data {
 166         unsigned int            width;
 167         unsigned int            height;
 168         unsigned int            sizeimage;
 169         struct emmaprp_fmt      *fmt;
 170 };
 171
 172 enum {
 173         V4L2_M2M_SRC = 0,
 174         V4L2_M2M_DST = 1,
 175 };
 176
 177 #define NUM_FORMATS ARRAY_SIZE(formats)
 178
 179 static struct emmaprp_fmt *find_format(struct v4l2_format *f)
 180 {
 181         struct emmaprp_fmt *fmt;
 182         unsigned int k;
 183
 184         for (k = 0; k < NUM_FORMATS; k++) {
 185                 fmt = &formats[k];
 186                 if (fmt->fourcc == f->fmt.pix.pixelformat)
 187                         break;
 188         }
 189
 190         if (k == NUM_FORMATS)
 191                 return NULL;
 192
 193         return &formats[k];
 194 }
 195
 196 struct emmaprp_dev {
 197         struct v4l2_device      v4l2_dev;
 198         struct video_device     *vfd;
 199
 200         struct mutex            dev_mutex;
 201         spinlock_t              irqlock;
 202
 203         void __iomem            *base_emma;
 204         struct clk              *clk_emma_ahb, *clk_emma_ipg;
 205
 206         struct v4l2_m2m_dev     *m2m_dev;
 207 };
 208
 209 struct emmaprp_ctx {
 210         struct v4l2_fh          fh;
 211         struct emmaprp_dev      *dev;
 212         /* Abort requested by m2m */
 213         int                     aborting;
 214         struct emmaprp_q_data   q_data[2];
 215 };
 216
 217 static struct emmaprp_q_data *get_q_data(struct emmaprp_ctx *ctx,
 218                                          enum v4l2_buf_type type)
 219 {
 220         switch (type) {
 221         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 222                 return &(ctx->q_data[V4L2_M2M_SRC]);
 223         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 224                 return &(ctx->q_data[V4L2_M2M_DST]);
 225         default:
 226                 BUG();
 227         }
 228         return NULL;
 229 }
 230
 231 /*
 232  * mem2mem callbacks
 233  */
 234 static void emmaprp_job_abort(void *priv)
 235 {
 236         struct emmaprp_ctx *ctx = priv;
 237         struct emmaprp_dev *pcdev = ctx->dev;
 238
 239         ctx->aborting = 1;
 240
 241         dprintk(pcdev, "Aborting task\n");
 242
 243         v4l2_m2m_job_finish(pcdev->m2m_dev, ctx->fh.m2m_ctx);
 244 }
 245
 246 static inline void emmaprp_dump_regs(struct emmaprp_dev *pcdev)
 247 {
 248         dprintk(pcdev,
 249                 "eMMa-PrP Registers:\n"
 250                 "  SOURCE_Y_PTR = 0x%08X\n"
 251                 "  SRC_FRAME_SIZE = 0x%08X\n"
 252                 "  DEST_Y_PTR = 0x%08X\n"
 253                 "  DEST_CR_PTR = 0x%08X\n"
 254                 "  DEST_CB_PTR = 0x%08X\n"
 255                 "  CH2_OUT_IMAGE_SIZE = 0x%08X\n"
 256                 "  CNTL = 0x%08X\n",
 257                 readl(pcdev->base_emma + PRP_SOURCE_Y_PTR),
 258                 readl(pcdev->base_emma + PRP_SRC_FRAME_SIZE),
 259                 readl(pcdev->base_emma + PRP_DEST_Y_PTR),
 260                 readl(pcdev->base_emma + PRP_DEST_CR_PTR),
 261                 readl(pcdev->base_emma + PRP_DEST_CB_PTR),
 262                 readl(pcdev->base_emma + PRP_CH2_OUT_IMAGE_SIZE),
 263                 readl(pcdev->base_emma + PRP_CNTL));
 264 }
 265
 266 static void emmaprp_device_run(void *priv)
 267 {
 268         struct emmaprp_ctx *ctx = priv;
 269         struct emmaprp_q_data *s_q_data, *d_q_data;
 270         struct vb2_v4l2_buffer *src_buf, *dst_buf;
 271         struct emmaprp_dev *pcdev = ctx->dev;
 272         unsigned int s_width, s_height;
 273         unsigned int d_width, d_height;
 274         unsigned int d_size;
 275         dma_addr_t p_in, p_out;
 276         u32 tmp;
 277
 278         src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
 279         dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
 280
 281         s_q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
 282         s_width = s_q_data->width;
 283         s_height = s_q_data->height;
 284
 285         d_q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
 286         d_width = d_q_data->width;
 287         d_height = d_q_data->height;
 288         d_size = d_width * d_height;
 289
 290         p_in = vb2_dma_contig_plane_dma_addr(&src_buf->vb2_buf, 0);
 291         p_out = vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf, 0);
 292         if (!p_in || !p_out) {
 293                 v4l2_err(&pcdev->v4l2_dev,
 294                          "Acquiring kernel pointers to buffers failed\n");
 295                 return;
 296         }
 297
 298         /* Input frame parameters */
 299         writel(p_in, pcdev->base_emma + PRP_SOURCE_Y_PTR);
 300         writel(PRP_SIZE_WIDTH(s_width) | PRP_SIZE_HEIGHT(s_height),
 301                pcdev->base_emma + PRP_SRC_FRAME_SIZE);
 302
 303         /* Output frame parameters */
 304         writel(p_out, pcdev->base_emma + PRP_DEST_Y_PTR);
 305         writel(p_out + d_size, pcdev->base_emma + PRP_DEST_CB_PTR);
 306         writel(p_out + d_size + (d_size >> 2),
 307                pcdev->base_emma + PRP_DEST_CR_PTR);
 308         writel(PRP_SIZE_WIDTH(d_width) | PRP_SIZE_HEIGHT(d_height),
 309                pcdev->base_emma + PRP_CH2_OUT_IMAGE_SIZE);
 310
 311         /* IRQ configuration */
 312         tmp = readl(pcdev->base_emma + PRP_INTR_CNTL);
 313         writel(tmp | PRP_INTR_RDERR |
 314                 PRP_INTR_CH2WERR |
 315                 PRP_INTR_CH2FC,
 316                 pcdev->base_emma + PRP_INTR_CNTL);
 317
 318         emmaprp_dump_regs(pcdev);
 319
 320         /* Enable transfer */
 321         tmp = readl(pcdev->base_emma + PRP_CNTL);
 322         writel(tmp | PRP_CNTL_CH2_OUT_YUV420 |
 323                 PRP_CNTL_DATA_IN_YUV422 |
 324                 PRP_CNTL_CH2EN,
 325                 pcdev->base_emma + PRP_CNTL);
 326 }
 327
 328 static irqreturn_t emmaprp_irq(int irq_emma, void *data)
 329 {
 330         struct emmaprp_dev *pcdev = data;
 331         struct emmaprp_ctx *curr_ctx;
 332         struct vb2_v4l2_buffer *src_vb, *dst_vb;
 333         unsigned long flags;
 334         u32 irqst;
 335
 336         /* Check irq flags and clear irq */
 337         irqst = readl(pcdev->base_emma + PRP_INTRSTATUS);
 338         writel(irqst, pcdev->base_emma + PRP_INTRSTATUS);
 339         dprintk(pcdev, "irqst = 0x%08x\n", irqst);
 340
 341         curr_ctx = v4l2_m2m_get_curr_priv(pcdev->m2m_dev);
 342         if (curr_ctx == NULL) {
 343                 pr_err("Instance released before the end of transaction\n");
 344                 return IRQ_HANDLED;
 345         }
 346
 347         if (!curr_ctx->aborting) {
 348                 if ((irqst & PRP_INTR_ST_RDERR) ||
 349                 (irqst & PRP_INTR_ST_CH2WERR)) {
 350                         pr_err("PrP bus error occurred, this transfer is probably corrupted\n");
 351                         writel(PRP_CNTL_SWRST, pcdev->base_emma + PRP_CNTL);
 352                 } else if (irqst & PRP_INTR_ST_CH2B1CI) { /* buffer ready */
 353                         src_vb = v4l2_m2m_src_buf_remove(curr_ctx->fh.m2m_ctx);
 354                         dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->fh.m2m_ctx);
 355
 356                         dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp;
 357                         dst_vb->flags &=
 358                                 ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 359                         dst_vb->flags |=
 360                                 src_vb->flags
 361                                 & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 362                         dst_vb->timecode = src_vb->timecode;
 363
 364                         spin_lock_irqsave(&pcdev->irqlock, flags);
 365                         v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
 366                         v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
 367                         spin_unlock_irqrestore(&pcdev->irqlock, flags);
 368                 }
 369         }
 370
 371         v4l2_m2m_job_finish(pcdev->m2m_dev, curr_ctx->fh.m2m_ctx);
 372         return IRQ_HANDLED;
 373 }
 374
 375 /*
 376  * video ioctls
 377  */
 378 static int vidioc_querycap(struct file *file, void *priv,
 379                            struct v4l2_capability *cap)
 380 {
 381         strscpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver));
 382         strscpy(cap->card, MEM2MEM_NAME, sizeof(cap->card));
 383         return 0;
 384 }
 385
 386 static int enum_fmt(struct v4l2_fmtdesc *f, u32 type)
 387 {
 388         int i, num;
 389         struct emmaprp_fmt *fmt;
 390
 391         num = 0;
 392
 393         for (i = 0; i < NUM_FORMATS; ++i) {
 394                 if (formats[i].types & type) {
 395                         /* index-th format of type type found ? */
 396                         if (num == f->index)
 397                                 break;
 398                         /* Correct type but haven't reached our index yet,
 399                          * just increment per-type index */
 400                         ++num;
 401                 }
 402         }
 403
 404         if (i < NUM_FORMATS) {
 405                 /* Format found */
 406                 fmt = &formats[i];
 407                 f->pixelformat = fmt->fourcc;
 408                 return 0;
 409         }
 410
 411         /* Format not found */
 412         return -EINVAL;
 413 }
 414
 415 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
 416                                    struct v4l2_fmtdesc *f)
 417 {
 418         return enum_fmt(f, MEM2MEM_CAPTURE);
 419 }
 420
 421 static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
 422                                    struct v4l2_fmtdesc *f)
 423 {
 424         return enum_fmt(f, MEM2MEM_OUTPUT);
 425 }
 426
 427 static int vidioc_g_fmt(struct emmaprp_ctx *ctx, struct v4l2_format *f)
 428 {
 429         struct vb2_queue *vq;
 430         struct emmaprp_q_data *q_data;
 431
 432         vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 433         if (!vq)
 434                 return -EINVAL;
 435
 436         q_data = get_q_data(ctx, f->type);
 437
 438         f->fmt.pix.width        = q_data->width;
 439         f->fmt.pix.height       = q_data->height;
 440         f->fmt.pix.field        = V4L2_FIELD_NONE;
 441         f->fmt.pix.pixelformat  = q_data->fmt->fourcc;
 442         if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420)
 443                 f->fmt.pix.bytesperline = q_data->width * 3 / 2;
 444         else /* YUYV */
 445                 f->fmt.pix.bytesperline = q_data->width * 2;
 446         f->fmt.pix.sizeimage    = q_data->sizeimage;
 447
 448         return 0;
 449 }
 450
 451 static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
 452                                 struct v4l2_format *f)
 453 {
 454         return vidioc_g_fmt(priv, f);
 455 }
 456
 457 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 458                                 struct v4l2_format *f)
 459 {
 460         return vidioc_g_fmt(priv, f);
 461 }
 462
 463 static int vidioc_try_fmt(struct v4l2_format *f)
 464 {
 465         enum v4l2_field field;
 466
 467
 468         if (!find_format(f))
 469                 return -EINVAL;
 470
 471         field = f->fmt.pix.field;
 472         if (field == V4L2_FIELD_ANY)
 473                 field = V4L2_FIELD_NONE;
 474         else if (V4L2_FIELD_NONE != field)
 475                 return -EINVAL;
 476
 477         /* V4L2 specification suggests the driver corrects the format struct
 478          * if any of the dimensions is unsupported */
 479         f->fmt.pix.field = field;
 480
 481         if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420) {
 482                 v4l_bound_align_image(&f->fmt.pix.width, MIN_W, MAX_W,
 483                                       W_ALIGN_YUV420, &f->fmt.pix.height,
 484                                       MIN_H, MAX_H, H_ALIGN, S_ALIGN);
 485                 f->fmt.pix.bytesperline = f->fmt.pix.width * 3 / 2;
 486         } else {
 487                 v4l_bound_align_image(&f->fmt.pix.width, MIN_W, MAX_W,
 488                                       W_ALIGN_OTHERS, &f->fmt.pix.height,
 489                                       MIN_H, MAX_H, H_ALIGN, S_ALIGN);
 490                 f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
 491         }
 492         f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
 493
 494         return 0;
 495 }
 496
 497 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 498                                   struct v4l2_format *f)
 499 {
 500         struct emmaprp_fmt *fmt;
 501         struct emmaprp_ctx *ctx = priv;
 502
 503         fmt = find_format(f);
 504         if (!fmt || !(fmt->types & MEM2MEM_CAPTURE)) {
 505                 v4l2_err(&ctx->dev->v4l2_dev,
 506                          "Fourcc format (0x%08x) invalid.\n",
 507                          f->fmt.pix.pixelformat);
 508                 return -EINVAL;
 509         }
 510
 511         return vidioc_try_fmt(f);
 512 }
 513
 514 static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
 515                                   struct v4l2_format *f)
 516 {
 517         struct emmaprp_fmt *fmt;
 518         struct emmaprp_ctx *ctx = priv;
 519
 520         fmt = find_format(f);
 521         if (!fmt || !(fmt->types & MEM2MEM_OUTPUT)) {
 522                 v4l2_err(&ctx->dev->v4l2_dev,
 523                          "Fourcc format (0x%08x) invalid.\n",
 524                          f->fmt.pix.pixelformat);
 525                 return -EINVAL;
 526         }
 527
 528         return vidioc_try_fmt(f);
 529 }
 530
 531 static int vidioc_s_fmt(struct emmaprp_ctx *ctx, struct v4l2_format *f)
 532 {
 533         struct emmaprp_q_data *q_data;
 534         struct vb2_queue *vq;
 535         int ret;
 536
 537         vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 538         if (!vq)
 539                 return -EINVAL;
 540
 541         q_data = get_q_data(ctx, f->type);
 542         if (!q_data)
 543                 return -EINVAL;
 544
 545         if (vb2_is_busy(vq)) {
 546                 v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
 547                 return -EBUSY;
 548         }
 549
 550         ret = vidioc_try_fmt(f);
 551         if (ret)
 552                 return ret;
 553
 554         q_data->fmt             = find_format(f);
 555         q_data->width           = f->fmt.pix.width;
 556         q_data->height          = f->fmt.pix.height;
 557         if (q_data->fmt->fourcc == V4L2_PIX_FMT_YUV420)
 558                 q_data->sizeimage = q_data->width * q_data->height * 3 / 2;
 559         else /* YUYV */
 560                 q_data->sizeimage = q_data->width * q_data->height * 2;
 561
 562         dprintk(ctx->dev,
 563                 "Setting format for type %d, wxh: %dx%d, fmt: %d\n",
 564                 f->type, q_data->width, q_data->height, q_data->fmt->fourcc);
 565
 566         return 0;
 567 }
 568
 569 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 570                                 struct v4l2_format *f)
 571 {
 572         int ret;
 573
 574         ret = vidioc_try_fmt_vid_cap(file, priv, f);
 575         if (ret)
 576                 return ret;
 577
 578         return vidioc_s_fmt(priv, f);
 579 }
 580
 581 static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
 582                                 struct v4l2_format *f)
 583 {
 584         int ret;
 585
 586         ret = vidioc_try_fmt_vid_out(file, priv, f);
 587         if (ret)
 588                 return ret;
 589
 590         return vidioc_s_fmt(priv, f);
 591 }
 592
 593 static const struct v4l2_ioctl_ops emmaprp_ioctl_ops = {
 594         .vidioc_querycap        = vidioc_querycap,
 595
 596         .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
 597         .vidioc_g_fmt_vid_cap   = vidioc_g_fmt_vid_cap,
 598         .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
 599         .vidioc_s_fmt_vid_cap   = vidioc_s_fmt_vid_cap,
 600
 601         .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
 602         .vidioc_g_fmt_vid_out   = vidioc_g_fmt_vid_out,
 603         .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out,
 604         .vidioc_s_fmt_vid_out   = vidioc_s_fmt_vid_out,
 605
 606         .vidioc_reqbufs         = v4l2_m2m_ioctl_reqbufs,
 607         .vidioc_querybuf        = v4l2_m2m_ioctl_querybuf,
 608         .vidioc_qbuf            = v4l2_m2m_ioctl_qbuf,
 609         .vidioc_dqbuf           = v4l2_m2m_ioctl_dqbuf,
 610         .vidioc_prepare_buf     = v4l2_m2m_ioctl_prepare_buf,
 611         .vidioc_expbuf          = v4l2_m2m_ioctl_expbuf,
 612         .vidioc_streamon        = v4l2_m2m_ioctl_streamon,
 613         .vidioc_streamoff       = v4l2_m2m_ioctl_streamoff,
 614 };
 615
 616
 617 /*
 618  * Queue operations
 619  */
 620 static int emmaprp_queue_setup(struct vb2_queue *vq,
 621                                 unsigned int *nbuffers, unsigned int *nplanes,
 622                                 unsigned int sizes[], struct device *alloc_devs[])
 623 {
 624         struct emmaprp_ctx *ctx = vb2_get_drv_priv(vq);
 625         struct emmaprp_q_data *q_data;
 626         unsigned int size, count = *nbuffers;
 627
 628         q_data = get_q_data(ctx, vq->type);
 629
 630         if (q_data->fmt->fourcc == V4L2_PIX_FMT_YUV420)
 631                 size = q_data->width * q_data->height * 3 / 2;
 632         else
 633                 size = q_data->width * q_data->height * 2;
 634
 635         while (size * count > MEM2MEM_VID_MEM_LIMIT)
 636                 (count)--;
 637
 638         *nplanes = 1;
 639         *nbuffers = count;
 640         sizes[0] = size;
 641
 642         dprintk(ctx->dev, "get %d buffer(s) of size %d each.\n", count, size);
 643
 644         return 0;
 645 }
 646
 647 static int emmaprp_buf_prepare(struct vb2_buffer *vb)
 648 {
 649         struct emmaprp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 650         struct emmaprp_q_data *q_data;
 651
 652         dprintk(ctx->dev, "type: %d\n", vb->vb2_queue->type);
 653
 654         q_data = get_q_data(ctx, vb->vb2_queue->type);
 655
 656         if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
 657                 dprintk(ctx->dev,
 658                         "%s data will not fit into plane(%lu < %lu)\n",
 659                         __func__, vb2_plane_size(vb, 0),
 660                         (long)q_data->sizeimage);
 661                 return -EINVAL;
 662         }
 663
 664         vb2_set_plane_payload(vb, 0, q_data->sizeimage);
 665
 666         return 0;
 667 }
 668
 669 static void emmaprp_buf_queue(struct vb2_buffer *vb)
 670 {
 671         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 672         struct emmaprp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 673         v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
 674 }
 675
 676 static const struct vb2_ops emmaprp_qops = {
 677         .queue_setup     = emmaprp_queue_setup,
 678         .buf_prepare     = emmaprp_buf_prepare,
 679         .buf_queue       = emmaprp_buf_queue,
 680         .wait_prepare    = vb2_ops_wait_prepare,
 681         .wait_finish     = vb2_ops_wait_finish,
 682 };
 683
 684 static int queue_init(void *priv, struct vb2_queue *src_vq,
 685                       struct vb2_queue *dst_vq)
 686 {
 687         struct emmaprp_ctx *ctx = priv;
 688         int ret;
 689
 690         src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
 691         src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
 692         src_vq->drv_priv = ctx;
 693         src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 694         src_vq->ops = &emmaprp_qops;
 695         src_vq->mem_ops = &vb2_dma_contig_memops;
 696         src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 697         src_vq->dev = ctx->dev->v4l2_dev.dev;
 698         src_vq->lock = &ctx->dev->dev_mutex;
 699
 700         ret = vb2_queue_init(src_vq);
 701         if (ret)
 702                 return ret;
 703
 704         dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 705         dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
 706         dst_vq->drv_priv = ctx;
 707         dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 708         dst_vq->ops = &emmaprp_qops;
 709         dst_vq->mem_ops = &vb2_dma_contig_memops;
 710         dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 711         dst_vq->dev = ctx->dev->v4l2_dev.dev;
 712         dst_vq->lock = &ctx->dev->dev_mutex;
 713
 714         return vb2_queue_init(dst_vq);
 715 }
 716
 717 /*
 718  * File operations
 719  */
 720 static int emmaprp_open(struct file *file)
 721 {
 722         struct emmaprp_dev *pcdev = video_drvdata(file);
 723         struct emmaprp_ctx *ctx;
 724
 725         ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
 726         if (!ctx)
 727                 return -ENOMEM;
 728
 729         v4l2_fh_init(&ctx->fh, video_devdata(file));
 730         file->private_data = &ctx->fh;
 731         ctx->dev = pcdev;
 732
 733         if (mutex_lock_interruptible(&pcdev->dev_mutex)) {
 734                 kfree(ctx);
 735                 return -ERESTARTSYS;
 736         }
 737
 738         ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(pcdev->m2m_dev, ctx, &queue_init);
 739
 740         if (IS_ERR(ctx->fh.m2m_ctx)) {
 741                 int ret = PTR_ERR(ctx->fh.m2m_ctx);
 742
 743                 mutex_unlock(&pcdev->dev_mutex);
 744                 kfree(ctx);
 745                 return ret;
 746         }
 747
 748         clk_prepare_enable(pcdev->clk_emma_ipg);
 749         clk_prepare_enable(pcdev->clk_emma_ahb);
 750         ctx->q_data[V4L2_M2M_SRC].fmt = &formats[1];
 751         ctx->q_data[V4L2_M2M_DST].fmt = &formats[0];
 752         v4l2_fh_add(&ctx->fh);
 753         mutex_unlock(&pcdev->dev_mutex);
 754
 755         dprintk(pcdev, "Created instance %p, m2m_ctx: %p\n", ctx, ctx->fh.m2m_ctx);
 756
 757         return 0;
 758 }
 759
 760 static int emmaprp_release(struct file *file)
 761 {
 762         struct emmaprp_dev *pcdev = video_drvdata(file);
 763         struct emmaprp_ctx *ctx = file->private_data;
 764
 765         dprintk(pcdev, "Releasing instance %p\n", ctx);
 766
 767         mutex_lock(&pcdev->dev_mutex);
 768         clk_disable_unprepare(pcdev->clk_emma_ahb);
 769         clk_disable_unprepare(pcdev->clk_emma_ipg);
 770         v4l2_fh_del(&ctx->fh);
 771         v4l2_fh_exit(&ctx->fh);
 772         v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
 773         mutex_unlock(&pcdev->dev_mutex);
 774         kfree(ctx);
 775
 776         return 0;
 777 }
 778
 779 static const struct v4l2_file_operations emmaprp_fops = {
 780         .owner          = THIS_MODULE,
 781         .open           = emmaprp_open,
 782         .release        = emmaprp_release,
 783         .poll           = v4l2_m2m_fop_poll,
 784         .unlocked_ioctl = video_ioctl2,
 785         .mmap           = v4l2_m2m_fop_mmap,
 786 };
 787
 788 static const struct video_device emmaprp_videodev = {
 789         .name           = MEM2MEM_NAME,
 790         .fops           = &emmaprp_fops,
 791         .ioctl_ops      = &emmaprp_ioctl_ops,
 792         .minor          = -1,
 793         .release        = video_device_release,
 794         .vfl_dir        = VFL_DIR_M2M,
 795         .device_caps    = V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING,
 796 };
 797
 798 static const struct v4l2_m2m_ops m2m_ops = {
 799         .device_run     = emmaprp_device_run,
 800         .job_abort      = emmaprp_job_abort,
 801 };
 802
 803 static int emmaprp_probe(struct platform_device *pdev)
 804 {
 805         struct emmaprp_dev *pcdev;
 806         struct video_device *vfd;
 807         struct resource *res;
 808         int irq, ret;
 809
 810         pcdev = devm_kzalloc(&pdev->dev, sizeof(*pcdev), GFP_KERNEL);
 811         if (!pcdev)
 812                 return -ENOMEM;
 813
 814         spin_lock_init(&pcdev->irqlock);
 815
 816         pcdev->clk_emma_ipg = devm_clk_get(&pdev->dev, "ipg");
 817         if (IS_ERR(pcdev->clk_emma_ipg)) {
 818                 return PTR_ERR(pcdev->clk_emma_ipg);
 819         }
 820
 821         pcdev->clk_emma_ahb = devm_clk_get(&pdev->dev, "ahb");
 822         if (IS_ERR(pcdev->clk_emma_ahb))
 823                 return PTR_ERR(pcdev->clk_emma_ahb);
 824
 825         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 826         pcdev->base_emma = devm_ioremap_resource(&pdev->dev, res);
 827         if (IS_ERR(pcdev->base_emma))
 828                 return PTR_ERR(pcdev->base_emma);
 829
 830         ret = v4l2_device_register(&pdev->dev, &pcdev->v4l2_dev);
 831         if (ret)
 832                 return ret;
 833
 834         mutex_init(&pcdev->dev_mutex);
 835
 836         vfd = video_device_alloc();
 837         if (!vfd) {
 838                 v4l2_err(&pcdev->v4l2_dev, "Failed to allocate video device\n");
 839                 ret = -ENOMEM;
 840                 goto unreg_dev;
 841         }
 842
 843         *vfd = emmaprp_videodev;
 844         vfd->lock = &pcdev->dev_mutex;
 845         vfd->v4l2_dev = &pcdev->v4l2_dev;
 846
 847         video_set_drvdata(vfd, pcdev);
 848         pcdev->vfd = vfd;
 849         v4l2_info(&pcdev->v4l2_dev, EMMAPRP_MODULE_NAME
 850                   " Device registered as /dev/video%d\n", vfd->num);
 851
 852         platform_set_drvdata(pdev, pcdev);
 853
 854         irq = platform_get_irq(pdev, 0);
 855         if (irq < 0)
 856                 return irq;
 857         ret = devm_request_irq(&pdev->dev, irq, emmaprp_irq, 0,
 858                                dev_name(&pdev->dev), pcdev);
 859         if (ret)
 860                 goto rel_vdev;
 861
 862         pcdev->m2m_dev = v4l2_m2m_init(&m2m_ops);
 863         if (IS_ERR(pcdev->m2m_dev)) {
 864                 v4l2_err(&pcdev->v4l2_dev, "Failed to init mem2mem device\n");
 865                 ret = PTR_ERR(pcdev->m2m_dev);
 866                 goto rel_vdev;
 867         }
 868
 869         ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
 870         if (ret) {
 871                 v4l2_err(&pcdev->v4l2_dev, "Failed to register video device\n");
 872                 goto rel_m2m;
 873         }
 874
 875         return 0;
 876
 877
 878 rel_m2m:
 879         v4l2_m2m_release(pcdev->m2m_dev);
 880 rel_vdev:
 881         video_device_release(vfd);
 882 unreg_dev:
 883         v4l2_device_unregister(&pcdev->v4l2_dev);
 884
 885         mutex_destroy(&pcdev->dev_mutex);
 886
 887         return ret;
 888 }
 889
 890 static int emmaprp_remove(struct platform_device *pdev)
 891 {
 892         struct emmaprp_dev *pcdev = platform_get_drvdata(pdev);
 893
 894         v4l2_info(&pcdev->v4l2_dev, "Removing " EMMAPRP_MODULE_NAME);
 895
 896         video_unregister_device(pcdev->vfd);
 897         v4l2_m2m_release(pcdev->m2m_dev);
 898         v4l2_device_unregister(&pcdev->v4l2_dev);
 899         mutex_destroy(&pcdev->dev_mutex);
 900
 901         return 0;
 902 }
 903
 904 static struct platform_driver emmaprp_pdrv = {
 905         .probe          = emmaprp_probe,
 906         .remove         = emmaprp_remove,
 907         .driver         = {
 908                 .name   = MEM2MEM_NAME,
 909         },
 910 };
 911 module_platform_driver(emmaprp_pdrv);