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       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
  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.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
 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_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
 544static 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
 554static 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;
 574out:
 575        mutex_unlock(&priv->lock);
 576        return ret;
 577}
 578
 579static 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
 620static 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;
 665out:
 666        mutex_unlock(&priv->lock);
 667        return ret;
 668}
 669
 670static 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;
 751out:
 752        mutex_unlock(&priv->lock);
 753        return ret;
 754}
 755
 756static 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
 781static 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
 798static 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;
 839out:
 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 */
 847static 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
 888static 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
 895static 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
 903static 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
 909static const struct media_entity_operations vdic_entity_ops = {
 910        .link_setup = vdic_link_setup,
 911        .link_validate = v4l2_subdev_link_validate,
 912};
 913
 914static const struct v4l2_subdev_ops vdic_subdev_ops = {
 915        .video = &vdic_video_ops,
 916        .pad = &vdic_pad_ops,
 917};
 918
 919static const struct v4l2_subdev_internal_ops vdic_internal_ops = {
 920        .registered = vdic_registered,
 921        .unregistered = vdic_unregistered,
 922};
 923
 924struct 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;
 957free:
 958        mutex_destroy(&priv->lock);
 959        return ERR_PTR(ret);
 960}
 961
 962int 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}
 974