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 <[email protected]>
   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 const 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 = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M_MPLANE;
 240
 241         __fimc_vidioc_querycap(&fimc->pdev->dev, cap, caps);
 242         return 0;
 243 }
 244
 245 static int fimc_m2m_enum_fmt_mplane(struct file *file, void *priv,
 246                                     struct v4l2_fmtdesc *f)
 247 {
 248         struct fimc_fmt *fmt;
 249
 250         fmt = fimc_find_format(NULL, NULL, get_m2m_fmt_flags(f->type),
 251                                f->index);
 252         if (!fmt)
 253                 return -EINVAL;
 254
 255         strscpy(f->description, fmt->name, sizeof(f->description));
 256         f->pixelformat = fmt->fourcc;
 257         return 0;
 258 }
 259
 260 static int fimc_m2m_g_fmt_mplane(struct file *file, void *fh,
 261                                  struct v4l2_format *f)
 262 {
 263         struct fimc_ctx *ctx = fh_to_ctx(fh);
 264         struct fimc_frame *frame = ctx_get_frame(ctx, f->type);
 265
 266         if (IS_ERR(frame))
 267                 return PTR_ERR(frame);
 268
 269         __fimc_get_format(frame, f);
 270         return 0;
 271 }
 272
 273 static int fimc_try_fmt_mplane(struct fimc_ctx *ctx, struct v4l2_format *f)
 274 {
 275         struct fimc_dev *fimc = ctx->fimc_dev;
 276         const struct fimc_variant *variant = fimc->variant;
 277         struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
 278         struct fimc_fmt *fmt;
 279         u32 max_w, mod_x, mod_y;
 280
 281         if (!IS_M2M(f->type))
 282                 return -EINVAL;
 283
 284         fmt = fimc_find_format(&pix->pixelformat, NULL,
 285                                get_m2m_fmt_flags(f->type), 0);
 286         if (WARN(fmt == NULL, "Pixel format lookup failed"))
 287                 return -EINVAL;
 288
 289         if (pix->field == V4L2_FIELD_ANY)
 290                 pix->field = V4L2_FIELD_NONE;
 291         else if (pix->field != V4L2_FIELD_NONE)
 292                 return -EINVAL;
 293
 294         if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
 295                 max_w = variant->pix_limit->scaler_dis_w;
 296                 mod_x = ffs(variant->min_inp_pixsize) - 1;
 297         } else {
 298                 max_w = variant->pix_limit->out_rot_dis_w;
 299                 mod_x = ffs(variant->min_out_pixsize) - 1;
 300         }
 301
 302         if (tiled_fmt(fmt)) {
 303                 mod_x = 6; /* 64 x 32 pixels tile */
 304                 mod_y = 5;
 305         } else {
 306                 if (variant->min_vsize_align == 1)
 307                         mod_y = fimc_fmt_is_rgb(fmt->color) ? 0 : 1;
 308                 else
 309                         mod_y = ffs(variant->min_vsize_align) - 1;
 310         }
 311
 312         v4l_bound_align_image(&pix->width, 16, max_w, mod_x,
 313                 &pix->height, 8, variant->pix_limit->scaler_dis_w, mod_y, 0);
 314
 315         fimc_adjust_mplane_format(fmt, pix->width, pix->height, &f->fmt.pix_mp);
 316         return 0;
 317 }
 318
 319 static int fimc_m2m_try_fmt_mplane(struct file *file, void *fh,
 320                                    struct v4l2_format *f)
 321 {
 322         struct fimc_ctx *ctx = fh_to_ctx(fh);
 323         return fimc_try_fmt_mplane(ctx, f);
 324 }
 325
 326 static void __set_frame_format(struct fimc_frame *frame, struct fimc_fmt *fmt,
 327                                struct v4l2_pix_format_mplane *pixm)
 328 {
 329         int i;
 330
 331         for (i = 0; i < fmt->memplanes; i++) {
 332                 frame->bytesperline[i] = pixm->plane_fmt[i].bytesperline;
 333                 frame->payload[i] = pixm->plane_fmt[i].sizeimage;
 334         }
 335
 336         frame->f_width = pixm->width;
 337         frame->f_height = pixm->height;
 338         frame->o_width = pixm->width;
 339         frame->o_height = pixm->height;
 340         frame->width = pixm->width;
 341         frame->height = pixm->height;
 342         frame->offs_h = 0;
 343         frame->offs_v = 0;
 344         frame->fmt = fmt;
 345 }
 346
 347 static int fimc_m2m_s_fmt_mplane(struct file *file, void *fh,
 348                                  struct v4l2_format *f)
 349 {
 350         struct fimc_ctx *ctx = fh_to_ctx(fh);
 351         struct fimc_dev *fimc = ctx->fimc_dev;
 352         struct fimc_fmt *fmt;
 353         struct vb2_queue *vq;
 354         struct fimc_frame *frame;
 355         int ret;
 356
 357         ret = fimc_try_fmt_mplane(ctx, f);
 358         if (ret)
 359                 return ret;
 360
 361         vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 362
 363         if (vb2_is_busy(vq)) {
 364                 v4l2_err(&fimc->m2m.vfd, "queue (%d) busy\n", f->type);
 365                 return -EBUSY;
 366         }
 367
 368         if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
 369                 frame = &ctx->s_frame;
 370         else
 371                 frame = &ctx->d_frame;
 372
 373         fmt = fimc_find_format(&f->fmt.pix_mp.pixelformat, NULL,
 374                                get_m2m_fmt_flags(f->type), 0);
 375         if (!fmt)
 376                 return -EINVAL;
 377
 378         __set_frame_format(frame, fmt, &f->fmt.pix_mp);
 379
 380         /* Update RGB Alpha control state and value range */
 381         fimc_alpha_ctrl_update(ctx);
 382
 383         return 0;
 384 }
 385
 386 static int fimc_m2m_g_selection(struct file *file, void *fh,
 387                                 struct v4l2_selection *s)
 388 {
 389         struct fimc_ctx *ctx = fh_to_ctx(fh);
 390         struct fimc_frame *frame;
 391
 392         frame = ctx_get_frame(ctx, s->type);
 393         if (IS_ERR(frame))
 394                 return PTR_ERR(frame);
 395
 396         switch (s->target) {
 397         case V4L2_SEL_TGT_CROP:
 398         case V4L2_SEL_TGT_CROP_DEFAULT:
 399         case V4L2_SEL_TGT_CROP_BOUNDS:
 400                 if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
 401                         return -EINVAL;
 402                 break;
 403         case V4L2_SEL_TGT_COMPOSE:
 404         case V4L2_SEL_TGT_COMPOSE_DEFAULT:
 405         case V4L2_SEL_TGT_COMPOSE_BOUNDS:
 406                 if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 407                         return -EINVAL;
 408                 break;
 409         default:
 410                 return -EINVAL;
 411         }
 412
 413         switch (s->target) {
 414         case V4L2_SEL_TGT_CROP:
 415         case V4L2_SEL_TGT_COMPOSE:
 416                 s->r.left = frame->offs_h;
 417                 s->r.top = frame->offs_v;
 418                 s->r.width = frame->width;
 419                 s->r.height = frame->height;
 420                 break;
 421         case V4L2_SEL_TGT_CROP_DEFAULT:
 422         case V4L2_SEL_TGT_CROP_BOUNDS:
 423         case V4L2_SEL_TGT_COMPOSE_DEFAULT:
 424         case V4L2_SEL_TGT_COMPOSE_BOUNDS:
 425                 s->r.left = 0;
 426                 s->r.top = 0;
 427                 s->r.width = frame->o_width;
 428                 s->r.height = frame->o_height;
 429                 break;
 430         default:
 431                 return -EINVAL;
 432         }
 433         return 0;
 434 }
 435
 436 static int fimc_m2m_try_selection(struct fimc_ctx *ctx,
 437                                   struct v4l2_selection *s)
 438 {
 439         struct fimc_dev *fimc = ctx->fimc_dev;
 440         struct fimc_frame *f;
 441         u32 min_size, halign, depth = 0;
 442         int i;
 443
 444         if (s->r.top < 0 || s->r.left < 0) {
 445                 v4l2_err(&fimc->m2m.vfd,
 446                         "doesn't support negative values for top & left\n");
 447                 return -EINVAL;
 448         }
 449         if (s->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
 450                 f = &ctx->d_frame;
 451                 if (s->target != V4L2_SEL_TGT_COMPOSE)
 452                         return -EINVAL;
 453         } else if (s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
 454                 f = &ctx->s_frame;
 455                 if (s->target != V4L2_SEL_TGT_CROP)
 456                         return -EINVAL;
 457         } else {
 458                 return -EINVAL;
 459         }
 460
 461         min_size = (f == &ctx->s_frame) ?
 462                 fimc->variant->min_inp_pixsize : fimc->variant->min_out_pixsize;
 463
 464         /* Get pixel alignment constraints. */
 465         if (fimc->variant->min_vsize_align == 1)
 466                 halign = fimc_fmt_is_rgb(f->fmt->color) ? 0 : 1;
 467         else
 468                 halign = ffs(fimc->variant->min_vsize_align) - 1;
 469
 470         for (i = 0; i < f->fmt->memplanes; i++)
 471                 depth += f->fmt->depth[i];
 472
 473         v4l_bound_align_image(&s->r.width, min_size, f->o_width,
 474                               ffs(min_size) - 1,
 475                               &s->r.height, min_size, f->o_height,
 476                               halign, 64/(ALIGN(depth, 8)));
 477
 478         /* adjust left/top if cropping rectangle is out of bounds */
 479         if (s->r.left + s->r.width > f->o_width)
 480                 s->r.left = f->o_width - s->r.width;
 481         if (s->r.top + s->r.height > f->o_height)
 482                 s->r.top = f->o_height - s->r.height;
 483
 484         s->r.left = round_down(s->r.left, min_size);
 485         s->r.top  = round_down(s->r.top, fimc->variant->hor_offs_align);
 486
 487         dbg("l:%d, t:%d, w:%d, h:%d, f_w: %d, f_h: %d",
 488             s->r.left, s->r.top, s->r.width, s->r.height,
 489             f->f_width, f->f_height);
 490
 491         return 0;
 492 }
 493
 494 static int fimc_m2m_s_selection(struct file *file, void *fh,
 495                                 struct v4l2_selection *s)
 496 {
 497         struct fimc_ctx *ctx = fh_to_ctx(fh);
 498         struct fimc_dev *fimc = ctx->fimc_dev;
 499         struct fimc_frame *f;
 500         int ret;
 501
 502         ret = fimc_m2m_try_selection(ctx, s);
 503         if (ret)
 504                 return ret;
 505
 506         f = (s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) ?
 507                 &ctx->s_frame : &ctx->d_frame;
 508
 509         /* Check to see if scaling ratio is within supported range */
 510         if (s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
 511                 ret = fimc_check_scaler_ratio(ctx, s->r.width,
 512                                 s->r.height, ctx->d_frame.width,
 513                                 ctx->d_frame.height, ctx->rotation);
 514         } else {
 515                 ret = fimc_check_scaler_ratio(ctx, ctx->s_frame.width,
 516                                 ctx->s_frame.height, s->r.width,
 517                                 s->r.height, ctx->rotation);
 518         }
 519         if (ret) {
 520                 v4l2_err(&fimc->m2m.vfd, "Out of scaler range\n");
 521                 return -EINVAL;
 522         }
 523
 524         f->offs_h = s->r.left;
 525         f->offs_v = s->r.top;
 526         f->width  = s->r.width;
 527         f->height = s->r.height;
 528
 529         fimc_ctx_state_set(FIMC_PARAMS, ctx);
 530
 531         return 0;
 532 }
 533
 534 static const struct v4l2_ioctl_ops fimc_m2m_ioctl_ops = {
 535         .vidioc_querycap                = fimc_m2m_querycap,
 536         .vidioc_enum_fmt_vid_cap_mplane = fimc_m2m_enum_fmt_mplane,
 537         .vidioc_enum_fmt_vid_out_mplane = fimc_m2m_enum_fmt_mplane,
 538         .vidioc_g_fmt_vid_cap_mplane    = fimc_m2m_g_fmt_mplane,
 539         .vidioc_g_fmt_vid_out_mplane    = fimc_m2m_g_fmt_mplane,
 540         .vidioc_try_fmt_vid_cap_mplane  = fimc_m2m_try_fmt_mplane,
 541         .vidioc_try_fmt_vid_out_mplane  = fimc_m2m_try_fmt_mplane,
 542         .vidioc_s_fmt_vid_cap_mplane    = fimc_m2m_s_fmt_mplane,
 543         .vidioc_s_fmt_vid_out_mplane    = fimc_m2m_s_fmt_mplane,
 544         .vidioc_reqbufs                 = v4l2_m2m_ioctl_reqbufs,
 545         .vidioc_querybuf                = v4l2_m2m_ioctl_querybuf,
 546         .vidioc_qbuf                    = v4l2_m2m_ioctl_qbuf,
 547         .vidioc_dqbuf                   = v4l2_m2m_ioctl_dqbuf,
 548         .vidioc_expbuf                  = v4l2_m2m_ioctl_expbuf,
 549         .vidioc_streamon                = v4l2_m2m_ioctl_streamon,
 550         .vidioc_streamoff               = v4l2_m2m_ioctl_streamoff,
 551         .vidioc_g_selection             = fimc_m2m_g_selection,
 552         .vidioc_s_selection             = fimc_m2m_s_selection,
 553
 554 };
 555
 556 static int queue_init(void *priv, struct vb2_queue *src_vq,
 557                       struct vb2_queue *dst_vq)
 558 {
 559         struct fimc_ctx *ctx = priv;
 560         int ret;
 561
 562         src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
 563         src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
 564         src_vq->drv_priv = ctx;
 565         src_vq->ops = &fimc_qops;
 566         src_vq->mem_ops = &vb2_dma_contig_memops;
 567         src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 568         src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 569         src_vq->lock = &ctx->fimc_dev->lock;
 570         src_vq->dev = &ctx->fimc_dev->pdev->dev;
 571
 572         ret = vb2_queue_init(src_vq);
 573         if (ret)
 574                 return ret;
 575
 576         dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
 577         dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
 578         dst_vq->drv_priv = ctx;
 579         dst_vq->ops = &fimc_qops;
 580         dst_vq->mem_ops = &vb2_dma_contig_memops;
 581         dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 582         dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 583         dst_vq->lock = &ctx->fimc_dev->lock;
 584         dst_vq->dev = &ctx->fimc_dev->pdev->dev;
 585
 586         return vb2_queue_init(dst_vq);
 587 }
 588
 589 static int fimc_m2m_set_default_format(struct fimc_ctx *ctx)
 590 {
 591         struct v4l2_pix_format_mplane pixm = {
 592                 .pixelformat    = V4L2_PIX_FMT_RGB32,
 593                 .width          = 800,
 594                 .height         = 600,
 595                 .plane_fmt[0]   = {
 596                         .bytesperline = 800 * 4,
 597                         .sizeimage = 800 * 4 * 600,
 598                 },
 599         };
 600         struct fimc_fmt *fmt;
 601
 602         fmt = fimc_find_format(&pixm.pixelformat, NULL, FMT_FLAGS_M2M, 0);
 603         if (!fmt)
 604                 return -EINVAL;
 605
 606         __set_frame_format(&ctx->s_frame, fmt, &pixm);
 607         __set_frame_format(&ctx->d_frame, fmt, &pixm);
 608
 609         return 0;
 610 }
 611
 612 static int fimc_m2m_open(struct file *file)
 613 {
 614         struct fimc_dev *fimc = video_drvdata(file);
 615         struct fimc_ctx *ctx;
 616         int ret = -EBUSY;
 617
 618         pr_debug("pid: %d, state: %#lx\n", task_pid_nr(current), fimc->state);
 619
 620         if (mutex_lock_interruptible(&fimc->lock))
 621                 return -ERESTARTSYS;
 622         /*
 623          * Don't allow simultaneous open() of the mem-to-mem and the
 624          * capture video node that belong to same FIMC IP instance.
 625          */
 626         if (test_bit(ST_CAPT_BUSY, &fimc->state))
 627                 goto unlock;
 628
 629         ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
 630         if (!ctx) {
 631                 ret = -ENOMEM;
 632                 goto unlock;
 633         }
 634         v4l2_fh_init(&ctx->fh, &fimc->m2m.vfd);
 635         ctx->fimc_dev = fimc;
 636
 637         /* Default color format */
 638         ctx->s_frame.fmt = fimc_get_format(0);
 639         ctx->d_frame.fmt = fimc_get_format(0);
 640
 641         ret = fimc_ctrls_create(ctx);
 642         if (ret)
 643                 goto error_fh;
 644
 645         /* Use separate control handler per file handle */
 646         ctx->fh.ctrl_handler = &ctx->ctrls.handler;
 647         file->private_data = &ctx->fh;
 648         v4l2_fh_add(&ctx->fh);
 649
 650         /* Setup the device context for memory-to-memory mode */
 651         ctx->state = FIMC_CTX_M2M;
 652         ctx->flags = 0;
 653         ctx->in_path = FIMC_IO_DMA;
 654         ctx->out_path = FIMC_IO_DMA;
 655         ctx->scaler.enabled = 1;
 656
 657         ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(fimc->m2m.m2m_dev, ctx, queue_init);
 658         if (IS_ERR(ctx->fh.m2m_ctx)) {
 659                 ret = PTR_ERR(ctx->fh.m2m_ctx);
 660                 goto error_c;
 661         }
 662
 663         if (fimc->m2m.refcnt++ == 0)
 664                 set_bit(ST_M2M_RUN, &fimc->state);
 665
 666         ret = fimc_m2m_set_default_format(ctx);
 667         if (ret < 0)
 668                 goto error_m2m_ctx;
 669
 670         mutex_unlock(&fimc->lock);
 671         return 0;
 672
 673 error_m2m_ctx:
 674         v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
 675 error_c:
 676         fimc_ctrls_delete(ctx);
 677         v4l2_fh_del(&ctx->fh);
 678 error_fh:
 679         v4l2_fh_exit(&ctx->fh);
 680         kfree(ctx);
 681 unlock:
 682         mutex_unlock(&fimc->lock);
 683         return ret;
 684 }
 685
 686 static int fimc_m2m_release(struct file *file)
 687 {
 688         struct fimc_ctx *ctx = fh_to_ctx(file->private_data);
 689         struct fimc_dev *fimc = ctx->fimc_dev;
 690
 691         dbg("pid: %d, state: 0x%lx, refcnt= %d",
 692                 task_pid_nr(current), fimc->state, fimc->m2m.refcnt);
 693
 694         mutex_lock(&fimc->lock);
 695
 696         v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
 697         fimc_ctrls_delete(ctx);
 698         v4l2_fh_del(&ctx->fh);
 699         v4l2_fh_exit(&ctx->fh);
 700
 701         if (--fimc->m2m.refcnt <= 0)
 702                 clear_bit(ST_M2M_RUN, &fimc->state);
 703         kfree(ctx);
 704
 705         mutex_unlock(&fimc->lock);
 706         return 0;
 707 }
 708
 709 static const struct v4l2_file_operations fimc_m2m_fops = {
 710         .owner          = THIS_MODULE,
 711         .open           = fimc_m2m_open,
 712         .release        = fimc_m2m_release,
 713         .poll           = v4l2_m2m_fop_poll,
 714         .unlocked_ioctl = video_ioctl2,
 715         .mmap           = v4l2_m2m_fop_mmap,
 716 };
 717
 718 static const struct v4l2_m2m_ops m2m_ops = {
 719         .device_run     = fimc_device_run,
 720         .job_abort      = fimc_job_abort,
 721 };
 722
 723 int fimc_register_m2m_device(struct fimc_dev *fimc,
 724                              struct v4l2_device *v4l2_dev)
 725 {
 726         struct video_device *vfd = &fimc->m2m.vfd;
 727         int ret;
 728
 729         fimc->v4l2_dev = v4l2_dev;
 730
 731         memset(vfd, 0, sizeof(*vfd));
 732         vfd->fops = &fimc_m2m_fops;
 733         vfd->ioctl_ops = &fimc_m2m_ioctl_ops;
 734         vfd->v4l2_dev = v4l2_dev;
 735         vfd->minor = -1;
 736         vfd->release = video_device_release_empty;
 737         vfd->lock = &fimc->lock;
 738         vfd->vfl_dir = VFL_DIR_M2M;
 739         set_bit(V4L2_FL_QUIRK_INVERTED_CROP, &vfd->flags);
 740
 741         snprintf(vfd->name, sizeof(vfd->name), "fimc.%d.m2m", fimc->id);
 742         video_set_drvdata(vfd, fimc);
 743
 744         fimc->m2m.m2m_dev = v4l2_m2m_init(&m2m_ops);
 745         if (IS_ERR(fimc->m2m.m2m_dev)) {
 746                 v4l2_err(v4l2_dev, "failed to initialize v4l2-m2m device\n");
 747                 return PTR_ERR(fimc->m2m.m2m_dev);
 748         }
 749
 750         ret = media_entity_pads_init(&vfd->entity, 0, NULL);
 751         if (ret)
 752                 goto err_me;
 753
 754         ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
 755         if (ret)
 756                 goto err_vd;
 757
 758         v4l2_info(v4l2_dev, "Registered %s as /dev/%s\n",
 759                   vfd->name, video_device_node_name(vfd));
 760         return 0;
 761
 762 err_vd:
 763         media_entity_cleanup(&vfd->entity);
 764 err_me:
 765         v4l2_m2m_release(fimc->m2m.m2m_dev);
 766         return ret;
 767 }
 768
 769 void fimc_unregister_m2m_device(struct fimc_dev *fimc)
 770 {
 771         if (!fimc)
 772                 return;
 773
 774         if (fimc->m2m.m2m_dev)
 775                 v4l2_m2m_release(fimc->m2m.m2m_dev);
 776
 777         if (video_is_registered(&fimc->m2m.vfd)) {
 778                 video_unregister_device(&fimc->m2m.vfd);
 779                 media_entity_cleanup(&fimc->m2m.vfd.entity);
 780         }
 781 }