[PATCH v3 19/31] media: synopsys: csi2: Add phy interface support

@@ -14,6 +14,7 @@
 #include <linux/of_graph.h>
 #include <linux/platform_device.h>
 #include <media/dw-mipi-csi2.h>
+#include <media/mipi-csi2.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>

@@ -268,7 +269,63 @@ static int csi2_get_active_lanes(struct dw_mipi_csi2_dev *csi2, unsigned int *la
 	return 0;
 }
 
-static int csi2_start(struct dw_mipi_csi2_dev *csi2)
+static int dw_csi2_get_dphy_configuration(struct dw_mipi_csi2_dev *csi2,
+					  union phy_configure_opts *opts,
+					  int bpp)
+{
+	struct phy_configure_opts_mipi_dphy *cfg = &opts->mipi_dphy;
+	struct v4l2_subdev *source = csi2->src_sd;
+	s64 link_freq;
+
+	link_freq = v4l2_get_link_freq(source->ctrl_handler,
+				       bpp,
+				       csi2->data_lanes * 2);
+	if (link_freq < 0) {
+		dev_err(csi2->dev, "Unable to obtain link frequency: %d\n",
+			(int)link_freq);
+		return link_freq;
+	}
+
+	memset(cfg, 0x0, sizeof(*cfg));
+	cfg->hs_clk_rate = link_freq * 2;
+	cfg->lanes = csi2->data_lanes;
+
+	return 0;
+}
+
+static int dw_mipi_csi2_phy_prep(struct dw_mipi_csi2_dev *csi2, int bpp)
+{
+	union phy_configure_opts opts;
+	int ret;
+
+	ret = dw_csi2_get_dphy_configuration(csi2, &opts, bpp);
+	if (ret)
+		return ret;
+
+	ret = phy_init(csi2->phy);
+	if (ret)
+		return ret;
+
+	ret = phy_reset(csi2->phy);
+	if (ret)
+		goto exit;
+
+	ret = phy_set_mode(csi2->phy, PHY_MODE_MIPI_DPHY);
+	if (ret)
+		goto exit;
+
+	ret = phy_configure(csi2->phy, &opts);
+	if (ret)
+		goto exit;
+
+	return 0;
+
+exit:
+	phy_exit(csi2->phy);
+	return ret;
+}
+
+static int csi2_start(struct dw_mipi_csi2_dev *csi2, int bpp)
 {
 	unsigned int lanes;
 	int ret;

@@ -277,6 +334,10 @@ static int csi2_start(struct dw_mipi_csi2_dev *csi2)
 	if (ret)
 		return ret;
 
+	ret = dw_mipi_csi2_phy_prep(csi2, bpp);
+	if (ret)
+		goto err_phy_prep;
+
 	/* setup the gasket */
 	if (csi2->config && csi2->config->gasket_init)
 		csi2->config->gasket_init(csi2);

@@ -285,15 +346,20 @@ static int csi2_start(struct dw_mipi_csi2_dev *csi2)
 	if (csi2->config && csi2->config->dphy_init) {
 		ret = csi2->config->dphy_init(csi2);
 		if (ret)
-			goto err_disable_clk;
+			goto err_dphy_init;
 	}
 
 	ret = csi2_get_active_lanes(csi2, &lanes);
 	if (ret)
-		goto err_disable_clk;
+		goto err_active_lanes;
 
 	/* Step 4 */
 	csi2_set_lanes(csi2, lanes);
+
+	ret = phy_power_on(csi2->phy);
+	if (ret)
+		goto err_phy_power_on;
+
 	csi2_enable(csi2, true);
 
 	/* Step 5 */

@@ -322,13 +388,21 @@ static int csi2_start(struct dw_mipi_csi2_dev *csi2)
 	v4l2_subdev_call(csi2->src_sd, video, post_streamoff);
 err_assert_reset:
 	csi2_enable(csi2, false);
-err_disable_clk:
+err_phy_power_on:
+	phy_power_off(csi2->phy);
+err_active_lanes:
+err_dphy_init:
+	phy_exit(csi2->phy);
+err_phy_prep:
 	pm_runtime_put(csi2->dev);
 	return ret;
 }
 
 static void csi2_stop(struct dw_mipi_csi2_dev *csi2)
 {
+	phy_power_off(csi2->phy);
+	phy_exit(csi2->phy);
+
 	/* stop upstream */
 	v4l2_subdev_call(csi2->src_sd, video, s_stream, 0);
 	v4l2_subdev_call(csi2->src_sd, video, post_streamoff);

@@ -364,7 +438,7 @@ static int dw_csi2_enable_streams(struct v4l2_subdev *sd,
 	if (csi2->stream_count)
 		return 0;
 
-	ret = csi2_start(csi2);
+	ret = csi2_start(csi2, media_bus_fmt_to_csi2_bpp(csi2->format_mbus.code));
 	if (ret)
 		return ret;
 

@@ -682,6 +756,13 @@ int dw_mipi_csi2_init(struct platform_device *pdev, struct dw_mipi_csi2_dev *csi
 	if (csi2->num_clks < 0)
 		return dev_err_probe(&pdev->dev, csi2->num_clks, "Failed to get clocks\n");
 
+	if (config->has_phy) {
+		csi2->phy = devm_phy_get(&pdev->dev, "rx");
+		if (IS_ERR(csi2->phy))
+			return dev_err_probe(&pdev->dev, PTR_ERR(csi2->phy),
+					     "Failed to get DPHY Rx\n");
+	}
+
 	devm_pm_runtime_enable(&pdev->dev);
 
 	return csi2_async_register(csi2);

@@ -3,6 +3,7 @@
 #ifndef __DW_MIPI_CSI2_COMMON_
 #define __DW_MIPI_CSI2_COMMON_
 
+#include <linux/phy/phy.h>
 #include <linux/pm_runtime.h>
 
 #include <media/v4l2-common.h>

@@ -25,6 +26,7 @@ struct dw_mipi_csi2_config {
 	int (*dphy_init)(struct dw_mipi_csi2_dev *dev);
 	u32	num_pads;	/* Max 64 pad now */
 	u32	sink_pad_mask;
+	bool	has_phy: 1;
 };
 
 struct dw_mipi_tstif {

@@ -56,6 +58,8 @@ struct dw_mipi_csi2_dev {
 	struct clk_bulk_data		*clks;
 	int				num_clks;
 
+	struct phy			*phy;
+
 	struct v4l2_subdev		*remote;
 	unsigned int			remote_pad;
 	unsigned short			data_lanes;