Add need_dphy_reset in config to indicate need keep dphy reset state during
config dphy because some SoC like i.MX93 need it.

Signed-off-by: Frank Li <Frank.Li@nxp.com>
---
 drivers/media/platform/synopsys/mipi-csi2.c | 45 +++++++++++++++++++++++++++++
 include/media/dw-mipi-csi2.h                |  1 +
 2 files changed, 46 insertions(+)

diff --git a/drivers/media/platform/synopsys/mipi-csi2.c b/drivers/media/platform/synopsys/mipi-csi2.c
index d6eb666646b916c2609a279f8badc1e8af557121..632ae5fd7da7850374c79dcff63b6ee4aee7b611 100644
--- a/drivers/media/platform/synopsys/mipi-csi2.c
+++ b/drivers/media/platform/synopsys/mipi-csi2.c
@@ -447,6 +447,45 @@ static void dw_csi2_disable_irq(struct dw_mipi_csi2_dev *csi2)
 	dw_writel(csi2, 0, int_msk_ipi_fatal);
 }
 
+static void dw_csi2_dphy_reset(struct dw_mipi_csi2_dev *csi2)
+{
+	u32 val;
+
+	/* Release synopsis DPHY test codes from reset */
+	dw_writel(csi2, 0, dphy_rstz);
+	dw_writel(csi2, 0, phy_shutdownz);
+
+	val = dw_readl(csi2, phy_tst_ctrl0);
+	val &= ~DPHY_TEST_CTRL0_TEST_CLR;
+	dw_writel(csi2, val, phy_tst_ctrl0);
+
+	/*
+	 * ndelay is not necessary have MMIO operation, need dummy read to make
+	 * sure above write reach target.
+	 */
+	val = dw_readl(csi2, phy_tst_ctrl0);
+	/* Wait for at least 15ns */
+	ndelay(15);
+	val |= DPHY_TEST_CTRL0_TEST_CLR;
+	dw_writel(csi2, val, phy_tst_ctrl0);
+}
+
+static void dw_csi2_dphy_release_reset(struct dw_mipi_csi2_dev *csi2)
+{
+	/* Release PHY from reset */
+	dw_writel(csi2, 0x1, phy_shutdownz);
+	/*
+	 * ndelay is not necessary have MMIO operation, need dummy read to make
+	 * sure above write reach target.
+	 */
+	dw_readl(csi2, phy_shutdownz);
+	ndelay(5);
+	dw_writel(csi2, 0x1, dphy_rstz);
+
+	dw_readl(csi2, dphy_rstz);
+	ndelay(5);
+}
+
 static int csi2_start(struct dw_mipi_csi2_dev *csi2, int bpp)
 {
 	unsigned int lanes;
@@ -460,6 +499,9 @@ static int csi2_start(struct dw_mipi_csi2_dev *csi2, int bpp)
 	if (ret)
 		goto err_phy_prep;
 
+	if (csi2->config->need_dphy_reset)
+		dw_csi2_dphy_reset(csi2);
+
 	/* setup the gasket */
 	if (csi2->config && csi2->config->gasket_init)
 		csi2->config->gasket_init(csi2);
@@ -482,6 +524,9 @@ static int csi2_start(struct dw_mipi_csi2_dev *csi2, int bpp)
 	if (ret)
 		goto err_phy_power_on;
 
+	if (csi2->config->need_dphy_reset)
+		dw_csi2_dphy_release_reset(csi2);
+
 	csi2_enable(csi2, true);
 
 	/* Step 5 */
diff --git a/include/media/dw-mipi-csi2.h b/include/media/dw-mipi-csi2.h
index 20ce6a21f0674cad54f01edb508dce14cd07a8dd..d1afd8c6d95457c28d7e1de6935908437372572d 100644
--- a/include/media/dw-mipi-csi2.h
+++ b/include/media/dw-mipi-csi2.h
@@ -28,6 +28,7 @@ struct dw_mipi_csi2_config {
 	u32	sink_pad_mask;
 	bool	has_phy: 1;
 	bool	has_irq: 1;
+	bool	need_dphy_reset: 1;
 };
 
 struct dw_mipi_tstif {

-- 
2.34.1