[PATCH v2 16/32] media: staging: media: imx6-mipi-csi2: move dphy init part to imx6-csi2.c

[Frank Li <Frank.Li@nxp.com>]

Move dphy init part to imx6-csi.c.

Prepare create common library for dw MIPI CSI2 controller.

Add callback for dphy_init in struct dw_mipi_csi2_config. New driver should
use phy interface.

Signed-off-by: Frank Li <Frank.Li@nxp.com>
---
 drivers/staging/media/imx/imx6-csi2.c      | 55 +++++++++++++++++++++++
 drivers/staging/media/imx/imx6-mipi-csi2.c | 72 ++++++------------------------
 include/media/dw-mipi-csi2.h               | 21 +++++++++
 3 files changed, 89 insertions(+), 59 deletions(-)

diff --git a/drivers/staging/media/imx/imx6-csi2.c b/drivers/staging/media/imx/imx6-csi2.c
index 062db72a36ef294566272d9f39d82e9414640e9a..696644aa04c83b1f2877b2b2315202627d927e96 100644
--- a/drivers/staging/media/imx/imx6-csi2.c
+++ b/drivers/staging/media/imx/imx6-csi2.c
@@ -16,6 +16,12 @@
 #include <media/dw-mipi-csi2.h>
 #include "imx-media.h"
 
+/*
+ * The default maximum bit-rate per lane in Mbps, if the
+ * source subdev does not provide V4L2_CID_LINK_FREQ.
+ */
+#define CSI2_DEFAULT_MAX_MBPS	849
+
 /*
  * i.MX CSI2IPU Gasket registers follow. The CSI2IPU gasket is
  * not part of the MIPI CSI-2 core, but its registers fall in the
@@ -28,6 +34,54 @@ struct imx6_csi2 {
 	struct dw_mipi_csi2_dev dw;
 };
 
+static const struct {
+	u32 max_mbps;
+	u32 hsfreqrange_sel;
+} hsfreq_map[] = {
+	{ 90, 0x00}, {100, 0x20}, {110, 0x40}, {125, 0x02},
+	{140, 0x22}, {150, 0x42}, {160, 0x04}, {180, 0x24},
+	{200, 0x44}, {210, 0x06}, {240, 0x26}, {250, 0x46},
+	{270, 0x08}, {300, 0x28}, {330, 0x48}, {360, 0x2a},
+	{400, 0x4a}, {450, 0x0c}, {500, 0x2c}, {550, 0x0e},
+	{600, 0x2e}, {650, 0x10}, {700, 0x30}, {750, 0x12},
+	{800, 0x32}, {850, 0x14}, {900, 0x34}, {950, 0x54},
+	{1000, 0x74},
+};
+
+static int max_mbps_to_hsfreqrange_sel(u32 max_mbps)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(hsfreq_map); i++)
+		if (hsfreq_map[i].max_mbps > max_mbps)
+			return hsfreq_map[i].hsfreqrange_sel;
+
+	return -EINVAL;
+}
+
+static int csi2_dphy_init(struct dw_mipi_csi2_dev *csi2)
+{
+	struct v4l2_ctrl *ctrl;
+	u32 mbps_per_lane;
+	int sel;
+
+	ctrl = v4l2_ctrl_find(csi2->src_sd->ctrl_handler,
+			      V4L2_CID_LINK_FREQ);
+	if (!ctrl)
+		mbps_per_lane = CSI2_DEFAULT_MAX_MBPS;
+	else
+		mbps_per_lane = DIV_ROUND_UP_ULL(2 * ctrl->qmenu_int[ctrl->val],
+						 USEC_PER_SEC);
+
+	sel = max_mbps_to_hsfreqrange_sel(mbps_per_lane);
+	if (sel < 0)
+		return sel;
+
+	dw_mipi_csi2_tstif_write(csi2, 0x44, sel);
+
+	return 0;
+}
+
 /* Setup the i.MX CSI2IPU Gasket */
 static void csi2ipu_gasket_init(struct dw_mipi_csi2_dev *csi2)
 {
@@ -74,6 +128,7 @@ static const struct dw_mipi_csi2_config imx6_config = {
 	.module = THIS_MODULE,
 	.name = "imx6-mipi-csi2",
 	.internal_ops = &csi2_internal_ops,
+	.dphy_init = csi2_dphy_init,
 	.grp_id = IMX_MEDIA_GRP_ID_CSI2,
 	.gasket_init = csi2ipu_gasket_init,
 	.num_pads = CSI2_NUM_PADS,
diff --git a/drivers/staging/media/imx/imx6-mipi-csi2.c b/drivers/staging/media/imx/imx6-mipi-csi2.c
index 546c6e7c5d13f3be618fc3eeca780695ead6bfe5..23656291d808ccdfc47f36d1f7f5104698812eba 100644
--- a/drivers/staging/media/imx/imx6-mipi-csi2.c
+++ b/drivers/staging/media/imx/imx6-mipi-csi2.c
@@ -21,12 +21,6 @@
 #include <media/v4l2-mc.h>
 #include <media/v4l2-subdev.h>
 
-/*
- * The default maximum bit-rate per lane in Mbps, if the
- * source subdev does not provide V4L2_CID_LINK_FREQ.
- */
-#define CSI2_DEFAULT_MAX_MBPS	849
-
 struct dw_csi2_regs {
 	u32	version;
 	u32	n_lanes;
@@ -146,9 +140,11 @@ static void csi2_set_lanes(struct dw_mipi_csi2_dev *csi2, unsigned int lanes)
 	dw_writel(csi2, lanes - 1, n_lanes);
 }
 
-static void dw_mipi_csi2_phy_write(struct dw_mipi_csi2_dev *csi2,
-				   u32 test_code, u32 test_data)
+static int dw_mipi_csi2_phy_write(struct dw_mipi_tstif *tstif,
+				  u32 test_code, u32 test_data)
 {
+	struct dw_mipi_csi2_dev *csi2 = container_of(tstif, struct dw_mipi_csi2_dev, tstif);
+
 	/* Clear PHY test interface */
 	dw_writel(csi2, PHY_TESTCLR, phy_tst_ctrl0);
 	dw_writel(csi2, 0x0, phy_tst_ctrl1);
@@ -167,6 +163,8 @@ static void dw_mipi_csi2_phy_write(struct dw_mipi_csi2_dev *csi2,
 
 	/* Clear strobe signal */
 	dw_writel(csi2, 0x0, phy_tst_ctrl0);
+
+	return 0;
 }
 
 /*
@@ -174,54 +172,6 @@ static void dw_mipi_csi2_phy_write(struct dw_mipi_csi2_dev *csi2,
  * https://community.nxp.com/docs/DOC-94312. It assumes
  * a 27MHz D-PHY pll reference clock.
  */
-static const struct {
-	u32 max_mbps;
-	u32 hsfreqrange_sel;
-} hsfreq_map[] = {
-	{ 90, 0x00}, {100, 0x20}, {110, 0x40}, {125, 0x02},
-	{140, 0x22}, {150, 0x42}, {160, 0x04}, {180, 0x24},
-	{200, 0x44}, {210, 0x06}, {240, 0x26}, {250, 0x46},
-	{270, 0x08}, {300, 0x28}, {330, 0x48}, {360, 0x2a},
-	{400, 0x4a}, {450, 0x0c}, {500, 0x2c}, {550, 0x0e},
-	{600, 0x2e}, {650, 0x10}, {700, 0x30}, {750, 0x12},
-	{800, 0x32}, {850, 0x14}, {900, 0x34}, {950, 0x54},
-	{1000, 0x74},
-};
-
-static int max_mbps_to_hsfreqrange_sel(u32 max_mbps)
-{
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(hsfreq_map); i++)
-		if (hsfreq_map[i].max_mbps > max_mbps)
-			return hsfreq_map[i].hsfreqrange_sel;
-
-	return -EINVAL;
-}
-
-static int csi2_dphy_init(struct dw_mipi_csi2_dev *csi2)
-{
-	struct v4l2_ctrl *ctrl;
-	u32 mbps_per_lane;
-	int sel;
-
-	ctrl = v4l2_ctrl_find(csi2->src_sd->ctrl_handler,
-			      V4L2_CID_LINK_FREQ);
-	if (!ctrl)
-		mbps_per_lane = CSI2_DEFAULT_MAX_MBPS;
-	else
-		mbps_per_lane = DIV_ROUND_UP_ULL(2 * ctrl->qmenu_int[ctrl->val],
-						 USEC_PER_SEC);
-
-	sel = max_mbps_to_hsfreqrange_sel(mbps_per_lane);
-	if (sel < 0)
-		return sel;
-
-	dw_mipi_csi2_phy_write(csi2, 0x44, sel);
-
-	return 0;
-}
-
 /*
  * Waits for ultra-low-power state on D-PHY clock lane. This is currently
  * unused and may not be needed at all, but keep around just in case.
@@ -332,9 +282,11 @@ static int csi2_start(struct dw_mipi_csi2_dev *csi2)
 		csi2->config->gasket_init(csi2);
 
 	/* Step 3 */
-	ret = csi2_dphy_init(csi2);
-	if (ret)
-		goto err_disable_clk;
+	if (csi2->config && csi2->config->dphy_init) {
+		ret = csi2->config->dphy_init(csi2);
+		if (ret)
+			goto err_disable_clk;
+	}
 
 	ret = csi2_get_active_lanes(csi2, &lanes);
 	if (ret)
@@ -680,6 +632,8 @@ int dw_mipi_csi2_init(struct platform_device *pdev, struct dw_mipi_csi2_dev *csi
 	csi2->sd.grp_id = config->grp_id;
 	csi2->config = config;
 
+	csi2->tstif.write = dw_mipi_csi2_phy_write;
+
 	if (!config)
 		return -EINVAL;
 
diff --git a/include/media/dw-mipi-csi2.h b/include/media/dw-mipi-csi2.h
index 12e1db1d149513fcd0db8c191c801cb144d18143..935c664440aae82d69f0253a551b91ec4ff2724e 100644
--- a/include/media/dw-mipi-csi2.h
+++ b/include/media/dw-mipi-csi2.h
@@ -20,10 +20,26 @@ struct dw_mipi_csi2_config {
 	const struct v4l2_subdev_internal_ops *internal_ops;
 	/* Deprecated, should go through phy interface */
 	void (*gasket_init)(struct dw_mipi_csi2_dev *dev);
+	int (*dphy_init)(struct dw_mipi_csi2_dev *dev);
 	u32	num_pads;	/* Max 64 pad now */
 	u32	sink_pad_mask;
 };
 
+struct dw_mipi_tstif {
+	int (*write)(struct dw_mipi_tstif *tstif, u32 indice, u32 data);
+};
+
+static inline int dw_mipi_tstif_write(struct dw_mipi_tstif *tstif, u32 indice, u32 data)
+{
+	if (!tstif)
+		return -EINVAL;
+
+	if (!tstif->write)
+		return -EINVAL;
+
+	return  tstif->write(tstif, indice, data);
+}
+
 struct dw_mipi_csi2_dev {
 	struct device			*dev;
 	struct v4l2_subdev		sd;
@@ -51,8 +67,13 @@ struct dw_mipi_csi2_dev {
 	struct v4l2_subdev		*src_sd;
 	bool				sink_linked[DW_MAX_PAD_NUM];
 	const struct dw_mipi_csi2_config *config;
+
+	struct dw_mipi_tstif		tstif;
 };
 
+#define dw_mipi_csi2_tstif_write(csi2, indice, data)		\
+dw_mipi_tstif_write(&(csi2)->tstif, indice, data)
+
 static inline struct dw_mipi_csi2_dev *
 sd_to_dw_mipi_csi2_dev(struct v4l2_subdev *sd)
 {

-- 
2.34.1