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 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         strncpy(f->description, fmt->name, sizeof(f->description) - 1);
 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_cropcap(struct file *file, void *fh,
 387                             struct v4l2_cropcap *cr)
 388 {
 389         struct fimc_ctx *ctx = fh_to_ctx(fh);
 390         struct fimc_frame *frame;
 391
 392         frame = ctx_get_frame(ctx, cr->type);
 393         if (IS_ERR(frame))
 394                 return PTR_ERR(frame);
 395
 396         cr->bounds.left = 0;
 397         cr->bounds.top = 0;
 398         cr->bounds.width = frame->o_width;
 399         cr->bounds.height = frame->o_height;
 400         cr->defrect = cr->bounds;
 401
 402         return 0;
 403 }
 404
 405 static int fimc_m2m_g_crop(struct file *file, void *fh, struct v4l2_crop *cr)
 406 {
 407         struct fimc_ctx *ctx = fh_to_ctx(fh);
 408         struct fimc_frame *frame;
 409
 410         frame = ctx_get_frame(ctx, cr->type);
 411         if (IS_ERR(frame))
 412                 return PTR_ERR(frame);
 413
 414         cr->c.left = frame->offs_h;
 415         cr->c.top = frame->offs_v;
 416         cr->c.width = frame->width;
 417         cr->c.height = frame->height;
 418
 419         return 0;
 420 }
 421
 422 static int fimc_m2m_try_crop(struct fimc_ctx *ctx, struct v4l2_crop *cr)
 423 {
 424         struct fimc_dev *fimc = ctx->fimc_dev;
 425         struct fimc_frame *f;
 426         u32 min_size, halign, depth = 0;
 427         int i;
 428
 429         if (cr->c.top < 0 || cr->c.left < 0) {
 430                 v4l2_err(&fimc->m2m.vfd,
 431                         "doesn't support negative values for top & left\n");
 432                 return -EINVAL;
 433         }
 434         if (cr->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
 435                 f = &ctx->d_frame;
 436         else if (cr->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
 437                 f = &ctx->s_frame;
 438         else
 439                 return -EINVAL;
 440
 441         min_size = (f == &ctx->s_frame) ?
 442                 fimc->variant->min_inp_pixsize : fimc->variant->min_out_pixsize;
 443
 444         /* Get pixel alignment constraints. */
 445         if (fimc->variant->min_vsize_align == 1)
 446                 halign = fimc_fmt_is_rgb(f->fmt->color) ? 0 : 1;
 447         else
 448                 halign = ffs(fimc->variant->min_vsize_align) - 1;
 449
 450         for (i = 0; i < f->fmt->memplanes; i++)
 451                 depth += f->fmt->depth[i];
 452
 453         v4l_bound_align_image(&cr->c.width, min_size, f->o_width,
 454                               ffs(min_size) - 1,
 455                               &cr->c.height, min_size, f->o_height,
 456                               halign, 64/(ALIGN(depth, 8)));
 457
 458         /* adjust left/top if cropping rectangle is out of bounds */
 459         if (cr->c.left + cr->c.width > f->o_width)
 460                 cr->c.left = f->o_width - cr->c.width;
 461         if (cr->c.top + cr->c.height > f->o_height)
 462                 cr->c.top = f->o_height - cr->c.height;
 463
 464         cr->c.left = round_down(cr->c.left, min_size);
 465         cr->c.top  = round_down(cr->c.top, fimc->variant->hor_offs_align);
 466
 467         dbg("l:%d, t:%d, w:%d, h:%d, f_w: %d, f_h: %d",
 468             cr->c.left, cr->c.top, cr->c.width, cr->c.height,
 469             f->f_width, f->f_height);
 470
 471         return 0;
 472 }
 473
 474 static int fimc_m2m_s_crop(struct file *file, void *fh, const struct v4l2_crop *crop)
 475 {
 476         struct fimc_ctx *ctx = fh_to_ctx(fh);
 477         struct fimc_dev *fimc = ctx->fimc_dev;
 478         struct v4l2_crop cr = *crop;
 479         struct fimc_frame *f;
 480         int ret;
 481
 482         ret = fimc_m2m_try_crop(ctx, &cr);
 483         if (ret)
 484                 return ret;
 485
 486         f = (cr.type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) ?
 487                 &ctx->s_frame : &ctx->d_frame;
 488
 489         /* Check to see if scaling ratio is within supported range */
 490         if (cr.type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
 491                 ret = fimc_check_scaler_ratio(ctx, cr.c.width,
 492                                 cr.c.height, ctx->d_frame.width,
 493                                 ctx->d_frame.height, ctx->rotation);
 494         } else {
 495                 ret = fimc_check_scaler_ratio(ctx, ctx->s_frame.width,
 496                                 ctx->s_frame.height, cr.c.width,
 497                                 cr.c.height, ctx->rotation);
 498         }
 499         if (ret) {
 500                 v4l2_err(&fimc->m2m.vfd, "Out of scaler range\n");
 501                 return -EINVAL;
 502         }
 503
 504         f->offs_h = cr.c.left;
 505         f->offs_v = cr.c.top;
 506         f->width  = cr.c.width;
 507         f->height = cr.c.height;
 508
 509         fimc_ctx_state_set(FIMC_PARAMS, ctx);
 510
 511         return 0;
 512 }
 513
 514 static const struct v4l2_ioctl_ops fimc_m2m_ioctl_ops = {
 515         .vidioc_querycap                = fimc_m2m_querycap,
 516         .vidioc_enum_fmt_vid_cap_mplane = fimc_m2m_enum_fmt_mplane,
 517         .vidioc_enum_fmt_vid_out_mplane = fimc_m2m_enum_fmt_mplane,
 518         .vidioc_g_fmt_vid_cap_mplane    = fimc_m2m_g_fmt_mplane,
 519         .vidioc_g_fmt_vid_out_mplane    = fimc_m2m_g_fmt_mplane,
 520         .vidioc_try_fmt_vid_cap_mplane  = fimc_m2m_try_fmt_mplane,
 521         .vidioc_try_fmt_vid_out_mplane  = fimc_m2m_try_fmt_mplane,
 522         .vidioc_s_fmt_vid_cap_mplane    = fimc_m2m_s_fmt_mplane,
 523         .vidioc_s_fmt_vid_out_mplane    = fimc_m2m_s_fmt_mplane,
 524         .vidioc_reqbufs                 = v4l2_m2m_ioctl_reqbufs,
 525         .vidioc_querybuf                = v4l2_m2m_ioctl_querybuf,
 526         .vidioc_qbuf                    = v4l2_m2m_ioctl_qbuf,
 527         .vidioc_dqbuf                   = v4l2_m2m_ioctl_dqbuf,
 528         .vidioc_expbuf                  = v4l2_m2m_ioctl_expbuf,
 529         .vidioc_streamon                = v4l2_m2m_ioctl_streamon,
 530         .vidioc_streamoff               = v4l2_m2m_ioctl_streamoff,
 531         .vidioc_g_crop                  = fimc_m2m_g_crop,
 532         .vidioc_s_crop                  = fimc_m2m_s_crop,
 533         .vidioc_cropcap                 = fimc_m2m_cropcap
 534
 535 };
 536
 537 static int queue_init(void *priv, struct vb2_queue *src_vq,
 538                       struct vb2_queue *dst_vq)
 539 {
 540         struct fimc_ctx *ctx = priv;
 541         int ret;
 542
 543         src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
 544         src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
 545         src_vq->drv_priv = ctx;
 546         src_vq->ops = &fimc_qops;
 547         src_vq->mem_ops = &vb2_dma_contig_memops;
 548         src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 549         src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 550         src_vq->lock = &ctx->fimc_dev->lock;
 551         src_vq->dev = &ctx->fimc_dev->pdev->dev;
 552
 553         ret = vb2_queue_init(src_vq);
 554         if (ret)
 555                 return ret;
 556
 557         dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
 558         dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
 559         dst_vq->drv_priv = ctx;
 560         dst_vq->ops = &fimc_qops;
 561         dst_vq->mem_ops = &vb2_dma_contig_memops;
 562         dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 563         dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 564         dst_vq->lock = &ctx->fimc_dev->lock;
 565         dst_vq->dev = &ctx->fimc_dev->pdev->dev;
 566
 567         return vb2_queue_init(dst_vq);
 568 }
 569
 570 static int fimc_m2m_set_default_format(struct fimc_ctx *ctx)
 571 {
 572         struct v4l2_pix_format_mplane pixm = {
 573                 .pixelformat    = V4L2_PIX_FMT_RGB32,
 574                 .width          = 800,
 575                 .height         = 600,
 576                 .plane_fmt[0]   = {
 577                         .bytesperline = 800 * 4,
 578                         .sizeimage = 800 * 4 * 600,
 579                 },
 580         };
 581         struct fimc_fmt *fmt;
 582
 583         fmt = fimc_find_format(&pixm.pixelformat, NULL, FMT_FLAGS_M2M, 0);
 584         if (!fmt)
 585                 return -EINVAL;
 586
 587         __set_frame_format(&ctx->s_frame, fmt, &pixm);
 588         __set_frame_format(&ctx->d_frame, fmt, &pixm);
 589
 590         return 0;
 591 }
 592
 593 static int fimc_m2m_open(struct file *file)
 594 {
 595         struct fimc_dev *fimc = video_drvdata(file);
 596         struct fimc_ctx *ctx;
 597         int ret = -EBUSY;
 598
 599         pr_debug("pid: %d, state: %#lx\n", task_pid_nr(current), fimc->state);
 600
 601         if (mutex_lock_interruptible(&fimc->lock))
 602                 return -ERESTARTSYS;
 603         /*
 604          * Don't allow simultaneous open() of the mem-to-mem and the
 605          * capture video node that belong to same FIMC IP instance.
 606          */
 607         if (test_bit(ST_CAPT_BUSY, &fimc->state))
 608                 goto unlock;
 609
 610         ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
 611         if (!ctx) {
 612                 ret = -ENOMEM;
 613                 goto unlock;
 614         }
 615         v4l2_fh_init(&ctx->fh, &fimc->m2m.vfd);
 616         ctx->fimc_dev = fimc;
 617
 618         /* Default color format */
 619         ctx->s_frame.fmt = fimc_get_format(0);
 620         ctx->d_frame.fmt = fimc_get_format(0);
 621
 622         ret = fimc_ctrls_create(ctx);
 623         if (ret)
 624                 goto error_fh;
 625
 626         /* Use separate control handler per file handle */
 627         ctx->fh.ctrl_handler = &ctx->ctrls.handler;
 628         file->private_data = &ctx->fh;
 629         v4l2_fh_add(&ctx->fh);
 630
 631         /* Setup the device context for memory-to-memory mode */
 632         ctx->state = FIMC_CTX_M2M;
 633         ctx->flags = 0;
 634         ctx->in_path = FIMC_IO_DMA;
 635         ctx->out_path = FIMC_IO_DMA;
 636         ctx->scaler.enabled = 1;
 637
 638         ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(fimc->m2m.m2m_dev, ctx, queue_init);
 639         if (IS_ERR(ctx->fh.m2m_ctx)) {
 640                 ret = PTR_ERR(ctx->fh.m2m_ctx);
 641                 goto error_c;
 642         }
 643
 644         if (fimc->m2m.refcnt++ == 0)
 645                 set_bit(ST_M2M_RUN, &fimc->state);
 646
 647         ret = fimc_m2m_set_default_format(ctx);
 648         if (ret < 0)
 649                 goto error_m2m_ctx;
 650
 651         mutex_unlock(&fimc->lock);
 652         return 0;
 653
 654 error_m2m_ctx:
 655         v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
 656 error_c:
 657         fimc_ctrls_delete(ctx);
 658         v4l2_fh_del(&ctx->fh);
 659 error_fh:
 660         v4l2_fh_exit(&ctx->fh);
 661         kfree(ctx);
 662 unlock:
 663         mutex_unlock(&fimc->lock);
 664         return ret;
 665 }
 666
 667 static int fimc_m2m_release(struct file *file)
 668 {
 669         struct fimc_ctx *ctx = fh_to_ctx(file->private_data);
 670         struct fimc_dev *fimc = ctx->fimc_dev;
 671
 672         dbg("pid: %d, state: 0x%lx, refcnt= %d",
 673                 task_pid_nr(current), fimc->state, fimc->m2m.refcnt);
 674
 675         mutex_lock(&fimc->lock);
 676
 677         v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
 678         fimc_ctrls_delete(ctx);
 679         v4l2_fh_del(&ctx->fh);
 680         v4l2_fh_exit(&ctx->fh);
 681
 682         if (--fimc->m2m.refcnt <= 0)
 683                 clear_bit(ST_M2M_RUN, &fimc->state);
 684         kfree(ctx);
 685
 686         mutex_unlock(&fimc->lock);
 687         return 0;
 688 }
 689
 690 static const struct v4l2_file_operations fimc_m2m_fops = {
 691         .owner          = THIS_MODULE,
 692         .open           = fimc_m2m_open,
 693         .release        = fimc_m2m_release,
 694         .poll           = v4l2_m2m_fop_poll,
 695         .unlocked_ioctl = video_ioctl2,
 696         .mmap           = v4l2_m2m_fop_mmap,
 697 };
 698
 699 static const struct v4l2_m2m_ops m2m_ops = {
 700         .device_run     = fimc_device_run,
 701         .job_abort      = fimc_job_abort,
 702 };
 703
 704 int fimc_register_m2m_device(struct fimc_dev *fimc,
 705                              struct v4l2_device *v4l2_dev)
 706 {
 707         struct video_device *vfd = &fimc->m2m.vfd;
 708         int ret;
 709
 710         fimc->v4l2_dev = v4l2_dev;
 711
 712         memset(vfd, 0, sizeof(*vfd));
 713         vfd->fops = &fimc_m2m_fops;
 714         vfd->ioctl_ops = &fimc_m2m_ioctl_ops;
 715         vfd->v4l2_dev = v4l2_dev;
 716         vfd->minor = -1;
 717         vfd->release = video_device_release_empty;
 718         vfd->lock = &fimc->lock;
 719         vfd->vfl_dir = VFL_DIR_M2M;
 720
 721         snprintf(vfd->name, sizeof(vfd->name), "fimc.%d.m2m", fimc->id);
 722         video_set_drvdata(vfd, fimc);
 723
 724         fimc->m2m.m2m_dev = v4l2_m2m_init(&m2m_ops);
 725         if (IS_ERR(fimc->m2m.m2m_dev)) {
 726                 v4l2_err(v4l2_dev, "failed to initialize v4l2-m2m device\n");
 727                 return PTR_ERR(fimc->m2m.m2m_dev);
 728         }
 729
 730         ret = media_entity_pads_init(&vfd->entity, 0, NULL);
 731         if (ret)
 732                 goto err_me;
 733
 734         ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
 735         if (ret)
 736                 goto err_vd;
 737
 738         v4l2_info(v4l2_dev, "Registered %s as /dev/%s\n",
 739                   vfd->name, video_device_node_name(vfd));
 740         return 0;
 741
 742 err_vd:
 743         media_entity_cleanup(&vfd->entity);
 744 err_me:
 745         v4l2_m2m_release(fimc->m2m.m2m_dev);
 746         return ret;
 747 }
 748
 749 void fimc_unregister_m2m_device(struct fimc_dev *fimc)
 750 {
 751         if (!fimc)
 752                 return;
 753
 754         if (fimc->m2m.m2m_dev)
 755                 v4l2_m2m_release(fimc->m2m.m2m_dev);
 756
 757         if (video_is_registered(&fimc->m2m.vfd)) {
 758                 video_unregister_device(&fimc->m2m.vfd);
 759                 media_entity_cleanup(&fimc->m2m.vfd.entity);
 760         }
 761 }