drivers/media/platform/exynos4-is/fimc-m2m.c

   1 /*
   2  * Samsung S5P/EXYNOS4 SoC series FIMC (video postprocessor) driver
   3  *
   4  * Copyright (C) 2012 - 2013 Samsung Electronics Co., Ltd.
   5  * Sylwester Nawrocki <s.nawrocki@samsung.com>
   6  *
   7  * This program is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License as published
   9  * by the Free Software Foundation, either version 2 of the License,
  10  * or (at your option) any later version.
  11  */
  12
  13 #include <linux/module.h>
  14 #include <linux/kernel.h>
  15 #include <linux/types.h>
  16 #include <linux/errno.h>
  17 #include <linux/bug.h>
  18 #include <linux/interrupt.h>
  19 #include <linux/device.h>
  20 #include <linux/platform_device.h>
  21 #include <linux/pm_runtime.h>
  22 #include <linux/list.h>
  23 #include <linux/io.h>
  24 #include <linux/slab.h>
  25 #include <linux/clk.h>
  26 #include <media/v4l2-ioctl.h>
  27 #include <media/videobuf2-v4l2.h>
  28 #include <media/videobuf2-dma-contig.h>
  29
  30 #include "common.h"
  31 #include "fimc-core.h"
  32 #include "fimc-reg.h"
  33 #include "media-dev.h"
  34
  35 static unsigned int get_m2m_fmt_flags(unsigned int stream_type)
  36 {
  37         if (stream_type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
  38                 return FMT_FLAGS_M2M_IN;
  39         else
  40                 return FMT_FLAGS_M2M_OUT;
  41 }
  42
  43 void fimc_m2m_job_finish(struct fimc_ctx *ctx, int vb_state)
  44 {
  45         struct vb2_v4l2_buffer *src_vb, *dst_vb;
  46
  47         if (!ctx || !ctx->fh.m2m_ctx)
  48                 return;
  49
  50         src_vb = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
  51         dst_vb = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
  52
  53         if (src_vb)
  54                 v4l2_m2m_buf_done(src_vb, vb_state);
  55         if (dst_vb)
  56                 v4l2_m2m_buf_done(dst_vb, vb_state);
  57         if (src_vb && dst_vb)
  58                 v4l2_m2m_job_finish(ctx->fimc_dev->m2m.m2m_dev,
  59                                     ctx->fh.m2m_ctx);
  60 }
  61
  62 /* Complete the transaction which has been scheduled for execution. */
  63 static void fimc_m2m_shutdown(struct fimc_ctx *ctx)
  64 {
  65         struct fimc_dev *fimc = ctx->fimc_dev;
  66
  67         if (!fimc_m2m_pending(fimc))
  68                 return;
  69
  70         fimc_ctx_state_set(FIMC_CTX_SHUT, ctx);
  71
  72         wait_event_timeout(fimc->irq_queue,
  73                         !fimc_ctx_state_is_set(FIMC_CTX_SHUT, ctx),
  74                         FIMC_SHUTDOWN_TIMEOUT);
  75 }
  76
  77 static int start_streaming(struct vb2_queue *q, unsigned int count)
  78 {
  79         struct fimc_ctx *ctx = q->drv_priv;
  80         int ret;
  81
  82         ret = pm_runtime_get_sync(&ctx->fimc_dev->pdev->dev);
  83         return ret > 0 ? 0 : ret;
  84 }
  85
  86 static void stop_streaming(struct vb2_queue *q)
  87 {
  88         struct fimc_ctx *ctx = q->drv_priv;
  89
  90
  91         fimc_m2m_shutdown(ctx);
  92         fimc_m2m_job_finish(ctx, VB2_BUF_STATE_ERROR);
  93         pm_runtime_put(&ctx->fimc_dev->pdev->dev);
  94 }
  95
  96 static void fimc_device_run(void *priv)
  97 {
  98         struct vb2_v4l2_buffer *src_vb, *dst_vb;
  99         struct fimc_ctx *ctx = priv;
 100         struct fimc_frame *sf, *df;
 101         struct fimc_dev *fimc;
 102         unsigned long flags;
 103         int ret;
 104
 105         if (WARN(!ctx, "Null context\n"))
 106                 return;
 107
 108         fimc = ctx->fimc_dev;
 109         spin_lock_irqsave(&fimc->slock, flags);
 110
 111         set_bit(ST_M2M_PEND, &fimc->state);
 112         sf = &ctx->s_frame;
 113         df = &ctx->d_frame;
 114
 115         if (ctx->state & FIMC_PARAMS) {
 116                 /* Prepare the DMA offsets for scaler */
 117                 fimc_prepare_dma_offset(ctx, sf);
 118                 fimc_prepare_dma_offset(ctx, df);
 119         }
 120
 121         src_vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
 122         ret = fimc_prepare_addr(ctx, &src_vb->vb2_buf, sf, &sf->paddr);
 123         if (ret)
 124                 goto dma_unlock;
 125
 126         dst_vb = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
 127         ret = fimc_prepare_addr(ctx, &dst_vb->vb2_buf, df, &df->paddr);
 128         if (ret)
 129                 goto dma_unlock;
 130
 131         dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp;
 132         dst_vb->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 133         dst_vb->flags |=
 134                 src_vb->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 135
 136         /* Reconfigure hardware if the context has changed. */
 137         if (fimc->m2m.ctx != ctx) {
 138                 ctx->state |= FIMC_PARAMS;
 139                 fimc->m2m.ctx = ctx;
 140         }
 141
 142         if (ctx->state & FIMC_PARAMS) {
 143                 fimc_set_yuv_order(ctx);
 144                 fimc_hw_set_input_path(ctx);
 145                 fimc_hw_set_in_dma(ctx);
 146                 ret = fimc_set_scaler_info(ctx);
 147                 if (ret)
 148                         goto dma_unlock;
 149                 fimc_hw_set_prescaler(ctx);
 150                 fimc_hw_set_mainscaler(ctx);
 151                 fimc_hw_set_target_format(ctx);
 152                 fimc_hw_set_rotation(ctx);
 153                 fimc_hw_set_effect(ctx);
 154                 fimc_hw_set_out_dma(ctx);
 155                 if (fimc->drv_data->alpha_color)
 156                         fimc_hw_set_rgb_alpha(ctx);
 157                 fimc_hw_set_output_path(ctx);
 158         }
 159         fimc_hw_set_input_addr(fimc, &sf->paddr);
 160         fimc_hw_set_output_addr(fimc, &df->paddr, -1);
 161
 162         fimc_activate_capture(ctx);
 163         ctx->state &= (FIMC_CTX_M2M | FIMC_CTX_CAP);
 164         fimc_hw_activate_input_dma(fimc, true);
 165
 166 dma_unlock:
 167         spin_unlock_irqrestore(&fimc->slock, flags);
 168 }
 169
 170 static void fimc_job_abort(void *priv)
 171 {
 172         fimc_m2m_shutdown(priv);
 173 }
 174
 175 static int fimc_queue_setup(struct vb2_queue *vq,
 176                             unsigned int *num_buffers, unsigned int *num_planes,
 177                             unsigned int sizes[], struct device *alloc_devs[])
 178 {
 179         struct fimc_ctx *ctx = vb2_get_drv_priv(vq);
 180         struct fimc_frame *f;
 181         int i;
 182
 183         f = ctx_get_frame(ctx, vq->type);
 184         if (IS_ERR(f))
 185                 return PTR_ERR(f);
 186         /*
 187          * Return number of non-contiguous planes (plane buffers)
 188          * depending on the configured color format.
 189          */
 190         if (!f->fmt)
 191                 return -EINVAL;
 192
 193         *num_planes = f->fmt->memplanes;
 194         for (i = 0; i < f->fmt->memplanes; i++)
 195                 sizes[i] = f->payload[i];
 196         return 0;
 197 }
 198
 199 static int fimc_buf_prepare(struct vb2_buffer *vb)
 200 {
 201         struct fimc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 202         struct fimc_frame *frame;
 203         int i;
 204
 205         frame = ctx_get_frame(ctx, vb->vb2_queue->type);
 206         if (IS_ERR(frame))
 207                 return PTR_ERR(frame);
 208
 209         for (i = 0; i < frame->fmt->memplanes; i++)
 210                 vb2_set_plane_payload(vb, i, frame->payload[i]);
 211
 212         return 0;
 213 }
 214
 215 static void fimc_buf_queue(struct vb2_buffer *vb)
 216 {
 217         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 218         struct fimc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 219         v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
 220 }
 221
 222 static struct vb2_ops fimc_qops = {
 223         .queue_setup     = fimc_queue_setup,
 224         .buf_prepare     = fimc_buf_prepare,
 225         .buf_queue       = fimc_buf_queue,
 226         .wait_prepare    = vb2_ops_wait_prepare,
 227         .wait_finish     = vb2_ops_wait_finish,
 228         .stop_streaming  = stop_streaming,
 229         .start_streaming = start_streaming,
 230 };
 231
 232 /*
 233  * V4L2 ioctl handlers
 234  */
 235 static int fimc_m2m_querycap(struct file *file, void *fh,
 236                                      struct v4l2_capability *cap)
 237 {
 238         struct fimc_dev *fimc = video_drvdata(file);
 239         unsigned int caps;
 240
 241         /*
 242          * This is only a mem-to-mem video device. The capture and output
 243          * device capability flags are left only for backward compatibility
 244          * and are scheduled for removal.
 245          */
 246         caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M_MPLANE |
 247                 V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_VIDEO_OUTPUT_MPLANE;
 248
 249         __fimc_vidioc_querycap(&fimc->pdev->dev, cap, caps);
 250         return 0;
 251 }
 252
 253 static int fimc_m2m_enum_fmt_mplane(struct file *file, void *priv,
 254                                     struct v4l2_fmtdesc *f)
 255 {
 256         struct fimc_fmt *fmt;
 257
 258         fmt = fimc_find_format(NULL, NULL, get_m2m_fmt_flags(f->type),
 259                                f->index);
 260         if (!fmt)
 261                 return -EINVAL;
 262
 263         strncpy(f->description, fmt->name, sizeof(f->description) - 1);
 264         f->pixelformat = fmt->fourcc;
 265         return 0;
 266 }
 267
 268 static int fimc_m2m_g_fmt_mplane(struct file *file, void *fh,
 269                                  struct v4l2_format *f)
 270 {
 271         struct fimc_ctx *ctx = fh_to_ctx(fh);
 272         struct fimc_frame *frame = ctx_get_frame(ctx, f->type);
 273
 274         if (IS_ERR(frame))
 275                 return PTR_ERR(frame);
 276
 277         __fimc_get_format(frame, f);
 278         return 0;
 279 }
 280
 281 static int fimc_try_fmt_mplane(struct fimc_ctx *ctx, struct v4l2_format *f)
 282 {
 283         struct fimc_dev *fimc = ctx->fimc_dev;
 284         const struct fimc_variant *variant = fimc->variant;
 285         struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
 286         struct fimc_fmt *fmt;
 287         u32 max_w, mod_x, mod_y;
 288
 289         if (!IS_M2M(f->type))
 290                 return -EINVAL;
 291
 292         fmt = fimc_find_format(&pix->pixelformat, NULL,
 293                                get_m2m_fmt_flags(f->type), 0);
 294         if (WARN(fmt == NULL, "Pixel format lookup failed"))
 295                 return -EINVAL;
 296
 297         if (pix->field == V4L2_FIELD_ANY)
 298                 pix->field = V4L2_FIELD_NONE;
 299         else if (pix->field != V4L2_FIELD_NONE)
 300                 return -EINVAL;
 301
 302         if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
 303                 max_w = variant->pix_limit->scaler_dis_w;
 304                 mod_x = ffs(variant->min_inp_pixsize) - 1;
 305         } else {
 306                 max_w = variant->pix_limit->out_rot_dis_w;
 307                 mod_x = ffs(variant->min_out_pixsize) - 1;
 308         }
 309
 310         if (tiled_fmt(fmt)) {
 311                 mod_x = 6; /* 64 x 32 pixels tile */
 312                 mod_y = 5;
 313         } else {
 314                 if (variant->min_vsize_align == 1)
 315                         mod_y = fimc_fmt_is_rgb(fmt->color) ? 0 : 1;
 316                 else
 317                         mod_y = ffs(variant->min_vsize_align) - 1;
 318         }
 319
 320         v4l_bound_align_image(&pix->width, 16, max_w, mod_x,
 321                 &pix->height, 8, variant->pix_limit->scaler_dis_w, mod_y, 0);
 322
 323         fimc_adjust_mplane_format(fmt, pix->width, pix->height, &f->fmt.pix_mp);
 324         return 0;
 325 }
 326
 327 static int fimc_m2m_try_fmt_mplane(struct file *file, void *fh,
 328                                    struct v4l2_format *f)
 329 {
 330         struct fimc_ctx *ctx = fh_to_ctx(fh);
 331         return fimc_try_fmt_mplane(ctx, f);
 332 }
 333
 334 static void __set_frame_format(struct fimc_frame *frame, struct fimc_fmt *fmt,
 335                                struct v4l2_pix_format_mplane *pixm)
 336 {
 337         int i;
 338
 339         for (i = 0; i < fmt->memplanes; i++) {
 340                 frame->bytesperline[i] = pixm->plane_fmt[i].bytesperline;
 341                 frame->payload[i] = pixm->plane_fmt[i].sizeimage;
 342         }
 343
 344         frame->f_width = pixm->width;
 345         frame->f_height = pixm->height;
 346         frame->o_width = pixm->width;
 347         frame->o_height = pixm->height;
 348         frame->width = pixm->width;
 349         frame->height = pixm->height;
 350         frame->offs_h = 0;
 351         frame->offs_v = 0;
 352         frame->fmt = fmt;
 353 }
 354
 355 static int fimc_m2m_s_fmt_mplane(struct file *file, void *fh,
 356                                  struct v4l2_format *f)
 357 {
 358         struct fimc_ctx *ctx = fh_to_ctx(fh);
 359         struct fimc_dev *fimc = ctx->fimc_dev;
 360         struct fimc_fmt *fmt;
 361         struct vb2_queue *vq;
 362         struct fimc_frame *frame;
 363         int ret;
 364
 365         ret = fimc_try_fmt_mplane(ctx, f);
 366         if (ret)
 367                 return ret;
 368
 369         vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 370
 371         if (vb2_is_busy(vq)) {
 372                 v4l2_err(&fimc->m2m.vfd, "queue (%d) busy\n", f->type);
 373                 return -EBUSY;
 374         }
 375
 376         if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
 377                 frame = &ctx->s_frame;
 378         else
 379                 frame = &ctx->d_frame;
 380
 381         fmt = fimc_find_format(&f->fmt.pix_mp.pixelformat, NULL,
 382                                get_m2m_fmt_flags(f->type), 0);
 383         if (!fmt)
 384                 return -EINVAL;
 385
 386         __set_frame_format(frame, fmt, &f->fmt.pix_mp);
 387
 388         /* Update RGB Alpha control state and value range */
 389         fimc_alpha_ctrl_update(ctx);
 390
 391         return 0;
 392 }
 393
 394 static int fimc_m2m_cropcap(struct file *file, void *fh,
 395                             struct v4l2_cropcap *cr)
 396 {
 397         struct fimc_ctx *ctx = fh_to_ctx(fh);
 398         struct fimc_frame *frame;
 399
 400         frame = ctx_get_frame(ctx, cr->type);
 401         if (IS_ERR(frame))
 402                 return PTR_ERR(frame);
 403
 404         cr->bounds.left = 0;
 405         cr->bounds.top = 0;
 406         cr->bounds.width = frame->o_width;
 407         cr->bounds.height = frame->o_height;
 408         cr->defrect = cr->bounds;
 409
 410         return 0;
 411 }
 412
 413 static int fimc_m2m_g_crop(struct file *file, void *fh, struct v4l2_crop *cr)
 414 {
 415         struct fimc_ctx *ctx = fh_to_ctx(fh);
 416         struct fimc_frame *frame;
 417
 418         frame = ctx_get_frame(ctx, cr->type);
 419         if (IS_ERR(frame))
 420                 return PTR_ERR(frame);
 421
 422         cr->c.left = frame->offs_h;
 423         cr->c.top = frame->offs_v;
 424         cr->c.width = frame->width;
 425         cr->c.height = frame->height;
 426
 427         return 0;
 428 }
 429
 430 static int fimc_m2m_try_crop(struct fimc_ctx *ctx, struct v4l2_crop *cr)
 431 {
 432         struct fimc_dev *fimc = ctx->fimc_dev;
 433         struct fimc_frame *f;
 434         u32 min_size, halign, depth = 0;
 435         int i;
 436
 437         if (cr->c.top < 0 || cr->c.left < 0) {
 438                 v4l2_err(&fimc->m2m.vfd,
 439                         "doesn't support negative values for top & left\n");
 440                 return -EINVAL;
 441         }
 442         if (cr->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
 443                 f = &ctx->d_frame;
 444         else if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
 445                 f = &ctx->s_frame;
 446         else
 447                 return -EINVAL;
 448
 449         min_size = (f == &ctx->s_frame) ?
 450                 fimc->variant->min_inp_pixsize : fimc->variant->min_out_pixsize;
 451
 452         /* Get pixel alignment constraints. */
 453         if (fimc->variant->min_vsize_align == 1)
 454                 halign = fimc_fmt_is_rgb(f->fmt->color) ? 0 : 1;
 455         else
 456                 halign = ffs(fimc->variant->min_vsize_align) - 1;
 457
 458         for (i = 0; i < f->fmt->memplanes; i++)
 459                 depth += f->fmt->depth[i];
 460
 461         v4l_bound_align_image(&cr->c.width, min_size, f->o_width,
 462                               ffs(min_size) - 1,
 463                               &cr->c.height, min_size, f->o_height,
 464                               halign, 64/(ALIGN(depth, 8)));
 465
 466         /* adjust left/top if cropping rectangle is out of bounds */
 467         if (cr->c.left + cr->c.width > f->o_width)
 468                 cr->c.left = f->o_width - cr->c.width;
 469         if (cr->c.top + cr->c.height > f->o_height)
 470                 cr->c.top = f->o_height - cr->c.height;
 471
 472         cr->c.left = round_down(cr->c.left, min_size);
 473         cr->c.top  = round_down(cr->c.top, fimc->variant->hor_offs_align);
 474
 475         dbg("l:%d, t:%d, w:%d, h:%d, f_w: %d, f_h: %d",
 476             cr->c.left, cr->c.top, cr->c.width, cr->c.height,
 477             f->f_width, f->f_height);
 478
 479         return 0;
 480 }
 481
 482 static int fimc_m2m_s_crop(struct file *file, void *fh, const struct v4l2_crop *crop)
 483 {
 484         struct fimc_ctx *ctx = fh_to_ctx(fh);
 485         struct fimc_dev *fimc = ctx->fimc_dev;
 486         struct v4l2_crop cr = *crop;
 487         struct fimc_frame *f;
 488         int ret;
 489
 490         ret = fimc_m2m_try_crop(ctx, &cr);
 491         if (ret)
 492                 return ret;
 493
 494         f = (cr.type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) ?
 495                 &ctx->s_frame : &ctx->d_frame;
 496
 497         /* Check to see if scaling ratio is within supported range */
 498         if (cr.type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
 499                 ret = fimc_check_scaler_ratio(ctx, cr.c.width,
 500                                 cr.c.height, ctx->d_frame.width,
 501                                 ctx->d_frame.height, ctx->rotation);
 502         } else {
 503                 ret = fimc_check_scaler_ratio(ctx, ctx->s_frame.width,
 504                                 ctx->s_frame.height, cr.c.width,
 505                                 cr.c.height, ctx->rotation);
 506         }
 507         if (ret) {
 508                 v4l2_err(&fimc->m2m.vfd, "Out of scaler range\n");
 509                 return -EINVAL;
 510         }
 511
 512         f->offs_h = cr.c.left;
 513         f->offs_v = cr.c.top;
 514         f->width  = cr.c.width;
 515         f->height = cr.c.height;
 516
 517         fimc_ctx_state_set(FIMC_PARAMS, ctx);
 518
 519         return 0;
 520 }
 521
 522 static const struct v4l2_ioctl_ops fimc_m2m_ioctl_ops = {
 523         .vidioc_querycap                = fimc_m2m_querycap,
 524         .vidioc_enum_fmt_vid_cap_mplane = fimc_m2m_enum_fmt_mplane,
 525         .vidioc_enum_fmt_vid_out_mplane = fimc_m2m_enum_fmt_mplane,
 526         .vidioc_g_fmt_vid_cap_mplane    = fimc_m2m_g_fmt_mplane,
 527         .vidioc_g_fmt_vid_out_mplane    = fimc_m2m_g_fmt_mplane,
 528         .vidioc_try_fmt_vid_cap_mplane  = fimc_m2m_try_fmt_mplane,
 529         .vidioc_try_fmt_vid_out_mplane  = fimc_m2m_try_fmt_mplane,
 530         .vidioc_s_fmt_vid_cap_mplane    = fimc_m2m_s_fmt_mplane,
 531         .vidioc_s_fmt_vid_out_mplane    = fimc_m2m_s_fmt_mplane,
 532         .vidioc_reqbufs                 = v4l2_m2m_ioctl_reqbufs,
 533         .vidioc_querybuf                = v4l2_m2m_ioctl_querybuf,
 534         .vidioc_qbuf                    = v4l2_m2m_ioctl_qbuf,
 535         .vidioc_dqbuf                   = v4l2_m2m_ioctl_dqbuf,
 536         .vidioc_expbuf                  = v4l2_m2m_ioctl_expbuf,
 537         .vidioc_streamon                = v4l2_m2m_ioctl_streamon,
 538         .vidioc_streamoff               = v4l2_m2m_ioctl_streamoff,
 539         .vidioc_g_crop                  = fimc_m2m_g_crop,
 540         .vidioc_s_crop                  = fimc_m2m_s_crop,
 541         .vidioc_cropcap                 = fimc_m2m_cropcap
 542
 543 };
 544
 545 static int queue_init(void *priv, struct vb2_queue *src_vq,
 546                       struct vb2_queue *dst_vq)
 547 {
 548         struct fimc_ctx *ctx = priv;
 549         int ret;
 550
 551         src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
 552         src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
 553         src_vq->drv_priv = ctx;
 554         src_vq->ops = &fimc_qops;
 555         src_vq->mem_ops = &vb2_dma_contig_memops;
 556         src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 557         src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 558         src_vq->lock = &ctx->fimc_dev->lock;
 559         src_vq->dev = &ctx->fimc_dev->pdev->dev;
 560
 561         ret = vb2_queue_init(src_vq);
 562         if (ret)
 563                 return ret;
 564
 565         dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
 566         dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
 567         dst_vq->drv_priv = ctx;
 568         dst_vq->ops = &fimc_qops;
 569         dst_vq->mem_ops = &vb2_dma_contig_memops;
 570         dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 571         dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 572         dst_vq->lock = &ctx->fimc_dev->lock;
 573         dst_vq->dev = &ctx->fimc_dev->pdev->dev;
 574
 575         return vb2_queue_init(dst_vq);
 576 }
 577
 578 static int fimc_m2m_set_default_format(struct fimc_ctx *ctx)
 579 {
 580         struct v4l2_pix_format_mplane pixm = {
 581                 .pixelformat    = V4L2_PIX_FMT_RGB32,
 582                 .width          = 800,
 583                 .height         = 600,
 584                 .plane_fmt[0]   = {
 585                         .bytesperline = 800 * 4,
 586                         .sizeimage = 800 * 4 * 600,
 587                 },
 588         };
 589         struct fimc_fmt *fmt;
 590
 591         fmt = fimc_find_format(&pixm.pixelformat, NULL, FMT_FLAGS_M2M, 0);
 592         if (!fmt)
 593                 return -EINVAL;
 594
 595         __set_frame_format(&ctx->s_frame, fmt, &pixm);
 596         __set_frame_format(&ctx->d_frame, fmt, &pixm);
 597
 598         return 0;
 599 }
 600
 601 static int fimc_m2m_open(struct file *file)
 602 {
 603         struct fimc_dev *fimc = video_drvdata(file);
 604         struct fimc_ctx *ctx;
 605         int ret = -EBUSY;
 606
 607         pr_debug("pid: %d, state: %#lx\n", task_pid_nr(current), fimc->state);
 608
 609         if (mutex_lock_interruptible(&fimc->lock))
 610                 return -ERESTARTSYS;
 611         /*
 612          * Don't allow simultaneous open() of the mem-to-mem and the
 613          * capture video node that belong to same FIMC IP instance.
 614          */
 615         if (test_bit(ST_CAPT_BUSY, &fimc->state))
 616                 goto unlock;
 617
 618         ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
 619         if (!ctx) {
 620                 ret = -ENOMEM;
 621                 goto unlock;
 622         }
 623         v4l2_fh_init(&ctx->fh, &fimc->m2m.vfd);
 624         ctx->fimc_dev = fimc;
 625
 626         /* Default color format */
 627         ctx->s_frame.fmt = fimc_get_format(0);
 628         ctx->d_frame.fmt = fimc_get_format(0);
 629
 630         ret = fimc_ctrls_create(ctx);
 631         if (ret)
 632                 goto error_fh;
 633
 634         /* Use separate control handler per file handle */
 635         ctx->fh.ctrl_handler = &ctx->ctrls.handler;
 636         file->private_data = &ctx->fh;
 637         v4l2_fh_add(&ctx->fh);
 638
 639         /* Setup the device context for memory-to-memory mode */
 640         ctx->state = FIMC_CTX_M2M;
 641         ctx->flags = 0;
 642         ctx->in_path = FIMC_IO_DMA;
 643         ctx->out_path = FIMC_IO_DMA;
 644         ctx->scaler.enabled = 1;
 645
 646         ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(fimc->m2m.m2m_dev, ctx, queue_init);
 647         if (IS_ERR(ctx->fh.m2m_ctx)) {
 648                 ret = PTR_ERR(ctx->fh.m2m_ctx);
 649                 goto error_c;
 650         }
 651
 652         if (fimc->m2m.refcnt++ == 0)
 653                 set_bit(ST_M2M_RUN, &fimc->state);
 654
 655         ret = fimc_m2m_set_default_format(ctx);
 656         if (ret < 0)
 657                 goto error_m2m_ctx;
 658
 659         mutex_unlock(&fimc->lock);
 660         return 0;
 661
 662 error_m2m_ctx:
 663         v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
 664 error_c:
 665         fimc_ctrls_delete(ctx);
 666 error_fh:
 667         v4l2_fh_del(&ctx->fh);
 668         v4l2_fh_exit(&ctx->fh);
 669         kfree(ctx);
 670 unlock:
 671         mutex_unlock(&fimc->lock);
 672         return ret;
 673 }
 674
 675 static int fimc_m2m_release(struct file *file)
 676 {
 677         struct fimc_ctx *ctx = fh_to_ctx(file->private_data);
 678         struct fimc_dev *fimc = ctx->fimc_dev;
 679
 680         dbg("pid: %d, state: 0x%lx, refcnt= %d",
 681                 task_pid_nr(current), fimc->state, fimc->m2m.refcnt);
 682
 683         mutex_lock(&fimc->lock);
 684
 685         v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
 686         fimc_ctrls_delete(ctx);
 687         v4l2_fh_del(&ctx->fh);
 688         v4l2_fh_exit(&ctx->fh);
 689
 690         if (--fimc->m2m.refcnt <= 0)
 691                 clear_bit(ST_M2M_RUN, &fimc->state);
 692         kfree(ctx);
 693
 694         mutex_unlock(&fimc->lock);
 695         return 0;
 696 }
 697
 698 static const struct v4l2_file_operations fimc_m2m_fops = {
 699         .owner          = THIS_MODULE,
 700         .open           = fimc_m2m_open,
 701         .release        = fimc_m2m_release,
 702         .poll           = v4l2_m2m_fop_poll,
 703         .unlocked_ioctl = video_ioctl2,
 704         .mmap           = v4l2_m2m_fop_mmap,
 705 };
 706
 707 static struct v4l2_m2m_ops m2m_ops = {
 708         .device_run     = fimc_device_run,
 709         .job_abort      = fimc_job_abort,
 710 };
 711
 712 int fimc_register_m2m_device(struct fimc_dev *fimc,
 713                              struct v4l2_device *v4l2_dev)
 714 {
 715         struct video_device *vfd = &fimc->m2m.vfd;
 716         int ret;
 717
 718         fimc->v4l2_dev = v4l2_dev;
 719
 720         memset(vfd, 0, sizeof(*vfd));
 721         vfd->fops = &fimc_m2m_fops;
 722         vfd->ioctl_ops = &fimc_m2m_ioctl_ops;
 723         vfd->v4l2_dev = v4l2_dev;
 724         vfd->minor = -1;
 725         vfd->release = video_device_release_empty;
 726         vfd->lock = &fimc->lock;
 727         vfd->vfl_dir = VFL_DIR_M2M;
 728
 729         snprintf(vfd->name, sizeof(vfd->name), "fimc.%d.m2m", fimc->id);
 730         video_set_drvdata(vfd, fimc);
 731
 732         fimc->m2m.m2m_dev = v4l2_m2m_init(&m2m_ops);
 733         if (IS_ERR(fimc->m2m.m2m_dev)) {
 734                 v4l2_err(v4l2_dev, "failed to initialize v4l2-m2m device\n");
 735                 return PTR_ERR(fimc->m2m.m2m_dev);
 736         }
 737
 738         ret = media_entity_pads_init(&vfd->entity, 0, NULL);
 739         if (ret)
 740                 goto err_me;
 741
 742         ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
 743         if (ret)
 744                 goto err_vd;
 745
 746         v4l2_info(v4l2_dev, "Registered %s as /dev/%s\n",
 747                   vfd->name, video_device_node_name(vfd));
 748         return 0;
 749
 750 err_vd:
 751         media_entity_cleanup(&vfd->entity);
 752 err_me:
 753         v4l2_m2m_release(fimc->m2m.m2m_dev);
 754         return ret;
 755 }
 756
 757 void fimc_unregister_m2m_device(struct fimc_dev *fimc)
 758 {
 759         if (!fimc)
 760                 return;
 761
 762         if (fimc->m2m.m2m_dev)
 763                 v4l2_m2m_release(fimc->m2m.m2m_dev);
 764
 765         if (video_is_registered(&fimc->m2m.vfd)) {
 766                 video_unregister_device(&fimc->m2m.vfd);
 767                 media_entity_cleanup(&fimc->m2m.vfd.entity);
 768         }
 769 }