// SPDX-License-Identifier: GPL-2.0+
/*
 * V4L2 Deinterlacer Subdev for Freescale i.MX5/6 SOC
 *
 * Copyright (c) 2017 Mentor Graphics Inc.
 */
#include <media/v4l2-ctrls.h>
#include <media/v4l2-device.h>
#include <media/v4l2-ioctl.h>
#include <media/v4l2-mc.h>
#include <media/v4l2-subdev.h>
#include <media/imx.h>
#include "imx-media.h"

/*
 * This subdev implements two different video pipelines:
 *
 * CSI -> VDIC
 *
 * In this pipeline, the CSI sends a single interlaced field F(n-1)
 * directly to the VDIC (and optionally the following field F(n)
 * can be sent to memory via IDMAC channel 13). This pipeline only works
 * in VDIC's high motion mode, which only requires a single field for
 * processing. The other motion modes (low and medium) require three
 * fields, so this pipeline does not work in those modes. Also, it is
 * not clear how this pipeline can deal with the various field orders
 * (sequential BT/TB, interlaced BT/TB).
 *
 * MEM -> CH8,9,10 -> VDIC
 *
 * In this pipeline, previous field F(n-1), current field F(n), and next
 * field F(n+1) are transferred to the VDIC via IDMAC channels 8,9,10.
 * These memory buffers can come from a video output or mem2mem device.
 * All motion modes are supported by this pipeline.
 *
 * The "direct" CSI->VDIC pipeline requires no DMA, but it can only be
 * used in high motion mode.
 */

struct vdic_priv;

struct vdic_pipeline_ops {
        int (*setup)(struct vdic_priv *priv);
        void (*start)(struct vdic_priv *priv);
        void (*stop)(struct vdic_priv *priv);
        void (*disable)(struct vdic_priv *priv);
};

/*
 * Min/Max supported width and heights.
 */
#define MIN_W       176
#define MIN_H       144
#define MAX_W_VDIC  968
#define MAX_H_VDIC 2048
#define W_ALIGN    4 /* multiple of 16 pixels */
#define H_ALIGN    1 /* multiple of 2 lines */
#define S_ALIGN    1 /* multiple of 2 */

struct vdic_priv {
        struct device *ipu_dev;
        struct ipu_soc *ipu;

        struct v4l2_subdev   sd;
        struct media_pad pad[VDIC_NUM_PADS];

        /* lock to protect all members below */
        struct mutex lock;

        /* IPU units we require */
        struct ipu_vdi *vdi;

        int active_input_pad;

        struct ipuv3_channel *vdi_in_ch_p; /* F(n-1) transfer channel */
        struct ipuv3_channel *vdi_in_ch;   /* F(n) transfer channel */
        struct ipuv3_channel *vdi_in_ch_n; /* F(n+1) transfer channel */

        /* pipeline operations */
        struct vdic_pipeline_ops *ops;

        /* current and previous input buffers indirect path */
        struct imx_media_buffer *curr_in_buf;
        struct imx_media_buffer *prev_in_buf;

        /*
         * translated field type, input line stride, and field size
         * for indirect path
         */
        u32 fieldtype;
        u32 in_stride;
        u32 field_size;

        /* the source (a video device or subdev) */
        struct media_entity *src;
        /* the sink that will receive the progressive out buffers */
        struct v4l2_subdev *sink_sd;

        struct v4l2_mbus_framefmt format_mbus[VDIC_NUM_PADS];
        const struct imx_media_pixfmt *cc[VDIC_NUM_PADS];
        struct v4l2_fract frame_interval[VDIC_NUM_PADS];

        /* the video device at IDMAC input pad */
        struct imx_media_video_dev *vdev;

        bool csi_direct;  /* using direct CSI->VDIC->IC pipeline */

        /* motion select control */
        struct v4l2_ctrl_handler ctrl_hdlr;
        enum ipu_motion_sel motion;

        int stream_count;
};

static void vdic_put_ipu_resources(struct vdic_priv *priv)
{
        if (priv->vdi_in_ch_p)
                ipu_idmac_put(priv->vdi_in_ch_p);
        priv->vdi_in_ch_p = NULL;

        if (priv->vdi_in_ch)
                ipu_idmac_put(priv->vdi_in_ch);
        priv->vdi_in_ch = NULL;

        if (priv->vdi_in_ch_n)
                ipu_idmac_put(priv->vdi_in_ch_n);
        priv->vdi_in_ch_n = NULL;

        if (!IS_ERR_OR_NULL(priv->vdi))
                ipu_vdi_put(priv->vdi);
        priv->vdi = NULL;
}

static int vdic_get_ipu_resources(struct vdic_priv *priv)
{
        int ret, err_chan;
        struct ipuv3_channel *ch;
        struct ipu_vdi *vdi;

        vdi = ipu_vdi_get(priv->ipu);
        if (IS_ERR(vdi)) {
                v4l2_err(&priv->sd, "failed to get VDIC\n");
                ret = PTR_ERR(vdi);
                goto out;
        }
        priv->vdi = vdi;

        if (!priv->csi_direct) {
                ch = ipu_idmac_get(priv->ipu, IPUV3_CHANNEL_MEM_VDI_PREV);
                if (IS_ERR(ch)) {
                        err_chan = IPUV3_CHANNEL_MEM_VDI_PREV;
                        ret = PTR_ERR(ch);
                        goto out_err_chan;
                }
                priv->vdi_in_ch_p = ch;

                ch = ipu_idmac_get(priv->ipu, IPUV3_CHANNEL_MEM_VDI_CUR);
                if (IS_ERR(ch)) {
                        err_chan = IPUV3_CHANNEL_MEM_VDI_CUR;
                        ret = PTR_ERR(ch);
                        goto out_err_chan;
                }
                priv->vdi_in_ch = ch;

                ch = ipu_idmac_get(priv->ipu, IPUV3_CHANNEL_MEM_VDI_NEXT);
                if (IS_ERR(ch)) {
                        err_chan = IPUV3_CHANNEL_MEM_VDI_NEXT;
                        ret = PTR_ERR(ch);
                        goto out_err_chan;
                }
                priv->vdi_in_ch_n = ch;
        }

        return 0;

out_err_chan:
        v4l2_err(&priv->sd, "could not get IDMAC channel %u\n", err_chan);
out:
        vdic_put_ipu_resources(priv);
        return ret;
}

/*
 * This function is currently unused, but will be called when the
 * output/mem2mem device at the IDMAC input pad sends us a new
 * buffer. It kicks off the IDMAC read channels to bring in the
 * buffer fields from memory and begin the conversions.
 */
static void __maybe_unused prepare_vdi_in_buffers(struct vdic_priv *priv,
                                                  struct imx_media_buffer *curr)
{
        dma_addr_t prev_phys, curr_phys, next_phys;
        struct imx_media_buffer *prev;
        struct vb2_buffer *curr_vb, *prev_vb;
        u32 fs = priv->field_size;
        u32 is = priv->in_stride;

        /* current input buffer is now previous */
        priv->prev_in_buf = priv->curr_in_buf;
        priv->curr_in_buf = curr;
        prev = priv->prev_in_buf ? priv->prev_in_buf : curr;

        prev_vb = &prev->vbuf.vb2_buf;
        curr_vb = &curr->vbuf.vb2_buf;

        switch (priv->fieldtype) {
        case V4L2_FIELD_SEQ_TB:
        case V4L2_FIELD_SEQ_BT:
                prev_phys = vb2_dma_contig_plane_dma_addr(prev_vb, 0) + fs;
                curr_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0);
                next_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0) + fs;
                break;
        case V4L2_FIELD_INTERLACED_TB:
        case V4L2_FIELD_INTERLACED_BT:
        case V4L2_FIELD_INTERLACED:
                prev_phys = vb2_dma_contig_plane_dma_addr(prev_vb, 0) + is;
                curr_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0);
                next_phys = vb2_dma_contig_plane_dma_addr(curr_vb, 0) + is;
                break;
        default:
                /*
                 * can't get here, priv->fieldtype can only be one of
                 * the above. This is to quiet smatch errors.
                 */
                return;
        }

        ipu_cpmem_set_buffer(priv->vdi_in_ch_p, 0, prev_phys);
        ipu_cpmem_set_buffer(priv->vdi_in_ch,   0, curr_phys);
        ipu_cpmem_set_buffer(priv->vdi_in_ch_n, 0, next_phys);

        ipu_idmac_select_buffer(priv->vdi_in_ch_p, 0);
        ipu_idmac_select_buffer(priv->vdi_in_ch, 0);
        ipu_idmac_select_buffer(priv->vdi_in_ch_n, 0);
}

static int setup_vdi_channel(struct vdic_priv *priv,
                             struct ipuv3_channel *channel,
                             dma_addr_t phys0, dma_addr_t phys1)
{
        struct imx_media_video_dev *vdev = priv->vdev;
        unsigned int burst_size;
        struct ipu_image image;
        int ret;

        ipu_cpmem_zero(channel);

        memset(&image, 0, sizeof(image));
        image.pix = vdev->fmt.fmt.pix;
        image.rect = vdev->compose;
        /* one field to VDIC channels */
        image.pix.height /= 2;
        image.rect.height /= 2;
        image.phys0 = phys0;
        image.phys1 = phys1;

        ret = ipu_cpmem_set_image(channel, &image);
        if (ret)
                return ret;

        burst_size = (image.pix.width & 0xf) ? 8 : 16;
        ipu_cpmem_set_burstsize(channel, burst_size);

        ipu_cpmem_set_axi_id(channel, 1);

        ipu_idmac_set_double_buffer(channel, false);

        return 0;
}

static int vdic_setup_direct(struct vdic_priv *priv)
{
        /* set VDIC to receive from CSI for direct path */
        ipu_fsu_link(priv->ipu, IPUV3_CHANNEL_CSI_DIRECT,
                     IPUV3_CHANNEL_CSI_VDI_PREV);

        return 0;
}

static void vdic_start_direct(struct vdic_priv *priv)
{
}

static void vdic_stop_direct(struct vdic_priv *priv)
{
}

static void vdic_disable_direct(struct vdic_priv *priv)
{
        ipu_fsu_unlink(priv->ipu, IPUV3_CHANNEL_CSI_DIRECT,
                       IPUV3_CHANNEL_CSI_VDI_PREV);
}

static int vdic_setup_indirect(struct vdic_priv *priv)
{
        struct v4l2_mbus_framefmt *infmt;
        const struct imx_media_pixfmt *incc;
        int in_size, ret;

        infmt = &priv->format_mbus[VDIC_SINK_PAD_IDMAC];
        incc = priv->cc[VDIC_SINK_PAD_IDMAC];

        in_size = (infmt->width * incc->bpp * infmt->height) >> 3;

        /* 1/2 full image size */
        priv->field_size = in_size / 2;
        priv->in_stride = incc->planar ?
                infmt->width : (infmt->width * incc->bpp) >> 3;

        priv->prev_in_buf = NULL;
        priv->curr_in_buf = NULL;

        priv->fieldtype = infmt->field;

        /* init the vdi-in channels */
        ret = setup_vdi_channel(priv, priv->vdi_in_ch_p, 0, 0);
        if (ret)
                return ret;
        ret = setup_vdi_channel(priv, priv->vdi_in_ch, 0, 0);
        if (ret)
                return ret;
        return setup_vdi_channel(priv, priv->vdi_in_ch_n, 0, 0);
}

static void vdic_start_indirect(struct vdic_priv *priv)
{
        /* enable the channels */
        ipu_idmac_enable_channel(priv->vdi_in_ch_p);
        ipu_idmac_enable_channel(priv->vdi_in_ch);
        ipu_idmac_enable_channel(priv->vdi_in_ch_n);
}

static void vdic_stop_indirect(struct vdic_priv *priv)
{
        /* disable channels */
        ipu_idmac_disable_channel(priv->vdi_in_ch_p);
        ipu_idmac_disable_channel(priv->vdi_in_ch);
        ipu_idmac_disable_channel(priv->vdi_in_ch_n);
}

static void vdic_disable_indirect(struct vdic_priv *priv)
{
}

static struct vdic_pipeline_ops direct_ops = {
        .setup = vdic_setup_direct,
        .start = vdic_start_direct,
        .stop = vdic_stop_direct,
        .disable = vdic_disable_direct,
};

static struct vdic_pipeline_ops indirect_ops = {
        .setup = vdic_setup_indirect,
        .start = vdic_start_indirect,
        .stop = vdic_stop_indirect,
        .disable = vdic_disable_indirect,
};

static int vdic_start(struct vdic_priv *priv)
{
        struct v4l2_mbus_framefmt *infmt;
        int ret;

        infmt = &priv->format_mbus[priv->active_input_pad];

        priv->ops = priv->csi_direct ? &direct_ops : &indirect_ops;

        ret = vdic_get_ipu_resources(priv);
        if (ret)
                return ret;

        /*
         * init the VDIC.
         *
         * note we don't give infmt->code to ipu_vdi_setup(). The VDIC
         * only supports 4:2:2 or 4:2:0, and this subdev will only
         * negotiate 4:2:2 at its sink pads.
         */
        ipu_vdi_setup(priv->vdi, MEDIA_BUS_FMT_UYVY8_2X8,
                      infmt->width, infmt->height);
        ipu_vdi_set_field_order(priv->vdi, V4L2_STD_UNKNOWN, infmt->field);
        ipu_vdi_set_motion(priv->vdi, priv->motion);

        ret = priv->ops->setup(priv);
        if (ret)
                goto out_put_ipu;

        ipu_vdi_enable(priv->vdi);

        priv->ops->start(priv);

        return 0;

out_put_ipu:
        vdic_put_ipu_resources(priv);
        return ret;
}

static void vdic_stop(struct vdic_priv *priv)
{
        priv->ops->stop(priv);
        ipu_vdi_disable(priv->vdi);
        priv->ops->disable(priv);

        vdic_put_ipu_resources(priv);
}

/*
 * V4L2 subdev operations.
 */

static int vdic_s_ctrl(struct v4l2_ctrl *ctrl)
{
        struct vdic_priv *priv = container_of(ctrl->handler,
                                              struct vdic_priv, ctrl_hdlr);
        enum ipu_motion_sel motion;
        int ret = 0;

        mutex_lock(&priv->lock);

        switch (ctrl->id) {
        case V4L2_CID_DEINTERLACING_MODE:
                motion = ctrl->val;
                if (motion != priv->motion) {
                        /* can't change motion control mid-streaming */
                        if (priv->stream_count > 0) {
                                ret = -EBUSY;
                                goto out;
                        }
                        priv->motion = motion;
                }
                break;
        default:
                v4l2_err(&priv->sd, "Invalid control\n");
                ret = -EINVAL;
        }

out:
        mutex_unlock(&priv->lock);
        return ret;
}

static const struct v4l2_ctrl_ops vdic_ctrl_ops = {
        .s_ctrl = vdic_s_ctrl,
};

static const char * const vdic_ctrl_motion_menu[] = {
        "No Motion Compensation",
        "Low Motion",
        "Medium Motion",
        "High Motion",
};

static int vdic_init_controls(struct vdic_priv *priv)
{
        struct v4l2_ctrl_handler *hdlr = &priv->ctrl_hdlr;
        int ret;

        v4l2_ctrl_handler_init(hdlr, 1);

        v4l2_ctrl_new_std_menu_items(hdlr, &vdic_ctrl_ops,
                                     V4L2_CID_DEINTERLACING_MODE,
                                     HIGH_MOTION, 0, HIGH_MOTION,
                                     vdic_ctrl_motion_menu);

        priv->sd.ctrl_handler = hdlr;

        if (hdlr->error) {
                ret = hdlr->error;
                goto out_free;
        }

        v4l2_ctrl_handler_setup(hdlr);
        return 0;

out_free:
        v4l2_ctrl_handler_free(hdlr);
        return ret;
}

static int vdic_s_stream(struct v4l2_subdev *sd, int enable)
{
        struct vdic_priv *priv = v4l2_get_subdevdata(sd);
        struct v4l2_subdev *src_sd = NULL;
        int ret = 0;

        mutex_lock(&priv->lock);

        if (!priv->src || !priv->sink_sd) {
                ret = -EPIPE;
                goto out;
        }

        if (priv->csi_direct)
                src_sd = media_entity_to_v4l2_subdev(priv->src);

        /*
         * enable/disable streaming only if stream_count is
         * going from 0 to 1 / 1 to 0.
         */
        if (priv->stream_count != !enable)
                goto update_count;

        dev_dbg(priv->ipu_dev, "%s: stream %s\n", sd->name,
                enable ? "ON" : "OFF");

        if (enable)
                ret = vdic_start(priv);
        else
                vdic_stop(priv);
        if (ret)
                goto out;

        if (src_sd) {
                /* start/stop upstream */
                ret = v4l2_subdev_call(src_sd, video, s_stream, enable);
                ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0;
                if (ret) {
                        if (enable)
                                vdic_stop(priv);
                        goto out;
                }
        }

update_count:
        priv->stream_count += enable ? 1 : -1;
        if (priv->stream_count < 0)
                priv->stream_count = 0;
out:
        mutex_unlock(&priv->lock);
        return ret;
}

static struct v4l2_mbus_framefmt *
__vdic_get_fmt(struct vdic_priv *priv, struct v4l2_subdev_pad_config *cfg,
               unsigned int pad, enum v4l2_subdev_format_whence which)
{
        if (which == V4L2_SUBDEV_FORMAT_TRY)
                return v4l2_subdev_get_try_format(&priv->sd, cfg, pad);
        else
                return &priv->format_mbus[pad];
}

static int vdic_enum_mbus_code(struct v4l2_subdev *sd,
                               struct v4l2_subdev_pad_config *cfg,
                               struct v4l2_subdev_mbus_code_enum *code)
{
        if (code->pad >= VDIC_NUM_PADS)
                return -EINVAL;

        return imx_media_enum_ipu_format(&code->code, code->index, CS_SEL_YUV);
}

static int vdic_get_fmt(struct v4l2_subdev *sd,
                        struct v4l2_subdev_pad_config *cfg,
                        struct v4l2_subdev_format *sdformat)
{
        struct vdic_priv *priv = v4l2_get_subdevdata(sd);
        struct v4l2_mbus_framefmt *fmt;
        int ret = 0;

        if (sdformat->pad >= VDIC_NUM_PADS)
                return -EINVAL;

        mutex_lock(&priv->lock);

        fmt = __vdic_get_fmt(priv, cfg, sdformat->pad, sdformat->which);
        if (!fmt) {
                ret = -EINVAL;
                goto out;
        }

        sdformat->format = *fmt;
out:
        mutex_unlock(&priv->lock);
        return ret;
}

static void vdic_try_fmt(struct vdic_priv *priv,
                         struct v4l2_subdev_pad_config *cfg,
                         struct v4l2_subdev_format *sdformat,
                         const struct imx_media_pixfmt **cc)
{
        struct v4l2_mbus_framefmt *infmt;

        *cc = imx_media_find_ipu_format(sdformat->format.code, CS_SEL_YUV);
        if (!*cc) {
                u32 code;

                imx_media_enum_ipu_format(&code, 0, CS_SEL_YUV);
                *cc = imx_media_find_ipu_format(code, CS_SEL_YUV);
                sdformat->format.code = (*cc)->codes[0];
        }

        infmt = __vdic_get_fmt(priv, cfg, priv->active_input_pad,
                               sdformat->which);

        switch (sdformat->pad) {
        case VDIC_SRC_PAD_DIRECT:
                sdformat->format = *infmt;
                /* output is always progressive! */
                sdformat->format.field = V4L2_FIELD_NONE;
                break;
        case VDIC_SINK_PAD_DIRECT:
        case VDIC_SINK_PAD_IDMAC:
                v4l_bound_align_image(&sdformat->format.width,
                                      MIN_W, MAX_W_VDIC, W_ALIGN,
                                      &sdformat->format.height,
                                      MIN_H, MAX_H_VDIC, H_ALIGN, S_ALIGN);

                /* input must be interlaced! Choose SEQ_TB if not */
                if (!V4L2_FIELD_HAS_BOTH(sdformat->format.field))
                        sdformat->format.field = V4L2_FIELD_SEQ_TB;
                break;
        }

        imx_media_try_colorimetry(&sdformat->format, true);
}

static int vdic_set_fmt(struct v4l2_subdev *sd,
                        struct v4l2_subdev_pad_config *cfg,
                        struct v4l2_subdev_format *sdformat)
{
        struct vdic_priv *priv = v4l2_get_subdevdata(sd);
        const struct imx_media_pixfmt *cc;
        struct v4l2_mbus_framefmt *fmt;
        int ret = 0;

        if (sdformat->pad >= VDIC_NUM_PADS)
                return -EINVAL;

        mutex_lock(&priv->lock);

        if (priv->stream_count > 0) {
                ret = -EBUSY;
                goto out;
        }

        vdic_try_fmt(priv, cfg, sdformat, &cc);

        fmt = __vdic_get_fmt(priv, cfg, sdformat->pad, sdformat->which);
        *fmt = sdformat->format;

        /* propagate format to source pad */
        if (sdformat->pad == VDIC_SINK_PAD_DIRECT ||
            sdformat->pad == VDIC_SINK_PAD_IDMAC) {
                const struct imx_media_pixfmt *outcc;
                struct v4l2_mbus_framefmt *outfmt;
                struct v4l2_subdev_format format;

                format.pad = VDIC_SRC_PAD_DIRECT;
                format.which = sdformat->which;
                format.format = sdformat->format;
                vdic_try_fmt(priv, cfg, &format, &outcc);

                outfmt = __vdic_get_fmt(priv, cfg, VDIC_SRC_PAD_DIRECT,
                                        sdformat->which);
                *outfmt = format.format;
                if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
                        priv->cc[VDIC_SRC_PAD_DIRECT] = outcc;
        }

        if (sdformat->which == V4L2_SUBDEV_FORMAT_ACTIVE)
                priv->cc[sdformat->pad] = cc;
out:
        mutex_unlock(&priv->lock);
        return ret;
}

static int vdic_link_setup(struct media_entity *entity,
                            const struct media_pad *local,
                            const struct media_pad *remote, u32 flags)
{
        struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
        struct vdic_priv *priv = v4l2_get_subdevdata(sd);
        struct v4l2_subdev *remote_sd;
        int ret = 0;

        dev_dbg(priv->ipu_dev, "%s: link setup %s -> %s",
                sd->name, remote->entity->name, local->entity->name);

        mutex_lock(&priv->lock);

        if (local->flags & MEDIA_PAD_FL_SOURCE) {
                if (!is_media_entity_v4l2_subdev(remote->entity)) {
                        ret = -EINVAL;
                        goto out;
                }

                remote_sd = media_entity_to_v4l2_subdev(remote->entity);

                if (flags & MEDIA_LNK_FL_ENABLED) {
                        if (priv->sink_sd) {
                                ret = -EBUSY;
                                goto out;
                        }
                        priv->sink_sd = remote_sd;
                } else {
                        priv->sink_sd = NULL;
                }

                goto out;
        }

        /* this is a sink pad */

        if (flags & MEDIA_LNK_FL_ENABLED) {
                if (priv->src) {
                        ret = -EBUSY;
                        goto out;
                }
        } else {
                priv->src = NULL;
                goto out;
        }

        if (local->index == VDIC_SINK_PAD_IDMAC) {
                struct imx_media_video_dev *vdev = priv->vdev;

                if (!is_media_entity_v4l2_video_device(remote->entity)) {
                        ret = -EINVAL;
                        goto out;
                }
                if (!vdev) {
                        ret = -ENODEV;
                        goto out;
                }

                priv->csi_direct = false;
        } else {
                if (!is_media_entity_v4l2_subdev(remote->entity)) {
                        ret = -EINVAL;
                        goto out;
                }

                remote_sd = media_entity_to_v4l2_subdev(remote->entity);

                /* direct pad must connect to a CSI */
                if (!(remote_sd->grp_id & IMX_MEDIA_GRP_ID_IPU_CSI) ||
                    remote->index != CSI_SRC_PAD_DIRECT) {
                        ret = -EINVAL;
                        goto out;
                }

                priv->csi_direct = true;
        }

        priv->src = remote->entity;
        /* record which input pad is now active */
        priv->active_input_pad = local->index;
out:
        mutex_unlock(&priv->lock);
        return ret;
}

static int vdic_link_validate(struct v4l2_subdev *sd,
                              struct media_link *link,
                              struct v4l2_subdev_format *source_fmt,
                              struct v4l2_subdev_format *sink_fmt)
{
        struct vdic_priv *priv = v4l2_get_subdevdata(sd);
        int ret;

        ret = v4l2_subdev_link_validate_default(sd, link,
                                                source_fmt, sink_fmt);
        if (ret)
                return ret;

        mutex_lock(&priv->lock);

        if (priv->csi_direct && priv->motion != HIGH_MOTION) {
                v4l2_err(&priv->sd,
                         "direct CSI pipeline requires high motion\n");
                ret = -EINVAL;
        }

        mutex_unlock(&priv->lock);
        return ret;
}

static int vdic_g_frame_interval(struct v4l2_subdev *sd,
                                struct v4l2_subdev_frame_interval *fi)
{
        struct vdic_priv *priv = v4l2_get_subdevdata(sd);

        if (fi->pad >= VDIC_NUM_PADS)
                return -EINVAL;

        mutex_lock(&priv->lock);

        fi->interval = priv->frame_interval[fi->pad];

        mutex_unlock(&priv->lock);

        return 0;
}

static int vdic_s_frame_interval(struct v4l2_subdev *sd,
                                struct v4l2_subdev_frame_interval *fi)
{
        struct vdic_priv *priv = v4l2_get_subdevdata(sd);
        struct v4l2_fract *input_fi, *output_fi;
        int ret = 0;

        mutex_lock(&priv->lock);

        input_fi = &priv->frame_interval[priv->active_input_pad];
        output_fi = &priv->frame_interval[VDIC_SRC_PAD_DIRECT];

        switch (fi->pad) {
        case VDIC_SINK_PAD_DIRECT:
        case VDIC_SINK_PAD_IDMAC:
                /* No limits on valid input frame intervals */
                if (fi->interval.numerator == 0 ||
                    fi->interval.denominator == 0)
                        fi->interval = priv->frame_interval[fi->pad];
                /* Reset output interval */
                *output_fi = fi->interval;
                if (priv->csi_direct)
                        output_fi->denominator *= 2;
                break;
        case VDIC_SRC_PAD_DIRECT:
                /*
                 * frame rate at output pad is double input
                 * rate when using direct CSI->VDIC pipeline.
                 *
                 * TODO: implement VDIC frame skipping
                 */
                fi->interval = *input_fi;
                if (priv->csi_direct)
                        fi->interval.denominator *= 2;
                break;
        default:
                ret = -EINVAL;
                goto out;
        }

        priv->frame_interval[fi->pad] = fi->interval;
out:
        mutex_unlock(&priv->lock);
        return ret;
}

/*
 * retrieve our pads parsed from the OF graph by the media device
 */
static int vdic_registered(struct v4l2_subdev *sd)
{
        struct vdic_priv *priv = v4l2_get_subdevdata(sd);
        int i, ret;
        u32 code;

        for (i = 0; i < VDIC_NUM_PADS; i++) {
                priv->pad[i].flags = (i == VDIC_SRC_PAD_DIRECT) ?
                        MEDIA_PAD_FL_SOURCE : MEDIA_PAD_FL_SINK;

                code = 0;
                if (i != VDIC_SINK_PAD_IDMAC)
                        imx_media_enum_ipu_format(&code, 0, CS_SEL_YUV);

                /* set a default mbus format  */
                ret = imx_media_init_mbus_fmt(&priv->format_mbus[i],
                                              640, 480, code, V4L2_FIELD_NONE,
                                              &priv->cc[i]);
                if (ret)
                        return ret;

                /* init default frame interval */
                priv->frame_interval[i].numerator = 1;
                priv->frame_interval[i].denominator = 30;
                if (i == VDIC_SRC_PAD_DIRECT)
                        priv->frame_interval[i].denominator *= 2;
        }

        priv->active_input_pad = VDIC_SINK_PAD_DIRECT;

        ret = vdic_init_controls(priv);
        if (ret)
                return ret;

        ret = media_entity_pads_init(&sd->entity, VDIC_NUM_PADS, priv->pad);
        if (ret)
                v4l2_ctrl_handler_free(&priv->ctrl_hdlr);

        return ret;
}

static void vdic_unregistered(struct v4l2_subdev *sd)
{
        struct vdic_priv *priv = v4l2_get_subdevdata(sd);

        v4l2_ctrl_handler_free(&priv->ctrl_hdlr);
}

static const struct v4l2_subdev_pad_ops vdic_pad_ops = {
        .init_cfg = imx_media_init_cfg,
        .enum_mbus_code = vdic_enum_mbus_code,
        .get_fmt = vdic_get_fmt,
        .set_fmt = vdic_set_fmt,
        .link_validate = vdic_link_validate,
};

static const struct v4l2_subdev_video_ops vdic_video_ops = {
        .g_frame_interval = vdic_g_frame_interval,
        .s_frame_interval = vdic_s_frame_interval,
        .s_stream = vdic_s_stream,
};

static const struct media_entity_operations vdic_entity_ops = {
        .link_setup = vdic_link_setup,
        .link_validate = v4l2_subdev_link_validate,
};

static const struct v4l2_subdev_ops vdic_subdev_ops = {
        .video = &vdic_video_ops,
        .pad = &vdic_pad_ops,
};

static const struct v4l2_subdev_internal_ops vdic_internal_ops = {
        .registered = vdic_registered,
        .unregistered = vdic_unregistered,
};

struct v4l2_subdev *imx_media_vdic_register(struct v4l2_device *v4l2_dev,
                                            struct device *ipu_dev,
                                            struct ipu_soc *ipu,
                                            u32 grp_id)
{
        struct vdic_priv *priv;
        int ret;

        priv = devm_kzalloc(ipu_dev, sizeof(*priv), GFP_KERNEL);
        if (!priv)
                return ERR_PTR(-ENOMEM);

        priv->ipu_dev = ipu_dev;
        priv->ipu = ipu;

        v4l2_subdev_init(&priv->sd, &vdic_subdev_ops);
        v4l2_set_subdevdata(&priv->sd, priv);
        priv->sd.internal_ops = &vdic_internal_ops;
        priv->sd.entity.ops = &vdic_entity_ops;
        priv->sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
        priv->sd.owner = ipu_dev->driver->owner;
        priv->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
        priv->sd.grp_id = grp_id;
        imx_media_grp_id_to_sd_name(priv->sd.name, sizeof(priv->sd.name),
                                    priv->sd.grp_id, ipu_get_num(ipu));

        mutex_init(&priv->lock);

        ret = v4l2_device_register_subdev(v4l2_dev, &priv->sd);
        if (ret)
                goto free;

        return &priv->sd;
free:
        mutex_destroy(&priv->lock);
        return ERR_PTR(ret);
}

int imx_media_vdic_unregister(struct v4l2_subdev *sd)
{
        struct vdic_priv *priv = v4l2_get_subdevdata(sd);

        v4l2_info(sd, "Removing\n");

        v4l2_device_unregister_subdev(sd);
        mutex_destroy(&priv->lock);
        media_entity_cleanup(&sd->entity);

        return 0;
}