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