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       176
  53 #define MIN_H       144
  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.fmt.pix;
 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_pad_config *cfg,
 536                unsigned int pad, enum v4l2_subdev_format_whence which)
 537 {
 538         if (which == V4L2_SUBDEV_FORMAT_TRY)
 539                 return v4l2_subdev_get_try_format(&priv->sd, cfg, 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_pad_config *cfg,
 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_format(&code->code, code->index, CS_SEL_YUV);
 552 }
 553
 554 static int vdic_get_fmt(struct v4l2_subdev *sd,
 555                         struct v4l2_subdev_pad_config *cfg,
 556                         struct v4l2_subdev_format *sdformat)
 557 {
 558         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 559         struct v4l2_mbus_framefmt *fmt;
 560         int ret = 0;
 561
 562         if (sdformat->pad >= VDIC_NUM_PADS)
 563                 return -EINVAL;
 564
 565         mutex_lock(&priv->lock);
 566
 567         fmt = __vdic_get_fmt(priv, cfg, sdformat->pad, sdformat->which);
 568         if (!fmt) {
 569                 ret = -EINVAL;
 570                 goto out;
 571         }
 572
 573         sdformat->format = *fmt;
 574 out:
 575         mutex_unlock(&priv->lock);
 576         return ret;
 577 }
 578
 579 static void vdic_try_fmt(struct vdic_priv *priv,
 580                          struct v4l2_subdev_pad_config *cfg,
 581                          struct v4l2_subdev_format *sdformat,
 582                          const struct imx_media_pixfmt **cc)
 583 {
 584         struct v4l2_mbus_framefmt *infmt;
 585
 586         *cc = imx_media_find_ipu_format(sdformat->format.code, CS_SEL_YUV);
 587         if (!*cc) {
 588                 u32 code;
 589
 590                 imx_media_enum_ipu_format(&code, 0, CS_SEL_YUV);
 591                 *cc = imx_media_find_ipu_format(code, CS_SEL_YUV);
 592                 sdformat->format.code = (*cc)->codes[0];
 593         }
 594
 595         infmt = __vdic_get_fmt(priv, cfg, priv->active_input_pad,
 596                                sdformat->which);
 597
 598         switch (sdformat->pad) {
 599         case VDIC_SRC_PAD_DIRECT:
 600                 sdformat->format = *infmt;
 601                 /* output is always progressive! */
 602                 sdformat->format.field = V4L2_FIELD_NONE;
 603                 break;
 604         case VDIC_SINK_PAD_DIRECT:
 605         case VDIC_SINK_PAD_IDMAC:
 606                 v4l_bound_align_image(&sdformat->format.width,
 607                                       MIN_W, MAX_W_VDIC, W_ALIGN,
 608                                       &sdformat->format.height,
 609                                       MIN_H, MAX_H_VDIC, H_ALIGN, S_ALIGN);
 610
 611                 /* input must be interlaced! Choose SEQ_TB if not */
 612                 if (!V4L2_FIELD_HAS_BOTH(sdformat->format.field))
 613                         sdformat->format.field = V4L2_FIELD_SEQ_TB;
 614                 break;
 615         }
 616
 617         imx_media_try_colorimetry(&sdformat->format, true);
 618 }
 619
 620 static int vdic_set_fmt(struct v4l2_subdev *sd,
 621                         struct v4l2_subdev_pad_config *cfg,
 622                         struct v4l2_subdev_format *sdformat)
 623 {
 624         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 625         const struct imx_media_pixfmt *cc;
 626         struct v4l2_mbus_framefmt *fmt;
 627         int ret = 0;
 628
 629         if (sdformat->pad >= VDIC_NUM_PADS)
 630                 return -EINVAL;
 631
 632         mutex_lock(&priv->lock);
 633
 634         if (priv->stream_count > 0) {
 635                 ret = -EBUSY;
 636                 goto out;
 637         }
 638
 639         vdic_try_fmt(priv, cfg, sdformat, &cc);
 640
 641         fmt = __vdic_get_fmt(priv, cfg, sdformat->pad, sdformat->which);
 642         *fmt = sdformat->format;
 643
 644         /* propagate format to source pad */
 645         if (sdformat->pad == VDIC_SINK_PAD_DIRECT ||
 646             sdformat->pad == VDIC_SINK_PAD_IDMAC) {
 647                 const struct imx_media_pixfmt *outcc;
 648                 struct v4l2_mbus_framefmt *outfmt;
 649                 struct v4l2_subdev_format format;
 650
 651                 format.pad = VDIC_SRC_PAD_DIRECT;
 652                 format.which = sdformat->which;
 653                 format.format = sdformat->format;
 654                 vdic_try_fmt(priv, cfg, &format, &outcc);
 655
 656                 outfmt = __vdic_get_fmt(priv, cfg, VDIC_SRC_PAD_DIRECT,
 657                                         sdformat->which);
 658                 *outfmt = format.format;
 659                 if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
 660                         priv->cc[VDIC_SRC_PAD_DIRECT] = outcc;
 661         }
 662
 663         if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
 664                 priv->cc[sdformat->pad] = cc;
 665 out:
 666         mutex_unlock(&priv->lock);
 667         return ret;
 668 }
 669
 670 static int vdic_link_setup(struct media_entity *entity,
 671                             const struct media_pad *local,
 672                             const struct media_pad *remote, u32 flags)
 673 {
 674         struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 675         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 676         struct v4l2_subdev *remote_sd;
 677         int ret = 0;
 678
 679         dev_dbg(priv->ipu_dev, "%s: link setup %s -> %s",
 680                 sd->name, remote->entity->name, local->entity->name);
 681
 682         mutex_lock(&priv->lock);
 683
 684         if (local->flags & MEDIA_PAD_FL_SOURCE) {
 685                 if (!is_media_entity_v4l2_subdev(remote->entity)) {
 686                         ret = -EINVAL;
 687                         goto out;
 688                 }
 689
 690                 remote_sd = media_entity_to_v4l2_subdev(remote->entity);
 691
 692                 if (flags & MEDIA_LNK_FL_ENABLED) {
 693                         if (priv->sink_sd) {
 694                                 ret = -EBUSY;
 695                                 goto out;
 696                         }
 697                         priv->sink_sd = remote_sd;
 698                 } else {
 699                         priv->sink_sd = NULL;
 700                 }
 701
 702                 goto out;
 703         }
 704
 705         /* this is a sink pad */
 706
 707         if (flags & MEDIA_LNK_FL_ENABLED) {
 708                 if (priv->src) {
 709                         ret = -EBUSY;
 710                         goto out;
 711                 }
 712         } else {
 713                 priv->src = NULL;
 714                 goto out;
 715         }
 716
 717         if (local->index == VDIC_SINK_PAD_IDMAC) {
 718                 struct imx_media_video_dev *vdev = priv->vdev;
 719
 720                 if (!is_media_entity_v4l2_video_device(remote->entity)) {
 721                         ret = -EINVAL;
 722                         goto out;
 723                 }
 724                 if (!vdev) {
 725                         ret = -ENODEV;
 726                         goto out;
 727                 }
 728
 729                 priv->csi_direct = false;
 730         } else {
 731                 if (!is_media_entity_v4l2_subdev(remote->entity)) {
 732                         ret = -EINVAL;
 733                         goto out;
 734                 }
 735
 736                 remote_sd = media_entity_to_v4l2_subdev(remote->entity);
 737
 738                 /* direct pad must connect to a CSI */
 739                 if (!(remote_sd->grp_id & IMX_MEDIA_GRP_ID_IPU_CSI) ||
 740                     remote->index != CSI_SRC_PAD_DIRECT) {
 741                         ret = -EINVAL;
 742                         goto out;
 743                 }
 744
 745                 priv->csi_direct = true;
 746         }
 747
 748         priv->src = remote->entity;
 749         /* record which input pad is now active */
 750         priv->active_input_pad = local->index;
 751 out:
 752         mutex_unlock(&priv->lock);
 753         return ret;
 754 }
 755
 756 static int vdic_link_validate(struct v4l2_subdev *sd,
 757                               struct media_link *link,
 758                               struct v4l2_subdev_format *source_fmt,
 759                               struct v4l2_subdev_format *sink_fmt)
 760 {
 761         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 762         int ret;
 763
 764         ret = v4l2_subdev_link_validate_default(sd, link,
 765                                                 source_fmt, sink_fmt);
 766         if (ret)
 767                 return ret;
 768
 769         mutex_lock(&priv->lock);
 770
 771         if (priv->csi_direct && priv->motion != HIGH_MOTION) {
 772                 v4l2_err(&priv->sd,
 773                          "direct CSI pipeline requires high motion\n");
 774                 ret = -EINVAL;
 775         }
 776
 777         mutex_unlock(&priv->lock);
 778         return ret;
 779 }
 780
 781 static int vdic_g_frame_interval(struct v4l2_subdev *sd,
 782                                 struct v4l2_subdev_frame_interval *fi)
 783 {
 784         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 785
 786         if (fi->pad >= VDIC_NUM_PADS)
 787                 return -EINVAL;
 788
 789         mutex_lock(&priv->lock);
 790
 791         fi->interval = priv->frame_interval[fi->pad];
 792
 793         mutex_unlock(&priv->lock);
 794
 795         return 0;
 796 }
 797
 798 static int vdic_s_frame_interval(struct v4l2_subdev *sd,
 799                                 struct v4l2_subdev_frame_interval *fi)
 800 {
 801         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 802         struct v4l2_fract *input_fi, *output_fi;
 803         int ret = 0;
 804
 805         mutex_lock(&priv->lock);
 806
 807         input_fi = &priv->frame_interval[priv->active_input_pad];
 808         output_fi = &priv->frame_interval[VDIC_SRC_PAD_DIRECT];
 809
 810         switch (fi->pad) {
 811         case VDIC_SINK_PAD_DIRECT:
 812         case VDIC_SINK_PAD_IDMAC:
 813                 /* No limits on valid input frame intervals */
 814                 if (fi->interval.numerator == 0 ||
 815                     fi->interval.denominator == 0)
 816                         fi->interval = priv->frame_interval[fi->pad];
 817                 /* Reset output interval */
 818                 *output_fi = fi->interval;
 819                 if (priv->csi_direct)
 820                         output_fi->denominator *= 2;
 821                 break;
 822         case VDIC_SRC_PAD_DIRECT:
 823                 /*
 824                  * frame rate at output pad is double input
 825                  * rate when using direct CSI->VDIC pipeline.
 826                  *
 827                  * TODO: implement VDIC frame skipping
 828                  */
 829                 fi->interval = *input_fi;
 830                 if (priv->csi_direct)
 831                         fi->interval.denominator *= 2;
 832                 break;
 833         default:
 834                 ret = -EINVAL;
 835                 goto out;
 836         }
 837
 838         priv->frame_interval[fi->pad] = fi->interval;
 839 out:
 840         mutex_unlock(&priv->lock);
 841         return ret;
 842 }
 843
 844 /*
 845  * retrieve our pads parsed from the OF graph by the media device
 846  */
 847 static int vdic_registered(struct v4l2_subdev *sd)
 848 {
 849         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 850         int i, ret;
 851         u32 code;
 852
 853         for (i = 0; i < VDIC_NUM_PADS; i++) {
 854                 priv->pad[i].flags = (i == VDIC_SRC_PAD_DIRECT) ?
 855                         MEDIA_PAD_FL_SOURCE : MEDIA_PAD_FL_SINK;
 856
 857                 code = 0;
 858                 if (i != VDIC_SINK_PAD_IDMAC)
 859                         imx_media_enum_ipu_format(&code, 0, CS_SEL_YUV);
 860
 861                 /* set a default mbus format  */
 862                 ret = imx_media_init_mbus_fmt(&priv->format_mbus[i],
 863                                               640, 480, code, V4L2_FIELD_NONE,
 864                                               &priv->cc[i]);
 865                 if (ret)
 866                         return ret;
 867
 868                 /* init default frame interval */
 869                 priv->frame_interval[i].numerator = 1;
 870                 priv->frame_interval[i].denominator = 30;
 871                 if (i == VDIC_SRC_PAD_DIRECT)
 872                         priv->frame_interval[i].denominator *= 2;
 873         }
 874
 875         priv->active_input_pad = VDIC_SINK_PAD_DIRECT;
 876
 877         ret = vdic_init_controls(priv);
 878         if (ret)
 879                 return ret;
 880
 881         ret = media_entity_pads_init(&sd->entity, VDIC_NUM_PADS, priv->pad);
 882         if (ret)
 883                 v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
 884
 885         return ret;
 886 }
 887
 888 static void vdic_unregistered(struct v4l2_subdev *sd)
 889 {
 890         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 891
 892         v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
 893 }
 894
 895 static const struct v4l2_subdev_pad_ops vdic_pad_ops = {
 896         .init_cfg = imx_media_init_cfg,
 897         .enum_mbus_code = vdic_enum_mbus_code,
 898         .get_fmt = vdic_get_fmt,
 899         .set_fmt = vdic_set_fmt,
 900         .link_validate = vdic_link_validate,
 901 };
 902
 903 static const struct v4l2_subdev_video_ops vdic_video_ops = {
 904         .g_frame_interval = vdic_g_frame_interval,
 905         .s_frame_interval = vdic_s_frame_interval,
 906         .s_stream = vdic_s_stream,
 907 };
 908
 909 static const struct media_entity_operations vdic_entity_ops = {
 910         .link_setup = vdic_link_setup,
 911         .link_validate = v4l2_subdev_link_validate,
 912 };
 913
 914 static const struct v4l2_subdev_ops vdic_subdev_ops = {
 915         .video = &vdic_video_ops,
 916         .pad = &vdic_pad_ops,
 917 };
 918
 919 static const struct v4l2_subdev_internal_ops vdic_internal_ops = {
 920         .registered = vdic_registered,
 921         .unregistered = vdic_unregistered,
 922 };
 923
 924 struct v4l2_subdev *imx_media_vdic_register(struct v4l2_device *v4l2_dev,
 925                                             struct device *ipu_dev,
 926                                             struct ipu_soc *ipu,
 927                                             u32 grp_id)
 928 {
 929         struct vdic_priv *priv;
 930         int ret;
 931
 932         priv = devm_kzalloc(ipu_dev, sizeof(*priv), GFP_KERNEL);
 933         if (!priv)
 934                 return ERR_PTR(-ENOMEM);
 935
 936         priv->ipu_dev = ipu_dev;
 937         priv->ipu = ipu;
 938
 939         v4l2_subdev_init(&priv->sd, &vdic_subdev_ops);
 940         v4l2_set_subdevdata(&priv->sd, priv);
 941         priv->sd.internal_ops = &vdic_internal_ops;
 942         priv->sd.entity.ops = &vdic_entity_ops;
 943         priv->sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
 944         priv->sd.owner = ipu_dev->driver->owner;
 945         priv->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
 946         priv->sd.grp_id = grp_id;
 947         imx_media_grp_id_to_sd_name(priv->sd.name, sizeof(priv->sd.name),
 948                                     priv->sd.grp_id, ipu_get_num(ipu));
 949
 950         mutex_init(&priv->lock);
 951
 952         ret = v4l2_device_register_subdev(v4l2_dev, &priv->sd);
 953         if (ret)
 954                 goto free;
 955
 956         return &priv->sd;
 957 free:
 958         mutex_destroy(&priv->lock);
 959         return ERR_PTR(ret);
 960 }
 961
 962 int imx_media_vdic_unregister(struct v4l2_subdev *sd)
 963 {
 964         struct vdic_priv *priv = v4l2_get_subdevdata(sd);
 965
 966         v4l2_info(sd, "Removing\n");
 967
 968         v4l2_device_unregister_subdev(sd);
 969         mutex_destroy(&priv->lock);
 970         media_entity_cleanup(&sd->entity);
 971
 972         return 0;
 973 }