From 022144524ab98e3e0824c2d9071f76ad914a376d Mon Sep 17 00:00:00 2001
From: Zoey <zoey.li@nxp.com>
Date: Fri, 14 Oct 2022 15:21:09 +0000
Subject: [PATCH] Add ISI weaving deinterlace support for i.MX8MP.

Signed-off-by: Zoey <zoey.li@nxp.com>
---
 arch/arm64/boot/dts/freescale/imx8mp-evk.dts  |  2 ++
 drivers/media/i2c/ov5640.c                    | 17 ++++++++++++
 drivers/staging/media/imx/imx8-isi-cap.c      | 14 +++++++++-
 drivers/staging/media/imx/imx8-isi-core.c     | 22 ++++++++++++++--
 drivers/staging/media/imx/imx8-isi-core.h     |  5 +++-
 drivers/staging/media/imx/imx8-isi-hw.c       | 26 +++++++++++++++----
 .../staging/media/imx/imx8-mipi-csi2-sam.c    | 10 +++++++
 7 files changed, 87 insertions(+), 9 deletions(-)

diff --git a/arch/arm64/boot/dts/freescale/imx8mp-evk.dts b/arch/arm64/boot/dts/freescale/imx8mp-evk.dts
index 03783ece0fcb..bd3c8c89df26 100644
--- a/arch/arm64/boot/dts/freescale/imx8mp-evk.dts
+++ b/arch/arm64/boot/dts/freescale/imx8mp-evk.dts
@@ -534,6 +534,7 @@ ov5640_0: ov5640_mipi@3c {
 		mclk = <24000000>;
 		mclk_source = <0>;
 		mipi_csi;
+		weaving_deinterlace_test;
 		status = "okay";
 
 		port {
@@ -1356,6 +1357,7 @@ &cameradev {
 };
 
 &isi_0 {
+	deinterlace_mode = <2>;
 	status = "okay";
 
 	cap_device {
diff --git a/drivers/media/i2c/ov5640.c b/drivers/media/i2c/ov5640.c
index 699a1e45b9c8..6922246887ae 100644
--- a/drivers/media/i2c/ov5640.c
+++ b/drivers/media/i2c/ov5640.c
@@ -108,6 +108,7 @@ enum ov5640_mode_id {
 	OV5640_MODE_720P_1280_720,
 	OV5640_MODE_1080P_1920_1080,
 	OV5640_MODE_QSXGA_2592_1944,
+	TEST_MODE_1280_1440,
 	OV5640_NUM_MODES,
 };
 
@@ -259,6 +260,7 @@ struct ov5640_dev {
 
 	bool pending_mode_change;
 	bool streaming;
+	bool interlaced;
 };
 
 static inline struct ov5640_dev *to_ov5640_dev(struct v4l2_subdev *sd)
@@ -642,6 +644,10 @@ ov5640_mode_data[OV5640_NUM_MODES] = {
 	 ov5640_setting_QSXGA_2592_1944,
 	 ARRAY_SIZE(ov5640_setting_QSXGA_2592_1944),
 	 OV5640_15_FPS},
+	{TEST_MODE_1280_1440, SCALING,
+	1280, 1892, 1440, 740,
+	0,
+	OV5640_15_FPS},
 };
 
 static int ov5640_init_slave_id(struct ov5640_dev *sensor)
@@ -999,6 +1005,8 @@ static int ov5640_check_valid_mode(struct ov5640_dev *sensor,
 		if (rate != OV5640_08_FPS)
 			ret = -EINVAL;
 		break;
+	case TEST_MODE_1280_1440:
+		break;
 	default:
 		dev_err(&client->dev, "Invalid mode (%d)\n", mode->id);
 		ret = -EINVAL;
@@ -2330,6 +2338,13 @@ static int ov5640_get_fmt(struct v4l2_subdev *sd,
 	else
 		fmt = &sensor->fmt;
 
+	if (sensor->interlaced)
+		fmt->field = V4L2_FIELD_INTERLACED;
+	else
+		fmt->field = V4L2_FIELD_NONE;
+
+	printk("%s, wxh=%dx%d\n", __func__, fmt->width, fmt->height);
+
 	fmt->reserved[1] = (sensor->current_fr == OV5640_30_FPS) ? 30 : 15;
 	format->format = *fmt;
 
@@ -3238,6 +3253,8 @@ static int ov5640_probe(struct i2c_client *client)
 		return -EINVAL;
 	}
 
+	sensor->interlaced = of_property_read_bool(dev->of_node, "weaving_deinterlace_test");
+
 	/* get system clock (xclk) */
 	sensor->xclk = devm_clk_get(dev, "xclk");
 	if (IS_ERR(sensor->xclk)) {
diff --git a/drivers/staging/media/imx/imx8-isi-cap.c b/drivers/staging/media/imx/imx8-isi-cap.c
index 6f83ba47f919..9d4c33a217d8 100644
--- a/drivers/staging/media/imx/imx8-isi-cap.c
+++ b/drivers/staging/media/imx/imx8-isi-cap.c
@@ -928,6 +928,7 @@ static int mxc_isi_cap_try_fmt_mplane(struct file *file, void *fh,
 /* Update input frame size and formate  */
 static int mxc_isi_source_fmt_init(struct mxc_isi_cap_dev *isi_cap)
 {
+	struct mxc_isi_dev *mxc_isi = mxc_isi_get_hostdata(isi_cap->pdev);
 	struct mxc_isi_frame *src_f = &isi_cap->src_f;
 	struct mxc_isi_frame *dst_f = &isi_cap->dst_f;
 	struct v4l2_subdev_format src_fmt;
@@ -950,7 +951,12 @@ static int mxc_isi_source_fmt_init(struct mxc_isi_cap_dev *isi_cap)
 	src_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 	src_fmt.format.code = MEDIA_BUS_FMT_UYVY8_2X8;
 	src_fmt.format.width = dst_f->width;
-	src_fmt.format.height = dst_f->height;
+	if ((mxc_isi->deinterlace == CHNL_IMG_CTRL_DEINT_WEAVE_ODD_EVEN) ||
+			(mxc_isi->deinterlace == CHNL_IMG_CTRL_DEINT_WEAVE_EVEN_ODD))
+		src_fmt.format.height = dst_f->height / 2;
+	else
+		src_fmt.format.height = dst_f->height;
+	printk("%s, set src to wxh=%dx%d\n", __func__, src_fmt.format.width, src_fmt.format.height);
 	ret = v4l2_subdev_call(src_sd, pad, set_fmt, NULL, &src_fmt);
 	if (ret < 0 && ret != -ENOIOCTLCMD) {
 		v4l2_err(&isi_cap->sd, "set remote fmt fail!\n");
@@ -968,9 +974,15 @@ static int mxc_isi_source_fmt_init(struct mxc_isi_cap_dev *isi_cap)
 
 	/* Pixel link master will transfer format to RGB32 or YUV32 */
 	src_f->fmt = mxc_isi_get_src_fmt(&src_fmt);
+	if ((mxc_isi->deinterlace == CHNL_IMG_CTRL_DEINT_WEAVE_ODD_EVEN) ||
+			(mxc_isi->deinterlace == CHNL_IMG_CTRL_DEINT_WEAVE_EVEN_ODD))
+		src_fmt.format.height *= 2;
 
 	set_frame_bounds(src_f, src_fmt.format.width, src_fmt.format.height);
 
+	printk("%s, src:wxh=%dx%d, dst:wxh=%dx%d\n", __func__, src_f->width, src_f->height,
+			dst_f->width, dst_f->height);
+
 	if (dst_f->width > src_f->width || dst_f->height > src_f->height) {
 		dev_err(&isi_cap->pdev->dev,
 			"%s: src:(%d,%d), dst:(%d,%d) Not support upscale\n",
diff --git a/drivers/staging/media/imx/imx8-isi-core.c b/drivers/staging/media/imx/imx8-isi-core.c
index 8f71f263112e..9da73acf8404 100644
--- a/drivers/staging/media/imx/imx8-isi-core.c
+++ b/drivers/staging/media/imx/imx8-isi-core.c
@@ -51,13 +51,25 @@ static irqreturn_t mxc_isi_irq_handler(int irq, void *priv)
 
 	status = mxc_isi_get_irq_status(mxc_isi);
 	mxc_isi->status = status;
+	//printk("%s, status=0x%x\n", __func__, status);
 	mxc_isi_clean_irq_status(mxc_isi, status);
 
 	if (status & CHNL_STS_FRM_STRD_MASK) {
 		if (mxc_isi->m2m_enabled)
 			mxc_isi_m2m_frame_write_done(mxc_isi);
-		else
-			mxc_isi_cap_frame_write_done(mxc_isi);
+		else {
+			if (mxc_isi->deinterlace == 0 || mxc_isi->deinterlace == 1)
+				mxc_isi_cap_frame_write_done(mxc_isi);
+			else if ((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 == true)
+					mxc_isi_cap_frame_write_done(mxc_isi);
+				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 |
@@ -618,6 +630,12 @@ static int mxc_isi_parse_dt(struct mxc_isi_dev *mxc_isi)
 		mxc_isi->interface[0],
 		mxc_isi->interface[1],
 		mxc_isi->interface[2]);
+
+	ret = of_property_read_u32(node, "deinterlace_mode", &mxc_isi->deinterlace);
+	if (ret < 0)
+		mxc_isi->deinterlace = 0;
+	printk("%s, deinterlace_mode=%d\n", __func__, mxc_isi->deinterlace);
+
 	return 0;
 }
 
diff --git a/drivers/staging/media/imx/imx8-isi-core.h b/drivers/staging/media/imx/imx8-isi-core.h
index 911b23698319..238525215196 100644
--- a/drivers/staging/media/imx/imx8-isi-core.h
+++ b/drivers/staging/media/imx/imx8-isi-core.h
@@ -394,8 +394,11 @@ struct mxc_isi_dev {
 	unsigned int scale:1;
 	unsigned int alphaen:1;
 	unsigned int crop:1;
-	unsigned int deinterlace:3;
+	//unsigned int deinterlace:3;
 	unsigned int is_streaming:1;
+
+	unsigned int deinterlace;
+	bool frame_irq;
 };
 
 static inline void set_frame_bounds(struct mxc_isi_frame *f,
diff --git a/drivers/staging/media/imx/imx8-isi-hw.c b/drivers/staging/media/imx/imx8-isi-hw.c
index 193ff5c5fd33..e47ed3ebc2be 100644
--- a/drivers/staging/media/imx/imx8-isi-hw.c
+++ b/drivers/staging/media/imx/imx8-isi-hw.c
@@ -444,11 +444,11 @@ void mxc_isi_channel_set_deinterlace(struct mxc_isi_dev *mxc_isi)
 
 	val = readl(mxc_isi->regs + CHNL_IMG_CTRL);
 	val &= ~CHNL_IMG_CTRL_DEINT_MASK;
-	if (mxc_isi->deinterlace)
+	if ((mxc_isi->deinterlace == CHNL_IMG_CTRL_DEINT_WEAVE_ODD_EVEN) ||
+			(mxc_isi->deinterlace == CHNL_IMG_CTRL_DEINT_WEAVE_EVEN_ODD))
 		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);
+	else
+		printk("only support weaving deinterlace currently\n");
 
 	writel(val, mxc_isi->regs + CHNL_IMG_CTRL);
 }
@@ -619,12 +619,28 @@ 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);
+	printk("%s, src_f->o_width=%d, src_f->o_height=%d\n", __func__,
+			src_f->o_width, src_f->o_height);
+	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 / 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->regs + CHNL_IMG_CFG);
 
+	val = src_f->o_width | (src_f->o_height << CHNL_IMG_CFG_HEIGHT_OFFSET);
 	/* scale size need to equal input size when scaling disabled*/
 	writel(val, mxc_isi->regs + CHNL_SCL_IMG_CFG);
 
+	/* set deinterlace */
+	if (mxc_isi->deinterlace) {
+		mxc_isi_channel_set_deinterlace(mxc_isi);
+		mxc_isi->frame_irq = false;
+	}
+
 	/* check csc and scaling  */
 	mxc_isi_channel_set_csc(mxc_isi, src_f, dst_f);
 
diff --git a/drivers/staging/media/imx/imx8-mipi-csi2-sam.c b/drivers/staging/media/imx/imx8-mipi-csi2-sam.c
index 1d12365520a6..ddfe30fb2a0a 100644
--- a/drivers/staging/media/imx/imx8-mipi-csi2-sam.c
+++ b/drivers/staging/media/imx/imx8-mipi-csi2-sam.c
@@ -228,6 +228,9 @@
 
 /* display_mix_clk_en_csr */
 #define DISP_MIX_GASKET_0_CTRL			0x00
+#define GASKET_0_CTRL_INTERLACE_MODE(x)		(((x) & (0x3)) << 6)
+#define GASKET_0_CTRL_INTERLACE_MODE_MASK	((0x3UL) << (6))
+
 #define GASKET_0_CTRL_DATA_TYPE(x)		(((x) & (0x3F)) << 8)
 #define GASKET_0_CTRL_DATA_TYPE_MASK		((0x3FUL) << (8))
 
@@ -988,6 +991,13 @@ static void disp_mix_gasket_config(struct csi_state *state)
 	if (fmt_val == GASKET_0_CTRL_DATA_TYPE_YUV422_8)
 		val |= GASKET_0_CTRL_DUAL_COMP_ENABLE;
 	val |= GASKET_0_CTRL_DATA_TYPE(fmt_val);
+
+	val &= ~GASKET_0_CTRL_INTERLACE_MODE_MASK;
+
+	printk("%s, field interlaced=%d\n", __func__, (state->format.field == V4L2_FIELD_INTERLACED? 1 : 0));
+	if (state->format.field == V4L2_FIELD_INTERLACED)
+		val |= GASKET_0_CTRL_INTERLACE_MODE(0x2);
+
 	regmap_write(gasket, DISP_MIX_GASKET_0_CTRL, val);
 
 	if (WARN_ON(!mf->width || !mf->height))
-- 
2.25.1

