linux/drivers/staging/media/imx/imx-media-vdic.c
<<
>>
Prefs
   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
  40struct vdic_priv;
  41
  42struct 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
  60struct 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
 115static 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
 134static 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
 176out_err_chan:
 177        v4l2_err(&priv->sd, "could not get IDMAC channel %u\n", err_chan);
 178out:
 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 */
 189static 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
 237static 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
 271static 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
 280static void vdic_start_direct(struct vdic_priv *priv)
 281{
 282}
 283
 284static void vdic_stop_direct(struct vdic_priv *priv)
 285{
 286}
 287
 288static 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
 294static 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
 325static 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
 333static 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
 341static void vdic_disable_indirect(struct vdic_priv *priv)
 342{
 343}
 344
 345static 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
 352static 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
 359static 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
 394out_put_ipu:
 395        vdic_put_ipu_resources(priv);
 396        return ret;
 397}
 398
 399static 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
 412static 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
 438out:
 439        mutex_unlock(&priv->lock);
 440        return ret;
 441}
 442
 443static const struct v4l2_ctrl_ops vdic_ctrl_ops = {
 444        .s_ctrl = vdic_s_ctrl,
 445};
 446
 447static const char * const vdic_ctrl_motion_menu[] = {
 448        "No Motion Compensation",
 449        "Low Motion",
 450        "Medium Motion",
 451        "High Motion",
 452};
 453
 454static 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
 476out_free:
 477        v4l2_ctrl_handler_free(hdlr);
 478        return ret;
 479}
 480
 481static 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
 525update_count:
 526        priv->stream_count += enable ? 1 : -1;
 527        if (priv->stream_count < 0)
 528                priv->stream_count = 0;
 529out:
 530        mutex_unlock(&priv->lock);
 531        return ret;
 532}
 533
 534static 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_get_try_format(&priv->sd, sd_state, pad);
 540        else
 541                return &priv->format_mbus[pad];
 542}
 543
 544static 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
 555static 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;
 575out:
 576        mutex_unlock(&priv->lock);
 577        return ret;
 578}
 579
 580static 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
 622static 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;
 667out:
 668        mutex_unlock(&priv->lock);
 669        return ret;
 670}
 671
 672static 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;
 753out:
 754        mutex_unlock(&priv->lock);
 755        return ret;
 756}
 757
 758static 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
 783static 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
 800static 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;
 841out:
 842        mutex_unlock(&priv->lock);
 843        return ret;
 844}
 845
 846static 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
 877static 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
 884static 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
 892static 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
 898static const struct media_entity_operations vdic_entity_ops = {
 899        .link_setup = vdic_link_setup,
 900        .link_validate = v4l2_subdev_link_validate,
 901};
 902
 903static const struct v4l2_subdev_ops vdic_subdev_ops = {
 904        .video = &vdic_video_ops,
 905        .pad = &vdic_pad_ops,
 906};
 907
 908static const struct v4l2_subdev_internal_ops vdic_internal_ops = {
 909        .registered = vdic_registered,
 910        .unregistered = vdic_unregistered,
 911};
 912
 913struct 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;
 955free:
 956        mutex_destroy(&priv->lock);
 957        return ERR_PTR(ret);
 958}
 959
 960int 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}
 972