Re: [PATCH 2/2] media: imx: vdic: Introduce mem2mem VDI deinterlacer driver

diff --git a/drivers/staging/media/imx/imx-media-dev.c b/drivers/staging/media/imx/imx-media-dev.c
index be54dca11465d..b75eec4513eab 100644
--- a/drivers/staging/media/imx/imx-media-dev.c
+++ b/drivers/staging/media/imx/imx-media-dev.c

@@ -57,7 +57,47 @@ static int imx6_media_probe_complete(struct v4l2_async_notifier *notifier)
 		goto unlock;
 	}
 
+	imxmd->m2m_vdic[0] = imx_media_mem2mem_vdic_init(imxmd, 0);
+	if (IS_ERR(imxmd->m2m_vdic[0])) {
+		ret = PTR_ERR(imxmd->m2m_vdic[0]);
+		imxmd->m2m_vdic[0] = NULL;
+		goto unlock;
+	}
+
+	/* MX6S/DL has one IPUv3, init second VDI only on MX6Q/QP */
+	if (imxmd->ipu[1]) {
+		imxmd->m2m_vdic[1] = imx_media_mem2mem_vdic_init(imxmd, 1);
+		if (IS_ERR(imxmd->m2m_vdic[1])) {
+			ret = PTR_ERR(imxmd->m2m_vdic[1]);
+			imxmd->m2m_vdic[1] = NULL;
+			goto unlock;

+		}
+	}
+
 	ret = imx_media_csc_scaler_device_register(imxmd->m2m_vdev);
+	if (ret)
+		goto unlock;
+
+	ret = imx_media_mem2mem_vdic_register(imxmd->m2m_vdic[0]);
+	if (ret)
+		goto unreg_csc;
+
+	/* MX6S/DL has one IPUv3, init second VDI only on MX6Q/QP */
+	if (imxmd->ipu[1]) {
+		ret = imx_media_mem2mem_vdic_register(imxmd->m2m_vdic[1]);
+		if (ret)
+			goto unreg_vdic;
+	}
+
+	mutex_unlock(&imxmd->mutex);
+	return ret;

+
+unreg_vdic:
+	imx_media_mem2mem_vdic_unregister(imxmd->m2m_vdic[0]);
+	imxmd->m2m_vdic[0] = NULL;
+unreg_csc:
+	imx_media_csc_scaler_device_unregister(imxmd->m2m_vdev);
+	imxmd->m2m_vdev = NULL;
 unlock:
 	mutex_unlock(&imxmd->mutex);
 	return ret;

+static void ipu_mem2mem_vdic_device_run(void *_ctx)
+{
+	struct ipu_mem2mem_vdic_ctx *ctx = _ctx;
+	struct ipu_mem2mem_vdic_priv *priv = ctx->priv;
+	struct vb2_v4l2_buffer *curr_buf, *dst_buf;
+	dma_addr_t prev_phys, curr_phys, out_phys;
+	struct v4l2_pix_format *infmt;
+	u32 phys_offset = 0;
+	unsigned long flags;
+	int ret;
+
+	infmt = ipu_mem2mem_vdic_get_format(priv, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+	if (V4L2_FIELD_IS_SEQUENTIAL(infmt->field))
+		phys_offset = infmt->sizeimage / 2;
+	else if (V4L2_FIELD_IS_INTERLACED(infmt->field))
+		phys_offset = infmt->bytesperline;
+	else
+		dev_err(priv->dev, "Invalid field %d\n", infmt->field);
+
+	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	out_phys = vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf, 0);
+
+	curr_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	if (!curr_buf) {
+		dev_err(priv->dev, "Not enough buffers %d\n", ret);

+		return;
+	}
+
+	spin_lock_irqsave(&priv->irqlock, flags);
+
+	if (ctx->curr_buf) {
+		ctx->prev_buf = ctx->curr_buf;
+		ctx->curr_buf = curr_buf;
+	} else {
+		ctx->prev_buf = curr_buf;
+		ctx->curr_buf = curr_buf;
+		dev_warn(priv->dev, "Single-buffer mode, fix your userspace\n");
+	}
+
+	prev_phys = vb2_dma_contig_plane_dma_addr(&ctx->prev_buf->vb2_buf, 0);
+	curr_phys = vb2_dma_contig_plane_dma_addr(&ctx->curr_buf->vb2_buf, 0);
+
+	priv->curr_ctx = ctx;
+	spin_unlock_irqrestore(&priv->irqlock, flags);
+
+	ipu_cpmem_set_buffer(priv->vdi_out_ch,  0, out_phys);
+	ipu_cpmem_set_buffer(priv->vdi_in_ch_p, 0, prev_phys + phys_offset);
+	ipu_cpmem_set_buffer(priv->vdi_in_ch,   0, curr_phys);
+	ipu_cpmem_set_buffer(priv->vdi_in_ch_n, 0, curr_phys + phys_offset);
+
+	/* No double buffering, always pick buffer 0 */
+	ipu_idmac_select_buffer(priv->vdi_out_ch, 0);
+	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);
+
+	/* Enable the channels */
+	ipu_idmac_enable_channel(priv->vdi_out_ch);
+	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 int ipu_mem2mem_vdic_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
+					unsigned int *nplanes, unsigned int sizes[],
+					struct device *alloc_devs[])
+{
+	struct ipu_mem2mem_vdic_ctx *ctx = vb2_get_drv_priv(vq);
+	struct ipu_mem2mem_vdic_priv *priv = ctx->priv;
+	struct v4l2_pix_format *fmt = ipu_mem2mem_vdic_get_format(priv, vq->type);
+	unsigned int count = *nbuffers;
+
+	*nbuffers = count;

+
+	if (*nplanes)
+		return sizes[0] < fmt->sizeimage ? -EINVAL : 0;
+
+	*nplanes = 1;
+	sizes[0] = fmt->sizeimage;
+
+	dev_dbg(ctx->priv->dev, "get %d buffer(s) of size %d each.\n",

+		count, fmt->sizeimage);
+
+	return 0;
+}

+static int ipu_mem2mem_vdic_get_ipu_resources(struct ipu_mem2mem_vdic_priv *priv,
+					      struct video_device *vfd)
+{
+	char *nfbname, *eofname;
+	int ret;
+
+	nfbname = devm_kasprintf(priv->dev, GFP_KERNEL, "%s_nfb4eof:%u",
+				 vfd->name, priv->ipu_id);
+	if (!nfbname)
+		return -ENOMEM;
+
+	eofname = devm_kasprintf(priv->dev, GFP_KERNEL, "%s_eof:%u",
+				 vfd->name, priv->ipu_id);
+	if (!eofname)
+		goto err_eof;
+
+	priv->vdi = ipu_vdi_get(priv->ipu_dev);
+	if (IS_ERR(priv->vdi)) {
+		ret = PTR_ERR(priv->vdi);
+		goto err_vdi;
+	}
+
+	priv->ic = ipu_ic_get(priv->ipu_dev, IC_TASK_VIEWFINDER);
+	if (IS_ERR(priv->ic)) {
+		ret = PTR_ERR(priv->ic);
+		goto err_ic;
+	}
+
+	priv->vdi_in_ch_p = ipu_idmac_get(priv->ipu_dev,
+					  IPUV3_CHANNEL_MEM_VDI_PREV);
+	if (IS_ERR(priv->vdi_in_ch_p)) {
+		ret = PTR_ERR(priv->vdi_in_ch_p);
+		goto err_prev;
+	}
+
+	priv->vdi_in_ch = ipu_idmac_get(priv->ipu_dev,
+					IPUV3_CHANNEL_MEM_VDI_CUR);
+	if (IS_ERR(priv->vdi_in_ch)) {
+		ret = PTR_ERR(priv->vdi_in_ch);
+		goto err_curr;
+	}
+
+	priv->vdi_in_ch_n = ipu_idmac_get(priv->ipu_dev,
+					  IPUV3_CHANNEL_MEM_VDI_NEXT);
+	if (IS_ERR(priv->vdi_in_ch_n)) {
+		ret = PTR_ERR(priv->vdi_in_ch_n);
+		goto err_next;
+	}
+
+	priv->vdi_out_ch = ipu_idmac_get(priv->ipu_dev,
+					 IPUV3_CHANNEL_IC_PRP_VF_MEM);
+	if (IS_ERR(priv->vdi_out_ch)) {
+		ret = PTR_ERR(priv->vdi_out_ch);
+		goto err_out;
+	}
+
+	priv->nfb4eof_irq = ipu_idmac_channel_irq(priv->ipu_dev,
+						  priv->vdi_out_ch,
+						  IPU_IRQ_NFB4EOF);
+	ret = devm_request_irq(priv->dev, priv->nfb4eof_irq,
+			       ipu_mem2mem_vdic_nfb4eof_interrupt, 0,
+			       nfbname, priv);
+	if (ret)
+		goto err_irq_nfb4eof;
+
+	priv->eof_irq = ipu_idmac_channel_irq(priv->ipu_dev,
+					      priv->vdi_out_ch,
+					      IPU_IRQ_EOF);
+	ret = devm_request_irq(priv->dev, priv->eof_irq,
+			       ipu_mem2mem_vdic_eof_interrupt, 0,
+			       eofname, priv);
+	if (ret)
+		goto err_irq_eof;
+
+	/*
+	 * Enable PRG, without PRG clock enabled (CCGR6:prg_clk_enable[0]
+	 * and CCGR6:prg_clk_enable[1]), the VDI does not produce any
+	 * interrupts at all.
+	 */
+	if (ipu_prg_present(priv->ipu_dev))
+		ipu_prg_enable(priv->ipu_dev);
+
+	return 0;
+
+err_irq_eof:
+	devm_free_irq(priv->dev, priv->nfb4eof_irq, priv);

+err_irq_nfb4eof:
+	ipu_idmac_put(priv->vdi_out_ch);
+err_out:
+	ipu_idmac_put(priv->vdi_in_ch_n);
+err_next:
+	ipu_idmac_put(priv->vdi_in_ch);
+err_curr:
+	ipu_idmac_put(priv->vdi_in_ch_p);
+err_prev:
+	ipu_ic_put(priv->ic);
+err_ic:
+	ipu_vdi_put(priv->vdi);
+err_vdi:
+	devm_kfree(priv->dev, eofname);

+	return ret;
+}
+
+static void ipu_mem2mem_vdic_put_ipu_resources(struct ipu_mem2mem_vdic_priv *priv)
+{
+	devm_free_irq(priv->dev, priv->eof_irq, priv);
+	devm_free_irq(priv->dev, priv->nfb4eof_irq, priv);
+	ipu_idmac_put(priv->vdi_out_ch);
+	ipu_idmac_put(priv->vdi_in_ch_n);
+	ipu_idmac_put(priv->vdi_in_ch);
+	ipu_idmac_put(priv->vdi_in_ch_p);
+	ipu_ic_put(priv->ic);
+	ipu_vdi_put(priv->vdi);
+}
+
+int imx_media_mem2mem_vdic_register(struct imx_media_video_dev *vdev)
+{
+	struct ipu_mem2mem_vdic_priv *priv = to_mem2mem_priv(vdev);
+	struct video_device *vfd = vdev->vfd;
+	int ret;
+
+	vfd->v4l2_dev = &priv->md->v4l2_dev;
+
+	ret = ipu_mem2mem_vdic_get_ipu_resources(priv, vfd);
+	if (ret) {
+		v4l2_err(vfd->v4l2_dev, "Failed to get VDIC resources (%d)\n", ret);
+		return ret;
+	}
+
+	ret = video_register_device(vfd, VFL_TYPE_VIDEO, -1);
+	if (ret) {
+		v4l2_err(vfd->v4l2_dev, "Failed to register video device\n");

+		return ret;
+	}
+
+	v4l2_info(vfd->v4l2_dev, "Registered %s as /dev/%s\n", vfd->name,
+		  video_device_node_name(vfd));
+
+	return 0;
+}
+
+void imx_media_mem2mem_vdic_unregister(struct imx_media_video_dev *vdev)
+{
+	struct ipu_mem2mem_vdic_priv *priv = to_mem2mem_priv(vdev);
+	struct video_device *vfd = priv->vdev.vfd;
+
+	video_unregister_device(vfd);
+
+	ipu_mem2mem_vdic_put_ipu_resources(priv);
+}
+
+struct imx_media_video_dev *
+imx_media_mem2mem_vdic_init(struct imx_media_dev *md, int ipu_id)
+{
+	struct ipu_mem2mem_vdic_priv *priv;
+	struct video_device *vfd;
+	int ret;
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return ERR_PTR(-ENOMEM);
+
+	priv->md = md;
+	priv->ipu_id = ipu_id;
+	priv->ipu_dev = md->ipu[ipu_id];
+	priv->dev = md->md.dev;
+
+	mutex_init(&priv->mutex);
+
+	vfd = video_device_alloc();
+	if (!vfd) {
+		ret = -ENOMEM;
+		goto err_vfd;
+	}
+
+	*vfd = mem2mem_template;
+	vfd->lock = &priv->mutex;
+	priv->vdev.vfd = vfd;
+
+	INIT_LIST_HEAD(&priv->vdev.list);
+	spin_lock_init(&priv->irqlock);
+	atomic_set(&priv->stream_count, 0);
+
+	video_set_drvdata(vfd, priv);
+
+	priv->m2m_dev = v4l2_m2m_init(&m2m_ops);
+	if (IS_ERR(priv->m2m_dev)) {
+		ret = PTR_ERR(priv->m2m_dev);
+		v4l2_err(&md->v4l2_dev, "Failed to init mem2mem device: %d\n",
+			 ret);
+		goto err_m2m;
+	}
+
+	/* Reset formats */
+	priv->fmt[V4L2_M2M_SRC] = ipu_mem2mem_vdic_default;
+	priv->fmt[V4L2_M2M_SRC].pixelformat = V4L2_PIX_FMT_YUV420;
+	priv->fmt[V4L2_M2M_SRC].field = V4L2_FIELD_SEQ_TB;
+	priv->fmt[V4L2_M2M_SRC].bytesperline = DEFAULT_WIDTH;
+	priv->fmt[V4L2_M2M_SRC].sizeimage = DEFAULT_WIDTH * DEFAULT_HEIGHT * 3 / 2;
+
+	priv->fmt[V4L2_M2M_DST] = ipu_mem2mem_vdic_default;
+	priv->fmt[V4L2_M2M_DST].pixelformat = V4L2_PIX_FMT_RGB565;
+	priv->fmt[V4L2_M2M_DST].field = V4L2_FIELD_NONE;
+	priv->fmt[V4L2_M2M_DST].bytesperline = DEFAULT_WIDTH * 2;
+	priv->fmt[V4L2_M2M_DST].sizeimage = DEFAULT_WIDTH * DEFAULT_HEIGHT * 2;
+
+	return &priv->vdev;
+
+err_m2m:
+	video_set_drvdata(vfd, NULL);

+err_vfd:
+	kfree(priv);
+	return ERR_PTR(ret);
+}