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

   1 /*
   2  * V4L2 Deinterlacer Subdev for Freescale i.MX5/6 SOC
   3  *
   4  * Copyright (c) 2017 Mentor Graphics Inc.
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation; either version 2 of the License, or
   9  * (at your option) any later version.
  10  */
  11 #include <linux/delay.h>
  12 #include <linux/interrupt.h>
  13 #include <linux/module.h>
  14 #include <linux/platform_device.h>
  15 #include <linux/sched.h>
  16 #include <linux/slab.h>
  17 #include <linux/timer.h>
  18 #include <media/v4l2-ctrls.h>
  19 #include <media/v4l2-device.h>
  20 #include <media/v4l2-ioctl.h>
  21 #include <media/v4l2-mc.h>
  22 #include <media/v4l2-subdev.h>
  23 #include <media/imx.h>
  24 #include "imx-media.h"
  25
  26 /*
  27  * This subdev implements two different video pipelines:
  28  *
  29  * CSI -> VDIC
  30  *
  31  * In this pipeline, the CSI sends a single interlaced field F(n-1)
  32  * directly to the VDIC (and optionally the following field F(n)
  33  * can be sent to memory via IDMAC channel 13). This pipeline only works
  34  * in VDIC's high motion mode, which only requires a single field for
  35  * processing. The other motion modes (low and medium) require three
  36  * fields, so this pipeline does not work in those modes. Also, it is
  37  * not clear how this pipeline can deal with the various field orders
  38  * (sequential BT/TB, interlaced BT/TB).
  39  *
  40  * MEM -> CH8,9,10 -> VDIC
  41  *
  42  * In this pipeline, previous field F(n-1), current field F(n), and next
  43  * field F(n+1) are transferred to the VDIC via IDMAC channels 8,9,10.
  44  * These memory buffers can come from a video output or mem2mem device.
  45  * All motion modes are supported by this pipeline.
  46  *
  47  * The "direct" CSI->VDIC pipeline requires no DMA, but it can only be
  48  * used in high motion mode.
  49  */
  50
  51 struct vdic_priv;
  52
  53 struct vdic_pipeline_ops {
  54         int (*setup)(struct vdic_priv *priv);
  55         void (*start)(struct vdic_priv *priv);
  56         void (*stop)(struct vdic_priv *priv);
  57         void (*disable)(struct vdic_priv *priv);
  58 };
  59
  60 /*
  61  * Min/Max supported width and heights.
  62  */
  63 #define MIN_W       176
  64 #define MIN_H       144
  65 #define MAX_W_VDIC  968
  66 #define MAX_H_VDIC 2048
  67 #define W_ALIGN    4 /* multiple of 16 pixels */
  68 #define H_ALIGN    1 /* multiple of 2 lines */
  69 #define S_ALIGN    1 /* multiple of 2 */
  70
  71 struct vdic_priv {
  72         struct device        *dev;
  73         struct ipu_soc       *ipu;
  74         struct imx_media_dev *md;
  75         struct v4l2_subdev   sd;
  76         struct media_pad pad[VDIC_NUM_PADS];
  77         int ipu_id;
  78
  79         /* lock to protect all members below */
  80         struct mutex lock;
  81
  82         /* IPU units we require */
  83         struct ipu_vdi *vdi;
  84
  85         int active_input_pad;
  86
  87         struct ipuv3_channel *vdi_in_ch_p; /* F(n-1) transfer channel */
  88         struct ipuv3_channel *vdi_in_ch;   /* F(n) transfer channel */
  89         struct ipuv3_channel *vdi_in_ch_n; /* F(n+1) transfer channel */
  90
  91         /* pipeline operations */
  92         struct vdic_pipeline_ops *ops;
  93
  94         /* current and previous input buffers indirect path */
  95         struct imx_media_buffer *curr_in_buf;
  96         struct imx_media_buffer *prev_in_buf;
  97
  98         /*
  99          * translated field type, input line stride, and field size
 100          * for indirect path
 101          */
 102         u32 fieldtype;
 103         u32 in_stride;
 104         u32 field_size;
 105
 106         /* the source (a video device or subdev) */
 107         struct media_entity *src;
 108         /* the sink that will receive the progressive out buffers */
 109         struct v4l2_subdev *sink_sd;
 110
 111         struct v4l2_mbus_framefmt format_mbus[VDIC_NUM_PADS];
 112         const struct imx_media_pixfmt *cc[VDIC_NUM_PADS];
 113         struct v4l2_fract frame_interval[VDIC_NUM_PADS];
 114
 115         /* the video device at IDMAC input pad */
 116         struct imx_media_video_dev *vdev;
 117
 118         bool csi_direct;  /* using direct CSI->VDIC->IC pipeline */
 119
 120         /* motion select control */
 121         struct v4l2_ctrl_handler ctrl_hdlr;
 122         enum ipu_motion_sel motion;
 123
 124         int stream_count;
 125 };
 126
 127 static void vdic_put_ipu_resources(struct vdic_priv *priv)
 128 {
 129         if (priv->vdi_in_ch_p)
 130                 ipu_idmac_put(priv->vdi_in_ch_p);
 131         priv->vdi_in_ch_p = NULL;
 132
 133         if (priv->vdi_in_ch)
 134                 ipu_idmac_put(priv->vdi_in_ch);
 135         priv->vdi_in_ch = NULL;
 136
 137         if (priv->vdi_in_ch_n)
 138                 ipu_idmac_put(priv->vdi_in_ch_n);
 139         priv->vdi_in_ch_n = NULL;
 140
 141         if (!IS_ERR_OR_NULL(priv->vdi))
 142                 ipu_vdi_put(priv->vdi);
 143         priv->vdi = NULL;
 144 }
 145
 146 static int vdic_get_ipu_resources(struct vdic_priv *priv)
 147 {
 148         int ret, err_chan;
 149         struct ipuv3_channel *ch;
 150         struct ipu_vdi *vdi;
 151
 152         priv->ipu = priv->md->ipu[priv->ipu_id];
 153
 154         vdi = ipu_vdi_get(priv->ipu);
 155         if (IS_ERR(vdi)) {
 156                 v4l2_err(&priv->sd, "failed to get VDIC\n");
 157                 ret = PTR_ERR(vdi);
 158                 goto out;
 159         }
 160         priv->vdi = vdi;
 161
 162         if (!priv->csi_direct) {
 163                 ch = ipu_idmac_get(priv->ipu, IPUV3_CHANNEL_MEM_VDI_PREV);
 164                 if (IS_ERR(ch)) {
 165                         err_chan = IPUV3_CHANNEL_MEM_VDI_PREV;
 166                         ret = PTR_ERR(ch);
 167                         goto out_err_chan;
 168                 }
 169                 priv->vdi_in_ch_p = ch;
 170
 171                 ch = ipu_idmac_get(priv->ipu, IPUV3_CHANNEL_MEM_VDI_CUR);
 172                 if (IS_ERR(ch)) {
 173                         err_chan = IPUV3_CHANNEL_MEM_VDI_CUR;
 174                         ret = PTR_ERR(ch);
 175                         goto out_err_chan;
 176                 }
 177                 priv->vdi_in_ch = ch;
 178
 179                 ch = ipu_idmac_get(priv->ipu, IPUV3_CHANNEL_MEM_VDI_NEXT);
 180                 if (IS_ERR(priv->vdi_in_ch_n)) {
 181                         err_chan = IPUV3_CHANNEL_MEM_VDI_NEXT;
 182                         ret = PTR_ERR(ch);
 183                         goto out_err_chan;
 184                 }
 185                 priv->vdi_in_ch_n = ch;
 186         }
 187
 188         return 0;
 189
 190 out_err_chan:
 191         v4l2_err(&priv->sd, "could not get IDMAC channel %u\n", err_chan);
 192 out:
 193         vdic_put_ipu_resources(priv);
 194         return ret;
 195 }
 196
 197 /*
 198  * This function is currently unused, but will be called when the
 199  * output/mem2mem device at the IDMAC input pad sends us a new
 200  * buffer. It kicks off the IDMAC read channels to bring in the
 201  * buffer fields from memory and begin the conversions.
 202  */
 203 static void __maybe_unused prepare_vdi_in_buffers(struct vdic_priv *priv,
 204                                                   struct imx_media_buffer *curr)
 205 {
 206         dma_addr_t prev_phys, curr_phys, next_phys;
 207         struct imx_media_buffer *prev;
 208         struct vb2_buffer *curr_vb, *prev_vb;
 209         u32 fs = priv->field_size;
 210         u32 is = priv->in_stride;
 211
 212         /* current input buffer is now previous */
 213         priv->prev_in_buf = priv->curr_in_buf;
 214         priv->curr_in_buf = curr;
 215         prev = priv->prev_in_buf ? priv->prev_in_buf : curr;
 216
 217         prev_vb = &prev->vbuf.vb2_buf;
 218         curr_vb = &curr->vbuf.vb2_buf;
 219
 220         switch (priv->fieldtype) {
 221         case V4L2_FIELD_SEQ_TB:
 222                 prev_phys = vb2_dma_contig_plane_dma_addr(prev_vb, 0);
 223                 curr_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0) + fs;
 224                 next_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0);
 225                 break;
 226         case V4L2_FIELD_SEQ_BT:
 227                 prev_phys = vb2_dma_contig_plane_dma_addr(prev_vb, 0) + fs;
 228                 curr_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0);
 229                 next_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0) + fs;
 230                 break;
 231         case V4L2_FIELD_INTERLACED_BT:
 232                 prev_phys = vb2_dma_contig_plane_dma_addr(prev_vb, 0) + is;
 233                 curr_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0);
 234                 next_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0) + is;
 235                 break;
 236         default:
 237                 /* assume V4L2_FIELD_INTERLACED_TB */
 238                 prev_phys = vb2_dma_contig_plane_dma_addr(prev_vb, 0);
 239                 curr_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0) + is;
 240                 next_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0);
 241                 break;
 242         }
 243
 244         ipu_cpmem_set_buffer(priv->vdi_in_ch_p, 0, prev_phys);
 245         ipu_cpmem_set_buffer(priv->vdi_in_ch,   0, curr_phys);
 246         ipu_cpmem_set_buffer(priv->vdi_in_ch_n, 0, next_phys);
 247
 248         ipu_idmac_select_buffer(priv->vdi_in_ch_p, 0);
 249         ipu_idmac_select_buffer(priv->vdi_in_ch, 0);
 250         ipu_idmac_select_buffer(priv->vdi_in_ch_n, 0);
 251 }
 252
 253 static int setup_vdi_channel(struct vdic_priv *priv,
 254                              struct ipuv3_channel *channel,
 255                              dma_addr_t phys0, dma_addr_t phys1)
 256 {
 257         struct imx_media_video_dev *vdev = priv->vdev;
 258         unsigned int burst_size;
 259         struct ipu_image image;
 260         int ret;
 261
 262         ipu_cpmem_zero(channel);
 263
 264         memset(&image, 0, sizeof(image));
 265         image.pix = vdev->fmt.fmt.pix;
 266         /* one field to VDIC channels */
 267         image.pix.height /= 2;
 268         image.rect.width = image.pix.width;
 269         image.rect.height = image.pix.height;
 270         image.phys0 = phys0;
 271         image.phys1 = phys1;
 272
 273         ret = ipu_cpmem_set_image(channel, &image);
 274         if (ret)
 275                 return ret;
 276
 277         burst_size = (image.pix.width & 0xf) ? 8 : 16;
 278         ipu_cpmem_set_burstsize(channel, burst_size);
 279
 280         ipu_cpmem_set_axi_id(channel, 1);
 281
 282         ipu_idmac_set_double_buffer(channel, false);
 283
 284         return 0;
 285 }
 286
 287 static int vdic_setup_direct(struct vdic_priv *priv)
 288 {
 289         /* set VDIC to receive from CSI for direct path */
 290         ipu_fsu_link(priv->ipu, IPUV3_CHANNEL_CSI_DIRECT,
 291                      IPUV3_CHANNEL_CSI_VDI_PREV);
 292
 293         return 0;
 294 }
 295
 296 static void vdic_start_direct(struct vdic_priv *priv)
 297 {
 298 }
 299
 300 static void vdic_stop_direct(struct vdic_priv *priv)
 301 {
 302 }
 303
 304 static void vdic_disable_direct(struct vdic_priv *priv)
 305 {
 306         ipu_fsu_unlink(priv->ipu, IPUV3_CHANNEL_CSI_DIRECT,
 307                        IPUV3_CHANNEL_CSI_VDI_PREV);
 308 }
 309
 310 static int vdic_setup_indirect(struct vdic_priv *priv)
 311 {
 312         struct v4l2_mbus_framefmt *infmt;
 313         const struct imx_media_pixfmt *incc;
 314         int in_size, ret;
 315
 316         infmt = &priv->format_mbus[VDIC_SINK_PAD_IDMAC];
 317         incc = priv->cc[VDIC_SINK_PAD_IDMAC];
 318
 319         in_size = (infmt->width * incc->bpp * infmt->height) >> 3;
 320
 321         /* 1/2 full image size */
 322         priv->field_size = in_size / 2;
 323         priv->in_stride = incc->planar ?
 324                 infmt->width : (infmt->width * incc->bpp) >> 3;
 325
 326         priv->prev_in_buf = NULL;
 327         priv->curr_in_buf = NULL;
 328
 329         priv->fieldtype = infmt->field;
 330
 331         /* init the vdi-in channels */
 332         ret = setup_vdi_channel(priv, priv->vdi_in_ch_p, 0, 0);
 333         if (ret)
 334                 return ret;
 335         ret = setup_vdi_channel(priv, priv->vdi_in_ch, 0, 0);
 336         if (ret)
 337                 return ret;
 338         return setup_vdi_channel(priv, priv->vdi_in_ch_n, 0, 0);
 339 }
 340
 341 static void vdic_start_indirect(struct vdic_priv *priv)
 342 {
 343         /* enable the channels */
 344         ipu_idmac_enable_channel(priv->vdi_in_ch_p);
 345         ipu_idmac_enable_channel(priv->vdi_in_ch);
 346         ipu_idmac_enable_channel(priv->vdi_in_ch_n);
 347 }
 348
 349 static void vdic_stop_indirect(struct vdic_priv *priv)
 350 {
 351         /* disable channels */
 352         ipu_idmac_disable_channel(priv->vdi_in_ch_p);
 353         ipu_idmac_disable_channel(priv->vdi_in_ch);
 354         ipu_idmac_disable_channel(priv->vdi_in_ch_n);
 355 }
 356
 357 static void vdic_disable_indirect(struct vdic_priv *priv)
 358 {
 359 }
 360
 361 static struct vdic_pipeline_ops direct_ops = {
 362         .setup = vdic_setup_direct,
 363         .start = vdic_start_direct,
 364         .stop = vdic_stop_direct,
 365         .disable = vdic_disable_direct,
 366 };
 367
 368 static struct vdic_pipeline_ops indirect_ops = {
 369         .setup = vdic_setup_indirect,
 370         .start = vdic_start_indirect,
 371         .stop = vdic_stop_indirect,
 372         .disable = vdic_disable_indirect,
 373 };
 374
 375 static int vdic_start(struct vdic_priv *priv)
 376 {
 377         struct v4l2_mbus_framefmt *infmt;
 378         int ret;
 379
 380         infmt = &priv->format_mbus[priv->active_input_pad];
 381
 382         priv->ops = priv->csi_direct ? &direct_ops : &indirect_ops;
 383
 384         ret = vdic_get_ipu_resources(priv);
 385         if (ret)
 386                 return ret;
 387
 388         /*
 389          * init the VDIC.
 390          *
 391          * note we don't give infmt->code to ipu_vdi_setup(). The VDIC
 392          * only supports 4:2:2 or 4:2:0, and this subdev will only
 393          * negotiate 4:2:2 at its sink pads.
 394          */
 395         ipu_vdi_setup(priv->vdi, MEDIA_BUS_FMT_UYVY8_2X8,
 396                       infmt->width, infmt->height);
 397         ipu_vdi_set_field_order(priv->vdi, V4L2_STD_UNKNOWN, infmt->field);
 398         ipu_vdi_set_motion(priv->vdi, priv->motion);
 399
 400         ret = priv->ops->setup(priv);
 401         if (ret)
 402                 goto out_put_ipu;
 403
 404         ipu_vdi_enable(priv->vdi);
 405
 406         priv->ops->start(priv);
 407
 408         return 0;
 409
 410 out_put_ipu:
 411         vdic_put_ipu_resources(priv);
 412         return ret;
 413 }
 414
 415 static void vdic_stop(struct vdic_priv *priv)
 416 {
 417         priv->ops->stop(priv);
 418         ipu_vdi_disable(priv->vdi);
 419         priv->ops->disable(priv);
 420
 421         vdic_put_ipu_resources(priv);
 422 }
 423
 424 /*
 425  * V4L2 subdev operations.
 426  */
 427
 428 static int vdic_s_ctrl(struct v4l2_ctrl *ctrl)
 429 {
 430         struct vdic_priv *priv = container_of(ctrl->handler,
 431                                               struct vdic_priv, ctrl_hdlr);
 432         enum ipu_motion_sel motion;
 433         int ret = 0;
 434
 435         mutex_lock(&priv->lock);
 436
 437         switch (ctrl->id) {
 438         case V4L2_CID_DEINTERLACING_MODE:
 439                 motion = ctrl->val;
 440                 if (motion != priv->motion) {
 441                         /* can't change motion control mid-streaming */
 442                         if (priv->stream_count > 0) {
 443                                 ret = -EBUSY;
 444                                 goto out;
 445                         }
 446                         priv->motion = motion;
 447                 }
 448                 break;
 449         default:
 450                 v4l2_err(&priv->sd, "Invalid control\n");
 451                 ret = -EINVAL;
 452         }
 453
 454 out:
 455         mutex_unlock(&priv->lock);
 456         return ret;
 457 }
 458
 459 static const struct v4l2_ctrl_ops vdic_ctrl_ops = {
 460         .s_ctrl = vdic_s_ctrl,
 461 };
 462
 463 static const char * const vdic_ctrl_motion_menu[] = {
 464         "No Motion Compensation",
 465         "Low Motion",
 466         "Medium Motion",
 467         "High Motion",
 468 };
 469
 470 static int vdic_init_controls(struct vdic_priv *priv)
 471 {
 472         struct v4l2_ctrl_handler *hdlr = &priv->ctrl_hdlr;
 473         int ret;
 474
 475         v4l2_ctrl_handler_init(hdlr, 1);
 476
 477         v4l2_ctrl_new_std_menu_items(hdlr, &vdic_ctrl_ops,
 478                                      V4L2_CID_DEINTERLACING_MODE,
 479                                      HIGH_MOTION, 0, HIGH_MOTION,
 480                                      vdic_ctrl_motion_menu);
 481
 482         priv->sd.ctrl_handler = hdlr;
 483
 484         if (hdlr->error) {
 485                 ret = hdlr->error;
 486                 goto out_free;
 487         }
 488
 489         v4l2_ctrl_handler_setup(hdlr);
 490         return 0;
 491
 492 out_free:
 493         v4l2_ctrl_handler_free(hdlr);
 494         return ret;
 495 }
 496
 497 static int vdic_s_stream(struct v4l2_subdev *sd, int enable)
 498 {
 499         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 500         struct v4l2_subdev *src_sd = NULL;
 501         int ret = 0;
 502
 503         mutex_lock(&priv->lock);
 504
 505         if (!priv->src || !priv->sink_sd) {
 506                 ret = -EPIPE;
 507                 goto out;
 508         }
 509
 510         if (priv->csi_direct)
 511                 src_sd = media_entity_to_v4l2_subdev(priv->src);
 512
 513         /*
 514          * enable/disable streaming only if stream_count is
 515          * going from 0 to 1 / 1 to 0.
 516          */
 517         if (priv->stream_count != !enable)
 518                 goto update_count;
 519
 520         dev_dbg(priv->dev, "stream %s\n", enable ? "ON" : "OFF");
 521
 522         if (enable)
 523                 ret = vdic_start(priv);
 524         else
 525                 vdic_stop(priv);
 526         if (ret)
 527                 goto out;
 528
 529         if (src_sd) {
 530                 /* start/stop upstream */
 531                 ret = v4l2_subdev_call(src_sd, video, s_stream, enable);
 532                 ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0;
 533                 if (ret) {
 534                         if (enable)
 535                                 vdic_stop(priv);
 536                         goto out;
 537                 }
 538         }
 539
 540 update_count:
 541         priv->stream_count += enable ? 1 : -1;
 542         if (priv->stream_count < 0)
 543                 priv->stream_count = 0;
 544 out:
 545         mutex_unlock(&priv->lock);
 546         return ret;
 547 }
 548
 549 static struct v4l2_mbus_framefmt *
 550 __vdic_get_fmt(struct vdic_priv *priv, struct v4l2_subdev_pad_config *cfg,
 551                unsigned int pad, enum v4l2_subdev_format_whence which)
 552 {
 553         if (which == V4L2_SUBDEV_FORMAT_TRY)
 554                 return v4l2_subdev_get_try_format(&priv->sd, cfg, pad);
 555         else
 556                 return &priv->format_mbus[pad];
 557 }
 558
 559 static int vdic_enum_mbus_code(struct v4l2_subdev *sd,
 560                                struct v4l2_subdev_pad_config *cfg,
 561                                struct v4l2_subdev_mbus_code_enum *code)
 562 {
 563         if (code->pad >= VDIC_NUM_PADS)
 564                 return -EINVAL;
 565
 566         return imx_media_enum_ipu_format(&code->code, code->index, CS_SEL_YUV);
 567 }
 568
 569 static int vdic_get_fmt(struct v4l2_subdev *sd,
 570                         struct v4l2_subdev_pad_config *cfg,
 571                         struct v4l2_subdev_format *sdformat)
 572 {
 573         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 574         struct v4l2_mbus_framefmt *fmt;
 575         int ret = 0;
 576
 577         if (sdformat->pad >= VDIC_NUM_PADS)
 578                 return -EINVAL;
 579
 580         mutex_lock(&priv->lock);
 581
 582         fmt = __vdic_get_fmt(priv, cfg, sdformat->pad, sdformat->which);
 583         if (!fmt) {
 584                 ret = -EINVAL;
 585                 goto out;
 586         }
 587
 588         sdformat->format = *fmt;
 589 out:
 590         mutex_unlock(&priv->lock);
 591         return ret;
 592 }
 593
 594 static void vdic_try_fmt(struct vdic_priv *priv,
 595                          struct v4l2_subdev_pad_config *cfg,
 596                          struct v4l2_subdev_format *sdformat,
 597                          const struct imx_media_pixfmt **cc)
 598 {
 599         struct v4l2_mbus_framefmt *infmt;
 600
 601         *cc = imx_media_find_ipu_format(sdformat->format.code, CS_SEL_YUV);
 602         if (!*cc) {
 603                 u32 code;
 604
 605                 imx_media_enum_ipu_format(&code, 0, CS_SEL_YUV);
 606                 *cc = imx_media_find_ipu_format(code, CS_SEL_YUV);
 607                 sdformat->format.code = (*cc)->codes[0];
 608         }
 609
 610         infmt = __vdic_get_fmt(priv, cfg, priv->active_input_pad,
 611                                sdformat->which);
 612
 613         switch (sdformat->pad) {
 614         case VDIC_SRC_PAD_DIRECT:
 615                 sdformat->format = *infmt;
 616                 /* output is always progressive! */
 617                 sdformat->format.field = V4L2_FIELD_NONE;
 618                 break;
 619         case VDIC_SINK_PAD_DIRECT:
 620         case VDIC_SINK_PAD_IDMAC:
 621                 v4l_bound_align_image(&sdformat->format.width,
 622                                       MIN_W, MAX_W_VDIC, W_ALIGN,
 623                                       &sdformat->format.height,
 624                                       MIN_H, MAX_H_VDIC, H_ALIGN, S_ALIGN);
 625
 626                 imx_media_fill_default_mbus_fields(&sdformat->format, infmt,
 627                                                    true);
 628
 629                 /* input must be interlaced! Choose SEQ_TB if not */
 630                 if (!V4L2_FIELD_HAS_BOTH(sdformat->format.field))
 631                         sdformat->format.field = V4L2_FIELD_SEQ_TB;
 632                 break;
 633         }
 634 }
 635
 636 static int vdic_set_fmt(struct v4l2_subdev *sd,
 637                         struct v4l2_subdev_pad_config *cfg,
 638                         struct v4l2_subdev_format *sdformat)
 639 {
 640         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 641         const struct imx_media_pixfmt *cc;
 642         struct v4l2_mbus_framefmt *fmt;
 643         int ret = 0;
 644
 645         if (sdformat->pad >= VDIC_NUM_PADS)
 646                 return -EINVAL;
 647
 648         mutex_lock(&priv->lock);
 649
 650         if (priv->stream_count > 0) {
 651                 ret = -EBUSY;
 652                 goto out;
 653         }
 654
 655         vdic_try_fmt(priv, cfg, sdformat, &cc);
 656
 657         fmt = __vdic_get_fmt(priv, cfg, sdformat->pad, sdformat->which);
 658         *fmt = sdformat->format;
 659
 660         /* propagate format to source pad */
 661         if (sdformat->pad == VDIC_SINK_PAD_DIRECT ||
 662             sdformat->pad == VDIC_SINK_PAD_IDMAC) {
 663                 const struct imx_media_pixfmt *outcc;
 664                 struct v4l2_mbus_framefmt *outfmt;
 665                 struct v4l2_subdev_format format;
 666
 667                 format.pad = VDIC_SRC_PAD_DIRECT;
 668                 format.which = sdformat->which;
 669                 format.format = sdformat->format;
 670                 vdic_try_fmt(priv, cfg, &format, &outcc);
 671
 672                 outfmt = __vdic_get_fmt(priv, cfg, VDIC_SRC_PAD_DIRECT,
 673                                         sdformat->which);
 674                 *outfmt = format.format;
 675                 if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
 676                         priv->cc[VDIC_SRC_PAD_DIRECT] = outcc;
 677         }
 678
 679         if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
 680                 priv->cc[sdformat->pad] = cc;
 681 out:
 682         mutex_unlock(&priv->lock);
 683         return ret;
 684 }
 685
 686 static int vdic_link_setup(struct media_entity *entity,
 687                             const struct media_pad *local,
 688                             const struct media_pad *remote, u32 flags)
 689 {
 690         struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 691         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 692         struct v4l2_subdev *remote_sd;
 693         int ret = 0;
 694
 695         dev_dbg(priv->dev, "link setup %s -> %s", remote->entity->name,
 696                 local->entity->name);
 697
 698         mutex_lock(&priv->lock);
 699
 700         if (local->flags & MEDIA_PAD_FL_SOURCE) {
 701                 if (!is_media_entity_v4l2_subdev(remote->entity)) {
 702                         ret = -EINVAL;
 703                         goto out;
 704                 }
 705
 706                 remote_sd = media_entity_to_v4l2_subdev(remote->entity);
 707
 708                 if (flags & MEDIA_LNK_FL_ENABLED) {
 709                         if (priv->sink_sd) {
 710                                 ret = -EBUSY;
 711                                 goto out;
 712                         }
 713                         priv->sink_sd = remote_sd;
 714                 } else {
 715                         priv->sink_sd = NULL;
 716                 }
 717
 718                 goto out;
 719         }
 720
 721         /* this is a sink pad */
 722
 723         if (flags & MEDIA_LNK_FL_ENABLED) {
 724                 if (priv->src) {
 725                         ret = -EBUSY;
 726                         goto out;
 727                 }
 728         } else {
 729                 priv->src = NULL;
 730                 goto out;
 731         }
 732
 733         if (local->index == VDIC_SINK_PAD_IDMAC) {
 734                 struct imx_media_video_dev *vdev = priv->vdev;
 735
 736                 if (!is_media_entity_v4l2_video_device(remote->entity)) {
 737                         ret = -EINVAL;
 738                         goto out;
 739                 }
 740                 if (!vdev) {
 741                         ret = -ENODEV;
 742                         goto out;
 743                 }
 744
 745                 priv->csi_direct = false;
 746         } else {
 747                 if (!is_media_entity_v4l2_subdev(remote->entity)) {
 748                         ret = -EINVAL;
 749                         goto out;
 750                 }
 751
 752                 remote_sd = media_entity_to_v4l2_subdev(remote->entity);
 753
 754                 /* direct pad must connect to a CSI */
 755                 if (!(remote_sd->grp_id & IMX_MEDIA_GRP_ID_CSI) ||
 756                     remote->index != CSI_SRC_PAD_DIRECT) {
 757                         ret = -EINVAL;
 758                         goto out;
 759                 }
 760
 761                 priv->csi_direct = true;
 762         }
 763
 764         priv->src = remote->entity;
 765         /* record which input pad is now active */
 766         priv->active_input_pad = local->index;
 767 out:
 768         mutex_unlock(&priv->lock);
 769         return ret;
 770 }
 771
 772 static int vdic_link_validate(struct v4l2_subdev *sd,
 773                               struct media_link *link,
 774                               struct v4l2_subdev_format *source_fmt,
 775                               struct v4l2_subdev_format *sink_fmt)
 776 {
 777         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 778         int ret;
 779
 780         ret = v4l2_subdev_link_validate_default(sd, link,
 781                                                 source_fmt, sink_fmt);
 782         if (ret)
 783                 return ret;
 784
 785         mutex_lock(&priv->lock);
 786
 787         if (priv->csi_direct && priv->motion != HIGH_MOTION) {
 788                 v4l2_err(&priv->sd,
 789                          "direct CSI pipeline requires high motion\n");
 790                 ret = -EINVAL;
 791         }
 792
 793         mutex_unlock(&priv->lock);
 794         return ret;
 795 }
 796
 797 static int vdic_g_frame_interval(struct v4l2_subdev *sd,
 798                                 struct v4l2_subdev_frame_interval *fi)
 799 {
 800         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 801
 802         if (fi->pad >= VDIC_NUM_PADS)
 803                 return -EINVAL;
 804
 805         mutex_lock(&priv->lock);
 806
 807         fi->interval = priv->frame_interval[fi->pad];
 808
 809         mutex_unlock(&priv->lock);
 810
 811         return 0;
 812 }
 813
 814 static int vdic_s_frame_interval(struct v4l2_subdev *sd,
 815                                 struct v4l2_subdev_frame_interval *fi)
 816 {
 817         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 818         struct v4l2_fract *input_fi, *output_fi;
 819         int ret = 0;
 820
 821         mutex_lock(&priv->lock);
 822
 823         input_fi = &priv->frame_interval[priv->active_input_pad];
 824         output_fi = &priv->frame_interval[VDIC_SRC_PAD_DIRECT];
 825
 826         switch (fi->pad) {
 827         case VDIC_SINK_PAD_DIRECT:
 828         case VDIC_SINK_PAD_IDMAC:
 829                 /* No limits on input frame interval */
 830                 /* Reset output interval */
 831                 *output_fi = fi->interval;
 832                 if (priv->csi_direct)
 833                         output_fi->denominator *= 2;
 834                 break;
 835         case VDIC_SRC_PAD_DIRECT:
 836                 /*
 837                  * frame rate at output pad is double input
 838                  * rate when using direct CSI->VDIC pipeline.
 839                  *
 840                  * TODO: implement VDIC frame skipping
 841                  */
 842                 fi->interval = *input_fi;
 843                 if (priv->csi_direct)
 844                         fi->interval.denominator *= 2;
 845                 break;
 846         default:
 847                 ret = -EINVAL;
 848                 goto out;
 849         }
 850
 851         priv->frame_interval[fi->pad] = fi->interval;
 852 out:
 853         mutex_unlock(&priv->lock);
 854         return ret;
 855 }
 856
 857 /*
 858  * retrieve our pads parsed from the OF graph by the media device
 859  */
 860 static int vdic_registered(struct v4l2_subdev *sd)
 861 {
 862         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 863         int i, ret;
 864         u32 code;
 865
 866         /* get media device */
 867         priv->md = dev_get_drvdata(sd->v4l2_dev->dev);
 868
 869         for (i = 0; i < VDIC_NUM_PADS; i++) {
 870                 priv->pad[i].flags = (i == VDIC_SRC_PAD_DIRECT) ?
 871                         MEDIA_PAD_FL_SOURCE : MEDIA_PAD_FL_SINK;
 872
 873                 code = 0;
 874                 if (i != VDIC_SINK_PAD_IDMAC)
 875                         imx_media_enum_ipu_format(&code, 0, CS_SEL_YUV);
 876
 877                 /* set a default mbus format  */
 878                 ret = imx_media_init_mbus_fmt(&priv->format_mbus[i],
 879                                               640, 480, code, V4L2_FIELD_NONE,
 880                                               &priv->cc[i]);
 881                 if (ret)
 882                         return ret;
 883
 884                 /* init default frame interval */
 885                 priv->frame_interval[i].numerator = 1;
 886                 priv->frame_interval[i].denominator = 30;
 887                 if (i == VDIC_SRC_PAD_DIRECT)
 888                         priv->frame_interval[i].denominator *= 2;
 889         }
 890
 891         priv->active_input_pad = VDIC_SINK_PAD_DIRECT;
 892
 893         ret = vdic_init_controls(priv);
 894         if (ret)
 895                 return ret;
 896
 897         ret = media_entity_pads_init(&sd->entity, VDIC_NUM_PADS, priv->pad);
 898         if (ret)
 899                 v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
 900
 901         return ret;
 902 }
 903
 904 static void vdic_unregistered(struct v4l2_subdev *sd)
 905 {
 906         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 907
 908         v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
 909 }
 910
 911 static const struct v4l2_subdev_pad_ops vdic_pad_ops = {
 912         .enum_mbus_code = vdic_enum_mbus_code,
 913         .get_fmt = vdic_get_fmt,
 914         .set_fmt = vdic_set_fmt,
 915         .link_validate = vdic_link_validate,
 916 };
 917
 918 static const struct v4l2_subdev_video_ops vdic_video_ops = {
 919         .g_frame_interval = vdic_g_frame_interval,
 920         .s_frame_interval = vdic_s_frame_interval,
 921         .s_stream = vdic_s_stream,
 922 };
 923
 924 static const struct media_entity_operations vdic_entity_ops = {
 925         .link_setup = vdic_link_setup,
 926         .link_validate = v4l2_subdev_link_validate,
 927 };
 928
 929 static const struct v4l2_subdev_ops vdic_subdev_ops = {
 930         .video = &vdic_video_ops,
 931         .pad = &vdic_pad_ops,
 932 };
 933
 934 static const struct v4l2_subdev_internal_ops vdic_internal_ops = {
 935         .registered = vdic_registered,
 936         .unregistered = vdic_unregistered,
 937 };
 938
 939 static int imx_vdic_probe(struct platform_device *pdev)
 940 {
 941         struct imx_media_internal_sd_platformdata *pdata;
 942         struct vdic_priv *priv;
 943         int ret;
 944
 945         priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
 946         if (!priv)
 947                 return -ENOMEM;
 948
 949         platform_set_drvdata(pdev, &priv->sd);
 950         priv->dev = &pdev->dev;
 951
 952         pdata = priv->dev->platform_data;
 953         priv->ipu_id = pdata->ipu_id;
 954
 955         v4l2_subdev_init(&priv->sd, &vdic_subdev_ops);
 956         v4l2_set_subdevdata(&priv->sd, priv);
 957         priv->sd.internal_ops = &vdic_internal_ops;
 958         priv->sd.entity.ops = &vdic_entity_ops;
 959         priv->sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
 960         priv->sd.dev = &pdev->dev;
 961         priv->sd.owner = THIS_MODULE;
 962         priv->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
 963         /* get our group id */
 964         priv->sd.grp_id = pdata->grp_id;
 965         strncpy(priv->sd.name, pdata->sd_name, sizeof(priv->sd.name));
 966
 967         mutex_init(&priv->lock);
 968
 969         ret = v4l2_async_register_subdev(&priv->sd);
 970         if (ret)
 971                 goto free;
 972
 973         return 0;
 974 free:
 975         mutex_destroy(&priv->lock);
 976         return ret;
 977 }
 978
 979 static int imx_vdic_remove(struct platform_device *pdev)
 980 {
 981         struct v4l2_subdev *sd = platform_get_drvdata(pdev);
 982         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 983
 984         v4l2_info(sd, "Removing\n");
 985
 986         v4l2_async_unregister_subdev(sd);
 987         mutex_destroy(&priv->lock);
 988         media_entity_cleanup(&sd->entity);
 989
 990         return 0;
 991 }
 992
 993 static const struct platform_device_id imx_vdic_ids[] = {
 994         { .name = "imx-ipuv3-vdic" },
 995         { },
 996 };
 997 MODULE_DEVICE_TABLE(platform, imx_vdic_ids);
 998
 999 static struct platform_driver imx_vdic_driver = {
1000         .probe = imx_vdic_probe,
1001         .remove = imx_vdic_remove,
1002         .id_table = imx_vdic_ids,
1003         .driver = {
1004                 .name = "imx-ipuv3-vdic",
1005         },
1006 };
1007 module_platform_driver(imx_vdic_driver);
1008
1009 MODULE_DESCRIPTION("i.MX VDIC subdev driver");
1010 MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
1011 MODULE_LICENSE("GPL");
1012 MODULE_ALIAS("platform:imx-ipuv3-vdic");