[PATCH] media: imx: mipi csi-2: Release DPHY reset in s_power

[Steve Longerbeam]

Some CSI-2 transmitters may only set their data lanes to LP-11 state for
a very short time after being powered on, after which they transition
the lanes to high speed mode.

If that occurs, the lanes will likely be in HS mode long before the imx6
DPHY receiver is initialized and brought out of reset at stream on.
According to the imx6 reference manual, the lanes need to be in LP-11
state at least for some period after the DPHY reset is released. This
might mean that the state machine in the DPHY core (a Synopsys DesignWare
core) needs to detect a LP-11 to HS transition on the lanes before it can
proceed to detect a clock on the clock lane and begin to accept packets.

In an attempt to accommodate such sensors, carry out steps 1-5 in the
s_power op (moved out of s_stream op). This moves steps 1-5 closer in
time to after the sensor is powered ON by v4l2_pipeline_pm_use(), and
provides a better chance that the receiver DPHY will catch an early
LP-11 to HS transition.

This works because the graph walk stack used by v4l2_pipeline_pm_use()
is setup such that the transmitter s_power op is called immediately
before the receiver's s_power op.

For sensors that can persist LP-11 state until stream on, then this
should still work fine. The receiver will detect the HS transition
at step 6, when streaming is enabled at the transmitter.

Tested on imx6q SabreSD with OV5640.

Signed-off-by: Steve Longerbeam <slongerbeam@xxxxxxxxx>
---
 drivers/staging/media/imx/imx6-mipi-csi2.c | 93 +++++++++++++++++-----
 1 file changed, 73 insertions(+), 20 deletions(-)

diff --git a/drivers/staging/media/imx/imx6-mipi-csi2.c b/drivers/staging/media/imx/imx6-mipi-csi2.c
index f29e28df36ed..06345de9f060 100644
--- a/drivers/staging/media/imx/imx6-mipi-csi2.c
+++ b/drivers/staging/media/imx/imx6-mipi-csi2.c
@@ -47,6 +47,7 @@ struct csi2_dev {
 
 	struct v4l2_mbus_framefmt format_mbus;
 
+	int                     power_count;
 	int                     stream_count;
 	struct v4l2_subdev      *src_sd;
 	bool                    sink_linked[CSI2_NUM_SRC_PADS];
@@ -113,9 +114,10 @@ static inline struct csi2_dev *sd_to_dev(struct v4l2_subdev *sdev)
  * 7. CSI2 Controller programming - Read the PHY status register (PHY_STATE)
  *    to confirm that the D-PHY is receiving a clock on the D-PHY clock lane.
  *
- * All steps 3 through 7 are carried out by csi2_s_stream(ON) here. Step
- * 6 is accomplished by calling the source subdev's s_stream(ON) between
- * steps 5 and 7.
+ * Steps 3 through 5 are carried out by csi2_s_power(ON) here.
+ *
+ * Steps 6 and 7 are carried out by stream(ON) here. Step 6 is accomplished
+ * by calling the source subdev's s_stream(ON).
  */
 
 static void csi2_enable(struct csi2_dev *csi2, bool enable)
@@ -295,7 +297,7 @@ static void csi2ipu_gasket_init(struct csi2_dev *csi2)
 	writel(reg, csi2->base + CSI2IPU_GASKET);
 }
 
-static int csi2_start(struct csi2_dev *csi2)
+static int csi2_power_on(struct csi2_dev *csi2)
 {
 	int ret;
 
@@ -320,41 +322,87 @@ static int csi2_start(struct csi2_dev *csi2)
 	if (ret)
 		goto err_assert_reset;
 
+	return 0;
+
+err_assert_reset:
+	csi2_enable(csi2, false);
+err_disable_clk:
+	clk_disable_unprepare(csi2->pix_clk);
+	return ret;
+}
+
+static void csi2_power_off(struct csi2_dev *csi2)
+{
+	csi2_enable(csi2, false);
+	clk_disable_unprepare(csi2->pix_clk);
+}
+
+static int csi2_stream_on(struct csi2_dev *csi2)
+{
+	int ret;
+
 	/* Step 6 */
 	ret = v4l2_subdev_call(csi2->src_sd, video, s_stream, 1);
 	ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0;
 	if (ret)
-		goto err_assert_reset;
+		return ret;
 
 	/* Step 7 */
 	ret = csi2_dphy_wait_clock_lane(csi2);
 	if (ret)
-		goto err_stop_upstream;
-
-	return 0;
+		v4l2_subdev_call(csi2->src_sd, video, s_stream, 0);
 
-err_stop_upstream:
-	v4l2_subdev_call(csi2->src_sd, video, s_stream, 0);
-err_assert_reset:
-	csi2_enable(csi2, false);
-err_disable_clk:
-	clk_disable_unprepare(csi2->pix_clk);
 	return ret;
 }
 
-static void csi2_stop(struct csi2_dev *csi2)
+static void csi2_stream_off(struct csi2_dev *csi2)
 {
 	/* stop upstream */
 	v4l2_subdev_call(csi2->src_sd, video, s_stream, 0);
-
-	csi2_enable(csi2, false);
-	clk_disable_unprepare(csi2->pix_clk);
 }
 
 /*
  * V4L2 subdev operations.
  */
 
+static int csi2_s_power(struct v4l2_subdev *sd, int on)
+{
+	struct csi2_dev *csi2 = sd_to_dev(sd);
+	int ret = 0;
+
+	mutex_lock(&csi2->lock);
+
+	if (!csi2->src_sd) {
+		ret = -EPIPE;
+		goto out;
+	}
+
+	/*
+	 * power on/off only if power_count is going from
+	 * 0 to 1 / 1 to 0.
+	 */
+	if (csi2->power_count != !on)
+		goto update_count;
+
+	dev_dbg(csi2->dev, "power %s\n", on ? "ON" : "OFF");
+
+	if (on)
+		ret = csi2_power_on(csi2);
+	else
+		csi2_power_off(csi2);
+	if (ret)
+		goto out;
+
+	/* Update the power count. */
+update_count:
+	csi2->power_count += on ? 1 : -1;
+	if (csi2->power_count < 0)
+		csi2->power_count = 0;
+out:
+	mutex_unlock(&csi2->lock);
+	return ret;
+}
+
 static int csi2_s_stream(struct v4l2_subdev *sd, int enable)
 {
 	struct csi2_dev *csi2 = sd_to_dev(sd);
@@ -385,9 +433,9 @@ static int csi2_s_stream(struct v4l2_subdev *sd, int enable)
 
 	dev_dbg(csi2->dev, "stream %s\n", enable ? "ON" : "OFF");
 	if (enable)
-		ret = csi2_start(csi2);
+		ret = csi2_stream_on(csi2);
 	else
-		csi2_stop(csi2);
+		csi2_stream_off(csi2);
 	if (ret)
 		goto out;
 
@@ -528,6 +576,10 @@ static const struct media_entity_operations csi2_entity_ops = {
 	.link_validate = v4l2_subdev_link_validate,
 };
 
+static const struct v4l2_subdev_core_ops csi2_core_ops = {
+	.s_power = csi2_s_power,
+};
+
 static const struct v4l2_subdev_video_ops csi2_video_ops = {
 	.s_stream = csi2_s_stream,
 };
@@ -539,6 +591,7 @@ static const struct v4l2_subdev_pad_ops csi2_pad_ops = {
 };
 
 static const struct v4l2_subdev_ops csi2_subdev_ops = {
+	.core = &csi2_core_ops,
 	.video = &csi2_video_ops,
 	.pad = &csi2_pad_ops,
 };
-- 
2.17.1