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

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