From cf8f0f9b610bae548833d71e7de3e2c1eb41479f Mon Sep 17 00:00:00 2001
From: Cai YiWei <cyw@rock-chips.com>
Date: Wed, 26 Dec 2018 10:37:56 +0800
Subject: [PATCH] media: rockchip: isp1: update sensor info during streaming

Change-Id: I4813be92f62f258ff97facb9f7658c23fb1f4138
Signed-off-by: Cai YiWei <cyw@rock-chips.com>
---
 .../media/platform/rockchip/isp1/capture.c    | 10 +++++
 drivers/media/platform/rockchip/isp1/dev.h    |  2 +
 drivers/media/platform/rockchip/isp1/rkisp1.c | 45 ++++++++++++-------
 drivers/media/platform/rockchip/isp1/rkisp1.h |  2 +
 4 files changed, 42 insertions(+), 17 deletions(-)

diff --git a/drivers/media/platform/rockchip/isp1/capture.c b/drivers/media/platform/rockchip/isp1/capture.c
index 852d55b2bebc..b31b97a4aed4 100644
--- a/drivers/media/platform/rockchip/isp1/capture.c
+++ b/drivers/media/platform/rockchip/isp1/capture.c
@@ -1283,6 +1283,16 @@ rkisp1_start_streaming(struct vb2_queue *queue, unsigned int count)
 	if (WARN_ON(stream->streaming))
 		return -EBUSY;
 
+	if (!dev->active_sensor) {
+		ret = rkisp1_update_sensor_info(dev);
+		if (ret < 0) {
+			v4l2_err(v4l2_dev,
+				 "update sensor info failed %d\n",
+				 ret);
+			return ret;
+		}
+	}
+
 	ret = rkisp1_create_dummy_buf(stream);
 	if (ret < 0)
 		return ret;
diff --git a/drivers/media/platform/rockchip/isp1/dev.h b/drivers/media/platform/rockchip/isp1/dev.h
index 27ae19211e01..ea5c1b93bcc7 100644
--- a/drivers/media/platform/rockchip/isp1/dev.h
+++ b/drivers/media/platform/rockchip/isp1/dev.h
@@ -94,6 +94,8 @@ struct rkisp1_pipeline {
 struct rkisp1_sensor_info {
 	struct v4l2_subdev *sd;
 	struct v4l2_mbus_config mbus;
+	struct v4l2_subdev_format fmt;
+	struct v4l2_subdev_pad_config cfg;
 };
 
 /*
diff --git a/drivers/media/platform/rockchip/isp1/rkisp1.c b/drivers/media/platform/rockchip/isp1/rkisp1.c
index b03dc66eb804..0c78beeeaaf4 100644
--- a/drivers/media/platform/rockchip/isp1/rkisp1.c
+++ b/drivers/media/platform/rockchip/isp1/rkisp1.c
@@ -140,6 +140,32 @@ static struct rkisp1_sensor_info *sd_to_sensor(struct rkisp1_device *dev,
 	return NULL;
 }
 
+int rkisp1_update_sensor_info(struct rkisp1_device *dev)
+{
+	struct v4l2_subdev *sd = &dev->isp_sdev.sd;
+	struct rkisp1_sensor_info *sensor;
+	struct v4l2_subdev *sensor_sd;
+	int ret = 0;
+
+	sensor_sd = get_remote_sensor(sd);
+	if (!sensor_sd)
+		return -ENODEV;
+
+	sensor = sd_to_sensor(dev, sensor_sd);
+	ret = v4l2_subdev_call(sensor->sd, video, g_mbus_config,
+			       &sensor->mbus);
+	if (ret && ret != -ENOIOCTLCMD)
+		return ret;
+	sensor->fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+	ret = v4l2_subdev_call(sensor->sd, pad, get_fmt,
+			       &sensor->cfg, &sensor->fmt);
+	if (ret && ret != -ENOIOCTLCMD)
+		return ret;
+	dev->active_sensor = sensor;
+
+	return ret;
+}
+
 /****************  register operations ****************/
 
 /*
@@ -549,6 +575,8 @@ static int rkisp1_isp_stop(struct rkisp1_device *dev)
 	if (dev->hdr_sensor)
 		dev->hdr_sensor = NULL;
 
+	dev->active_sensor = NULL;
+
 	return 0;
 }
 
@@ -1157,28 +1185,11 @@ static void rkisp1_isp_read_add_fifo_data(struct rkisp1_device *dev)
 static int rkisp1_isp_sd_s_stream(struct v4l2_subdev *sd, int on)
 {
 	struct rkisp1_device *isp_dev = sd_to_isp_dev(sd);
-	struct rkisp1_sensor_info *sensor;
-	struct v4l2_subdev *sensor_sd;
 	int ret = 0;
 
 	if (!on)
 		return rkisp1_isp_stop(isp_dev);
 
-	sensor_sd = get_remote_sensor(sd);
-	if (!sensor_sd)
-		return -ENODEV;
-
-	sensor = sd_to_sensor(isp_dev, sensor_sd);
-	/*
-	 * Update sensor bus configuration. This is only effective
-	 * for sensors chained off an external CSI2 PHY.
-	 */
-	ret = v4l2_subdev_call(sensor->sd, video, g_mbus_config,
-			       &sensor->mbus);
-	if (ret && ret != -ENOIOCTLCMD)
-		return ret;
-	isp_dev->active_sensor = sensor;
-
 	atomic_set(&isp_dev->isp_sdev.frm_sync_seq, 0);
 	ret = rkisp1_config_cif(isp_dev);
 	if (ret < 0)
diff --git a/drivers/media/platform/rockchip/isp1/rkisp1.h b/drivers/media/platform/rockchip/isp1/rkisp1.h
index f3bdc47ce741..3fdc0089a467 100644
--- a/drivers/media/platform/rockchip/isp1/rkisp1.h
+++ b/drivers/media/platform/rockchip/isp1/rkisp1.h
@@ -124,6 +124,8 @@ void rkisp1_isp_isr(unsigned int isp_mis, struct rkisp1_device *dev);
 
 irqreturn_t rkisp1_vs_isr_handler(int irq, void *ctx);
 
+int rkisp1_update_sensor_info(struct rkisp1_device *dev);
+
 static inline
 struct ispsd_out_fmt *rkisp1_get_ispsd_out_fmt(struct rkisp1_isp_subdev *isp_sdev)
 {