linux/drivers/media/platform/mx2_emmaprp.c
<<
>>
Prefs
   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
  30MODULE_DESCRIPTION("Mem-to-mem device which supports eMMa-PrP present in mx2 SoCs");
  31MODULE_AUTHOR("Javier Martin <javier.martin@vista-silicon.com");
  32MODULE_LICENSE("GPL");
  33MODULE_VERSION("0.0.1");
  34
  35static bool debug;
  36module_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
 147struct emmaprp_fmt {
 148        u32     fourcc;
 149        /* Types the format can be used for */
 150        u32     types;
 151};
 152
 153static 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 */
 165struct emmaprp_q_data {
 166        unsigned int            width;
 167        unsigned int            height;
 168        unsigned int            sizeimage;
 169        struct emmaprp_fmt      *fmt;
 170};
 171
 172enum {
 173        V4L2_M2M_SRC = 0,
 174        V4L2_M2M_DST = 1,
 175};
 176
 177#define NUM_FORMATS ARRAY_SIZE(formats)
 178
 179static 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
 196struct 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
 209struct 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
 217static 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 */
 234static 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
 246static 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
 266static 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
 328static 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 */
 378static 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
 386static 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
 415static 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
 421static 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
 427static 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
 451static 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
 457static 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
 463static 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
 497static 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
 514static 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
 531static 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
 569static 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
 581static 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
 593static 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 */
 620static 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
 647static 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
 669static 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
 676static 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
 684static 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 */
 720static 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
 760static 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
 779static 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
 788static 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
 798static const struct v4l2_m2m_ops m2m_ops = {
 799        .device_run     = emmaprp_device_run,
 800        .job_abort      = emmaprp_job_abort,
 801};
 802
 803static 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
 878rel_m2m:
 879        v4l2_m2m_release(pcdev->m2m_dev);
 880rel_vdev:
 881        video_device_release(vfd);
 882unreg_dev:
 883        v4l2_device_unregister(&pcdev->v4l2_dev);
 884
 885        mutex_destroy(&pcdev->dev_mutex);
 886
 887        return ret;
 888}
 889
 890static 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
 904static struct platform_driver emmaprp_pdrv = {
 905        .probe          = emmaprp_probe,
 906        .remove         = emmaprp_remove,
 907        .driver         = {
 908                .name   = MEM2MEM_NAME,
 909        },
 910};
 911module_platform_driver(emmaprp_pdrv);
 912