linux/drivers/media/platform/vim2m.c
<<
>>
Prefs
   1/*
   2 * A virtual v4l2-mem2mem example device.
   3 *
   4 * This is a virtual device driver for testing mem-to-mem videobuf framework.
   5 * It simulates a device that uses memory buffers for both source and
   6 * destination, processes the data and issues an "irq" (simulated by a timer).
   7 * The device is capable of multi-instance, multi-buffer-per-transaction
   8 * operation (via the mem2mem framework).
   9 *
  10 * Copyright (c) 2009-2010 Samsung Electronics Co., Ltd.
  11 * Pawel Osciak, <pawel@osciak.com>
  12 * Marek Szyprowski, <m.szyprowski@samsung.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/delay.h>
  21#include <linux/fs.h>
  22#include <linux/timer.h>
  23#include <linux/sched.h>
  24#include <linux/slab.h>
  25
  26#include <linux/platform_device.h>
  27#include <media/v4l2-mem2mem.h>
  28#include <media/v4l2-device.h>
  29#include <media/v4l2-ioctl.h>
  30#include <media/v4l2-ctrls.h>
  31#include <media/v4l2-event.h>
  32#include <media/videobuf2-vmalloc.h>
  33
  34MODULE_DESCRIPTION("Virtual device for mem2mem framework testing");
  35MODULE_AUTHOR("Pawel Osciak, <pawel@osciak.com>");
  36MODULE_LICENSE("GPL");
  37MODULE_VERSION("0.1.1");
  38MODULE_ALIAS("mem2mem_testdev");
  39
  40static unsigned debug;
  41module_param(debug, uint, 0644);
  42MODULE_PARM_DESC(debug, "activates debug info");
  43
  44#define MIN_W 32
  45#define MIN_H 32
  46#define MAX_W 640
  47#define MAX_H 480
  48#define DIM_ALIGN_MASK 7 /* 8-byte alignment for line length */
  49
  50/* Flags that indicate a format can be used for capture/output */
  51#define MEM2MEM_CAPTURE (1 << 0)
  52#define MEM2MEM_OUTPUT  (1 << 1)
  53
  54#define MEM2MEM_NAME            "vim2m"
  55
  56/* Per queue */
  57#define MEM2MEM_DEF_NUM_BUFS    VIDEO_MAX_FRAME
  58/* In bytes, per queue */
  59#define MEM2MEM_VID_MEM_LIMIT   (16 * 1024 * 1024)
  60
  61/* Default transaction time in msec */
  62#define MEM2MEM_DEF_TRANSTIME   40
  63#define MEM2MEM_COLOR_STEP      (0xff >> 4)
  64#define MEM2MEM_NUM_TILES       8
  65
  66/* Flags that indicate processing mode */
  67#define MEM2MEM_HFLIP   (1 << 0)
  68#define MEM2MEM_VFLIP   (1 << 1)
  69
  70#define dprintk(dev, fmt, arg...) \
  71        v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg)
  72
  73
  74static void vim2m_dev_release(struct device *dev)
  75{}
  76
  77static struct platform_device vim2m_pdev = {
  78        .name           = MEM2MEM_NAME,
  79        .dev.release    = vim2m_dev_release,
  80};
  81
  82struct vim2m_fmt {
  83        u32     fourcc;
  84        int     depth;
  85        /* Types the format can be used for */
  86        u32     types;
  87};
  88
  89static struct vim2m_fmt formats[] = {
  90        {
  91                .fourcc = V4L2_PIX_FMT_RGB565X, /* rrrrrggg gggbbbbb */
  92                .depth  = 16,
  93                /* Both capture and output format */
  94                .types  = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
  95        },
  96        {
  97                .fourcc = V4L2_PIX_FMT_YUYV,
  98                .depth  = 16,
  99                /* Output-only format */
 100                .types  = MEM2MEM_OUTPUT,
 101        },
 102};
 103
 104#define NUM_FORMATS ARRAY_SIZE(formats)
 105
 106/* Per-queue, driver-specific private data */
 107struct vim2m_q_data {
 108        unsigned int            width;
 109        unsigned int            height;
 110        unsigned int            sizeimage;
 111        unsigned int            sequence;
 112        struct vim2m_fmt        *fmt;
 113};
 114
 115enum {
 116        V4L2_M2M_SRC = 0,
 117        V4L2_M2M_DST = 1,
 118};
 119
 120#define V4L2_CID_TRANS_TIME_MSEC        (V4L2_CID_USER_BASE + 0x1000)
 121#define V4L2_CID_TRANS_NUM_BUFS         (V4L2_CID_USER_BASE + 0x1001)
 122
 123static struct vim2m_fmt *find_format(struct v4l2_format *f)
 124{
 125        struct vim2m_fmt *fmt;
 126        unsigned int k;
 127
 128        for (k = 0; k < NUM_FORMATS; k++) {
 129                fmt = &formats[k];
 130                if (fmt->fourcc == f->fmt.pix.pixelformat)
 131                        break;
 132        }
 133
 134        if (k == NUM_FORMATS)
 135                return NULL;
 136
 137        return &formats[k];
 138}
 139
 140struct vim2m_dev {
 141        struct v4l2_device      v4l2_dev;
 142        struct video_device     vfd;
 143
 144        atomic_t                num_inst;
 145        struct mutex            dev_mutex;
 146        spinlock_t              irqlock;
 147
 148        struct timer_list       timer;
 149
 150        struct v4l2_m2m_dev     *m2m_dev;
 151};
 152
 153struct vim2m_ctx {
 154        struct v4l2_fh          fh;
 155        struct vim2m_dev        *dev;
 156
 157        struct v4l2_ctrl_handler hdl;
 158
 159        /* Processed buffers in this transaction */
 160        u8                      num_processed;
 161
 162        /* Transaction length (i.e. how many buffers per transaction) */
 163        u32                     translen;
 164        /* Transaction time (i.e. simulated processing time) in milliseconds */
 165        u32                     transtime;
 166
 167        /* Abort requested by m2m */
 168        int                     aborting;
 169
 170        /* Processing mode */
 171        int                     mode;
 172
 173        enum v4l2_colorspace    colorspace;
 174
 175        /* Source and destination queue data */
 176        struct vim2m_q_data   q_data[2];
 177};
 178
 179static inline struct vim2m_ctx *file2ctx(struct file *file)
 180{
 181        return container_of(file->private_data, struct vim2m_ctx, fh);
 182}
 183
 184static struct vim2m_q_data *get_q_data(struct vim2m_ctx *ctx,
 185                                         enum v4l2_buf_type type)
 186{
 187        switch (type) {
 188        case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 189                return &ctx->q_data[V4L2_M2M_SRC];
 190        case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 191                return &ctx->q_data[V4L2_M2M_DST];
 192        default:
 193                BUG();
 194        }
 195        return NULL;
 196}
 197
 198
 199static int device_process(struct vim2m_ctx *ctx,
 200                          struct vb2_v4l2_buffer *in_vb,
 201                          struct vb2_v4l2_buffer *out_vb)
 202{
 203        struct vim2m_dev *dev = ctx->dev;
 204        struct vim2m_q_data *q_data;
 205        u8 *p_in, *p_out;
 206        int x, y, t, w;
 207        int tile_w, bytes_left;
 208        int width, height, bytesperline;
 209
 210        q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
 211
 212        width   = q_data->width;
 213        height  = q_data->height;
 214        bytesperline    = (q_data->width * q_data->fmt->depth) >> 3;
 215
 216        p_in = vb2_plane_vaddr(&in_vb->vb2_buf, 0);
 217        p_out = vb2_plane_vaddr(&out_vb->vb2_buf, 0);
 218        if (!p_in || !p_out) {
 219                v4l2_err(&dev->v4l2_dev,
 220                         "Acquiring kernel pointers to buffers failed\n");
 221                return -EFAULT;
 222        }
 223
 224        if (vb2_plane_size(&in_vb->vb2_buf, 0) >
 225                        vb2_plane_size(&out_vb->vb2_buf, 0)) {
 226                v4l2_err(&dev->v4l2_dev, "Output buffer is too small\n");
 227                return -EINVAL;
 228        }
 229
 230        tile_w = (width * (q_data[V4L2_M2M_DST].fmt->depth >> 3))
 231                / MEM2MEM_NUM_TILES;
 232        bytes_left = bytesperline - tile_w * MEM2MEM_NUM_TILES;
 233        w = 0;
 234
 235        out_vb->sequence =
 236                get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE)->sequence++;
 237        in_vb->sequence = q_data->sequence++;
 238        out_vb->timestamp = in_vb->timestamp;
 239
 240        if (in_vb->flags & V4L2_BUF_FLAG_TIMECODE)
 241                out_vb->timecode = in_vb->timecode;
 242        out_vb->field = in_vb->field;
 243        out_vb->flags = in_vb->flags &
 244                (V4L2_BUF_FLAG_TIMECODE |
 245                 V4L2_BUF_FLAG_KEYFRAME |
 246                 V4L2_BUF_FLAG_PFRAME |
 247                 V4L2_BUF_FLAG_BFRAME |
 248                 V4L2_BUF_FLAG_TSTAMP_SRC_MASK);
 249
 250        switch (ctx->mode) {
 251        case MEM2MEM_HFLIP | MEM2MEM_VFLIP:
 252                p_out += bytesperline * height - bytes_left;
 253                for (y = 0; y < height; ++y) {
 254                        for (t = 0; t < MEM2MEM_NUM_TILES; ++t) {
 255                                if (w & 0x1) {
 256                                        for (x = 0; x < tile_w; ++x)
 257                                                *--p_out = *p_in++ +
 258                                                        MEM2MEM_COLOR_STEP;
 259                                } else {
 260                                        for (x = 0; x < tile_w; ++x)
 261                                                *--p_out = *p_in++ -
 262                                                        MEM2MEM_COLOR_STEP;
 263                                }
 264                                ++w;
 265                        }
 266                        p_in += bytes_left;
 267                        p_out -= bytes_left;
 268                }
 269                break;
 270
 271        case MEM2MEM_HFLIP:
 272                for (y = 0; y < height; ++y) {
 273                        p_out += MEM2MEM_NUM_TILES * tile_w;
 274                        for (t = 0; t < MEM2MEM_NUM_TILES; ++t) {
 275                                if (w & 0x01) {
 276                                        for (x = 0; x < tile_w; ++x)
 277                                                *--p_out = *p_in++ +
 278                                                        MEM2MEM_COLOR_STEP;
 279                                } else {
 280                                        for (x = 0; x < tile_w; ++x)
 281                                                *--p_out = *p_in++ -
 282                                                        MEM2MEM_COLOR_STEP;
 283                                }
 284                                ++w;
 285                        }
 286                        p_in += bytes_left;
 287                        p_out += bytesperline;
 288                }
 289                break;
 290
 291        case MEM2MEM_VFLIP:
 292                p_out += bytesperline * (height - 1);
 293                for (y = 0; y < height; ++y) {
 294                        for (t = 0; t < MEM2MEM_NUM_TILES; ++t) {
 295                                if (w & 0x1) {
 296                                        for (x = 0; x < tile_w; ++x)
 297                                                *p_out++ = *p_in++ +
 298                                                        MEM2MEM_COLOR_STEP;
 299                                } else {
 300                                        for (x = 0; x < tile_w; ++x)
 301                                                *p_out++ = *p_in++ -
 302                                                        MEM2MEM_COLOR_STEP;
 303                                }
 304                                ++w;
 305                        }
 306                        p_in += bytes_left;
 307                        p_out += bytes_left - 2 * bytesperline;
 308                }
 309                break;
 310
 311        default:
 312                for (y = 0; y < height; ++y) {
 313                        for (t = 0; t < MEM2MEM_NUM_TILES; ++t) {
 314                                if (w & 0x1) {
 315                                        for (x = 0; x < tile_w; ++x)
 316                                                *p_out++ = *p_in++ +
 317                                                        MEM2MEM_COLOR_STEP;
 318                                } else {
 319                                        for (x = 0; x < tile_w; ++x)
 320                                                *p_out++ = *p_in++ -
 321                                                        MEM2MEM_COLOR_STEP;
 322                                }
 323                                ++w;
 324                        }
 325                        p_in += bytes_left;
 326                        p_out += bytes_left;
 327                }
 328        }
 329
 330        return 0;
 331}
 332
 333static void schedule_irq(struct vim2m_dev *dev, int msec_timeout)
 334{
 335        dprintk(dev, "Scheduling a simulated irq\n");
 336        mod_timer(&dev->timer, jiffies + msecs_to_jiffies(msec_timeout));
 337}
 338
 339/*
 340 * mem2mem callbacks
 341 */
 342
 343/**
 344 * job_ready() - check whether an instance is ready to be scheduled to run
 345 */
 346static int job_ready(void *priv)
 347{
 348        struct vim2m_ctx *ctx = priv;
 349
 350        if (v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx) < ctx->translen
 351            || v4l2_m2m_num_dst_bufs_ready(ctx->fh.m2m_ctx) < ctx->translen) {
 352                dprintk(ctx->dev, "Not enough buffers available\n");
 353                return 0;
 354        }
 355
 356        return 1;
 357}
 358
 359static void job_abort(void *priv)
 360{
 361        struct vim2m_ctx *ctx = priv;
 362
 363        /* Will cancel the transaction in the next interrupt handler */
 364        ctx->aborting = 1;
 365}
 366
 367/* device_run() - prepares and starts the device
 368 *
 369 * This simulates all the immediate preparations required before starting
 370 * a device. This will be called by the framework when it decides to schedule
 371 * a particular instance.
 372 */
 373static void device_run(void *priv)
 374{
 375        struct vim2m_ctx *ctx = priv;
 376        struct vim2m_dev *dev = ctx->dev;
 377        struct vb2_v4l2_buffer *src_buf, *dst_buf;
 378
 379        src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
 380        dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
 381
 382        device_process(ctx, src_buf, dst_buf);
 383
 384        /* Run a timer, which simulates a hardware irq  */
 385        schedule_irq(dev, ctx->transtime);
 386}
 387
 388static void device_isr(unsigned long priv)
 389{
 390        struct vim2m_dev *vim2m_dev = (struct vim2m_dev *)priv;
 391        struct vim2m_ctx *curr_ctx;
 392        struct vb2_v4l2_buffer *src_vb, *dst_vb;
 393        unsigned long flags;
 394
 395        curr_ctx = v4l2_m2m_get_curr_priv(vim2m_dev->m2m_dev);
 396
 397        if (NULL == curr_ctx) {
 398                pr_err("Instance released before the end of transaction\n");
 399                return;
 400        }
 401
 402        src_vb = v4l2_m2m_src_buf_remove(curr_ctx->fh.m2m_ctx);
 403        dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->fh.m2m_ctx);
 404
 405        curr_ctx->num_processed++;
 406
 407        spin_lock_irqsave(&vim2m_dev->irqlock, flags);
 408        v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
 409        v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
 410        spin_unlock_irqrestore(&vim2m_dev->irqlock, flags);
 411
 412        if (curr_ctx->num_processed == curr_ctx->translen
 413            || curr_ctx->aborting) {
 414                dprintk(curr_ctx->dev, "Finishing transaction\n");
 415                curr_ctx->num_processed = 0;
 416                v4l2_m2m_job_finish(vim2m_dev->m2m_dev, curr_ctx->fh.m2m_ctx);
 417        } else {
 418                device_run(curr_ctx);
 419        }
 420}
 421
 422/*
 423 * video ioctls
 424 */
 425static int vidioc_querycap(struct file *file, void *priv,
 426                           struct v4l2_capability *cap)
 427{
 428        strncpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver) - 1);
 429        strncpy(cap->card, MEM2MEM_NAME, sizeof(cap->card) - 1);
 430        snprintf(cap->bus_info, sizeof(cap->bus_info),
 431                        "platform:%s", MEM2MEM_NAME);
 432        cap->device_caps = V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING;
 433        cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 434        return 0;
 435}
 436
 437static int enum_fmt(struct v4l2_fmtdesc *f, u32 type)
 438{
 439        int i, num;
 440        struct vim2m_fmt *fmt;
 441
 442        num = 0;
 443
 444        for (i = 0; i < NUM_FORMATS; ++i) {
 445                if (formats[i].types & type) {
 446                        /* index-th format of type type found ? */
 447                        if (num == f->index)
 448                                break;
 449                        /* Correct type but haven't reached our index yet,
 450                         * just increment per-type index */
 451                        ++num;
 452                }
 453        }
 454
 455        if (i < NUM_FORMATS) {
 456                /* Format found */
 457                fmt = &formats[i];
 458                f->pixelformat = fmt->fourcc;
 459                return 0;
 460        }
 461
 462        /* Format not found */
 463        return -EINVAL;
 464}
 465
 466static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
 467                                   struct v4l2_fmtdesc *f)
 468{
 469        return enum_fmt(f, MEM2MEM_CAPTURE);
 470}
 471
 472static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
 473                                   struct v4l2_fmtdesc *f)
 474{
 475        return enum_fmt(f, MEM2MEM_OUTPUT);
 476}
 477
 478static int vidioc_g_fmt(struct vim2m_ctx *ctx, struct v4l2_format *f)
 479{
 480        struct vb2_queue *vq;
 481        struct vim2m_q_data *q_data;
 482
 483        vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 484        if (!vq)
 485                return -EINVAL;
 486
 487        q_data = get_q_data(ctx, f->type);
 488
 489        f->fmt.pix.width        = q_data->width;
 490        f->fmt.pix.height       = q_data->height;
 491        f->fmt.pix.field        = V4L2_FIELD_NONE;
 492        f->fmt.pix.pixelformat  = q_data->fmt->fourcc;
 493        f->fmt.pix.bytesperline = (q_data->width * q_data->fmt->depth) >> 3;
 494        f->fmt.pix.sizeimage    = q_data->sizeimage;
 495        f->fmt.pix.colorspace   = ctx->colorspace;
 496
 497        return 0;
 498}
 499
 500static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
 501                                struct v4l2_format *f)
 502{
 503        return vidioc_g_fmt(file2ctx(file), f);
 504}
 505
 506static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 507                                struct v4l2_format *f)
 508{
 509        return vidioc_g_fmt(file2ctx(file), f);
 510}
 511
 512static int vidioc_try_fmt(struct v4l2_format *f, struct vim2m_fmt *fmt)
 513{
 514        /* V4L2 specification suggests the driver corrects the format struct
 515         * if any of the dimensions is unsupported */
 516        if (f->fmt.pix.height < MIN_H)
 517                f->fmt.pix.height = MIN_H;
 518        else if (f->fmt.pix.height > MAX_H)
 519                f->fmt.pix.height = MAX_H;
 520
 521        if (f->fmt.pix.width < MIN_W)
 522                f->fmt.pix.width = MIN_W;
 523        else if (f->fmt.pix.width > MAX_W)
 524                f->fmt.pix.width = MAX_W;
 525
 526        f->fmt.pix.width &= ~DIM_ALIGN_MASK;
 527        f->fmt.pix.bytesperline = (f->fmt.pix.width * fmt->depth) >> 3;
 528        f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
 529        f->fmt.pix.field = V4L2_FIELD_NONE;
 530
 531        return 0;
 532}
 533
 534static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 535                                  struct v4l2_format *f)
 536{
 537        struct vim2m_fmt *fmt;
 538        struct vim2m_ctx *ctx = file2ctx(file);
 539
 540        fmt = find_format(f);
 541        if (!fmt) {
 542                f->fmt.pix.pixelformat = formats[0].fourcc;
 543                fmt = find_format(f);
 544        }
 545        if (!(fmt->types & MEM2MEM_CAPTURE)) {
 546                v4l2_err(&ctx->dev->v4l2_dev,
 547                         "Fourcc format (0x%08x) invalid.\n",
 548                         f->fmt.pix.pixelformat);
 549                return -EINVAL;
 550        }
 551        f->fmt.pix.colorspace = ctx->colorspace;
 552
 553        return vidioc_try_fmt(f, fmt);
 554}
 555
 556static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
 557                                  struct v4l2_format *f)
 558{
 559        struct vim2m_fmt *fmt;
 560        struct vim2m_ctx *ctx = file2ctx(file);
 561
 562        fmt = find_format(f);
 563        if (!fmt) {
 564                f->fmt.pix.pixelformat = formats[0].fourcc;
 565                fmt = find_format(f);
 566        }
 567        if (!(fmt->types & MEM2MEM_OUTPUT)) {
 568                v4l2_err(&ctx->dev->v4l2_dev,
 569                         "Fourcc format (0x%08x) invalid.\n",
 570                         f->fmt.pix.pixelformat);
 571                return -EINVAL;
 572        }
 573        if (!f->fmt.pix.colorspace)
 574                f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
 575
 576        return vidioc_try_fmt(f, fmt);
 577}
 578
 579static int vidioc_s_fmt(struct vim2m_ctx *ctx, struct v4l2_format *f)
 580{
 581        struct vim2m_q_data *q_data;
 582        struct vb2_queue *vq;
 583
 584        vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 585        if (!vq)
 586                return -EINVAL;
 587
 588        q_data = get_q_data(ctx, f->type);
 589        if (!q_data)
 590                return -EINVAL;
 591
 592        if (vb2_is_busy(vq)) {
 593                v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
 594                return -EBUSY;
 595        }
 596
 597        q_data->fmt             = find_format(f);
 598        q_data->width           = f->fmt.pix.width;
 599        q_data->height          = f->fmt.pix.height;
 600        q_data->sizeimage       = q_data->width * q_data->height
 601                                * q_data->fmt->depth >> 3;
 602
 603        dprintk(ctx->dev,
 604                "Setting format for type %d, wxh: %dx%d, fmt: %d\n",
 605                f->type, q_data->width, q_data->height, q_data->fmt->fourcc);
 606
 607        return 0;
 608}
 609
 610static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 611                                struct v4l2_format *f)
 612{
 613        int ret;
 614
 615        ret = vidioc_try_fmt_vid_cap(file, priv, f);
 616        if (ret)
 617                return ret;
 618
 619        return vidioc_s_fmt(file2ctx(file), f);
 620}
 621
 622static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
 623                                struct v4l2_format *f)
 624{
 625        struct vim2m_ctx *ctx = file2ctx(file);
 626        int ret;
 627
 628        ret = vidioc_try_fmt_vid_out(file, priv, f);
 629        if (ret)
 630                return ret;
 631
 632        ret = vidioc_s_fmt(file2ctx(file), f);
 633        if (!ret)
 634                ctx->colorspace = f->fmt.pix.colorspace;
 635        return ret;
 636}
 637
 638static int vim2m_s_ctrl(struct v4l2_ctrl *ctrl)
 639{
 640        struct vim2m_ctx *ctx =
 641                container_of(ctrl->handler, struct vim2m_ctx, hdl);
 642
 643        switch (ctrl->id) {
 644        case V4L2_CID_HFLIP:
 645                if (ctrl->val)
 646                        ctx->mode |= MEM2MEM_HFLIP;
 647                else
 648                        ctx->mode &= ~MEM2MEM_HFLIP;
 649                break;
 650
 651        case V4L2_CID_VFLIP:
 652                if (ctrl->val)
 653                        ctx->mode |= MEM2MEM_VFLIP;
 654                else
 655                        ctx->mode &= ~MEM2MEM_VFLIP;
 656                break;
 657
 658        case V4L2_CID_TRANS_TIME_MSEC:
 659                ctx->transtime = ctrl->val;
 660                break;
 661
 662        case V4L2_CID_TRANS_NUM_BUFS:
 663                ctx->translen = ctrl->val;
 664                break;
 665
 666        default:
 667                v4l2_err(&ctx->dev->v4l2_dev, "Invalid control\n");
 668                return -EINVAL;
 669        }
 670
 671        return 0;
 672}
 673
 674static const struct v4l2_ctrl_ops vim2m_ctrl_ops = {
 675        .s_ctrl = vim2m_s_ctrl,
 676};
 677
 678
 679static const struct v4l2_ioctl_ops vim2m_ioctl_ops = {
 680        .vidioc_querycap        = vidioc_querycap,
 681
 682        .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
 683        .vidioc_g_fmt_vid_cap   = vidioc_g_fmt_vid_cap,
 684        .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
 685        .vidioc_s_fmt_vid_cap   = vidioc_s_fmt_vid_cap,
 686
 687        .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
 688        .vidioc_g_fmt_vid_out   = vidioc_g_fmt_vid_out,
 689        .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out,
 690        .vidioc_s_fmt_vid_out   = vidioc_s_fmt_vid_out,
 691
 692        .vidioc_reqbufs         = v4l2_m2m_ioctl_reqbufs,
 693        .vidioc_querybuf        = v4l2_m2m_ioctl_querybuf,
 694        .vidioc_qbuf            = v4l2_m2m_ioctl_qbuf,
 695        .vidioc_dqbuf           = v4l2_m2m_ioctl_dqbuf,
 696        .vidioc_prepare_buf     = v4l2_m2m_ioctl_prepare_buf,
 697        .vidioc_create_bufs     = v4l2_m2m_ioctl_create_bufs,
 698        .vidioc_expbuf          = v4l2_m2m_ioctl_expbuf,
 699
 700        .vidioc_streamon        = v4l2_m2m_ioctl_streamon,
 701        .vidioc_streamoff       = v4l2_m2m_ioctl_streamoff,
 702
 703        .vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
 704        .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 705};
 706
 707
 708/*
 709 * Queue operations
 710 */
 711
 712static int vim2m_queue_setup(struct vb2_queue *vq,
 713                                const void *parg,
 714                                unsigned int *nbuffers, unsigned int *nplanes,
 715                                unsigned int sizes[], void *alloc_ctxs[])
 716{
 717        const struct v4l2_format *fmt = parg;
 718        struct vim2m_ctx *ctx = vb2_get_drv_priv(vq);
 719        struct vim2m_q_data *q_data;
 720        unsigned int size, count = *nbuffers;
 721
 722        q_data = get_q_data(ctx, vq->type);
 723
 724        size = q_data->width * q_data->height * q_data->fmt->depth >> 3;
 725
 726        if (fmt) {
 727                if (fmt->fmt.pix.sizeimage < size)
 728                        return -EINVAL;
 729                size = fmt->fmt.pix.sizeimage;
 730        }
 731
 732        while (size * count > MEM2MEM_VID_MEM_LIMIT)
 733                (count)--;
 734
 735        *nplanes = 1;
 736        *nbuffers = count;
 737        sizes[0] = size;
 738
 739        /*
 740         * videobuf2-vmalloc allocator is context-less so no need to set
 741         * alloc_ctxs array.
 742         */
 743
 744        dprintk(ctx->dev, "get %d buffer(s) of size %d each.\n", count, size);
 745
 746        return 0;
 747}
 748
 749static int vim2m_buf_prepare(struct vb2_buffer *vb)
 750{
 751        struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 752        struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 753        struct vim2m_q_data *q_data;
 754
 755        dprintk(ctx->dev, "type: %d\n", vb->vb2_queue->type);
 756
 757        q_data = get_q_data(ctx, vb->vb2_queue->type);
 758        if (V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) {
 759                if (vbuf->field == V4L2_FIELD_ANY)
 760                        vbuf->field = V4L2_FIELD_NONE;
 761                if (vbuf->field != V4L2_FIELD_NONE) {
 762                        dprintk(ctx->dev, "%s field isn't supported\n",
 763                                        __func__);
 764                        return -EINVAL;
 765                }
 766        }
 767
 768        if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
 769                dprintk(ctx->dev, "%s data will not fit into plane (%lu < %lu)\n",
 770                                __func__, vb2_plane_size(vb, 0), (long)q_data->sizeimage);
 771                return -EINVAL;
 772        }
 773
 774        vb2_set_plane_payload(vb, 0, q_data->sizeimage);
 775
 776        return 0;
 777}
 778
 779static void vim2m_buf_queue(struct vb2_buffer *vb)
 780{
 781        struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 782        struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 783
 784        v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
 785}
 786
 787static int vim2m_start_streaming(struct vb2_queue *q, unsigned count)
 788{
 789        struct vim2m_ctx *ctx = vb2_get_drv_priv(q);
 790        struct vim2m_q_data *q_data = get_q_data(ctx, q->type);
 791
 792        q_data->sequence = 0;
 793        return 0;
 794}
 795
 796static void vim2m_stop_streaming(struct vb2_queue *q)
 797{
 798        struct vim2m_ctx *ctx = vb2_get_drv_priv(q);
 799        struct vb2_v4l2_buffer *vbuf;
 800        unsigned long flags;
 801
 802        for (;;) {
 803                if (V4L2_TYPE_IS_OUTPUT(q->type))
 804                        vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
 805                else
 806                        vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
 807                if (vbuf == NULL)
 808                        return;
 809                spin_lock_irqsave(&ctx->dev->irqlock, flags);
 810                v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
 811                spin_unlock_irqrestore(&ctx->dev->irqlock, flags);
 812        }
 813}
 814
 815static struct vb2_ops vim2m_qops = {
 816        .queue_setup     = vim2m_queue_setup,
 817        .buf_prepare     = vim2m_buf_prepare,
 818        .buf_queue       = vim2m_buf_queue,
 819        .start_streaming = vim2m_start_streaming,
 820        .stop_streaming  = vim2m_stop_streaming,
 821        .wait_prepare    = vb2_ops_wait_prepare,
 822        .wait_finish     = vb2_ops_wait_finish,
 823};
 824
 825static int queue_init(void *priv, struct vb2_queue *src_vq, struct vb2_queue *dst_vq)
 826{
 827        struct vim2m_ctx *ctx = priv;
 828        int ret;
 829
 830        src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
 831        src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
 832        src_vq->drv_priv = ctx;
 833        src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 834        src_vq->ops = &vim2m_qops;
 835        src_vq->mem_ops = &vb2_vmalloc_memops;
 836        src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 837        src_vq->lock = &ctx->dev->dev_mutex;
 838
 839        ret = vb2_queue_init(src_vq);
 840        if (ret)
 841                return ret;
 842
 843        dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 844        dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
 845        dst_vq->drv_priv = ctx;
 846        dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 847        dst_vq->ops = &vim2m_qops;
 848        dst_vq->mem_ops = &vb2_vmalloc_memops;
 849        dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 850        dst_vq->lock = &ctx->dev->dev_mutex;
 851
 852        return vb2_queue_init(dst_vq);
 853}
 854
 855static const struct v4l2_ctrl_config vim2m_ctrl_trans_time_msec = {
 856        .ops = &vim2m_ctrl_ops,
 857        .id = V4L2_CID_TRANS_TIME_MSEC,
 858        .name = "Transaction Time (msec)",
 859        .type = V4L2_CTRL_TYPE_INTEGER,
 860        .def = MEM2MEM_DEF_TRANSTIME,
 861        .min = 1,
 862        .max = 10001,
 863        .step = 1,
 864};
 865
 866static const struct v4l2_ctrl_config vim2m_ctrl_trans_num_bufs = {
 867        .ops = &vim2m_ctrl_ops,
 868        .id = V4L2_CID_TRANS_NUM_BUFS,
 869        .name = "Buffers Per Transaction",
 870        .type = V4L2_CTRL_TYPE_INTEGER,
 871        .def = 1,
 872        .min = 1,
 873        .max = MEM2MEM_DEF_NUM_BUFS,
 874        .step = 1,
 875};
 876
 877/*
 878 * File operations
 879 */
 880static int vim2m_open(struct file *file)
 881{
 882        struct vim2m_dev *dev = video_drvdata(file);
 883        struct vim2m_ctx *ctx = NULL;
 884        struct v4l2_ctrl_handler *hdl;
 885        int rc = 0;
 886
 887        if (mutex_lock_interruptible(&dev->dev_mutex))
 888                return -ERESTARTSYS;
 889        ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
 890        if (!ctx) {
 891                rc = -ENOMEM;
 892                goto open_unlock;
 893        }
 894
 895        v4l2_fh_init(&ctx->fh, video_devdata(file));
 896        file->private_data = &ctx->fh;
 897        ctx->dev = dev;
 898        hdl = &ctx->hdl;
 899        v4l2_ctrl_handler_init(hdl, 4);
 900        v4l2_ctrl_new_std(hdl, &vim2m_ctrl_ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
 901        v4l2_ctrl_new_std(hdl, &vim2m_ctrl_ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
 902        v4l2_ctrl_new_custom(hdl, &vim2m_ctrl_trans_time_msec, NULL);
 903        v4l2_ctrl_new_custom(hdl, &vim2m_ctrl_trans_num_bufs, NULL);
 904        if (hdl->error) {
 905                rc = hdl->error;
 906                v4l2_ctrl_handler_free(hdl);
 907                goto open_unlock;
 908        }
 909        ctx->fh.ctrl_handler = hdl;
 910        v4l2_ctrl_handler_setup(hdl);
 911
 912        ctx->q_data[V4L2_M2M_SRC].fmt = &formats[0];
 913        ctx->q_data[V4L2_M2M_SRC].width = 640;
 914        ctx->q_data[V4L2_M2M_SRC].height = 480;
 915        ctx->q_data[V4L2_M2M_SRC].sizeimage =
 916                ctx->q_data[V4L2_M2M_SRC].width *
 917                ctx->q_data[V4L2_M2M_SRC].height *
 918                (ctx->q_data[V4L2_M2M_SRC].fmt->depth >> 3);
 919        ctx->q_data[V4L2_M2M_DST] = ctx->q_data[V4L2_M2M_SRC];
 920        ctx->colorspace = V4L2_COLORSPACE_REC709;
 921
 922        ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx, &queue_init);
 923
 924        if (IS_ERR(ctx->fh.m2m_ctx)) {
 925                rc = PTR_ERR(ctx->fh.m2m_ctx);
 926
 927                v4l2_ctrl_handler_free(hdl);
 928                kfree(ctx);
 929                goto open_unlock;
 930        }
 931
 932        v4l2_fh_add(&ctx->fh);
 933        atomic_inc(&dev->num_inst);
 934
 935        dprintk(dev, "Created instance: %p, m2m_ctx: %p\n",
 936                ctx, ctx->fh.m2m_ctx);
 937
 938open_unlock:
 939        mutex_unlock(&dev->dev_mutex);
 940        return rc;
 941}
 942
 943static int vim2m_release(struct file *file)
 944{
 945        struct vim2m_dev *dev = video_drvdata(file);
 946        struct vim2m_ctx *ctx = file2ctx(file);
 947
 948        dprintk(dev, "Releasing instance %p\n", ctx);
 949
 950        v4l2_fh_del(&ctx->fh);
 951        v4l2_fh_exit(&ctx->fh);
 952        v4l2_ctrl_handler_free(&ctx->hdl);
 953        mutex_lock(&dev->dev_mutex);
 954        v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
 955        mutex_unlock(&dev->dev_mutex);
 956        kfree(ctx);
 957
 958        atomic_dec(&dev->num_inst);
 959
 960        return 0;
 961}
 962
 963static const struct v4l2_file_operations vim2m_fops = {
 964        .owner          = THIS_MODULE,
 965        .open           = vim2m_open,
 966        .release        = vim2m_release,
 967        .poll           = v4l2_m2m_fop_poll,
 968        .unlocked_ioctl = video_ioctl2,
 969        .mmap           = v4l2_m2m_fop_mmap,
 970};
 971
 972static struct video_device vim2m_videodev = {
 973        .name           = MEM2MEM_NAME,
 974        .vfl_dir        = VFL_DIR_M2M,
 975        .fops           = &vim2m_fops,
 976        .ioctl_ops      = &vim2m_ioctl_ops,
 977        .minor          = -1,
 978        .release        = video_device_release_empty,
 979};
 980
 981static struct v4l2_m2m_ops m2m_ops = {
 982        .device_run     = device_run,
 983        .job_ready      = job_ready,
 984        .job_abort      = job_abort,
 985};
 986
 987static int vim2m_probe(struct platform_device *pdev)
 988{
 989        struct vim2m_dev *dev;
 990        struct video_device *vfd;
 991        int ret;
 992
 993        dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
 994        if (!dev)
 995                return -ENOMEM;
 996
 997        spin_lock_init(&dev->irqlock);
 998
 999        ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
1000        if (ret)
1001                return ret;
1002
1003        atomic_set(&dev->num_inst, 0);
1004        mutex_init(&dev->dev_mutex);
1005
1006        dev->vfd = vim2m_videodev;
1007        vfd = &dev->vfd;
1008        vfd->lock = &dev->dev_mutex;
1009        vfd->v4l2_dev = &dev->v4l2_dev;
1010
1011        ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0);
1012        if (ret) {
1013                v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
1014                goto unreg_dev;
1015        }
1016
1017        video_set_drvdata(vfd, dev);
1018        snprintf(vfd->name, sizeof(vfd->name), "%s", vim2m_videodev.name);
1019        v4l2_info(&dev->v4l2_dev,
1020                        "Device registered as /dev/video%d\n", vfd->num);
1021
1022        setup_timer(&dev->timer, device_isr, (long)dev);
1023        platform_set_drvdata(pdev, dev);
1024
1025        dev->m2m_dev = v4l2_m2m_init(&m2m_ops);
1026        if (IS_ERR(dev->m2m_dev)) {
1027                v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem device\n");
1028                ret = PTR_ERR(dev->m2m_dev);
1029                goto err_m2m;
1030        }
1031
1032        return 0;
1033
1034err_m2m:
1035        v4l2_m2m_release(dev->m2m_dev);
1036        video_unregister_device(&dev->vfd);
1037unreg_dev:
1038        v4l2_device_unregister(&dev->v4l2_dev);
1039
1040        return ret;
1041}
1042
1043static int vim2m_remove(struct platform_device *pdev)
1044{
1045        struct vim2m_dev *dev = platform_get_drvdata(pdev);
1046
1047        v4l2_info(&dev->v4l2_dev, "Removing " MEM2MEM_NAME);
1048        v4l2_m2m_release(dev->m2m_dev);
1049        del_timer_sync(&dev->timer);
1050        video_unregister_device(&dev->vfd);
1051        v4l2_device_unregister(&dev->v4l2_dev);
1052
1053        return 0;
1054}
1055
1056static struct platform_driver vim2m_pdrv = {
1057        .probe          = vim2m_probe,
1058        .remove         = vim2m_remove,
1059        .driver         = {
1060                .name   = MEM2MEM_NAME,
1061        },
1062};
1063
1064static void __exit vim2m_exit(void)
1065{
1066        platform_driver_unregister(&vim2m_pdrv);
1067        platform_device_unregister(&vim2m_pdev);
1068}
1069
1070static int __init vim2m_init(void)
1071{
1072        int ret;
1073
1074        ret = platform_device_register(&vim2m_pdev);
1075        if (ret)
1076                return ret;
1077
1078        ret = platform_driver_register(&vim2m_pdrv);
1079        if (ret)
1080                platform_device_unregister(&vim2m_pdev);
1081
1082        return 0;
1083}
1084
1085module_init(vim2m_init);
1086module_exit(vim2m_exit);
1087