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