drivers/staging/media/imx/imx-media-capture.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * Video Capture Subdev for Freescale i.MX5/6 SOC
   4  *
   5  * Copyright (c) 2012-2016 Mentor Graphics Inc.
   6  */
   7 #include <linux/delay.h>
   8 #include <linux/fs.h>
   9 #include <linux/module.h>
  10 #include <linux/of_platform.h>
  11 #include <linux/pinctrl/consumer.h>
  12 #include <linux/platform_device.h>
  13 #include <linux/sched.h>
  14 #include <linux/slab.h>
  15 #include <linux/spinlock.h>
  16 #include <linux/timer.h>
  17 #include <media/v4l2-ctrls.h>
  18 #include <media/v4l2-device.h>
  19 #include <media/v4l2-event.h>
  20 #include <media/v4l2-fwnode.h>
  21 #include <media/v4l2-ioctl.h>
  22 #include <media/v4l2-mc.h>
  23 #include <media/v4l2-subdev.h>
  24 #include <media/videobuf2-dma-contig.h>
  25 #include <video/imx-ipu-v3.h>
  26 #include <media/imx.h>
  27 #include "imx-media.h"
  28
  29 #define IMX_CAPTURE_NAME "imx-capture"
  30
  31 struct capture_priv {
  32         struct imx_media_video_dev vdev;
  33
  34         struct v4l2_subdev    *src_sd;
  35         int                   src_sd_pad;
  36         struct device         *dev;
  37
  38         struct imx_media_dev  *md;
  39
  40         struct media_pad      vdev_pad;
  41
  42         struct mutex          mutex;       /* capture device mutex */
  43
  44         /* the videobuf2 queue */
  45         struct vb2_queue       q;
  46         /* list of ready imx_media_buffer's from q */
  47         struct list_head       ready_q;
  48         /* protect ready_q */
  49         spinlock_t             q_lock;
  50
  51         /* controls inherited from subdevs */
  52         struct v4l2_ctrl_handler ctrl_hdlr;
  53
  54         /* misc status */
  55         bool                  stop;          /* streaming is stopping */
  56 };
  57
  58 #define to_capture_priv(v) container_of(v, struct capture_priv, vdev)
  59
  60 /* In bytes, per queue */
  61 #define VID_MEM_LIMIT   SZ_64M
  62
  63 static const struct vb2_ops capture_qops;
  64
  65 /*
  66  * Video ioctls follow
  67  */
  68
  69 static int vidioc_querycap(struct file *file, void *fh,
  70                            struct v4l2_capability *cap)
  71 {
  72         struct capture_priv *priv = video_drvdata(file);
  73
  74         strscpy(cap->driver, IMX_CAPTURE_NAME, sizeof(cap->driver));
  75         strscpy(cap->card, IMX_CAPTURE_NAME, sizeof(cap->card));
  76         snprintf(cap->bus_info, sizeof(cap->bus_info),
  77                  "platform:%s", priv->src_sd->name);
  78
  79         return 0;
  80 }
  81
  82 static int capture_enum_framesizes(struct file *file, void *fh,
  83                                    struct v4l2_frmsizeenum *fsize)
  84 {
  85         struct capture_priv *priv = video_drvdata(file);
  86         const struct imx_media_pixfmt *cc;
  87         struct v4l2_subdev_frame_size_enum fse = {
  88                 .index = fsize->index,
  89                 .pad = priv->src_sd_pad,
  90                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
  91         };
  92         int ret;
  93
  94         cc = imx_media_find_pixel_format(fsize->pixel_format, PIXFMT_SEL_ANY);
  95         if (!cc)
  96                 return -EINVAL;
  97
  98         fse.code = cc->codes ? cc->codes[0] : 0;
  99
 100         ret = v4l2_subdev_call(priv->src_sd, pad, enum_frame_size, NULL, &fse);
 101         if (ret)
 102                 return ret;
 103
 104         if (fse.min_width == fse.max_width &&
 105             fse.min_height == fse.max_height) {
 106                 fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
 107                 fsize->discrete.width = fse.min_width;
 108                 fsize->discrete.height = fse.min_height;
 109         } else {
 110                 fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
 111                 fsize->stepwise.min_width = fse.min_width;
 112                 fsize->stepwise.max_width = fse.max_width;
 113                 fsize->stepwise.min_height = fse.min_height;
 114                 fsize->stepwise.max_height = fse.max_height;
 115                 fsize->stepwise.step_width = 1;
 116                 fsize->stepwise.step_height = 1;
 117         }
 118
 119         return 0;
 120 }
 121
 122 static int capture_enum_frameintervals(struct file *file, void *fh,
 123                                        struct v4l2_frmivalenum *fival)
 124 {
 125         struct capture_priv *priv = video_drvdata(file);
 126         const struct imx_media_pixfmt *cc;
 127         struct v4l2_subdev_frame_interval_enum fie = {
 128                 .index = fival->index,
 129                 .pad = priv->src_sd_pad,
 130                 .width = fival->width,
 131                 .height = fival->height,
 132                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 133         };
 134         int ret;
 135
 136         cc = imx_media_find_pixel_format(fival->pixel_format, PIXFMT_SEL_ANY);
 137         if (!cc)
 138                 return -EINVAL;
 139
 140         fie.code = cc->codes ? cc->codes[0] : 0;
 141
 142         ret = v4l2_subdev_call(priv->src_sd, pad, enum_frame_interval,
 143                                NULL, &fie);
 144         if (ret)
 145                 return ret;
 146
 147         fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
 148         fival->discrete = fie.interval;
 149
 150         return 0;
 151 }
 152
 153 static int capture_enum_fmt_vid_cap(struct file *file, void *fh,
 154                                     struct v4l2_fmtdesc *f)
 155 {
 156         struct capture_priv *priv = video_drvdata(file);
 157         const struct imx_media_pixfmt *cc_src;
 158         struct v4l2_subdev_format fmt_src;
 159         u32 fourcc;
 160         int ret;
 161
 162         fmt_src.pad = priv->src_sd_pad;
 163         fmt_src.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 164         ret = v4l2_subdev_call(priv->src_sd, pad, get_fmt, NULL, &fmt_src);
 165         if (ret) {
 166                 v4l2_err(priv->src_sd, "failed to get src_sd format\n");
 167                 return ret;
 168         }
 169
 170         cc_src = imx_media_find_ipu_format(fmt_src.format.code,
 171                                            PIXFMT_SEL_YUV_RGB);
 172         if (cc_src) {
 173                 enum imx_pixfmt_sel fmt_sel =
 174                         (cc_src->cs == IPUV3_COLORSPACE_YUV) ?
 175                         PIXFMT_SEL_YUV : PIXFMT_SEL_RGB;
 176
 177                 ret = imx_media_enum_pixel_formats(&fourcc, f->index, fmt_sel);
 178                 if (ret)
 179                         return ret;
 180         } else {
 181                 cc_src = imx_media_find_mbus_format(fmt_src.format.code,
 182                                                     PIXFMT_SEL_ANY);
 183                 if (WARN_ON(!cc_src))
 184                         return -EINVAL;
 185
 186                 if (f->index != 0)
 187                         return -EINVAL;
 188                 fourcc = cc_src->fourcc;
 189         }
 190
 191         f->pixelformat = fourcc;
 192
 193         return 0;
 194 }
 195
 196 static int capture_g_fmt_vid_cap(struct file *file, void *fh,
 197                                  struct v4l2_format *f)
 198 {
 199         struct capture_priv *priv = video_drvdata(file);
 200
 201         *f = priv->vdev.fmt;
 202
 203         return 0;
 204 }
 205
 206 static int __capture_try_fmt_vid_cap(struct capture_priv *priv,
 207                                      struct v4l2_subdev_format *fmt_src,
 208                                      struct v4l2_format *f,
 209                                      const struct imx_media_pixfmt **retcc,
 210                                      struct v4l2_rect *compose)
 211 {
 212         const struct imx_media_pixfmt *cc, *cc_src;
 213
 214         cc_src = imx_media_find_ipu_format(fmt_src->format.code,
 215                                            PIXFMT_SEL_YUV_RGB);
 216         if (cc_src) {
 217                 enum imx_pixfmt_sel fmt_sel;
 218                 u32 fourcc;
 219
 220                 fmt_sel = (cc_src->cs == IPUV3_COLORSPACE_YUV) ?
 221                         PIXFMT_SEL_YUV : PIXFMT_SEL_RGB;
 222                 fourcc = f->fmt.pix.pixelformat;
 223
 224                 cc = imx_media_find_pixel_format(fourcc, fmt_sel);
 225                 if (!cc) {
 226                         imx_media_enum_pixel_formats(&fourcc, 0, fmt_sel);
 227                         cc = imx_media_find_pixel_format(fourcc, fmt_sel);
 228                 }
 229         } else {
 230                 cc_src = imx_media_find_mbus_format(fmt_src->format.code,
 231                                                     PIXFMT_SEL_ANY);
 232                 if (WARN_ON(!cc_src))
 233                         return -EINVAL;
 234
 235                 cc = cc_src;
 236         }
 237
 238         /* allow IDMAC interweave but enforce field order from source */
 239         if (V4L2_FIELD_IS_INTERLACED(f->fmt.pix.field)) {
 240                 switch (fmt_src->format.field) {
 241                 case V4L2_FIELD_SEQ_TB:
 242                         fmt_src->format.field = V4L2_FIELD_INTERLACED_TB;
 243                         break;
 244                 case V4L2_FIELD_SEQ_BT:
 245                         fmt_src->format.field = V4L2_FIELD_INTERLACED_BT;
 246                         break;
 247                 default:
 248                         break;
 249                 }
 250         }
 251
 252         imx_media_mbus_fmt_to_pix_fmt(&f->fmt.pix, &fmt_src->format, cc);
 253
 254         if (retcc)
 255                 *retcc = cc;
 256
 257         if (compose) {
 258                 compose->left = 0;
 259                 compose->top = 0;
 260                 compose->width = fmt_src->format.width;
 261                 compose->height = fmt_src->format.height;
 262         }
 263
 264         return 0;
 265 }
 266
 267 static int capture_try_fmt_vid_cap(struct file *file, void *fh,
 268                                    struct v4l2_format *f)
 269 {
 270         struct capture_priv *priv = video_drvdata(file);
 271         struct v4l2_subdev_format fmt_src;
 272         int ret;
 273
 274         fmt_src.pad = priv->src_sd_pad;
 275         fmt_src.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 276         ret = v4l2_subdev_call(priv->src_sd, pad, get_fmt, NULL, &fmt_src);
 277         if (ret)
 278                 return ret;
 279
 280         return __capture_try_fmt_vid_cap(priv, &fmt_src, f, NULL, NULL);
 281 }
 282
 283 static int capture_s_fmt_vid_cap(struct file *file, void *fh,
 284                                  struct v4l2_format *f)
 285 {
 286         struct capture_priv *priv = video_drvdata(file);
 287         struct v4l2_subdev_format fmt_src;
 288         int ret;
 289
 290         if (vb2_is_busy(&priv->q)) {
 291                 v4l2_err(priv->src_sd, "%s queue busy\n", __func__);
 292                 return -EBUSY;
 293         }
 294
 295         fmt_src.pad = priv->src_sd_pad;
 296         fmt_src.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 297         ret = v4l2_subdev_call(priv->src_sd, pad, get_fmt, NULL, &fmt_src);
 298         if (ret)
 299                 return ret;
 300
 301         ret = __capture_try_fmt_vid_cap(priv, &fmt_src, f, &priv->vdev.cc,
 302                                         &priv->vdev.compose);
 303         if (ret)
 304                 return ret;
 305
 306         priv->vdev.fmt.fmt.pix = f->fmt.pix;
 307
 308         return 0;
 309 }
 310
 311 static int capture_querystd(struct file *file, void *fh, v4l2_std_id *std)
 312 {
 313         struct capture_priv *priv = video_drvdata(file);
 314
 315         return v4l2_subdev_call(priv->src_sd, video, querystd, std);
 316 }
 317
 318 static int capture_g_std(struct file *file, void *fh, v4l2_std_id *std)
 319 {
 320         struct capture_priv *priv = video_drvdata(file);
 321
 322         return v4l2_subdev_call(priv->src_sd, video, g_std, std);
 323 }
 324
 325 static int capture_s_std(struct file *file, void *fh, v4l2_std_id std)
 326 {
 327         struct capture_priv *priv = video_drvdata(file);
 328
 329         if (vb2_is_busy(&priv->q))
 330                 return -EBUSY;
 331
 332         return v4l2_subdev_call(priv->src_sd, video, s_std, std);
 333 }
 334
 335 static int capture_g_selection(struct file *file, void *fh,
 336                                struct v4l2_selection *s)
 337 {
 338         struct capture_priv *priv = video_drvdata(file);
 339
 340         switch (s->target) {
 341         case V4L2_SEL_TGT_COMPOSE:
 342         case V4L2_SEL_TGT_COMPOSE_DEFAULT:
 343         case V4L2_SEL_TGT_COMPOSE_BOUNDS:
 344                 /* The compose rectangle is fixed to the source format. */
 345                 s->r = priv->vdev.compose;
 346                 break;
 347         case V4L2_SEL_TGT_COMPOSE_PADDED:
 348                 /*
 349                  * The hardware writes with a configurable but fixed DMA burst
 350                  * size. If the source format width is not burst size aligned,
 351                  * the written frame contains padding to the right.
 352                  */
 353                 s->r.left = 0;
 354                 s->r.top = 0;
 355                 s->r.width = priv->vdev.fmt.fmt.pix.width;
 356                 s->r.height = priv->vdev.fmt.fmt.pix.height;
 357                 break;
 358         default:
 359                 return -EINVAL;
 360         }
 361
 362         return 0;
 363 }
 364
 365 static int capture_g_parm(struct file *file, void *fh,
 366                           struct v4l2_streamparm *a)
 367 {
 368         struct capture_priv *priv = video_drvdata(file);
 369         struct v4l2_subdev_frame_interval fi;
 370         int ret;
 371
 372         if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 373                 return -EINVAL;
 374
 375         memset(&fi, 0, sizeof(fi));
 376         fi.pad = priv->src_sd_pad;
 377         ret = v4l2_subdev_call(priv->src_sd, video, g_frame_interval, &fi);
 378         if (ret < 0)
 379                 return ret;
 380
 381         a->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
 382         a->parm.capture.timeperframe = fi.interval;
 383
 384         return 0;
 385 }
 386
 387 static int capture_s_parm(struct file *file, void *fh,
 388                           struct v4l2_streamparm *a)
 389 {
 390         struct capture_priv *priv = video_drvdata(file);
 391         struct v4l2_subdev_frame_interval fi;
 392         int ret;
 393
 394         if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 395                 return -EINVAL;
 396
 397         memset(&fi, 0, sizeof(fi));
 398         fi.pad = priv->src_sd_pad;
 399         fi.interval = a->parm.capture.timeperframe;
 400         ret = v4l2_subdev_call(priv->src_sd, video, s_frame_interval, &fi);
 401         if (ret < 0)
 402                 return ret;
 403
 404         a->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
 405         a->parm.capture.timeperframe = fi.interval;
 406
 407         return 0;
 408 }
 409
 410 static int capture_subscribe_event(struct v4l2_fh *fh,
 411                                    const struct v4l2_event_subscription *sub)
 412 {
 413         switch (sub->type) {
 414         case V4L2_EVENT_IMX_FRAME_INTERVAL_ERROR:
 415                 return v4l2_event_subscribe(fh, sub, 0, NULL);
 416         case V4L2_EVENT_SOURCE_CHANGE:
 417                 return v4l2_src_change_event_subscribe(fh, sub);
 418         case V4L2_EVENT_CTRL:
 419                 return v4l2_ctrl_subscribe_event(fh, sub);
 420         default:
 421                 return -EINVAL;
 422         }
 423 }
 424
 425 static const struct v4l2_ioctl_ops capture_ioctl_ops = {
 426         .vidioc_querycap        = vidioc_querycap,
 427
 428         .vidioc_enum_framesizes = capture_enum_framesizes,
 429         .vidioc_enum_frameintervals = capture_enum_frameintervals,
 430
 431         .vidioc_enum_fmt_vid_cap        = capture_enum_fmt_vid_cap,
 432         .vidioc_g_fmt_vid_cap           = capture_g_fmt_vid_cap,
 433         .vidioc_try_fmt_vid_cap         = capture_try_fmt_vid_cap,
 434         .vidioc_s_fmt_vid_cap           = capture_s_fmt_vid_cap,
 435
 436         .vidioc_querystd        = capture_querystd,
 437         .vidioc_g_std           = capture_g_std,
 438         .vidioc_s_std           = capture_s_std,
 439
 440         .vidioc_g_selection     = capture_g_selection,
 441
 442         .vidioc_g_parm          = capture_g_parm,
 443         .vidioc_s_parm          = capture_s_parm,
 444
 445         .vidioc_reqbufs         = vb2_ioctl_reqbufs,
 446         .vidioc_create_bufs     = vb2_ioctl_create_bufs,
 447         .vidioc_prepare_buf     = vb2_ioctl_prepare_buf,
 448         .vidioc_querybuf        = vb2_ioctl_querybuf,
 449         .vidioc_qbuf            = vb2_ioctl_qbuf,
 450         .vidioc_dqbuf           = vb2_ioctl_dqbuf,
 451         .vidioc_expbuf          = vb2_ioctl_expbuf,
 452         .vidioc_streamon        = vb2_ioctl_streamon,
 453         .vidioc_streamoff       = vb2_ioctl_streamoff,
 454
 455         .vidioc_subscribe_event = capture_subscribe_event,
 456         .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 457 };
 458
 459 /*
 460  * Queue operations
 461  */
 462
 463 static int capture_queue_setup(struct vb2_queue *vq,
 464                                unsigned int *nbuffers,
 465                                unsigned int *nplanes,
 466                                unsigned int sizes[],
 467                                struct device *alloc_devs[])
 468 {
 469         struct capture_priv *priv = vb2_get_drv_priv(vq);
 470         struct v4l2_pix_format *pix = &priv->vdev.fmt.fmt.pix;
 471         unsigned int count = *nbuffers;
 472
 473         if (vq->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 474                 return -EINVAL;
 475
 476         if (*nplanes) {
 477                 if (*nplanes != 1 || sizes[0] < pix->sizeimage)
 478                         return -EINVAL;
 479                 count += vq->num_buffers;
 480         }
 481
 482         count = min_t(__u32, VID_MEM_LIMIT / pix->sizeimage, count);
 483
 484         if (*nplanes)
 485                 *nbuffers = (count < vq->num_buffers) ? 0 :
 486                         count - vq->num_buffers;
 487         else
 488                 *nbuffers = count;
 489
 490         *nplanes = 1;
 491         sizes[0] = pix->sizeimage;
 492
 493         return 0;
 494 }
 495
 496 static int capture_buf_init(struct vb2_buffer *vb)
 497 {
 498         struct imx_media_buffer *buf = to_imx_media_vb(vb);
 499
 500         INIT_LIST_HEAD(&buf->list);
 501
 502         return 0;
 503 }
 504
 505 static int capture_buf_prepare(struct vb2_buffer *vb)
 506 {
 507         struct vb2_queue *vq = vb->vb2_queue;
 508         struct capture_priv *priv = vb2_get_drv_priv(vq);
 509         struct v4l2_pix_format *pix = &priv->vdev.fmt.fmt.pix;
 510
 511         if (vb2_plane_size(vb, 0) < pix->sizeimage) {
 512                 v4l2_err(priv->src_sd,
 513                          "data will not fit into plane (%lu < %lu)\n",
 514                          vb2_plane_size(vb, 0), (long)pix->sizeimage);
 515                 return -EINVAL;
 516         }
 517
 518         vb2_set_plane_payload(vb, 0, pix->sizeimage);
 519
 520         return 0;
 521 }
 522
 523 static void capture_buf_queue(struct vb2_buffer *vb)
 524 {
 525         struct capture_priv *priv = vb2_get_drv_priv(vb->vb2_queue);
 526         struct imx_media_buffer *buf = to_imx_media_vb(vb);
 527         unsigned long flags;
 528
 529         spin_lock_irqsave(&priv->q_lock, flags);
 530
 531         list_add_tail(&buf->list, &priv->ready_q);
 532
 533         spin_unlock_irqrestore(&priv->q_lock, flags);
 534 }
 535
 536 static int capture_validate_fmt(struct capture_priv *priv)
 537 {
 538         struct v4l2_subdev_format fmt_src;
 539         const struct imx_media_pixfmt *cc;
 540         struct v4l2_rect compose;
 541         struct v4l2_format f;
 542         int ret;
 543
 544         fmt_src.pad = priv->src_sd_pad;
 545         fmt_src.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 546         ret = v4l2_subdev_call(priv->src_sd, pad, get_fmt, NULL, &fmt_src);
 547         if (ret)
 548                 return ret;
 549
 550         v4l2_fill_pix_format(&f.fmt.pix, &fmt_src.format);
 551
 552         ret = __capture_try_fmt_vid_cap(priv, &fmt_src, &f, &cc, &compose);
 553         if (ret)
 554                 return ret;
 555
 556         return (priv->vdev.fmt.fmt.pix.width != f.fmt.pix.width ||
 557                 priv->vdev.fmt.fmt.pix.height != f.fmt.pix.height ||
 558                 priv->vdev.cc->cs != cc->cs ||
 559                 priv->vdev.compose.width != compose.width ||
 560                 priv->vdev.compose.height != compose.height) ? -EPIPE : 0;
 561 }
 562
 563 static int capture_start_streaming(struct vb2_queue *vq, unsigned int count)
 564 {
 565         struct capture_priv *priv = vb2_get_drv_priv(vq);
 566         struct imx_media_buffer *buf, *tmp;
 567         unsigned long flags;
 568         int ret;
 569
 570         ret = capture_validate_fmt(priv);
 571         if (ret) {
 572                 v4l2_err(priv->src_sd, "capture format not valid\n");
 573                 goto return_bufs;
 574         }
 575
 576         ret = imx_media_pipeline_set_stream(priv->md, &priv->src_sd->entity,
 577                                             true);
 578         if (ret) {
 579                 v4l2_err(priv->src_sd, "pipeline start failed with %d\n", ret);
 580                 goto return_bufs;
 581         }
 582
 583         priv->stop = false;
 584
 585         return 0;
 586
 587 return_bufs:
 588         spin_lock_irqsave(&priv->q_lock, flags);
 589         list_for_each_entry_safe(buf, tmp, &priv->ready_q, list) {
 590                 list_del(&buf->list);
 591                 vb2_buffer_done(&buf->vbuf.vb2_buf, VB2_BUF_STATE_QUEUED);
 592         }
 593         spin_unlock_irqrestore(&priv->q_lock, flags);
 594         return ret;
 595 }
 596
 597 static void capture_stop_streaming(struct vb2_queue *vq)
 598 {
 599         struct capture_priv *priv = vb2_get_drv_priv(vq);
 600         struct imx_media_buffer *frame;
 601         struct imx_media_buffer *tmp;
 602         unsigned long flags;
 603         int ret;
 604
 605         spin_lock_irqsave(&priv->q_lock, flags);
 606         priv->stop = true;
 607         spin_unlock_irqrestore(&priv->q_lock, flags);
 608
 609         ret = imx_media_pipeline_set_stream(priv->md, &priv->src_sd->entity,
 610                                             false);
 611         if (ret)
 612                 v4l2_warn(priv->src_sd, "pipeline stop failed with %d\n", ret);
 613
 614         /* release all active buffers */
 615         spin_lock_irqsave(&priv->q_lock, flags);
 616         list_for_each_entry_safe(frame, tmp, &priv->ready_q, list) {
 617                 list_del(&frame->list);
 618                 vb2_buffer_done(&frame->vbuf.vb2_buf, VB2_BUF_STATE_ERROR);
 619         }
 620         spin_unlock_irqrestore(&priv->q_lock, flags);
 621 }
 622
 623 static const struct vb2_ops capture_qops = {
 624         .queue_setup     = capture_queue_setup,
 625         .buf_init        = capture_buf_init,
 626         .buf_prepare     = capture_buf_prepare,
 627         .buf_queue       = capture_buf_queue,
 628         .wait_prepare    = vb2_ops_wait_prepare,
 629         .wait_finish     = vb2_ops_wait_finish,
 630         .start_streaming = capture_start_streaming,
 631         .stop_streaming  = capture_stop_streaming,
 632 };
 633
 634 /*
 635  * File operations
 636  */
 637 static int capture_open(struct file *file)
 638 {
 639         struct capture_priv *priv = video_drvdata(file);
 640         struct video_device *vfd = priv->vdev.vfd;
 641         int ret;
 642
 643         if (mutex_lock_interruptible(&priv->mutex))
 644                 return -ERESTARTSYS;
 645
 646         ret = v4l2_fh_open(file);
 647         if (ret)
 648                 v4l2_err(priv->src_sd, "v4l2_fh_open failed\n");
 649
 650         ret = v4l2_pipeline_pm_get(&vfd->entity);
 651         if (ret)
 652                 v4l2_fh_release(file);
 653
 654         mutex_unlock(&priv->mutex);
 655         return ret;
 656 }
 657
 658 static int capture_release(struct file *file)
 659 {
 660         struct capture_priv *priv = video_drvdata(file);
 661         struct video_device *vfd = priv->vdev.vfd;
 662         struct vb2_queue *vq = &priv->q;
 663
 664         mutex_lock(&priv->mutex);
 665
 666         if (file->private_data == vq->owner) {
 667                 vb2_queue_release(vq);
 668                 vq->owner = NULL;
 669         }
 670
 671         v4l2_pipeline_pm_put(&vfd->entity);
 672
 673         v4l2_fh_release(file);
 674         mutex_unlock(&priv->mutex);
 675         return 0;
 676 }
 677
 678 static const struct v4l2_file_operations capture_fops = {
 679         .owner          = THIS_MODULE,
 680         .open           = capture_open,
 681         .release        = capture_release,
 682         .poll           = vb2_fop_poll,
 683         .unlocked_ioctl = video_ioctl2,
 684         .mmap           = vb2_fop_mmap,
 685 };
 686
 687 static struct video_device capture_videodev = {
 688         .fops           = &capture_fops,
 689         .ioctl_ops      = &capture_ioctl_ops,
 690         .minor          = -1,
 691         .release        = video_device_release,
 692         .vfl_dir        = VFL_DIR_RX,
 693         .tvnorms        = V4L2_STD_NTSC | V4L2_STD_PAL | V4L2_STD_SECAM,
 694         .device_caps    = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING,
 695 };
 696
 697 struct imx_media_buffer *
 698 imx_media_capture_device_next_buf(struct imx_media_video_dev *vdev)
 699 {
 700         struct capture_priv *priv = to_capture_priv(vdev);
 701         struct imx_media_buffer *buf = NULL;
 702         unsigned long flags;
 703
 704         spin_lock_irqsave(&priv->q_lock, flags);
 705
 706         /* get next queued buffer */
 707         if (!list_empty(&priv->ready_q)) {
 708                 buf = list_entry(priv->ready_q.next, struct imx_media_buffer,
 709                                  list);
 710                 list_del(&buf->list);
 711         }
 712
 713         spin_unlock_irqrestore(&priv->q_lock, flags);
 714
 715         return buf;
 716 }
 717 EXPORT_SYMBOL_GPL(imx_media_capture_device_next_buf);
 718
 719 void imx_media_capture_device_error(struct imx_media_video_dev *vdev)
 720 {
 721         struct capture_priv *priv = to_capture_priv(vdev);
 722         struct vb2_queue *vq = &priv->q;
 723         unsigned long flags;
 724
 725         if (!vb2_is_streaming(vq))
 726                 return;
 727
 728         spin_lock_irqsave(&priv->q_lock, flags);
 729         vb2_queue_error(vq);
 730         spin_unlock_irqrestore(&priv->q_lock, flags);
 731 }
 732 EXPORT_SYMBOL_GPL(imx_media_capture_device_error);
 733
 734 int imx_media_capture_device_register(struct imx_media_video_dev *vdev)
 735 {
 736         struct capture_priv *priv = to_capture_priv(vdev);
 737         struct v4l2_subdev *sd = priv->src_sd;
 738         struct v4l2_device *v4l2_dev = sd->v4l2_dev;
 739         struct video_device *vfd = vdev->vfd;
 740         struct vb2_queue *vq = &priv->q;
 741         struct v4l2_subdev_format fmt_src;
 742         int ret;
 743
 744         /* get media device */
 745         priv->md = container_of(v4l2_dev->mdev, struct imx_media_dev, md);
 746
 747         vfd->v4l2_dev = v4l2_dev;
 748
 749         ret = video_register_device(vfd, VFL_TYPE_VIDEO, -1);
 750         if (ret) {
 751                 v4l2_err(sd, "Failed to register video device\n");
 752                 return ret;
 753         }
 754
 755         vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 756         vq->io_modes = VB2_MMAP | VB2_DMABUF;
 757         vq->drv_priv = priv;
 758         vq->buf_struct_size = sizeof(struct imx_media_buffer);
 759         vq->ops = &capture_qops;
 760         vq->mem_ops = &vb2_dma_contig_memops;
 761         vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 762         vq->lock = &priv->mutex;
 763         vq->min_buffers_needed = 2;
 764         vq->dev = priv->dev;
 765
 766         ret = vb2_queue_init(vq);
 767         if (ret) {
 768                 v4l2_err(sd, "vb2_queue_init failed\n");
 769                 goto unreg;
 770         }
 771
 772         INIT_LIST_HEAD(&priv->ready_q);
 773
 774         /* create the link from the src_sd devnode pad to device node */
 775         ret = media_create_pad_link(&sd->entity, priv->src_sd_pad,
 776                                     &vfd->entity, 0, 0);
 777         if (ret) {
 778                 v4l2_err(sd, "failed to create link to device node\n");
 779                 goto unreg;
 780         }
 781
 782         /* setup default format */
 783         fmt_src.pad = priv->src_sd_pad;
 784         fmt_src.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 785         ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt_src);
 786         if (ret) {
 787                 v4l2_err(sd, "failed to get src_sd format\n");
 788                 goto unreg;
 789         }
 790
 791         vdev->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 792         imx_media_mbus_fmt_to_pix_fmt(&vdev->fmt.fmt.pix,
 793                                       &fmt_src.format, NULL);
 794         vdev->compose.width = fmt_src.format.width;
 795         vdev->compose.height = fmt_src.format.height;
 796         vdev->cc = imx_media_find_pixel_format(vdev->fmt.fmt.pix.pixelformat,
 797                                                PIXFMT_SEL_ANY);
 798
 799         v4l2_info(sd, "Registered %s as /dev/%s\n", vfd->name,
 800                   video_device_node_name(vfd));
 801
 802         vfd->ctrl_handler = &priv->ctrl_hdlr;
 803
 804         /* add vdev to the video device list */
 805         imx_media_add_video_device(priv->md, vdev);
 806
 807         return 0;
 808 unreg:
 809         video_unregister_device(vfd);
 810         return ret;
 811 }
 812 EXPORT_SYMBOL_GPL(imx_media_capture_device_register);
 813
 814 void imx_media_capture_device_unregister(struct imx_media_video_dev *vdev)
 815 {
 816         struct capture_priv *priv = to_capture_priv(vdev);
 817         struct video_device *vfd = priv->vdev.vfd;
 818
 819         mutex_lock(&priv->mutex);
 820
 821         if (video_is_registered(vfd)) {
 822                 video_unregister_device(vfd);
 823                 media_entity_cleanup(&vfd->entity);
 824         }
 825
 826         mutex_unlock(&priv->mutex);
 827 }
 828 EXPORT_SYMBOL_GPL(imx_media_capture_device_unregister);
 829
 830 struct imx_media_video_dev *
 831 imx_media_capture_device_init(struct device *dev, struct v4l2_subdev *src_sd,
 832                               int pad)
 833 {
 834         struct capture_priv *priv;
 835         struct video_device *vfd;
 836         int ret;
 837
 838         priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
 839         if (!priv)
 840                 return ERR_PTR(-ENOMEM);
 841
 842         priv->src_sd = src_sd;
 843         priv->src_sd_pad = pad;
 844         priv->dev = dev;
 845
 846         mutex_init(&priv->mutex);
 847         spin_lock_init(&priv->q_lock);
 848
 849         snprintf(capture_videodev.name, sizeof(capture_videodev.name),
 850                  "%s capture", src_sd->name);
 851
 852         vfd = video_device_alloc();
 853         if (!vfd)
 854                 return ERR_PTR(-ENOMEM);
 855
 856         *vfd = capture_videodev;
 857         vfd->lock = &priv->mutex;
 858         vfd->queue = &priv->q;
 859         priv->vdev.vfd = vfd;
 860
 861         priv->vdev_pad.flags = MEDIA_PAD_FL_SINK;
 862         ret = media_entity_pads_init(&vfd->entity, 1, &priv->vdev_pad);
 863         if (ret) {
 864                 video_device_release(vfd);
 865                 return ERR_PTR(ret);
 866         }
 867
 868         INIT_LIST_HEAD(&priv->vdev.list);
 869
 870         video_set_drvdata(vfd, priv);
 871
 872         v4l2_ctrl_handler_init(&priv->ctrl_hdlr, 0);
 873
 874         return &priv->vdev;
 875 }
 876 EXPORT_SYMBOL_GPL(imx_media_capture_device_init);
 877
 878 void imx_media_capture_device_remove(struct imx_media_video_dev *vdev)
 879 {
 880         struct capture_priv *priv = to_capture_priv(vdev);
 881
 882         v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
 883 }
 884 EXPORT_SYMBOL_GPL(imx_media_capture_device_remove);
 885
 886 MODULE_DESCRIPTION("i.MX5/6 v4l2 video capture interface driver");
 887 MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
 888 MODULE_LICENSE("GPL");