media: staging: rkisp1: cap: refactor enable/disable stream to allow multistreaming

Allow streaming from self picture path and main picture path at the same
time.

Take care for s_stream() callbacks to not be called twice.
When starting a stream, s_stream(true) shouldn't be called for the isp
and the sensor if the other stream is already enabled (since it was
already called).
When stopping a stream, s_stream(false) shouldn't be called for isp and
the sensor if the other stream is still enabled.

Remove the callback function scheme for navigating through the topology,
simplifying the code, improving readability.

Remove multistreaming item from the TODO list.

Signed-off-by: Helen Koike <helen.koike@collabora.com>
Reviewed-by: Dafna Hirschfeld <dafna.hirschfeld@collabora.com>
Signed-off-by: Mauro Carvalho Chehab <mchehab+huawei@kernel.org>

2 files changed, 110 insertions, 112 deletions

diff --git a/drivers/staging/media/rkisp1/TODO b/drivers/staging/media/rkisp1/TODO
index d9360607fbfb..75aae98242d0 100644
--- a/drivers/staging/media/rkisp1/TODO
+++ b/drivers/staging/media/rkisp1/TODO
@@ -1,8 +1,5 @@
 * Fix pad format size for statistics and parameters entities.
 * Fix checkpatch errors.
-* streaming paths (mainpath and selfpath) check if the other path is streaming
-in several places of the code, review this, specially that it doesn't seem it
-supports streaming from both paths at the same time.
 
 NOTES:
 * All v4l2-compliance test must pass.
diff --git a/drivers/staging/media/rkisp1/rkisp1-capture.c b/drivers/staging/media/rkisp1/rkisp1-capture.c
index 1c0f79284162..a7a013204177 100644
--- a/drivers/staging/media/rkisp1/rkisp1-capture.c
+++ b/drivers/staging/media/rkisp1/rkisp1-capture.c
@@ -830,71 +830,43 @@ static void rkisp1_return_all_buffers(struct rkisp1_capture *cap,
 }
 
 /*
- * rkisp1_pipeline_sink_walk - Walk through the pipeline and call cb
- * @from: entity at which to start pipeline walk
- * @until: entity at which to stop pipeline walk
- *
- * Walk the entities chain starting at the pipeline video node and stop
- * all subdevices in the chain.
- *
- * If the until argument isn't NULL, stop the pipeline walk when reaching the
- * until entity. This is used to disable a partially started pipeline due to a
- * subdev start error.
+ * Most of registers inside rockchip ISP1 have shadow register since
+ * they must be not be changed during processing a frame.
+ * Usually, each sub-module updates its shadow register after
+ * processing the last pixel of a frame.
  */
-static int rkisp1_pipeline_sink_walk(struct media_entity *from,
-				     struct media_entity *until,
-				     int (*cb)(struct media_entity *from,
-					       struct media_entity *curr))
+static void rkisp1_cap_stream_enable(struct rkisp1_capture *cap)
 {
-	struct media_entity *entity = from;
-	struct media_pad *pad;
-	unsigned int i;
-	int ret;
-
-	while (1) {
-		pad = NULL;
-		/* Find remote source pad */
-		for (i = 0; i < entity->num_pads; i++) {
-			struct media_pad *spad = &entity->pads[i];
-
-			if (!(spad->flags & MEDIA_PAD_FL_SINK))
-				continue;
-			pad = media_entity_remote_pad(spad);
-			if (pad && is_media_entity_v4l2_subdev(pad->entity))
-				break;
-		}
-		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
-			break;
+	struct rkisp1_device *rkisp1 = cap->rkisp1;
+	struct rkisp1_capture *other = &rkisp1->capture_devs[cap->id ^ 1];
 
-		entity = pad->entity;
-		if (entity == until)
-			break;
+	cap->ops->set_data_path(cap);
+	cap->ops->config(cap);
 
-		ret = cb(from, entity);
-		if (ret)
-			return ret;
+	/* Setup a buffer for the next frame */
+	spin_lock_irq(&cap->buf.lock);
+	rkisp1_set_next_buf(cap);
+	cap->ops->enable(cap);
+	/* It's safe to config ACTIVE and SHADOW regs for the
+	 * first stream. While when the second is starting, do NOT
+	 * force update because it also update the first one.
+	 *
+	 * The latter case would drop one more buf(that is 2) since
+	 * there's not buf in shadow when the second FE received. This's
+	 * also required because the second FE maybe corrupt especially
+	 * when run at 120fps.
+	 */
+	if (!other->is_streaming) {
+		/* force cfg update */
+		rkisp1_write(rkisp1,
+			     RKISP1_CIF_MI_INIT_SOFT_UPD, RKISP1_CIF_MI_INIT);
+		rkisp1_set_next_buf(cap);
 	}
-
-	return 0;
-}
-
-static int rkisp1_pipeline_disable_cb(struct media_entity *from,
-				      struct media_entity *curr)
-{
-	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(curr);
-
-	return v4l2_subdev_call(sd, video, s_stream, false);
-}
-
-static int rkisp1_pipeline_enable_cb(struct media_entity *from,
-				     struct media_entity *curr)
-{
-	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(curr);
-
-	return v4l2_subdev_call(sd, video, s_stream, true);
+	spin_unlock_irq(&cap->buf.lock);
+	cap->is_streaming = true;
 }
 
-static void rkisp1_stream_stop(struct rkisp1_capture *cap)
+static void rkisp1_cap_stream_disable(struct rkisp1_capture *cap)
 {
 	int ret;
 
@@ -911,6 +883,82 @@ static void rkisp1_stream_stop(struct rkisp1_capture *cap)
 	}
 }
 
+/*
+ * rkisp1_pipeline_stream_disable - disable nodes in the pipeline
+ *
+ * Call s_stream(false) in the reverse order from
+ * rkisp1_pipeline_stream_enable() and disable the DMA engine.
+ * Should be called before media_pipeline_stop()
+ */
+static void rkisp1_pipeline_stream_disable(struct rkisp1_capture *cap)
+	__must_hold(&cap->rkisp1->stream_lock)
+{
+	struct rkisp1_device *rkisp1 = cap->rkisp1;
+
+	/*
+	 * If the other capture is streaming, isp and sensor nodes shouldn't
+	 * be disabled, skip them.
+	 */
+	if (rkisp1->pipe.streaming_count < 2) {
+		v4l2_subdev_call(rkisp1->active_sensor->sd, video, s_stream,
+				 false);
+		v4l2_subdev_call(&rkisp1->isp.sd, video, s_stream, false);
+	}
+
+	v4l2_subdev_call(&rkisp1->resizer_devs[cap->id].sd, video, s_stream,
+			 false);
+
+	rkisp1_cap_stream_disable(cap);
+}
+
+/*
+ * rkisp1_pipeline_stream_enable - enable nodes in the pipeline
+ *
+ * Enable the DMA Engine and call s_stream(true) through the pipeline.
+ * Should be called after media_pipeline_start()
+ */
+static int rkisp1_pipeline_stream_enable(struct rkisp1_capture *cap)
+	__must_hold(&cap->rkisp1->stream_lock)
+{
+	struct rkisp1_device *rkisp1 = cap->rkisp1;
+	int ret;
+
+	rkisp1_cap_stream_enable(cap);
+
+	ret = v4l2_subdev_call(&rkisp1->resizer_devs[cap->id].sd, video,
+			       s_stream, true);
+	if (ret)
+		goto err_disable_cap;
+
+	/*
+	 * If the other capture is streaming, isp and sensor nodes are already
+	 * enabled, skip them.
+	 */
+	if (rkisp1->pipe.streaming_count > 1)
+		return 0;
+
+	ret = v4l2_subdev_call(&rkisp1->isp.sd, video, s_stream, true);
+	if (ret)
+		goto err_disable_rsz;
+
+	ret = v4l2_subdev_call(rkisp1->active_sensor->sd, video, s_stream,
+			       true);
+	if (ret)
+		goto err_disable_isp;
+
+	return 0;
+
+err_disable_isp:
+	v4l2_subdev_call(&rkisp1->isp.sd, video, s_stream, false);
+err_disable_rsz:
+	v4l2_subdev_call(&rkisp1->resizer_devs[cap->id].sd, video, s_stream,
+			 false);
+err_disable_cap:
+	rkisp1_cap_stream_disable(cap);
+
+	return ret;
+}
+
 static void rkisp1_vb2_stop_streaming(struct vb2_queue *queue)
 {
 	struct rkisp1_capture *cap = queue->drv_priv;
@@ -920,12 +968,7 @@ static void rkisp1_vb2_stop_streaming(struct vb2_queue *queue)
 
 	mutex_lock(&cap->rkisp1->stream_lock);
 
-	rkisp1_stream_stop(cap);
-	ret = rkisp1_pipeline_sink_walk(&node->vdev.entity, NULL,
-					rkisp1_pipeline_disable_cb);
-	if (ret)
-		dev_err(rkisp1->dev,
-			"pipeline stream-off failed error:%d\n", ret);
+	rkisp1_pipeline_stream_disable(cap);
 
 	rkisp1_return_all_buffers(cap, VB2_BUF_STATE_ERROR);
 
@@ -941,43 +984,6 @@ static void rkisp1_vb2_stop_streaming(struct vb2_queue *queue)
 	mutex_unlock(&cap->rkisp1->stream_lock);
 }
 
-/*
- * Most of registers inside rockchip ISP1 have shadow register since
- * they must be not be changed during processing a frame.
- * Usually, each sub-module updates its shadow register after
- * processing the last pixel of a frame.
- */
-static void rkisp1_stream_start(struct rkisp1_capture *cap)
-{
-	struct rkisp1_device *rkisp1 = cap->rkisp1;
-	struct rkisp1_capture *other = &rkisp1->capture_devs[cap->id ^ 1];
-
-	cap->ops->set_data_path(cap);
-	cap->ops->config(cap);
-
-	/* Setup a buffer for the next frame */
-	spin_lock_irq(&cap->buf.lock);
-	rkisp1_set_next_buf(cap);
-	cap->ops->enable(cap);
-	/* It's safe to config ACTIVE and SHADOW regs for the
-	 * first stream. While when the second is starting, do NOT
-	 * force update because it also update the first one.
-	 *
-	 * The latter case would drop one more buf(that is 2) since
-	 * there's not buf in shadow when the second FE received. This's
-	 * also required because the second FE maybe corrupt especially
-	 * when run at 120fps.
-	 */
-	if (!other->is_streaming) {
-		/* force cfg update */
-		rkisp1_write(rkisp1,
-			     RKISP1_CIF_MI_INIT_SOFT_UPD, RKISP1_CIF_MI_INIT);
-		rkisp1_set_next_buf(cap);
-	}
-	spin_unlock_irq(&cap->buf.lock);
-	cap->is_streaming = true;
-}
-
 static int
 rkisp1_vb2_start_streaming(struct vb2_queue *queue, unsigned int count)
 {
@@ -1008,20 +1014,15 @@ rkisp1_vb2_start_streaming(struct vb2_queue *queue, unsigned int count)
 		goto err_pipe_pm_put;
 	}
 
-	rkisp1_stream_start(cap);
-
-	/* start sub-devices */
-	ret = rkisp1_pipeline_sink_walk(entity, NULL,
-					rkisp1_pipeline_enable_cb);
+	ret = rkisp1_pipeline_stream_enable(cap);
 	if (ret)
-		goto err_stop_stream;
+		goto err_v4l2_pm_put;
 
 	mutex_unlock(&cap->rkisp1->stream_lock);
 
 	return 0;
 
-err_stop_stream:
-	rkisp1_stream_stop(cap);
+err_v4l2_pm_put:
 	v4l2_pipeline_pm_put(entity);
 err_pipe_pm_put:
 	pm_runtime_put(cap->rkisp1->dev);