drivers/media/platform/exynos-gsc/gsc-m2m.c

   1 /*
   2  * Copyright (c) 2011 - 2012 Samsung Electronics Co., Ltd.
   3  *              http://www.samsung.com
   4  *
   5  * Samsung EXYNOS5 SoC series G-Scaler driver
   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/workqueue.h>
  20 #include <linux/device.h>
  21 #include <linux/platform_device.h>
  22 #include <linux/list.h>
  23 #include <linux/io.h>
  24 #include <linux/slab.h>
  25 #include <linux/clk.h>
  26
  27 #include <media/v4l2-ioctl.h>
  28
  29 #include "gsc-core.h"
  30
  31 static int gsc_m2m_ctx_stop_req(struct gsc_ctx *ctx)
  32 {
  33         struct gsc_ctx *curr_ctx;
  34         struct gsc_dev *gsc = ctx->gsc_dev;
  35         int ret;
  36
  37         curr_ctx = v4l2_m2m_get_curr_priv(gsc->m2m.m2m_dev);
  38         if (!gsc_m2m_pending(gsc) || (curr_ctx != ctx))
  39                 return 0;
  40
  41         gsc_ctx_state_lock_set(GSC_CTX_STOP_REQ, ctx);
  42         ret = wait_event_timeout(gsc->irq_queue,
  43                         !gsc_ctx_state_is_set(GSC_CTX_STOP_REQ, ctx),
  44                         GSC_SHUTDOWN_TIMEOUT);
  45
  46         return ret == 0 ? -ETIMEDOUT : ret;
  47 }
  48
  49 static int gsc_m2m_start_streaming(struct vb2_queue *q, unsigned int count)
  50 {
  51         struct gsc_ctx *ctx = q->drv_priv;
  52         int ret;
  53
  54         ret = pm_runtime_get_sync(&ctx->gsc_dev->pdev->dev);
  55         return ret > 0 ? 0 : ret;
  56 }
  57
  58 static int gsc_m2m_stop_streaming(struct vb2_queue *q)
  59 {
  60         struct gsc_ctx *ctx = q->drv_priv;
  61         int ret;
  62
  63         ret = gsc_m2m_ctx_stop_req(ctx);
  64         if (ret == -ETIMEDOUT)
  65                 gsc_m2m_job_finish(ctx, VB2_BUF_STATE_ERROR);
  66
  67         pm_runtime_put(&ctx->gsc_dev->pdev->dev);
  68
  69         return 0;
  70 }
  71
  72 void gsc_m2m_job_finish(struct gsc_ctx *ctx, int vb_state)
  73 {
  74         struct vb2_buffer *src_vb, *dst_vb;
  75
  76         if (!ctx || !ctx->m2m_ctx)
  77                 return;
  78
  79         src_vb = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
  80         dst_vb = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
  81
  82         if (src_vb && dst_vb) {
  83                 v4l2_m2m_buf_done(src_vb, vb_state);
  84                 v4l2_m2m_buf_done(dst_vb, vb_state);
  85
  86                 v4l2_m2m_job_finish(ctx->gsc_dev->m2m.m2m_dev,
  87                                     ctx->m2m_ctx);
  88         }
  89 }
  90
  91
  92 static void gsc_m2m_job_abort(void *priv)
  93 {
  94         struct gsc_ctx *ctx = priv;
  95         int ret;
  96
  97         ret = gsc_m2m_ctx_stop_req(ctx);
  98         if (ret == -ETIMEDOUT)
  99                 gsc_m2m_job_finish(ctx, VB2_BUF_STATE_ERROR);
 100 }
 101
 102 static int gsc_fill_addr(struct gsc_ctx *ctx)
 103 {
 104         struct gsc_frame *s_frame, *d_frame;
 105         struct vb2_buffer *vb = NULL;
 106         int ret;
 107
 108         s_frame = &ctx->s_frame;
 109         d_frame = &ctx->d_frame;
 110
 111         vb = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
 112         ret = gsc_prepare_addr(ctx, vb, s_frame, &s_frame->addr);
 113         if (ret)
 114                 return ret;
 115
 116         vb = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
 117         return gsc_prepare_addr(ctx, vb, d_frame, &d_frame->addr);
 118 }
 119
 120 static void gsc_m2m_device_run(void *priv)
 121 {
 122         struct gsc_ctx *ctx = priv;
 123         struct gsc_dev *gsc;
 124         unsigned long flags;
 125         int ret;
 126         bool is_set = false;
 127
 128         if (WARN(!ctx, "null hardware context\n"))
 129                 return;
 130
 131         gsc = ctx->gsc_dev;
 132         spin_lock_irqsave(&gsc->slock, flags);
 133
 134         set_bit(ST_M2M_PEND, &gsc->state);
 135
 136         /* Reconfigure hardware if the context has changed. */
 137         if (gsc->m2m.ctx != ctx) {
 138                 pr_debug("gsc->m2m.ctx = 0x%p, current_ctx = 0x%p",
 139                                 gsc->m2m.ctx, ctx);
 140                 ctx->state |= GSC_PARAMS;
 141                 gsc->m2m.ctx = ctx;
 142         }
 143
 144         is_set = (ctx->state & GSC_CTX_STOP_REQ) ? 1 : 0;
 145         ctx->state &= ~GSC_CTX_STOP_REQ;
 146         if (is_set) {
 147                 wake_up(&gsc->irq_queue);
 148                 goto put_device;
 149         }
 150
 151         ret = gsc_fill_addr(ctx);
 152         if (ret) {
 153                 pr_err("Wrong address");
 154                 goto put_device;
 155         }
 156
 157         gsc_set_prefbuf(gsc, &ctx->s_frame);
 158         gsc_hw_set_input_addr(gsc, &ctx->s_frame.addr, GSC_M2M_BUF_NUM);
 159         gsc_hw_set_output_addr(gsc, &ctx->d_frame.addr, GSC_M2M_BUF_NUM);
 160
 161         if (ctx->state & GSC_PARAMS) {
 162                 gsc_hw_set_input_buf_masking(gsc, GSC_M2M_BUF_NUM, false);
 163                 gsc_hw_set_output_buf_masking(gsc, GSC_M2M_BUF_NUM, false);
 164                 gsc_hw_set_frm_done_irq_mask(gsc, false);
 165                 gsc_hw_set_gsc_irq_enable(gsc, true);
 166
 167                 if (gsc_set_scaler_info(ctx)) {
 168                         pr_err("Scaler setup error");
 169                         goto put_device;
 170                 }
 171
 172                 gsc_hw_set_input_path(ctx);
 173                 gsc_hw_set_in_size(ctx);
 174                 gsc_hw_set_in_image_format(ctx);
 175
 176                 gsc_hw_set_output_path(ctx);
 177                 gsc_hw_set_out_size(ctx);
 178                 gsc_hw_set_out_image_format(ctx);
 179
 180                 gsc_hw_set_prescaler(ctx);
 181                 gsc_hw_set_mainscaler(ctx);
 182                 gsc_hw_set_rotation(ctx);
 183                 gsc_hw_set_global_alpha(ctx);
 184         }
 185
 186         /* update shadow registers */
 187         gsc_hw_set_sfr_update(ctx);
 188
 189         ctx->state &= ~GSC_PARAMS;
 190         gsc_hw_enable_control(gsc, true);
 191
 192         spin_unlock_irqrestore(&gsc->slock, flags);
 193         return;
 194
 195 put_device:
 196         ctx->state &= ~GSC_PARAMS;
 197         spin_unlock_irqrestore(&gsc->slock, flags);
 198 }
 199
 200 static int gsc_m2m_queue_setup(struct vb2_queue *vq,
 201                         const struct v4l2_format *fmt,
 202                         unsigned int *num_buffers, unsigned int *num_planes,
 203                         unsigned int sizes[], void *allocators[])
 204 {
 205         struct gsc_ctx *ctx = vb2_get_drv_priv(vq);
 206         struct gsc_frame *frame;
 207         int i;
 208
 209         frame = ctx_get_frame(ctx, vq->type);
 210         if (IS_ERR(frame))
 211                 return PTR_ERR(frame);
 212
 213         if (!frame->fmt)
 214                 return -EINVAL;
 215
 216         *num_planes = frame->fmt->num_planes;
 217         for (i = 0; i < frame->fmt->num_planes; i++) {
 218                 sizes[i] = frame->payload[i];
 219                 allocators[i] = ctx->gsc_dev->alloc_ctx;
 220         }
 221         return 0;
 222 }
 223
 224 static int gsc_m2m_buf_prepare(struct vb2_buffer *vb)
 225 {
 226         struct gsc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 227         struct gsc_frame *frame;
 228         int i;
 229
 230         frame = ctx_get_frame(ctx, vb->vb2_queue->type);
 231         if (IS_ERR(frame))
 232                 return PTR_ERR(frame);
 233
 234         if (!V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) {
 235                 for (i = 0; i < frame->fmt->num_planes; i++)
 236                         vb2_set_plane_payload(vb, i, frame->payload[i]);
 237         }
 238
 239         return 0;
 240 }
 241
 242 static void gsc_m2m_buf_queue(struct vb2_buffer *vb)
 243 {
 244         struct gsc_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 245
 246         pr_debug("ctx: %p, ctx->state: 0x%x", ctx, ctx->state);
 247
 248         if (ctx->m2m_ctx)
 249                 v4l2_m2m_buf_queue(ctx->m2m_ctx, vb);
 250 }
 251
 252 static struct vb2_ops gsc_m2m_qops = {
 253         .queue_setup     = gsc_m2m_queue_setup,
 254         .buf_prepare     = gsc_m2m_buf_prepare,
 255         .buf_queue       = gsc_m2m_buf_queue,
 256         .wait_prepare    = gsc_unlock,
 257         .wait_finish     = gsc_lock,
 258         .stop_streaming  = gsc_m2m_stop_streaming,
 259         .start_streaming = gsc_m2m_start_streaming,
 260 };
 261
 262 static int gsc_m2m_querycap(struct file *file, void *fh,
 263                            struct v4l2_capability *cap)
 264 {
 265         struct gsc_ctx *ctx = fh_to_ctx(fh);
 266         struct gsc_dev *gsc = ctx->gsc_dev;
 267
 268         strlcpy(cap->driver, gsc->pdev->name, sizeof(cap->driver));
 269         strlcpy(cap->card, gsc->pdev->name, sizeof(cap->card));
 270         strlcpy(cap->bus_info, "platform", sizeof(cap->bus_info));
 271         cap->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M_MPLANE |
 272                 V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_VIDEO_OUTPUT_MPLANE;
 273
 274         cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 275         return 0;
 276 }
 277
 278 static int gsc_m2m_enum_fmt_mplane(struct file *file, void *priv,
 279                                 struct v4l2_fmtdesc *f)
 280 {
 281         return gsc_enum_fmt_mplane(f);
 282 }
 283
 284 static int gsc_m2m_g_fmt_mplane(struct file *file, void *fh,
 285                              struct v4l2_format *f)
 286 {
 287         struct gsc_ctx *ctx = fh_to_ctx(fh);
 288
 289         return gsc_g_fmt_mplane(ctx, f);
 290 }
 291
 292 static int gsc_m2m_try_fmt_mplane(struct file *file, void *fh,
 293                                   struct v4l2_format *f)
 294 {
 295         struct gsc_ctx *ctx = fh_to_ctx(fh);
 296
 297         return gsc_try_fmt_mplane(ctx, f);
 298 }
 299
 300 static int gsc_m2m_s_fmt_mplane(struct file *file, void *fh,
 301                                  struct v4l2_format *f)
 302 {
 303         struct gsc_ctx *ctx = fh_to_ctx(fh);
 304         struct vb2_queue *vq;
 305         struct gsc_frame *frame;
 306         struct v4l2_pix_format_mplane *pix;
 307         int i, ret = 0;
 308
 309         ret = gsc_m2m_try_fmt_mplane(file, fh, f);
 310         if (ret)
 311                 return ret;
 312
 313         vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
 314
 315         if (vb2_is_streaming(vq)) {
 316                 pr_err("queue (%d) busy", f->type);
 317                 return -EBUSY;
 318         }
 319
 320         if (V4L2_TYPE_IS_OUTPUT(f->type))
 321                 frame = &ctx->s_frame;
 322         else
 323                 frame = &ctx->d_frame;
 324
 325         pix = &f->fmt.pix_mp;
 326         frame->fmt = find_fmt(&pix->pixelformat, NULL, 0);
 327         frame->colorspace = pix->colorspace;
 328         if (!frame->fmt)
 329                 return -EINVAL;
 330
 331         for (i = 0; i < frame->fmt->num_planes; i++)
 332                 frame->payload[i] = pix->plane_fmt[i].sizeimage;
 333
 334         gsc_set_frame_size(frame, pix->width, pix->height);
 335
 336         if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
 337                 gsc_ctx_state_lock_set(GSC_PARAMS | GSC_DST_FMT, ctx);
 338         else
 339                 gsc_ctx_state_lock_set(GSC_PARAMS | GSC_SRC_FMT, ctx);
 340
 341         pr_debug("f_w: %d, f_h: %d", frame->f_width, frame->f_height);
 342
 343         return 0;
 344 }
 345
 346 static int gsc_m2m_reqbufs(struct file *file, void *fh,
 347                           struct v4l2_requestbuffers *reqbufs)
 348 {
 349         struct gsc_ctx *ctx = fh_to_ctx(fh);
 350         struct gsc_dev *gsc = ctx->gsc_dev;
 351         struct gsc_frame *frame;
 352         u32 max_cnt;
 353
 354         max_cnt = (reqbufs->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) ?
 355                 gsc->variant->in_buf_cnt : gsc->variant->out_buf_cnt;
 356         if (reqbufs->count > max_cnt) {
 357                 return -EINVAL;
 358         } else if (reqbufs->count == 0) {
 359                 if (reqbufs->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
 360                         gsc_ctx_state_lock_clear(GSC_SRC_FMT, ctx);
 361                 else
 362                         gsc_ctx_state_lock_clear(GSC_DST_FMT, ctx);
 363         }
 364
 365         frame = ctx_get_frame(ctx, reqbufs->type);
 366
 367         return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
 368 }
 369
 370 static int gsc_m2m_querybuf(struct file *file, void *fh,
 371                                         struct v4l2_buffer *buf)
 372 {
 373         struct gsc_ctx *ctx = fh_to_ctx(fh);
 374         return v4l2_m2m_querybuf(file, ctx->m2m_ctx, buf);
 375 }
 376
 377 static int gsc_m2m_qbuf(struct file *file, void *fh,
 378                           struct v4l2_buffer *buf)
 379 {
 380         struct gsc_ctx *ctx = fh_to_ctx(fh);
 381         return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
 382 }
 383
 384 static int gsc_m2m_dqbuf(struct file *file, void *fh,
 385                            struct v4l2_buffer *buf)
 386 {
 387         struct gsc_ctx *ctx = fh_to_ctx(fh);
 388         return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
 389 }
 390
 391 static int gsc_m2m_streamon(struct file *file, void *fh,
 392                            enum v4l2_buf_type type)
 393 {
 394         struct gsc_ctx *ctx = fh_to_ctx(fh);
 395
 396         /* The source and target color format need to be set */
 397         if (V4L2_TYPE_IS_OUTPUT(type)) {
 398                 if (!gsc_ctx_state_is_set(GSC_SRC_FMT, ctx))
 399                         return -EINVAL;
 400         } else if (!gsc_ctx_state_is_set(GSC_DST_FMT, ctx)) {
 401                 return -EINVAL;
 402         }
 403
 404         return v4l2_m2m_streamon(file, ctx->m2m_ctx, type);
 405 }
 406
 407 static int gsc_m2m_streamoff(struct file *file, void *fh,
 408                             enum v4l2_buf_type type)
 409 {
 410         struct gsc_ctx *ctx = fh_to_ctx(fh);
 411         return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type);
 412 }
 413
 414 /* Return 1 if rectangle a is enclosed in rectangle b, or 0 otherwise. */
 415 static int is_rectangle_enclosed(struct v4l2_rect *a, struct v4l2_rect *b)
 416 {
 417         if (a->left < b->left || a->top < b->top)
 418                 return 0;
 419
 420         if (a->left + a->width > b->left + b->width)
 421                 return 0;
 422
 423         if (a->top + a->height > b->top + b->height)
 424                 return 0;
 425
 426         return 1;
 427 }
 428
 429 static int gsc_m2m_g_selection(struct file *file, void *fh,
 430                         struct v4l2_selection *s)
 431 {
 432         struct gsc_frame *frame;
 433         struct gsc_ctx *ctx = fh_to_ctx(fh);
 434
 435         if ((s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) &&
 436             (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE))
 437                 return -EINVAL;
 438
 439         frame = ctx_get_frame(ctx, s->type);
 440         if (IS_ERR(frame))
 441                 return PTR_ERR(frame);
 442
 443         switch (s->target) {
 444         case V4L2_SEL_TGT_COMPOSE_DEFAULT:
 445         case V4L2_SEL_TGT_COMPOSE_BOUNDS:
 446         case V4L2_SEL_TGT_CROP_BOUNDS:
 447         case V4L2_SEL_TGT_CROP_DEFAULT:
 448                 s->r.left = 0;
 449                 s->r.top = 0;
 450                 s->r.width = frame->f_width;
 451                 s->r.height = frame->f_height;
 452                 return 0;
 453
 454         case V4L2_SEL_TGT_COMPOSE:
 455         case V4L2_SEL_TGT_CROP:
 456                 s->r.left = frame->crop.left;
 457                 s->r.top = frame->crop.top;
 458                 s->r.width = frame->crop.width;
 459                 s->r.height = frame->crop.height;
 460                 return 0;
 461         }
 462
 463         return -EINVAL;
 464 }
 465
 466 static int gsc_m2m_s_selection(struct file *file, void *fh,
 467                                 struct v4l2_selection *s)
 468 {
 469         struct gsc_frame *frame;
 470         struct gsc_ctx *ctx = fh_to_ctx(fh);
 471         struct v4l2_crop cr;
 472         struct gsc_variant *variant = ctx->gsc_dev->variant;
 473         int ret;
 474
 475         cr.type = s->type;
 476         cr.c = s->r;
 477
 478         if ((s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) &&
 479             (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE))
 480                 return -EINVAL;
 481
 482         ret = gsc_try_crop(ctx, &cr);
 483         if (ret)
 484                 return ret;
 485
 486         if (s->flags & V4L2_SEL_FLAG_LE &&
 487             !is_rectangle_enclosed(&cr.c, &s->r))
 488                 return -ERANGE;
 489
 490         if (s->flags & V4L2_SEL_FLAG_GE &&
 491             !is_rectangle_enclosed(&s->r, &cr.c))
 492                 return -ERANGE;
 493
 494         s->r = cr.c;
 495
 496         switch (s->target) {
 497         case V4L2_SEL_TGT_COMPOSE_BOUNDS:
 498         case V4L2_SEL_TGT_COMPOSE_DEFAULT:
 499         case V4L2_SEL_TGT_COMPOSE:
 500                 frame = &ctx->s_frame;
 501                 break;
 502
 503         case V4L2_SEL_TGT_CROP_BOUNDS:
 504         case V4L2_SEL_TGT_CROP:
 505         case V4L2_SEL_TGT_CROP_DEFAULT:
 506                 frame = &ctx->d_frame;
 507                 break;
 508
 509         default:
 510                 return -EINVAL;
 511         }
 512
 513         /* Check to see if scaling ratio is within supported range */
 514         if (gsc_ctx_state_is_set(GSC_DST_FMT | GSC_SRC_FMT, ctx)) {
 515                 if (s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
 516                         ret = gsc_check_scaler_ratio(variant, cr.c.width,
 517                                 cr.c.height, ctx->d_frame.crop.width,
 518                                 ctx->d_frame.crop.height,
 519                                 ctx->gsc_ctrls.rotate->val, ctx->out_path);
 520                 } else {
 521                         ret = gsc_check_scaler_ratio(variant,
 522                                 ctx->s_frame.crop.width,
 523                                 ctx->s_frame.crop.height, cr.c.width,
 524                                 cr.c.height, ctx->gsc_ctrls.rotate->val,
 525                                 ctx->out_path);
 526                 }
 527
 528                 if (ret) {
 529                         pr_err("Out of scaler range");
 530                         return -EINVAL;
 531                 }
 532         }
 533
 534         frame->crop = cr.c;
 535
 536         gsc_ctx_state_lock_set(GSC_PARAMS, ctx);
 537         return 0;
 538 }
 539
 540 static const struct v4l2_ioctl_ops gsc_m2m_ioctl_ops = {
 541         .vidioc_querycap                = gsc_m2m_querycap,
 542         .vidioc_enum_fmt_vid_cap_mplane = gsc_m2m_enum_fmt_mplane,
 543         .vidioc_enum_fmt_vid_out_mplane = gsc_m2m_enum_fmt_mplane,
 544         .vidioc_g_fmt_vid_cap_mplane    = gsc_m2m_g_fmt_mplane,
 545         .vidioc_g_fmt_vid_out_mplane    = gsc_m2m_g_fmt_mplane,
 546         .vidioc_try_fmt_vid_cap_mplane  = gsc_m2m_try_fmt_mplane,
 547         .vidioc_try_fmt_vid_out_mplane  = gsc_m2m_try_fmt_mplane,
 548         .vidioc_s_fmt_vid_cap_mplane    = gsc_m2m_s_fmt_mplane,
 549         .vidioc_s_fmt_vid_out_mplane    = gsc_m2m_s_fmt_mplane,
 550         .vidioc_reqbufs                 = gsc_m2m_reqbufs,
 551         .vidioc_querybuf                = gsc_m2m_querybuf,
 552         .vidioc_qbuf                    = gsc_m2m_qbuf,
 553         .vidioc_dqbuf                   = gsc_m2m_dqbuf,
 554         .vidioc_streamon                = gsc_m2m_streamon,
 555         .vidioc_streamoff               = gsc_m2m_streamoff,
 556         .vidioc_g_selection             = gsc_m2m_g_selection,
 557         .vidioc_s_selection             = gsc_m2m_s_selection
 558 };
 559
 560 static int queue_init(void *priv, struct vb2_queue *src_vq,
 561                         struct vb2_queue *dst_vq)
 562 {
 563         struct gsc_ctx *ctx = priv;
 564         int ret;
 565
 566         memset(src_vq, 0, sizeof(*src_vq));
 567         src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
 568         src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
 569         src_vq->drv_priv = ctx;
 570         src_vq->ops = &gsc_m2m_qops;
 571         src_vq->mem_ops = &vb2_dma_contig_memops;
 572         src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 573
 574         ret = vb2_queue_init(src_vq);
 575         if (ret)
 576                 return ret;
 577
 578         memset(dst_vq, 0, sizeof(*dst_vq));
 579         dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
 580         dst_vq->io_modes = VB2_MMAP | VB2_USERPTR;
 581         dst_vq->drv_priv = ctx;
 582         dst_vq->ops = &gsc_m2m_qops;
 583         dst_vq->mem_ops = &vb2_dma_contig_memops;
 584         dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
 585
 586         return vb2_queue_init(dst_vq);
 587 }
 588
 589 static int gsc_m2m_open(struct file *file)
 590 {
 591         struct gsc_dev *gsc = video_drvdata(file);
 592         struct gsc_ctx *ctx = NULL;
 593         int ret;
 594
 595         pr_debug("pid: %d, state: 0x%lx", task_pid_nr(current), gsc->state);
 596
 597         if (mutex_lock_interruptible(&gsc->lock))
 598                 return -ERESTARTSYS;
 599
 600         ctx = kzalloc(sizeof (*ctx), GFP_KERNEL);
 601         if (!ctx) {
 602                 ret = -ENOMEM;
 603                 goto unlock;
 604         }
 605
 606         v4l2_fh_init(&ctx->fh, gsc->m2m.vfd);
 607         ret = gsc_ctrls_create(ctx);
 608         if (ret)
 609                 goto error_fh;
 610
 611         /* Use separate control handler per file handle */
 612         ctx->fh.ctrl_handler = &ctx->ctrl_handler;
 613         file->private_data = &ctx->fh;
 614         v4l2_fh_add(&ctx->fh);
 615
 616         ctx->gsc_dev = gsc;
 617         /* Default color format */
 618         ctx->s_frame.fmt = get_format(0);
 619         ctx->d_frame.fmt = get_format(0);
 620         /* Setup the device context for mem2mem mode. */
 621         ctx->state = GSC_CTX_M2M;
 622         ctx->flags = 0;
 623         ctx->in_path = GSC_DMA;
 624         ctx->out_path = GSC_DMA;
 625
 626         ctx->m2m_ctx = v4l2_m2m_ctx_init(gsc->m2m.m2m_dev, ctx, queue_init);
 627         if (IS_ERR(ctx->m2m_ctx)) {
 628                 pr_err("Failed to initialize m2m context");
 629                 ret = PTR_ERR(ctx->m2m_ctx);
 630                 goto error_ctrls;
 631         }
 632
 633         if (gsc->m2m.refcnt++ == 0)
 634                 set_bit(ST_M2M_OPEN, &gsc->state);
 635
 636         pr_debug("gsc m2m driver is opened, ctx(0x%p)", ctx);
 637
 638         mutex_unlock(&gsc->lock);
 639         return 0;
 640
 641 error_ctrls:
 642         gsc_ctrls_delete(ctx);
 643 error_fh:
 644         v4l2_fh_del(&ctx->fh);
 645         v4l2_fh_exit(&ctx->fh);
 646         kfree(ctx);
 647 unlock:
 648         mutex_unlock(&gsc->lock);
 649         return ret;
 650 }
 651
 652 static int gsc_m2m_release(struct file *file)
 653 {
 654         struct gsc_ctx *ctx = fh_to_ctx(file->private_data);
 655         struct gsc_dev *gsc = ctx->gsc_dev;
 656
 657         pr_debug("pid: %d, state: 0x%lx, refcnt= %d",
 658                 task_pid_nr(current), gsc->state, gsc->m2m.refcnt);
 659
 660         if (mutex_lock_interruptible(&gsc->lock))
 661                 return -ERESTARTSYS;
 662
 663         v4l2_m2m_ctx_release(ctx->m2m_ctx);
 664         gsc_ctrls_delete(ctx);
 665         v4l2_fh_del(&ctx->fh);
 666         v4l2_fh_exit(&ctx->fh);
 667
 668         if (--gsc->m2m.refcnt <= 0)
 669                 clear_bit(ST_M2M_OPEN, &gsc->state);
 670         kfree(ctx);
 671
 672         mutex_unlock(&gsc->lock);
 673         return 0;
 674 }
 675
 676 static unsigned int gsc_m2m_poll(struct file *file,
 677                                         struct poll_table_struct *wait)
 678 {
 679         struct gsc_ctx *ctx = fh_to_ctx(file->private_data);
 680         struct gsc_dev *gsc = ctx->gsc_dev;
 681         int ret;
 682
 683         if (mutex_lock_interruptible(&gsc->lock))
 684                 return -ERESTARTSYS;
 685
 686         ret = v4l2_m2m_poll(file, ctx->m2m_ctx, wait);
 687         mutex_unlock(&gsc->lock);
 688
 689         return ret;
 690 }
 691
 692 static int gsc_m2m_mmap(struct file *file, struct vm_area_struct *vma)
 693 {
 694         struct gsc_ctx *ctx = fh_to_ctx(file->private_data);
 695         struct gsc_dev *gsc = ctx->gsc_dev;
 696         int ret;
 697
 698         if (mutex_lock_interruptible(&gsc->lock))
 699                 return -ERESTARTSYS;
 700
 701         ret = v4l2_m2m_mmap(file, ctx->m2m_ctx, vma);
 702         mutex_unlock(&gsc->lock);
 703
 704         return ret;
 705 }
 706
 707 static const struct v4l2_file_operations gsc_m2m_fops = {
 708         .owner          = THIS_MODULE,
 709         .open           = gsc_m2m_open,
 710         .release        = gsc_m2m_release,
 711         .poll           = gsc_m2m_poll,
 712         .unlocked_ioctl = video_ioctl2,
 713         .mmap           = gsc_m2m_mmap,
 714 };
 715
 716 static struct v4l2_m2m_ops gsc_m2m_ops = {
 717         .device_run     = gsc_m2m_device_run,
 718         .job_abort      = gsc_m2m_job_abort,
 719 };
 720
 721 int gsc_register_m2m_device(struct gsc_dev *gsc)
 722 {
 723         struct platform_device *pdev;
 724         int ret;
 725
 726         if (!gsc)
 727                 return -ENODEV;
 728
 729         pdev = gsc->pdev;
 730
 731         gsc->vdev.fops          = &gsc_m2m_fops;
 732         gsc->vdev.ioctl_ops     = &gsc_m2m_ioctl_ops;
 733         gsc->vdev.release       = video_device_release_empty;
 734         gsc->vdev.lock          = &gsc->lock;
 735         snprintf(gsc->vdev.name, sizeof(gsc->vdev.name), "%s.%d:m2m",
 736                                         GSC_MODULE_NAME, gsc->id);
 737
 738         video_set_drvdata(&gsc->vdev, gsc);
 739
 740         gsc->m2m.vfd = &gsc->vdev;
 741         gsc->m2m.m2m_dev = v4l2_m2m_init(&gsc_m2m_ops);
 742         if (IS_ERR(gsc->m2m.m2m_dev)) {
 743                 dev_err(&pdev->dev, "failed to initialize v4l2-m2m device\n");
 744                 ret = PTR_ERR(gsc->m2m.m2m_dev);
 745                 goto err_m2m_r1;
 746         }
 747
 748         ret = video_register_device(&gsc->vdev, VFL_TYPE_GRABBER, -1);
 749         if (ret) {
 750                 dev_err(&pdev->dev,
 751                          "%s(): failed to register video device\n", __func__);
 752                 goto err_m2m_r2;
 753         }
 754
 755         pr_debug("gsc m2m driver registered as /dev/video%d", gsc->vdev.num);
 756         return 0;
 757
 758 err_m2m_r2:
 759         v4l2_m2m_release(gsc->m2m.m2m_dev);
 760 err_m2m_r1:
 761         video_device_release(gsc->m2m.vfd);
 762
 763         return ret;
 764 }
 765
 766 void gsc_unregister_m2m_device(struct gsc_dev *gsc)
 767 {
 768         if (gsc)
 769                 v4l2_m2m_release(gsc->m2m.m2m_dev);
 770 }