From 3d5216147fd758341dd92b397e70162101a61adc Mon Sep 17 00:00:00 2001
From: b19715 <b19715@freescale.com>
Date: Fri, 11 Jun 2021 17:04:49 +0800
Subject: [PATCH 1/3] ISI: Add de-interlace support

---
 arch/arm64/configs/imx_v8_defconfig           |   2 +-
 drivers/staging/media/imx/imx8-isi-cap.c      |  78 +++++++++++++-
 drivers/staging/media/imx/imx8-isi-core.c     |  25 ++++-
 drivers/staging/media/imx/imx8-isi-core.h     |  10 +-
 drivers/staging/media/imx/imx8-isi-hw.c       | 147 ++++++++++++++++++++++++--
 drivers/staging/media/imx/imx8-isi-hw.h       |   4 +
 drivers/staging/media/imx/imx8-mipi-csi2.c    |  48 +++++++++
 drivers/staging/media/imx/imx8-parallel-csi.c |  95 ++++++++++++++---
 8 files changed, 386 insertions(+), 23 deletions(-)

diff --git a/arch/arm64/configs/imx_v8_defconfig b/arch/arm64/configs/imx_v8_defconfig
index 1de0c72..12b4833 100644
--- a/arch/arm64/configs/imx_v8_defconfig
+++ b/arch/arm64/configs/imx_v8_defconfig
@@ -720,7 +720,7 @@ CONFIG_XEN_GRANT_DEV_ALLOC=y
 CONFIG_STAGING=y
 CONFIG_STAGING_MEDIA=y
 CONFIG_VIDEO_IMX_CAPTURE=y
-CONFIG_IMX8_MEDIA_DEVICE=m
+CONFIG_IMX8_MEDIA_DEVICE=y
 CONFIG_ION=y
 CONFIG_ION_SYSTEM_HEAP=y
 CONFIG_ION_CMA_HEAP=y
diff --git a/drivers/staging/media/imx/imx8-isi-cap.c b/drivers/staging/media/imx/imx8-isi-cap.c
index 6ba5b3a..c129178 100644
--- a/drivers/staging/media/imx/imx8-isi-cap.c
+++ b/drivers/staging/media/imx/imx8-isi-cap.c
@@ -689,6 +689,7 @@ static int mxc_isi_capture_open(struct file *file)
 {
 	struct mxc_isi_cap_dev *isi_cap = video_drvdata(file);
 	struct mxc_isi_dev *mxc_isi = mxc_isi_get_hostdata(isi_cap->pdev);
+	struct mxc_isi_dev *mxc_isi_temp;
 	struct device *dev = &isi_cap->pdev->dev;
 	struct v4l2_subdev *sd;
 	int ret = -EBUSY;
@@ -733,6 +734,11 @@ static int mxc_isi_capture_open(struct file *file)
 	mxc_isi->m2m_enabled = false;
 	mutex_unlock(&mxc_isi->lock);
 
+	if (mxc_isi->blending) {
+		mxc_isi_temp = mxc_isi_get_weaving_channel();
+		pm_runtime_get_sync(&mxc_isi_temp->pdev->dev);
+	}
+
 	return 0;
 }
 
@@ -740,6 +746,7 @@ static int mxc_isi_capture_release(struct file *file)
 {
 	struct mxc_isi_cap_dev *isi_cap = video_drvdata(file);
 	struct mxc_isi_dev *mxc_isi = mxc_isi_get_hostdata(isi_cap->pdev);
+	struct mxc_isi_dev *mxc_isi_temp;
 	struct device *dev = &isi_cap->pdev->dev;
 	struct v4l2_subdev *sd;
 	int ret = -1;
@@ -761,9 +768,23 @@ static int mxc_isi_capture_release(struct file *file)
 	mutex_unlock(&isi_cap->lock);
 
 	if (atomic_read(&mxc_isi->usage_count) > 0 &&
-	    atomic_dec_and_test(&mxc_isi->usage_count))
+	    atomic_dec_and_test(&mxc_isi->usage_count)) {
 		mxc_isi_channel_deinit(mxc_isi);
 
+		if (mxc_isi->blending) {
+			mxc_isi_temp = mxc_isi_get_weaving_channel();
+			if (mxc_isi_temp->bld_buf_vaddr) {
+				dma_free_coherent(&mxc_isi->pdev->dev,
+					PAGE_ALIGN(mxc_isi_temp->bld_buf_size),
+					mxc_isi_temp->bld_buf_vaddr, mxc_isi_temp->bld_buf_paddr);
+				mxc_isi_temp->bld_buf_vaddr = NULL;
+				mxc_isi_temp->bld_buf_paddr = 0;
+			}
+			mxc_isi_channel_deinit(mxc_isi_temp);
+			pm_runtime_put(&mxc_isi_temp->pdev->dev);
+		}
+	}
+
 	ret = v4l2_subdev_call(sd, core, s_power, 0);
 	if (ret < 0 && ret != -ENOIOCTLCMD) {
 		dev_err(dev, "%s s_power fail\n", __func__);
@@ -888,6 +909,7 @@ static int mxc_isi_source_fmt_init(struct mxc_isi_cap_dev *isi_cap)
 	struct v4l2_subdev_format src_fmt;
 	struct media_pad *source_pad;
 	struct v4l2_subdev *src_sd;
+	struct mxc_isi_dev *mxc_isi = mxc_isi_get_hostdata(isi_cap->pdev);
 	int ret;
 
 	source_pad = mxc_isi_get_remote_source_pad(&isi_cap->sd);
@@ -935,6 +957,10 @@ static int mxc_isi_source_fmt_init(struct mxc_isi_cap_dev *isi_cap)
 		return -EINVAL;
 	}
 
+	if (src_fmt.format.field == V4L2_FIELD_INTERLACED) {
+		mxc_isi_channel_set_deinterlace(mxc_isi);
+	}
+
 	return 0;
 }
 
@@ -1020,6 +1046,7 @@ static int mxc_isi_cap_s_fmt_mplane(struct file *file, void *priv,
 static int mxc_isi_config_parm(struct mxc_isi_cap_dev *isi_cap)
 {
 	struct mxc_isi_dev *mxc_isi = mxc_isi_get_hostdata(isi_cap->pdev);
+	struct mxc_isi_dev *mxc_isi_temp;
 	int ret;
 
 	ret = mxc_isi_source_fmt_init(isi_cap);
@@ -1027,6 +1054,25 @@ static int mxc_isi_config_parm(struct mxc_isi_cap_dev *isi_cap)
 		return -EINVAL;
 
 	mxc_isi_channel_init(mxc_isi);
+
+	if (mxc_isi->blending) {
+		mxc_isi_temp = mxc_isi_get_weaving_channel();
+
+		/* weaving buffer size for YUV422: width * height * bytes_per_pixel * 2 */
+		mxc_isi_temp->bld_buf_size = isi_cap->src_f.o_height
+				* isi_cap->src_f.o_width * 4;
+
+		mxc_isi_temp->bld_buf_vaddr = dma_alloc_coherent(&mxc_isi->pdev->dev,
+				PAGE_ALIGN(mxc_isi_temp->bld_buf_size),
+				&mxc_isi_temp->bld_buf_paddr, GFP_DMA | GFP_KERNEL);
+		if (mxc_isi_temp->bld_buf_vaddr == NULL) {
+			dev_err(&mxc_isi->pdev->dev, "No enough dma memory\n");
+			return -ENOMEM;
+		}
+
+		mxc_isi_channel_init(mxc_isi_temp);
+	}
+
 	mxc_isi_channel_config(mxc_isi, &isi_cap->src_f, &isi_cap->dst_f);
 
 	return 0;
@@ -1189,6 +1235,34 @@ static int mxc_isi_cap_s_selection(struct file *file, void *fh,
 	return 0;
 }
 
+static int mxc_isi_cap_g_std(struct file *file, void *priv, v4l2_std_id *a)
+{
+	struct mxc_isi_cap_dev *isi_cap = video_drvdata(file);
+	struct v4l2_subdev *sd;
+	struct media_pad *source_pad;
+	struct mxc_isi_dev *mxc_isi = mxc_isi_get_hostdata(isi_cap->pdev);
+	int ret;
+
+	/* Get remote source pad */
+	source_pad = mxc_isi_get_remote_source_pad(&isi_cap->sd);
+	if (source_pad == NULL) {
+		v4l2_err(&isi_cap->sd, "%s, No remote pad found!\n", __func__);
+		return -EINVAL;
+	}
+
+	/* Get remote source pad subdev */
+	sd = media_entity_to_v4l2_subdev(source_pad->entity);
+	if (sd == NULL) {
+		v4l2_err(&isi_cap->sd, "%s, No remote subdev found!\n", __func__);
+		return -EINVAL;
+	}
+
+	ret = v4l2_subdev_call(sd, video, g_std, a);
+	mxc_isi->std_id = *a;
+
+	return ret;
+}
+
 static int mxc_isi_cap_enum_framesizes(struct file *file, void *priv,
 				       struct v4l2_frmsizeenum *fsize)
 {
@@ -1306,6 +1380,8 @@ static int mxc_isi_cap_enum_frameintervals(struct file *file, void *fh,
 	.vidioc_g_selection		= mxc_isi_cap_g_selection,
 	.vidioc_s_selection		= mxc_isi_cap_s_selection,
 
+	.vidioc_g_std			= mxc_isi_cap_g_std,
+
 	.vidioc_enum_framesizes = mxc_isi_cap_enum_framesizes,
 	.vidioc_enum_frameintervals = mxc_isi_cap_enum_frameintervals,
 };
diff --git a/drivers/staging/media/imx/imx8-isi-core.c b/drivers/staging/media/imx/imx8-isi-core.c
index d5c432c..e3cb85d 100644
--- a/drivers/staging/media/imx/imx8-isi-core.c
+++ b/drivers/staging/media/imx/imx8-isi-core.c
@@ -49,10 +49,18 @@ static irqreturn_t mxc_isi_irq_handler(int irq, void *priv)
 	if (status & CHNL_STS_FRM_STRD_MASK) {
 		if (mxc_isi->m2m_enabled)
 			mxc_isi_m2m_frame_write_done(mxc_isi);
-		else
+		else if (mxc_isi->frame_irq == true)
 			mxc_isi_cap_frame_write_done(mxc_isi);
 	}
 
+	if ((!mxc_isi->m2m_enabled) && ((mxc_isi->deinterlace == CHNL_IMG_CTRL_DEINT_WEAVE_ODD_EVEN) ||
+		(mxc_isi->deinterlace == CHNL_IMG_CTRL_DEINT_WEAVE_EVEN_ODD))) {
+		if (mxc_isi->frame_irq == false)
+			mxc_isi->frame_irq = true;
+		else
+			mxc_isi->frame_irq = false;
+	}
+
 	if (status & (CHNL_STS_AXI_WR_ERR_Y_MASK |
 		      CHNL_STS_AXI_WR_ERR_U_MASK |
 		      CHNL_STS_AXI_WR_ERR_V_MASK))
@@ -311,6 +319,11 @@ static int mxc_isi_parse_dt(struct mxc_isi_dev *mxc_isi)
 	if (ret < 0)
 		return ret;
 
+	of_property_read_u32(node, "deinterlace_mode", &mxc_isi->deinterlace);
+
+	mxc_isi->blending = of_property_read_bool(node, "deinterlace_blending");
+	of_property_read_u32(node, "ntsc_top", &mxc_isi->ntsc_top);
+
 	dev_dbg(dev, "%s, isi_%d,interface(%d, %d, %d)\n", __func__,
 		mxc_isi->id,
 		mxc_isi->interface[0],
@@ -516,6 +529,11 @@ static int mxc_isi_probe(struct platform_device *pdev)
 		goto err;
 	}
 
+	if (mxc_isi->deinterlace == CHNL_IMG_CTRL_DEINT_BLEND_ODD_EVEN ||
+			mxc_isi->deinterlace == CHNL_IMG_CTRL_DEINT_BLEND_EVEN_ODD) {
+		mxc_isi_set_weaving_channel(mxc_isi);
+	}
+
 	mxc_isi_channel_set_chain_buf(mxc_isi);
 
 	ret = of_platform_populate(dev->of_node, NULL, NULL, dev);
@@ -540,6 +558,11 @@ static int mxc_isi_probe(struct platform_device *pdev)
 static int mxc_isi_remove(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
+	struct mxc_isi_dev *mxc_isi = platform_get_drvdata(pdev);
+
+	if (mxc_isi->deinterlace == CHNL_IMG_CTRL_DEINT_BLEND_ODD_EVEN ||
+			mxc_isi->deinterlace == CHNL_IMG_CTRL_DEINT_BLEND_EVEN_ODD)
+		mxc_isi_clear_weaving_channel(mxc_isi);
 
 	of_platform_depopulate(dev);
 	pm_runtime_disable(dev);
diff --git a/drivers/staging/media/imx/imx8-isi-core.h b/drivers/staging/media/imx/imx8-isi-core.h
index c955ec2..4c0bc94 100644
--- a/drivers/staging/media/imx/imx8-isi-core.h
+++ b/drivers/staging/media/imx/imx8-isi-core.h
@@ -370,13 +370,21 @@ struct mxc_isi_dev {
 	u32 interface[MAX_PORTS];
 	int id;
 
+	unsigned int	deinterlace;
+	bool		frame_irq;
+	v4l2_std_id 	std_id;
+	unsigned int	ntsc_top;
+	unsigned int	bld_buf_size;
+	dma_addr_t	bld_buf_paddr;
+	void		*bld_buf_vaddr;
+
 	unsigned int hflip:1;
 	unsigned int vflip:1;
 	unsigned int cscen:1;
 	unsigned int scale:1;
 	unsigned int alphaen:1;
 	unsigned int crop:1;
-	unsigned int deinterlace:1;
+	unsigned int blending:1;
 	unsigned int is_streaming:1;
 };
 
diff --git a/drivers/staging/media/imx/imx8-isi-hw.c b/drivers/staging/media/imx/imx8-isi-hw.c
index f8e8044..f64d779 100644
--- a/drivers/staging/media/imx/imx8-isi-hw.c
+++ b/drivers/staging/media/imx/imx8-isi-hw.c
@@ -16,6 +16,8 @@
 
 #define	ISI_DOWNSCALE_THRESHOLD		0x4000
 
+static struct mxc_isi_dev *mxc_isi_weaving;
+
 #ifdef DEBUG
 void dump_isi_regs(struct mxc_isi_dev *mxc_isi)
 {
@@ -384,6 +386,13 @@ void mxc_isi_channel_set_csc(struct mxc_isi_dev *mxc_isi,
 		writel(coeffs[csc][3], mxc_isi->regs + CHNL_CSC_COEFF3);
 		writel(coeffs[csc][4], mxc_isi->regs + CHNL_CSC_COEFF4);
 		writel(coeffs[csc][5], mxc_isi->regs + CHNL_CSC_COEFF5);
+	} else {
+		writel(0, mxc_isi->regs + CHNL_CSC_COEFF0);
+		writel(0, mxc_isi->regs + CHNL_CSC_COEFF1);
+		writel(0, mxc_isi->regs + CHNL_CSC_COEFF2);
+		writel(0, mxc_isi->regs + CHNL_CSC_COEFF3);
+		writel(0, mxc_isi->regs + CHNL_CSC_COEFF4);
+		writel(0, mxc_isi->regs + CHNL_CSC_COEFF5);
 	}
 
 	writel(val, mxc_isi->regs + CHNL_IMG_CTRL);
@@ -446,9 +455,47 @@ void mxc_isi_channel_set_chain_buf(struct mxc_isi_dev *mxc_isi)
 	}
 }
 
-void mxc_isi_channel_deinterlace_init(struct mxc_isi_dev *mxc_isi)
+void mxc_isi_channel_weaving_config(struct mxc_isi_dev *mxc_isi)
 {
 	/* Config for Blending deinterlace */
+	int val;
+	struct mxc_isi_frame *src_fmt = &mxc_isi->isi_cap->src_f;
+
+	mxc_isi_channel_source_config(mxc_isi_weaving);
+
+	val = MXC_ISI_OUT_FMT_YUV422_1P8P << CHNL_IMG_CTRL_FORMAT_OFFSET |
+		mxc_isi_weaving->deinterlace << CHNL_IMG_CTRL_DEINT_OFFSET |
+		CHNL_IMG_CTRL_CSC_BYPASS_ENABLE;
+	writel(val, mxc_isi_weaving->regs + CHNL_IMG_CTRL);
+
+	if (mxc_isi_weaving->ntsc_top)
+		val = src_fmt->o_width |
+			((src_fmt->o_height + mxc_isi_weaving->ntsc_top )/ 2 << CHNL_IMG_CFG_HEIGHT_OFFSET);
+	else
+		val = src_fmt->o_width | (src_fmt->o_height / 2 << CHNL_IMG_CFG_HEIGHT_OFFSET);
+	writel(val, mxc_isi_weaving->regs + CHNL_IMG_CFG);
+
+	val = mxc_isi_weaving->bld_buf_paddr;
+	writel(val, mxc_isi_weaving->regs + CHNL_IN_BUF_ADDR);
+	writel(src_fmt->o_width * 2, mxc_isi_weaving->regs + CHNL_IN_BUF_PITCH);
+
+	val = 1 << CHNL_OUT_BUF_CTRL_LOAD_BUF1_ADDR_OFFSET;
+	writel(val, mxc_isi_weaving->regs + CHNL_OUT_BUF_CTRL);
+
+	if (mxc_isi_weaving->ntsc_top) {
+		val = mxc_isi_weaving->ntsc_top / 2;
+		writel(val, mxc_isi_weaving->regs + CHNL_CROP_ULC);
+		val = ((src_fmt->o_width - 1) << 16) |
+			((src_fmt->o_height + mxc_isi_weaving->ntsc_top) / 2 - 1);
+		writel(val, mxc_isi_weaving->regs + CHNL_CROP_LRC);
+		val = readl(mxc_isi_weaving->regs + CHNL_IMG_CTRL);
+		val &= ~CHNL_IMG_CTRL_CROP_EN_MASK;
+		val |= (CHNL_IMG_CTRL_CROP_EN_ENABLE << CHNL_IMG_CTRL_CROP_EN_OFFSET);
+		writel(val, mxc_isi_weaving->regs + CHNL_IMG_CTRL);
+	}
+
+	val = src_fmt->o_width | (src_fmt->o_height << CHNL_IMG_CFG_HEIGHT_OFFSET);
+	writel(val, mxc_isi_weaving->regs + CHNL_SCL_IMG_CFG);
 }
 
 void mxc_isi_channel_set_deinterlace(struct mxc_isi_dev *mxc_isi)
@@ -459,16 +506,30 @@ void mxc_isi_channel_set_deinterlace(struct mxc_isi_dev *mxc_isi)
 	 * Blending -----------TODO
 	 */
 	u32 val;
+	struct mxc_isi_frame *src_fmt = &mxc_isi->isi_cap->src_f;
 
 	val = readl(mxc_isi->regs + CHNL_IMG_CTRL);
 	val &= ~CHNL_IMG_CTRL_DEINT_MASK;
 	if (mxc_isi->deinterlace)
 		val |= mxc_isi->deinterlace << CHNL_IMG_CTRL_DEINT_OFFSET;
-	if (mxc_isi->deinterlace == CHNL_IMG_CTRL_DEINT_LDOUBLE_ODD_EVEN ||
-	    mxc_isi->deinterlace == CHNL_IMG_CTRL_DEINT_LDOUBLE_EVEN_ODD)
-		mxc_isi_channel_deinterlace_init(mxc_isi);
+	if (mxc_isi->blending) {
+		mxc_isi_channel_weaving_config(mxc_isi);
+		return;
+	}
 
 	writel(val, mxc_isi->regs + CHNL_IMG_CTRL);
+
+	if (mxc_isi->ntsc_top) {
+		val = mxc_isi->ntsc_top / 2;
+		writel(val, mxc_isi->regs + CHNL_CROP_ULC);
+		val = ((src_fmt->o_width - 1) << 16) |
+			((src_fmt->o_height + mxc_isi->ntsc_top) / 2 - 1);
+		writel(val, mxc_isi->regs + CHNL_CROP_LRC);
+		val = readl(mxc_isi->regs + CHNL_IMG_CTRL);
+		val &= ~CHNL_IMG_CTRL_CROP_EN_MASK;
+		val |= (CHNL_IMG_CTRL_CROP_EN_ENABLE << CHNL_IMG_CTRL_CROP_EN_OFFSET);
+		writel(val, mxc_isi->regs + CHNL_IMG_CTRL);
+	}
 }
 
 void mxc_isi_channel_set_crop(struct mxc_isi_dev *mxc_isi)
@@ -513,6 +574,21 @@ void mxc_isi_channel_set_crop(struct mxc_isi_dev *mxc_isi)
 	writel(val, mxc_isi->regs + CHNL_IMG_CTRL);
 }
 
+void mxc_isi_set_weaving_channel(struct mxc_isi_dev *mxc_isi)
+{
+	mxc_isi_weaving = mxc_isi;
+}
+
+struct mxc_isi_dev *mxc_isi_get_weaving_channel(void)
+{
+	return mxc_isi_weaving;
+}
+
+void mxc_isi_clear_weaving_channel(struct mxc_isi_dev *mxc_isi)
+{
+	mxc_isi_weaving = NULL;
+}
+
 static void mxc_isi_channel_clear_scaling(struct mxc_isi_dev *mxc_isi)
 {
 	u32 val0;
@@ -619,6 +695,13 @@ void mxc_isi_channel_init(struct mxc_isi_dev *mxc_isi)
 	val = readl(mxc_isi->regs + CHNL_CTRL);
 	val |= (CHNL_CTRL_CLK_EN_ENABLE << CHNL_CTRL_CLK_EN_OFFSET);
 	writel(val, mxc_isi->regs + CHNL_CTRL);
+
+	if ((!mxc_isi->m2m_enabled) && ((mxc_isi->deinterlace == CHNL_IMG_CTRL_DEINT_WEAVE_ODD_EVEN) ||
+		(mxc_isi->deinterlace == CHNL_IMG_CTRL_DEINT_WEAVE_EVEN_ODD))) {
+		mxc_isi->frame_irq = false;
+	} else {
+		mxc_isi->frame_irq = true;
+	}
 }
 EXPORT_SYMBOL_GPL(mxc_isi_channel_init);
 
@@ -648,8 +731,15 @@ void mxc_isi_channel_config(struct mxc_isi_dev *mxc_isi,
 	chain_buf(mxc_isi, src_f);
 
 	/* config output frame size and format */
-	val = src_f->o_width | (src_f->o_height << CHNL_IMG_CFG_HEIGHT_OFFSET);
-	writel(val, mxc_isi->regs + CHNL_IMG_CFG);
+	if ((mxc_isi->deinterlace == CHNL_IMG_CTRL_DEINT_WEAVE_ODD_EVEN) ||
+		(mxc_isi->deinterlace == CHNL_IMG_CTRL_DEINT_WEAVE_EVEN_ODD)) {
+		val = src_f->o_width | (((src_f->o_height + 1) / 2) << CHNL_IMG_CFG_HEIGHT_OFFSET);
+	} else {
+		val = src_f->o_width | (src_f->o_height << CHNL_IMG_CFG_HEIGHT_OFFSET);
+	}
+	writel(val + ((mxc_isi->ntsc_top / 2) << CHNL_IMG_CFG_HEIGHT_OFFSET), mxc_isi->regs + CHNL_IMG_CFG);
+
+	mxc_isi_channel_set_deinterlace(mxc_isi);
 
 	/* scale size need to equal input size when scaling disabled*/
 	writel(val, mxc_isi->regs + CHNL_SCL_IMG_CFG);
@@ -673,6 +763,34 @@ void mxc_isi_channel_config(struct mxc_isi_dev *mxc_isi,
 
 	mxc_isi_channel_set_panic_threshold(mxc_isi);
 
+	if (mxc_isi->blending) {
+		val = MXC_ISI_M2M_IN_FMT_YUV422_1P8P << CHNL_MEM_RD_CTRL_IMG_TYPE_OFFSET;
+		writel(val, mxc_isi->regs + CHNL_MEM_RD_CTRL);
+
+		val = mxc_isi_weaving->bld_buf_paddr;
+		writel(val, mxc_isi->regs + CHNL_IN_BUF_ADDR);
+		val = dst_f->bytesperline[0];
+		writel(src_f->o_width * 2, mxc_isi->regs + CHNL_IN_BUF_PITCH);
+		writel(0xd0000000, mxc_isi->regs + CHNL_MEM_RD_CTRL);
+		val = src_f->o_width | (src_f->o_height << CHNL_IMG_CFG_HEIGHT_OFFSET);
+		writel(val, mxc_isi->regs + CHNL_SCL_IMG_CFG);
+
+		val = readl(mxc_isi->regs + CHNL_CTRL);
+		val |= 1 << CHNL_CTRL_SRC_TYPE_OFFSET;
+		val &= ~CHNL_CTRL_SRC_INPUT_MASK;
+		val |= CHNL_CTRL_SRC_INPUT_MEMORY;
+		val &= ~(1 << CHNL_CTRL_CHNL_BYPASS_OFFSET);
+		writel(val, mxc_isi->regs + CHNL_CTRL);
+		mxc_isi->scale = 1;
+
+		val = readl(mxc_isi->regs + CHNL_IMG_CTRL);
+		val |= 1 << CHNL_IMG_CTRL_YCBCR_MODE_OFFSET;
+		writel(val, mxc_isi->regs + CHNL_IMG_CTRL);
+
+		val = 0x800 << CHNL_SCALE_OFFSET_Y_SCALE_OFFSET;
+		writel(val, mxc_isi->regs + CHNL_SCALE_OFFSET);
+	}
+
 	val = readl(mxc_isi->regs + CHNL_CTRL);
 	val &= ~CHNL_CTRL_CHNL_BYPASS_MASK;
 
@@ -710,6 +828,13 @@ void mxc_isi_channel_enable(struct mxc_isi_dev *mxc_isi, bool m2m_enabled)
 	val |= CHNL_CTRL_CHNL_EN_ENABLE << CHNL_CTRL_CHNL_EN_OFFSET;
 	writel(val, mxc_isi->regs + CHNL_CTRL);
 
+	if (mxc_isi->blending) {
+		val = readl(mxc_isi_weaving->regs + CHNL_CTRL);
+		val |= (CHNL_CTRL_CHNL_EN_ENABLE << CHNL_CTRL_CHNL_EN_OFFSET);
+		val |= 0xff << CHNL_CTRL_BLANK_PXL_OFFSET;
+		writel(val, mxc_isi_weaving->regs + CHNL_CTRL);
+	}
+
 	mxc_isi_clean_registers(mxc_isi);
 	mxc_isi_enable_irq(mxc_isi);
 
@@ -719,6 +844,8 @@ void mxc_isi_channel_enable(struct mxc_isi_dev *mxc_isi, bool m2m_enabled)
 	}
 
 	dump_isi_regs(mxc_isi);
+	if (mxc_isi->blending)
+		dump_isi_regs(mxc_isi_weaving);
 	msleep(300);
 }
 EXPORT_SYMBOL_GPL(mxc_isi_channel_enable);
@@ -734,6 +861,14 @@ void mxc_isi_channel_disable(struct mxc_isi_dev *mxc_isi)
 	val |= (CHNL_CTRL_CHNL_EN_DISABLE << CHNL_CTRL_CHNL_EN_OFFSET);
 	val |= (CHNL_CTRL_CLK_EN_DISABLE << CHNL_CTRL_CLK_EN_OFFSET);
 	writel(val, mxc_isi->regs + CHNL_CTRL);
+
+	if (mxc_isi->blending) {
+		val = readl(mxc_isi_weaving->regs + CHNL_CTRL);
+		val &= ~(CHNL_CTRL_CHNL_EN_MASK | CHNL_CTRL_CLK_EN_MASK);
+		val |= (CHNL_CTRL_CHNL_EN_DISABLE << CHNL_CTRL_CHNL_EN_OFFSET);
+		val |= (CHNL_CTRL_CLK_EN_DISABLE << CHNL_CTRL_CLK_EN_OFFSET);
+		writel(val, mxc_isi_weaving->regs + CHNL_CTRL);
+	}
 }
 EXPORT_SYMBOL_GPL(mxc_isi_channel_disable);
 
diff --git a/drivers/staging/media/imx/imx8-isi-hw.h b/drivers/staging/media/imx/imx8-isi-hw.h
index 54cde42..abdc3f7 100644
--- a/drivers/staging/media/imx/imx8-isi-hw.h
+++ b/drivers/staging/media/imx/imx8-isi-hw.h
@@ -476,6 +476,10 @@ void mxc_isi_m2m_config_dst(struct mxc_isi_dev *mxc_isi,
 void mxc_isi_disable_irq(struct mxc_isi_dev *mxc_isi);
 void dump_isi_regs(struct mxc_isi_dev *mxc_isi);
 
+void mxc_isi_set_weaving_channel(struct mxc_isi_dev *mxc_isi);
+struct mxc_isi_dev *mxc_isi_get_weaving_channel(void);
+void mxc_isi_clear_weaving_channel(struct mxc_isi_dev *mxc_isi);
+
 u32 mxc_isi_get_irq_status(struct mxc_isi_dev *mxc_isi);
 bool is_buf_active(struct mxc_isi_dev *mxc_isi, int buf_id);
 
diff --git a/drivers/staging/media/imx/imx8-mipi-csi2.c b/drivers/staging/media/imx/imx8-mipi-csi2.c
index 8881a5a..18e966b 100644
--- a/drivers/staging/media/imx/imx8-mipi-csi2.c
+++ b/drivers/staging/media/imx/imx8-mipi-csi2.c
@@ -271,6 +271,9 @@ struct mxc_mipi_csi2_dev {
 
 	struct mutex lock;
 
+	v4l2_std_id norm;
+	u8 tvdec;
+
 	int id;
 	u32 hs_settle;
 	u32 send_level;
@@ -523,6 +526,17 @@ static void mxc_mipi_csi2_csr_config(struct mxc_mipi_csi2_dev *csi2dev)
 	val = 0;
 	writel(val, csi2dev->csr_regs + CSI2SS_DATA_TYPE);
 
+	if (csi2dev->tvdec == V4L2_FIELD_INTERLACED) {
+		if (csi2dev->vchannel == true)
+			val = (CSI2SS_VC_INTERLACED_VC0 |
+				CSI2SS_VC_INTERLACED_VC1 |
+				CSI2SS_VC_INTERLACED_VC2 |
+				CSI2SS_VC_INTERLACED_VC3);
+		else
+			val = CSI2SS_VC_INTERLACED_VC0;
+		writel(val, csi2dev->csr_regs + CSI2SS_VC_INTERLACED);
+	}
+
 	/* polarity */
 	val = readl(csi2dev->csr_regs + CSI2SS_PLM_CTRL);
 	val &= ~(CSI2SS_PLM_CTRL_VSYNC_OVERRIDE |
@@ -663,6 +677,8 @@ static int mxc_csi2_get_sensor_fmt(struct mxc_mipi_csi2_dev *csi2dev)
 			csi2dev->hs_settle = rxhs_settle[1];
 	}
 
+	csi2dev->tvdec = mf->field;
+
 	return 0;
 }
 
@@ -851,6 +867,37 @@ static int mipi_csi2_s_frame_interval(struct v4l2_subdev *sd,
 	return v4l2_subdev_call(sen_sd, video, s_frame_interval, interval);
 }
 
+static int mipi_csi2_g_std(struct v4l2_subdev *sd, v4l2_std_id *a)
+{
+	struct mxc_mipi_csi2_dev *csi2dev = sd_to_mxc_mipi_csi2_dev(sd);
+	struct media_pad *source_pad;
+	struct v4l2_subdev *sen_sd;
+	int ret;
+
+	/* Get remote source pad */
+	source_pad = mxc_csi2_get_remote_sensor_pad(csi2dev);
+	if (source_pad == NULL) {
+		v4l2_err(&csi2dev->v4l2_dev, "%s, No remote pad found!\n", __func__);
+		return -EINVAL;
+	}
+
+	/* Get remote source pad subdev */
+	sen_sd = media_entity_to_v4l2_subdev(source_pad->entity);
+	if (sen_sd == NULL) {
+		v4l2_err(&csi2dev->v4l2_dev, "%s, No remote subdev found!\n", __func__);
+		return -EINVAL;
+	}
+
+	ret = v4l2_subdev_call(sen_sd, video, g_std, a);
+	if (ret < 0 && ret != -ENOIOCTLCMD) {
+		v4l2_err(&csi2dev->v4l2_dev, "%s, get remote std faile!\n", __func__);
+		return -EINVAL;
+	}
+	csi2dev->norm = *a;
+
+	return 0;
+}
+
 static int mipi_csi2_s_stream(struct v4l2_subdev *sd, int enable)
 {
 	struct mxc_mipi_csi2_dev *csi2dev = sd_to_mxc_mipi_csi2_dev(sd);
@@ -970,6 +1017,7 @@ static int mipi_csi2_set_fmt(struct v4l2_subdev *sd,
 	.g_frame_interval = mipi_csi2_g_frame_interval,
 	.s_frame_interval = mipi_csi2_s_frame_interval,
 	.s_stream	  = mipi_csi2_s_stream,
+	.g_std = mipi_csi2_g_std,
 };
 
 static struct v4l2_subdev_ops mipi_csi2_subdev_ops = {
diff --git a/drivers/staging/media/imx/imx8-parallel-csi.c b/drivers/staging/media/imx/imx8-parallel-csi.c
index 408082a..fb9e937 100644
--- a/drivers/staging/media/imx/imx8-parallel-csi.c
+++ b/drivers/staging/media/imx/imx8-parallel-csi.c
@@ -162,6 +162,8 @@ struct mxc_parallel_csi_dev {
 
 	struct mutex lock;
 
+	v4l2_std_id norm;
+
 	u8 running;
 	u8 mode;
 	u8 uv_swap;
@@ -343,23 +345,56 @@ static void mxc_pcsi_csr_config(struct mxc_parallel_csi_dev *pcsidev)
 	val = IF_CTRL_REG_PL_VALID;
 	writel(val, pcsidev->csr_regs + IF_CTRL_REG_SET);
 
-	/* Config CTRL REG */
-	val = readl(pcsidev->csr_regs + CSI_CTRL_REG);
-	val |= (CSI_CTRL_REG_DATA_TYPE_IN(DATA_TYPE_IN_UYVY_BT656_8BITS) |
-		CSI_CTRL_REG_HSYNC_POL |
-		CSI_CTRL_REG_MASK_VSYNC_COUNTER(3) |
-		CSI_CTRL_REG_HSYNC_PULSE(2));
+	if (pcsidev->tvdec == V4L2_FIELD_NONE) {
+		/* Config CTRL REG */
+		val = readl(pcsidev->csr_regs + CSI_CTRL_REG);
+		val |= (
+			CSI_CTRL_REG_DATA_TYPE_IN(DATA_TYPE_IN_YVYU_8BITS) |
+			CSI_CTRL_REG_HSYNC_POL |
+			CSI_CTRL_REG_MASK_VSYNC_COUNTER(3) |
+			CSI_CTRL_REG_HSYNC_PULSE(2));
+
+		if (pcsidev->uv_swap)
+			val |= CSI_CTRL_REG_UV_SWAP_EN;
+
+		if (pcsidev->mode == PI_GATE_CLOCK_MODE)
+			val |= CSI_CTRL_REG_GCLK_MODE_EN;
+		else if (pcsidev->mode == PI_CCIR_MODE) {
+			val |= (CSI_CTRL_REG_CCIR_EN |
+				CSI_CTRL_REG_CCIR_VSYNC_RESET_EN |
+				CSI_CTRL_REG_CCIR_EXT_VSYNC_EN |
+				CSI_CTRL_REG_CCIR_ECC_ERR_CORRECT_EN);
+		}
+	} else if (pcsidev->tvdec == V4L2_FIELD_INTERLACED) {
+		val = CSI_CTRL_REG_DATA_TYPE_IN(DATA_TYPE_IN_UYVY_BT656_8BITS) |
+			CSI_CTRL_REG_MASK_VSYNC_COUNTER(3) |
+			CSI_CTRL_REG_CCIR_EN |
+			CSI_CTRL_REG_CCIR_VIDEO_MODE;
+
+		if (pcsidev->uv_swap)
+			val |= CSI_CTRL_REG_UV_SWAP_EN;
+
+		if (pcsidev->mode == PI_GATE_CLOCK_MODE)
+			val |= CSI_CTRL_REG_GCLK_MODE_EN;
 
-	if (pcsidev->uv_swap)
-		val |= CSI_CTRL_REG_UV_SWAP_EN;
+		if (pcsidev->norm == V4L2_STD_NTSC)
+			val |= CSI_CTRL_REG_CCIR_NTSC_EN;
+	} else if (pcsidev->tvdec == V4L2_FIELD_SEQ_TB) {
+		val = CSI_CTRL_REG_DATA_TYPE_IN(DATA_TYPE_IN_UYVY_BT656_8BITS) |
+			CSI_CTRL_REG_MASK_VSYNC_COUNTER(3) |
+			CSI_CTRL_REG_CCIR_EN;
 
-	if (pcsidev->mode & PI_GATE_CLOCK_MODE) {
-		val |= CSI_CTRL_REG_GCLK_MODE_EN;
-	} else if (pcsidev->mode & PI_CCIR_MODE) {
-		val |= (CSI_CTRL_REG_CCIR_EN |
+		if (pcsidev->uv_swap)
+			val |= CSI_CTRL_REG_UV_SWAP_EN;
+
+		if (pcsidev->mode == PI_GATE_CLOCK_MODE)
+			val |= CSI_CTRL_REG_GCLK_MODE_EN;
+
+		val |= (CSI_CTRL_REG_CCIR_EXT_VSYNC_EN |
 			CSI_CTRL_REG_CCIR_VSYNC_RESET_EN |
-			CSI_CTRL_REG_CCIR_EXT_VSYNC_EN |
 			CSI_CTRL_REG_CCIR_ECC_ERR_CORRECT_EN);
+	} else {
+		;/* to do */
 	}
 
 	writel(val, pcsidev->csr_regs + CSI_CTRL_REG);
@@ -498,6 +533,8 @@ static int mxc_pcsi_get_sensor_fmt(struct mxc_parallel_csi_dev *pcsidev)
 	    mf->code == MEDIA_BUS_FMT_UYVY8_2X8)
 		pcsidev->uv_swap = 1;
 
+	pcsidev->tvdec = mf->field;
+
 	dev_dbg(&pcsidev->pdev->dev,
 		"width=%d, height=%d, fmt.code=0x%x\n",
 		mf->width, mf->height, mf->code);
@@ -617,6 +654,37 @@ static int mxc_pcsi_s_frame_interval(struct v4l2_subdev *sd,
 	return v4l2_subdev_call(sen_sd, video, s_frame_interval, interval);
 }
 
+static int mxc_pcsi_g_std(struct v4l2_subdev *sd, v4l2_std_id *a)
+{
+	struct mxc_parallel_csi_dev *pcsidev = sd_to_mxc_pcsi_dev(sd);
+	struct media_pad *source_pad;
+	struct v4l2_subdev *sen_sd;
+	int ret;
+
+	/* Get remote source pad */
+	source_pad = mxc_pcsi_get_remote_sensor_pad(pcsidev);
+	if (source_pad == NULL) {
+		v4l2_err(&pcsidev->v4l2_dev, "%s, No remote pad found!\n", __func__);
+		return -EINVAL;
+	}
+
+	/* Get remote source pad subdev */
+	sen_sd = media_entity_to_v4l2_subdev(source_pad->entity);
+	if (sen_sd == NULL) {
+		v4l2_err(&pcsidev->v4l2_dev, "%s, No remote subdev found!\n", __func__);
+		return -EINVAL;
+	}
+
+	ret = v4l2_subdev_call(sen_sd, video, g_std, a);
+	if (ret < 0 && ret != -ENOIOCTLCMD) {
+		v4l2_err(&pcsidev->v4l2_dev, "%s, get remote std faile!\n", __func__);
+		return -EINVAL;
+	}
+	pcsidev->norm = *a;
+
+	return 0;
+}
+
 static int mxc_pcsi_s_stream(struct v4l2_subdev *sd, int enable)
 {
 	struct mxc_parallel_csi_dev *pcsidev = sd_to_mxc_pcsi_dev(sd);
@@ -685,6 +753,7 @@ static int mxc_pcsi_link_setup(struct media_entity *entity,
 static struct v4l2_subdev_video_ops pcsi_video_ops = {
 	.g_frame_interval = mxc_pcsi_g_frame_interval,
 	.s_frame_interval = mxc_pcsi_s_frame_interval,
+	.g_std = mxc_pcsi_g_std,
 	.s_stream	  = mxc_pcsi_s_stream,
 };
 
-- 
1.9.1

