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

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