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(ch)) {
 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         case V4L2_FIELD_SEQ_BT:
 223                 prev_phys = vb2_dma_contig_plane_dma_addr(prev_vb, 0) + fs;
 224                 curr_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0);
 225                 next_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0) + fs;
 226                 break;
 227         case V4L2_FIELD_INTERLACED_TB:
 228         case V4L2_FIELD_INTERLACED_BT:
 229         case V4L2_FIELD_INTERLACED:
 230                 prev_phys = vb2_dma_contig_plane_dma_addr(prev_vb, 0) + is;
 231                 curr_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0);
 232                 next_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0) + is;
 233                 break;
 234         }
 235
 236         ipu_cpmem_set_buffer(priv->vdi_in_ch_p, 0, prev_phys);
 237         ipu_cpmem_set_buffer(priv->vdi_in_ch,   0, curr_phys);
 238         ipu_cpmem_set_buffer(priv->vdi_in_ch_n, 0, next_phys);
 239
 240         ipu_idmac_select_buffer(priv->vdi_in_ch_p, 0);
 241         ipu_idmac_select_buffer(priv->vdi_in_ch, 0);
 242         ipu_idmac_select_buffer(priv->vdi_in_ch_n, 0);
 243 }
 244
 245 static int setup_vdi_channel(struct vdic_priv *priv,
 246                              struct ipuv3_channel *channel,
 247                              dma_addr_t phys0, dma_addr_t phys1)
 248 {
 249         struct imx_media_video_dev *vdev = priv->vdev;
 250         unsigned int burst_size;
 251         struct ipu_image image;
 252         int ret;
 253
 254         ipu_cpmem_zero(channel);
 255
 256         memset(&image, 0, sizeof(image));
 257         image.pix = vdev->fmt.fmt.pix;
 258         image.rect = vdev->compose;
 259         /* one field to VDIC channels */
 260         image.pix.height /= 2;
 261         image.rect.height /= 2;
 262         image.phys0 = phys0;
 263         image.phys1 = phys1;
 264
 265         ret = ipu_cpmem_set_image(channel, &image);
 266         if (ret)
 267                 return ret;
 268
 269         burst_size = (image.pix.width & 0xf) ? 8 : 16;
 270         ipu_cpmem_set_burstsize(channel, burst_size);
 271
 272         ipu_cpmem_set_axi_id(channel, 1);
 273
 274         ipu_idmac_set_double_buffer(channel, false);
 275
 276         return 0;
 277 }
 278
 279 static int vdic_setup_direct(struct vdic_priv *priv)
 280 {
 281         /* set VDIC to receive from CSI for direct path */
 282         ipu_fsu_link(priv->ipu, IPUV3_CHANNEL_CSI_DIRECT,
 283                      IPUV3_CHANNEL_CSI_VDI_PREV);
 284
 285         return 0;
 286 }
 287
 288 static void vdic_start_direct(struct vdic_priv *priv)
 289 {
 290 }
 291
 292 static void vdic_stop_direct(struct vdic_priv *priv)
 293 {
 294 }
 295
 296 static void vdic_disable_direct(struct vdic_priv *priv)
 297 {
 298         ipu_fsu_unlink(priv->ipu, IPUV3_CHANNEL_CSI_DIRECT,
 299                        IPUV3_CHANNEL_CSI_VDI_PREV);
 300 }
 301
 302 static int vdic_setup_indirect(struct vdic_priv *priv)
 303 {
 304         struct v4l2_mbus_framefmt *infmt;
 305         const struct imx_media_pixfmt *incc;
 306         int in_size, ret;
 307
 308         infmt = &priv->format_mbus[VDIC_SINK_PAD_IDMAC];
 309         incc = priv->cc[VDIC_SINK_PAD_IDMAC];
 310
 311         in_size = (infmt->width * incc->bpp * infmt->height) >> 3;
 312
 313         /* 1/2 full image size */
 314         priv->field_size = in_size / 2;
 315         priv->in_stride = incc->planar ?
 316                 infmt->width : (infmt->width * incc->bpp) >> 3;
 317
 318         priv->prev_in_buf = NULL;
 319         priv->curr_in_buf = NULL;
 320
 321         priv->fieldtype = infmt->field;
 322
 323         /* init the vdi-in channels */
 324         ret = setup_vdi_channel(priv, priv->vdi_in_ch_p, 0, 0);
 325         if (ret)
 326                 return ret;
 327         ret = setup_vdi_channel(priv, priv->vdi_in_ch, 0, 0);
 328         if (ret)
 329                 return ret;
 330         return setup_vdi_channel(priv, priv->vdi_in_ch_n, 0, 0);
 331 }
 332
 333 static void vdic_start_indirect(struct vdic_priv *priv)
 334 {
 335         /* enable the channels */
 336         ipu_idmac_enable_channel(priv->vdi_in_ch_p);
 337         ipu_idmac_enable_channel(priv->vdi_in_ch);
 338         ipu_idmac_enable_channel(priv->vdi_in_ch_n);
 339 }
 340
 341 static void vdic_stop_indirect(struct vdic_priv *priv)
 342 {
 343         /* disable channels */
 344         ipu_idmac_disable_channel(priv->vdi_in_ch_p);
 345         ipu_idmac_disable_channel(priv->vdi_in_ch);
 346         ipu_idmac_disable_channel(priv->vdi_in_ch_n);
 347 }
 348
 349 static void vdic_disable_indirect(struct vdic_priv *priv)
 350 {
 351 }
 352
 353 static struct vdic_pipeline_ops direct_ops = {
 354         .setup = vdic_setup_direct,
 355         .start = vdic_start_direct,
 356         .stop = vdic_stop_direct,
 357         .disable = vdic_disable_direct,
 358 };
 359
 360 static struct vdic_pipeline_ops indirect_ops = {
 361         .setup = vdic_setup_indirect,
 362         .start = vdic_start_indirect,
 363         .stop = vdic_stop_indirect,
 364         .disable = vdic_disable_indirect,
 365 };
 366
 367 static int vdic_start(struct vdic_priv *priv)
 368 {
 369         struct v4l2_mbus_framefmt *infmt;
 370         int ret;
 371
 372         infmt = &priv->format_mbus[priv->active_input_pad];
 373
 374         priv->ops = priv->csi_direct ? &direct_ops : &indirect_ops;
 375
 376         ret = vdic_get_ipu_resources(priv);
 377         if (ret)
 378                 return ret;
 379
 380         /*
 381          * init the VDIC.
 382          *
 383          * note we don't give infmt->code to ipu_vdi_setup(). The VDIC
 384          * only supports 4:2:2 or 4:2:0, and this subdev will only
 385          * negotiate 4:2:2 at its sink pads.
 386          */
 387         ipu_vdi_setup(priv->vdi, MEDIA_BUS_FMT_UYVY8_2X8,
 388                       infmt->width, infmt->height);
 389         ipu_vdi_set_field_order(priv->vdi, V4L2_STD_UNKNOWN, infmt->field);
 390         ipu_vdi_set_motion(priv->vdi, priv->motion);
 391
 392         ret = priv->ops->setup(priv);
 393         if (ret)
 394                 goto out_put_ipu;
 395
 396         ipu_vdi_enable(priv->vdi);
 397
 398         priv->ops->start(priv);
 399
 400         return 0;
 401
 402 out_put_ipu:
 403         vdic_put_ipu_resources(priv);
 404         return ret;
 405 }
 406
 407 static void vdic_stop(struct vdic_priv *priv)
 408 {
 409         priv->ops->stop(priv);
 410         ipu_vdi_disable(priv->vdi);
 411         priv->ops->disable(priv);
 412
 413         vdic_put_ipu_resources(priv);
 414 }
 415
 416 /*
 417  * V4L2 subdev operations.
 418  */
 419
 420 static int vdic_s_ctrl(struct v4l2_ctrl *ctrl)
 421 {
 422         struct vdic_priv *priv = container_of(ctrl->handler,
 423                                               struct vdic_priv, ctrl_hdlr);
 424         enum ipu_motion_sel motion;
 425         int ret = 0;
 426
 427         mutex_lock(&priv->lock);
 428
 429         switch (ctrl->id) {
 430         case V4L2_CID_DEINTERLACING_MODE:
 431                 motion = ctrl->val;
 432                 if (motion != priv->motion) {
 433                         /* can't change motion control mid-streaming */
 434                         if (priv->stream_count > 0) {
 435                                 ret = -EBUSY;
 436                                 goto out;
 437                         }
 438                         priv->motion = motion;
 439                 }
 440                 break;
 441         default:
 442                 v4l2_err(&priv->sd, "Invalid control\n");
 443                 ret = -EINVAL;
 444         }
 445
 446 out:
 447         mutex_unlock(&priv->lock);
 448         return ret;
 449 }
 450
 451 static const struct v4l2_ctrl_ops vdic_ctrl_ops = {
 452         .s_ctrl = vdic_s_ctrl,
 453 };
 454
 455 static const char * const vdic_ctrl_motion_menu[] = {
 456         "No Motion Compensation",
 457         "Low Motion",
 458         "Medium Motion",
 459         "High Motion",
 460 };
 461
 462 static int vdic_init_controls(struct vdic_priv *priv)
 463 {
 464         struct v4l2_ctrl_handler *hdlr = &priv->ctrl_hdlr;
 465         int ret;
 466
 467         v4l2_ctrl_handler_init(hdlr, 1);
 468
 469         v4l2_ctrl_new_std_menu_items(hdlr, &vdic_ctrl_ops,
 470                                      V4L2_CID_DEINTERLACING_MODE,
 471                                      HIGH_MOTION, 0, HIGH_MOTION,
 472                                      vdic_ctrl_motion_menu);
 473
 474         priv->sd.ctrl_handler = hdlr;
 475
 476         if (hdlr->error) {
 477                 ret = hdlr->error;
 478                 goto out_free;
 479         }
 480
 481         v4l2_ctrl_handler_setup(hdlr);
 482         return 0;
 483
 484 out_free:
 485         v4l2_ctrl_handler_free(hdlr);
 486         return ret;
 487 }
 488
 489 static int vdic_s_stream(struct v4l2_subdev *sd, int enable)
 490 {
 491         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 492         struct v4l2_subdev *src_sd = NULL;
 493         int ret = 0;
 494
 495         mutex_lock(&priv->lock);
 496
 497         if (!priv->src || !priv->sink_sd) {
 498                 ret = -EPIPE;
 499                 goto out;
 500         }
 501
 502         if (priv->csi_direct)
 503                 src_sd = media_entity_to_v4l2_subdev(priv->src);
 504
 505         /*
 506          * enable/disable streaming only if stream_count is
 507          * going from 0 to 1 / 1 to 0.
 508          */
 509         if (priv->stream_count != !enable)
 510                 goto update_count;
 511
 512         dev_dbg(priv->dev, "stream %s\n", enable ? "ON" : "OFF");
 513
 514         if (enable)
 515                 ret = vdic_start(priv);
 516         else
 517                 vdic_stop(priv);
 518         if (ret)
 519                 goto out;
 520
 521         if (src_sd) {
 522                 /* start/stop upstream */
 523                 ret = v4l2_subdev_call(src_sd, video, s_stream, enable);
 524                 ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0;
 525                 if (ret) {
 526                         if (enable)
 527                                 vdic_stop(priv);
 528                         goto out;
 529                 }
 530         }
 531
 532 update_count:
 533         priv->stream_count += enable ? 1 : -1;
 534         if (priv->stream_count < 0)
 535                 priv->stream_count = 0;
 536 out:
 537         mutex_unlock(&priv->lock);
 538         return ret;
 539 }
 540
 541 static struct v4l2_mbus_framefmt *
 542 __vdic_get_fmt(struct vdic_priv *priv, struct v4l2_subdev_pad_config *cfg,
 543                unsigned int pad, enum v4l2_subdev_format_whence which)
 544 {
 545         if (which == V4L2_SUBDEV_FORMAT_TRY)
 546                 return v4l2_subdev_get_try_format(&priv->sd, cfg, pad);
 547         else
 548                 return &priv->format_mbus[pad];
 549 }
 550
 551 static int vdic_enum_mbus_code(struct v4l2_subdev *sd,
 552                                struct v4l2_subdev_pad_config *cfg,
 553                                struct v4l2_subdev_mbus_code_enum *code)
 554 {
 555         if (code->pad >= VDIC_NUM_PADS)
 556                 return -EINVAL;
 557
 558         return imx_media_enum_ipu_format(&code->code, code->index, CS_SEL_YUV);
 559 }
 560
 561 static int vdic_get_fmt(struct v4l2_subdev *sd,
 562                         struct v4l2_subdev_pad_config *cfg,
 563                         struct v4l2_subdev_format *sdformat)
 564 {
 565         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 566         struct v4l2_mbus_framefmt *fmt;
 567         int ret = 0;
 568
 569         if (sdformat->pad >= VDIC_NUM_PADS)
 570                 return -EINVAL;
 571
 572         mutex_lock(&priv->lock);
 573
 574         fmt = __vdic_get_fmt(priv, cfg, sdformat->pad, sdformat->which);
 575         if (!fmt) {
 576                 ret = -EINVAL;
 577                 goto out;
 578         }
 579
 580         sdformat->format = *fmt;
 581 out:
 582         mutex_unlock(&priv->lock);
 583         return ret;
 584 }
 585
 586 static void vdic_try_fmt(struct vdic_priv *priv,
 587                          struct v4l2_subdev_pad_config *cfg,
 588                          struct v4l2_subdev_format *sdformat,
 589                          const struct imx_media_pixfmt **cc)
 590 {
 591         struct v4l2_mbus_framefmt *infmt;
 592
 593         *cc = imx_media_find_ipu_format(sdformat->format.code, CS_SEL_YUV);
 594         if (!*cc) {
 595                 u32 code;
 596
 597                 imx_media_enum_ipu_format(&code, 0, CS_SEL_YUV);
 598                 *cc = imx_media_find_ipu_format(code, CS_SEL_YUV);
 599                 sdformat->format.code = (*cc)->codes[0];
 600         }
 601
 602         infmt = __vdic_get_fmt(priv, cfg, priv->active_input_pad,
 603                                sdformat->which);
 604
 605         switch (sdformat->pad) {
 606         case VDIC_SRC_PAD_DIRECT:
 607                 sdformat->format = *infmt;
 608                 /* output is always progressive! */
 609                 sdformat->format.field = V4L2_FIELD_NONE;
 610                 break;
 611         case VDIC_SINK_PAD_DIRECT:
 612         case VDIC_SINK_PAD_IDMAC:
 613                 v4l_bound_align_image(&sdformat->format.width,
 614                                       MIN_W, MAX_W_VDIC, W_ALIGN,
 615                                       &sdformat->format.height,
 616                                       MIN_H, MAX_H_VDIC, H_ALIGN, S_ALIGN);
 617
 618                 imx_media_fill_default_mbus_fields(&sdformat->format, infmt,
 619                                                    true);
 620
 621                 /* input must be interlaced! Choose SEQ_TB if not */
 622                 if (!V4L2_FIELD_HAS_BOTH(sdformat->format.field))
 623                         sdformat->format.field = V4L2_FIELD_SEQ_TB;
 624                 break;
 625         }
 626 }
 627
 628 static int vdic_set_fmt(struct v4l2_subdev *sd,
 629                         struct v4l2_subdev_pad_config *cfg,
 630                         struct v4l2_subdev_format *sdformat)
 631 {
 632         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 633         const struct imx_media_pixfmt *cc;
 634         struct v4l2_mbus_framefmt *fmt;
 635         int ret = 0;
 636
 637         if (sdformat->pad >= VDIC_NUM_PADS)
 638                 return -EINVAL;
 639
 640         mutex_lock(&priv->lock);
 641
 642         if (priv->stream_count > 0) {
 643                 ret = -EBUSY;
 644                 goto out;
 645         }
 646
 647         vdic_try_fmt(priv, cfg, sdformat, &cc);
 648
 649         fmt = __vdic_get_fmt(priv, cfg, sdformat->pad, sdformat->which);
 650         *fmt = sdformat->format;
 651
 652         /* propagate format to source pad */
 653         if (sdformat->pad == VDIC_SINK_PAD_DIRECT ||
 654             sdformat->pad == VDIC_SINK_PAD_IDMAC) {
 655                 const struct imx_media_pixfmt *outcc;
 656                 struct v4l2_mbus_framefmt *outfmt;
 657                 struct v4l2_subdev_format format;
 658
 659                 format.pad = VDIC_SRC_PAD_DIRECT;
 660                 format.which = sdformat->which;
 661                 format.format = sdformat->format;
 662                 vdic_try_fmt(priv, cfg, &format, &outcc);
 663
 664                 outfmt = __vdic_get_fmt(priv, cfg, VDIC_SRC_PAD_DIRECT,
 665                                         sdformat->which);
 666                 *outfmt = format.format;
 667                 if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
 668                         priv->cc[VDIC_SRC_PAD_DIRECT] = outcc;
 669         }
 670
 671         if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
 672                 priv->cc[sdformat->pad] = cc;
 673 out:
 674         mutex_unlock(&priv->lock);
 675         return ret;
 676 }
 677
 678 static int vdic_link_setup(struct media_entity *entity,
 679                             const struct media_pad *local,
 680                             const struct media_pad *remote, u32 flags)
 681 {
 682         struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 683         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 684         struct v4l2_subdev *remote_sd;
 685         int ret = 0;
 686
 687         dev_dbg(priv->dev, "link setup %s -> %s", remote->entity->name,
 688                 local->entity->name);
 689
 690         mutex_lock(&priv->lock);
 691
 692         if (local->flags & MEDIA_PAD_FL_SOURCE) {
 693                 if (!is_media_entity_v4l2_subdev(remote->entity)) {
 694                         ret = -EINVAL;
 695                         goto out;
 696                 }
 697
 698                 remote_sd = media_entity_to_v4l2_subdev(remote->entity);
 699
 700                 if (flags & MEDIA_LNK_FL_ENABLED) {
 701                         if (priv->sink_sd) {
 702                                 ret = -EBUSY;
 703                                 goto out;
 704                         }
 705                         priv->sink_sd = remote_sd;
 706                 } else {
 707                         priv->sink_sd = NULL;
 708                 }
 709
 710                 goto out;
 711         }
 712
 713         /* this is a sink pad */
 714
 715         if (flags & MEDIA_LNK_FL_ENABLED) {
 716                 if (priv->src) {
 717                         ret = -EBUSY;
 718                         goto out;
 719                 }
 720         } else {
 721                 priv->src = NULL;
 722                 goto out;
 723         }
 724
 725         if (local->index == VDIC_SINK_PAD_IDMAC) {
 726                 struct imx_media_video_dev *vdev = priv->vdev;
 727
 728                 if (!is_media_entity_v4l2_video_device(remote->entity)) {
 729                         ret = -EINVAL;
 730                         goto out;
 731                 }
 732                 if (!vdev) {
 733                         ret = -ENODEV;
 734                         goto out;
 735                 }
 736
 737                 priv->csi_direct = false;
 738         } else {
 739                 if (!is_media_entity_v4l2_subdev(remote->entity)) {
 740                         ret = -EINVAL;
 741                         goto out;
 742                 }
 743
 744                 remote_sd = media_entity_to_v4l2_subdev(remote->entity);
 745
 746                 /* direct pad must connect to a CSI */
 747                 if (!(remote_sd->grp_id & IMX_MEDIA_GRP_ID_CSI) ||
 748                     remote->index != CSI_SRC_PAD_DIRECT) {
 749                         ret = -EINVAL;
 750                         goto out;
 751                 }
 752
 753                 priv->csi_direct = true;
 754         }
 755
 756         priv->src = remote->entity;
 757         /* record which input pad is now active */
 758         priv->active_input_pad = local->index;
 759 out:
 760         mutex_unlock(&priv->lock);
 761         return ret;
 762 }
 763
 764 static int vdic_link_validate(struct v4l2_subdev *sd,
 765                               struct media_link *link,
 766                               struct v4l2_subdev_format *source_fmt,
 767                               struct v4l2_subdev_format *sink_fmt)
 768 {
 769         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 770         int ret;
 771
 772         ret = v4l2_subdev_link_validate_default(sd, link,
 773                                                 source_fmt, sink_fmt);
 774         if (ret)
 775                 return ret;
 776
 777         mutex_lock(&priv->lock);
 778
 779         if (priv->csi_direct && priv->motion != HIGH_MOTION) {
 780                 v4l2_err(&priv->sd,
 781                          "direct CSI pipeline requires high motion\n");
 782                 ret = -EINVAL;
 783         }
 784
 785         mutex_unlock(&priv->lock);
 786         return ret;
 787 }
 788
 789 static int vdic_g_frame_interval(struct v4l2_subdev *sd,
 790                                 struct v4l2_subdev_frame_interval *fi)
 791 {
 792         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 793
 794         if (fi->pad >= VDIC_NUM_PADS)
 795                 return -EINVAL;
 796
 797         mutex_lock(&priv->lock);
 798
 799         fi->interval = priv->frame_interval[fi->pad];
 800
 801         mutex_unlock(&priv->lock);
 802
 803         return 0;
 804 }
 805
 806 static int vdic_s_frame_interval(struct v4l2_subdev *sd,
 807                                 struct v4l2_subdev_frame_interval *fi)
 808 {
 809         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 810         struct v4l2_fract *input_fi, *output_fi;
 811         int ret = 0;
 812
 813         mutex_lock(&priv->lock);
 814
 815         input_fi = &priv->frame_interval[priv->active_input_pad];
 816         output_fi = &priv->frame_interval[VDIC_SRC_PAD_DIRECT];
 817
 818         switch (fi->pad) {
 819         case VDIC_SINK_PAD_DIRECT:
 820         case VDIC_SINK_PAD_IDMAC:
 821                 /* No limits on input frame interval */
 822                 /* Reset output interval */
 823                 *output_fi = fi->interval;
 824                 if (priv->csi_direct)
 825                         output_fi->denominator *= 2;
 826                 break;
 827         case VDIC_SRC_PAD_DIRECT:
 828                 /*
 829                  * frame rate at output pad is double input
 830                  * rate when using direct CSI->VDIC pipeline.
 831                  *
 832                  * TODO: implement VDIC frame skipping
 833                  */
 834                 fi->interval = *input_fi;
 835                 if (priv->csi_direct)
 836                         fi->interval.denominator *= 2;
 837                 break;
 838         default:
 839                 ret = -EINVAL;
 840                 goto out;
 841         }
 842
 843         priv->frame_interval[fi->pad] = fi->interval;
 844 out:
 845         mutex_unlock(&priv->lock);
 846         return ret;
 847 }
 848
 849 /*
 850  * retrieve our pads parsed from the OF graph by the media device
 851  */
 852 static int vdic_registered(struct v4l2_subdev *sd)
 853 {
 854         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 855         int i, ret;
 856         u32 code;
 857
 858         /* get media device */
 859         priv->md = dev_get_drvdata(sd->v4l2_dev->dev);
 860
 861         for (i = 0; i < VDIC_NUM_PADS; i++) {
 862                 priv->pad[i].flags = (i == VDIC_SRC_PAD_DIRECT) ?
 863                         MEDIA_PAD_FL_SOURCE : MEDIA_PAD_FL_SINK;
 864
 865                 code = 0;
 866                 if (i != VDIC_SINK_PAD_IDMAC)
 867                         imx_media_enum_ipu_format(&code, 0, CS_SEL_YUV);
 868
 869                 /* set a default mbus format  */
 870                 ret = imx_media_init_mbus_fmt(&priv->format_mbus[i],
 871                                               640, 480, code, V4L2_FIELD_NONE,
 872                                               &priv->cc[i]);
 873                 if (ret)
 874                         return ret;
 875
 876                 /* init default frame interval */
 877                 priv->frame_interval[i].numerator = 1;
 878                 priv->frame_interval[i].denominator = 30;
 879                 if (i == VDIC_SRC_PAD_DIRECT)
 880                         priv->frame_interval[i].denominator *= 2;
 881         }
 882
 883         priv->active_input_pad = VDIC_SINK_PAD_DIRECT;
 884
 885         ret = vdic_init_controls(priv);
 886         if (ret)
 887                 return ret;
 888
 889         ret = media_entity_pads_init(&sd->entity, VDIC_NUM_PADS, priv->pad);
 890         if (ret)
 891                 v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
 892
 893         return ret;
 894 }
 895
 896 static void vdic_unregistered(struct v4l2_subdev *sd)
 897 {
 898         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 899
 900         v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
 901 }
 902
 903 static const struct v4l2_subdev_pad_ops vdic_pad_ops = {
 904         .init_cfg = imx_media_init_cfg,
 905         .enum_mbus_code = vdic_enum_mbus_code,
 906         .get_fmt = vdic_get_fmt,
 907         .set_fmt = vdic_set_fmt,
 908         .link_validate = vdic_link_validate,
 909 };
 910
 911 static const struct v4l2_subdev_video_ops vdic_video_ops = {
 912         .g_frame_interval = vdic_g_frame_interval,
 913         .s_frame_interval = vdic_s_frame_interval,
 914         .s_stream = vdic_s_stream,
 915 };
 916
 917 static const struct media_entity_operations vdic_entity_ops = {
 918         .link_setup = vdic_link_setup,
 919         .link_validate = v4l2_subdev_link_validate,
 920 };
 921
 922 static const struct v4l2_subdev_ops vdic_subdev_ops = {
 923         .video = &vdic_video_ops,
 924         .pad = &vdic_pad_ops,
 925 };
 926
 927 static const struct v4l2_subdev_internal_ops vdic_internal_ops = {
 928         .registered = vdic_registered,
 929         .unregistered = vdic_unregistered,
 930 };
 931
 932 static int imx_vdic_probe(struct platform_device *pdev)
 933 {
 934         struct imx_media_internal_sd_platformdata *pdata;
 935         struct vdic_priv *priv;
 936         int ret;
 937
 938         priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
 939         if (!priv)
 940                 return -ENOMEM;
 941
 942         platform_set_drvdata(pdev, &priv->sd);
 943         priv->dev = &pdev->dev;
 944
 945         pdata = priv->dev->platform_data;
 946         priv->ipu_id = pdata->ipu_id;
 947
 948         v4l2_subdev_init(&priv->sd, &vdic_subdev_ops);
 949         v4l2_set_subdevdata(&priv->sd, priv);
 950         priv->sd.internal_ops = &vdic_internal_ops;
 951         priv->sd.entity.ops = &vdic_entity_ops;
 952         priv->sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
 953         priv->sd.dev = &pdev->dev;
 954         priv->sd.owner = THIS_MODULE;
 955         priv->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
 956         /* get our group id */
 957         priv->sd.grp_id = pdata->grp_id;
 958         strncpy(priv->sd.name, pdata->sd_name, sizeof(priv->sd.name));
 959
 960         mutex_init(&priv->lock);
 961
 962         ret = v4l2_async_register_subdev(&priv->sd);
 963         if (ret)
 964                 goto free;
 965
 966         return 0;
 967 free:
 968         mutex_destroy(&priv->lock);
 969         return ret;
 970 }
 971
 972 static int imx_vdic_remove(struct platform_device *pdev)
 973 {
 974         struct v4l2_subdev *sd = platform_get_drvdata(pdev);
 975         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 976
 977         v4l2_info(sd, "Removing\n");
 978
 979         v4l2_async_unregister_subdev(sd);
 980         mutex_destroy(&priv->lock);
 981         media_entity_cleanup(&sd->entity);
 982
 983         return 0;
 984 }
 985
 986 static const struct platform_device_id imx_vdic_ids[] = {
 987         { .name = "imx-ipuv3-vdic" },
 988         { },
 989 };
 990 MODULE_DEVICE_TABLE(platform, imx_vdic_ids);
 991
 992 static struct platform_driver imx_vdic_driver = {
 993         .probe = imx_vdic_probe,
 994         .remove = imx_vdic_remove,
 995         .id_table = imx_vdic_ids,
 996         .driver = {
 997                 .name = "imx-ipuv3-vdic",
 998         },
 999 };
1000 module_platform_driver(imx_vdic_driver);
1001
1002 MODULE_DESCRIPTION("i.MX VDIC subdev driver");
1003 MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
1004 MODULE_LICENSE("GPL");
1005 MODULE_ALIAS("platform:imx-ipuv3-vdic");