[PATCH v6 10/11] media: rcar-csi2: Add full streams support

[Tomi Valkeinen <tomi.valkeinen+renesas@ideasonboard.com>]

Add the missing pieces to enable full streams support:

- Add set_routing
- Drop the explicit uses of a single stream, and instead use the streams
  mask
- Track enabled sink streams so that in rcsi2_irq_thread() we can
  disable and enable all currently enabled streams

Signed-off-by: Tomi Valkeinen <tomi.valkeinen+renesas@ideasonboard.com>
---
 drivers/media/platform/renesas/rcar-csi2.c | 95 ++++++++++++++++++++----------
 1 file changed, 64 insertions(+), 31 deletions(-)

diff --git a/drivers/media/platform/renesas/rcar-csi2.c b/drivers/media/platform/renesas/rcar-csi2.c
index 8cd38b4cbc7f..34899e5409da 100644
--- a/drivers/media/platform/renesas/rcar-csi2.c
+++ b/drivers/media/platform/renesas/rcar-csi2.c
@@ -702,6 +702,17 @@ static const struct rcar_csi2_format rcar_csi2_formats[] = {
 	},
 };
 
+static const struct v4l2_mbus_framefmt rcar_csi2_default_fmt = {
+	.width		= 1920,
+	.height		= 1080,
+	.code		= MEDIA_BUS_FMT_RGB888_1X24,
+	.colorspace	= V4L2_COLORSPACE_SRGB,
+	.field		= V4L2_FIELD_NONE,
+	.ycbcr_enc	= V4L2_YCBCR_ENC_DEFAULT,
+	.quantization	= V4L2_QUANTIZATION_DEFAULT,
+	.xfer_func	= V4L2_XFER_FUNC_DEFAULT,
+};
+
 static const struct rcar_csi2_format *rcsi2_code_to_fmt(unsigned int code)
 {
 	unsigned int i;
@@ -773,7 +784,7 @@ struct rcar_csi2 {
 
 	int channel_vc[4];
 
-	int stream_count;
+	u64 enabled_sink_streams_mask;
 
 	bool cphy;
 	unsigned short lanes;
@@ -1883,29 +1894,32 @@ static int rcsi2_enable_streams(struct v4l2_subdev *sd,
 				u64 source_streams_mask)
 {
 	struct rcar_csi2 *priv = sd_to_csi2(sd);
-	int ret = 0;
-
-	if (source_streams_mask != 1)
-		return -EINVAL;
+	u64 sink_streams;
+	int ret;
 
 	if (!priv->remote)
 		return -ENODEV;
 
-	if (priv->stream_count == 0) {
+	if (!priv->enabled_sink_streams_mask) {
 		ret = rcsi2_start(priv, state);
 		if (ret)
 			return ret;
 	}
 
+	sink_streams = v4l2_subdev_state_xlate_streams(state,
+						       source_pad,
+						       RCAR_CSI2_SINK,
+						       &source_streams_mask);
+
 	ret = v4l2_subdev_enable_streams(priv->remote, priv->remote_pad,
-					 BIT_ULL(0));
+					 sink_streams);
 	if (ret) {
-		if (priv->stream_count == 0)
+		if (!priv->enabled_sink_streams_mask)
 			rcsi2_stop(priv);
 		return ret;
 	}
 
-	priv->stream_count += 1;
+	priv->enabled_sink_streams_mask |= sink_streams;
 
 	return ret;
 }
@@ -1915,23 +1929,26 @@ static int rcsi2_disable_streams(struct v4l2_subdev *sd,
 				 u32 source_pad, u64 source_streams_mask)
 {
 	struct rcar_csi2 *priv = sd_to_csi2(sd);
+	u64 sink_streams;
 	int ret;
 
-	if (source_streams_mask != 1)
-		return -EINVAL;
-
 	if (!priv->remote)
 		return -ENODEV;
 
-	if (priv->stream_count == 1)
+	sink_streams = v4l2_subdev_state_xlate_streams(state,
+						       source_pad,
+						       RCAR_CSI2_SINK,
+						       &source_streams_mask);
+
+	if (priv->enabled_sink_streams_mask == sink_streams)
 		rcsi2_stop(priv);
 
 	ret = v4l2_subdev_disable_streams(priv->remote, priv->remote_pad,
-					  BIT_ULL(0));
+					  sink_streams);
 	if (ret)
 		return ret;
 
-	priv->stream_count -= 1;
+	priv->enabled_sink_streams_mask &= ~sink_streams;
 
 	return 0;
 }
@@ -1966,6 +1983,34 @@ static int rcsi2_set_pad_format(struct v4l2_subdev *sd,
 	return 0;
 }
 
+static int rcsi2_set_routing(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *state,
+			     enum v4l2_subdev_format_whence which,
+			     struct v4l2_subdev_krouting *routing)
+{
+	struct rcar_csi2 *priv = sd_to_csi2(sd);
+	int ret;
+
+	if (priv->info->use_isp) {
+		ret = v4l2_subdev_routing_validate(sd, routing,
+						   V4L2_SUBDEV_ROUTING_ONLY_1_TO_1);
+	} else {
+		ret = v4l2_subdev_routing_validate(sd, routing,
+						   V4L2_SUBDEV_ROUTING_ONLY_1_TO_1 |
+						   V4L2_SUBDEV_ROUTING_NO_SOURCE_MULTIPLEXING);
+	}
+
+	if (ret)
+		return ret;
+
+	ret = v4l2_subdev_set_routing_with_fmt(sd, state, routing,
+					       &rcar_csi2_default_fmt);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
 static int rcsi2_get_frame_desc_fallback(struct v4l2_subdev *sd,
 					 unsigned int pad,
 					 struct v4l2_mbus_frame_desc *fd)
@@ -2045,6 +2090,7 @@ static const struct v4l2_subdev_pad_ops rcar_csi2_pad_ops = {
 	.set_fmt = rcsi2_set_pad_format,
 	.get_fmt = v4l2_subdev_get_fmt,
 
+	.set_routing = rcsi2_set_routing,
 	.get_frame_desc = rcsi2_get_frame_desc,
 };
 
@@ -2065,17 +2111,6 @@ static int rcsi2_init_state(struct v4l2_subdev *sd,
 		},
 	};
 
-	static const struct v4l2_mbus_framefmt rcar_csi2_default_fmt = {
-		.width		= 1920,
-		.height		= 1080,
-		.code		= MEDIA_BUS_FMT_RGB888_1X24,
-		.colorspace	= V4L2_COLORSPACE_SRGB,
-		.field		= V4L2_FIELD_NONE,
-		.ycbcr_enc	= V4L2_YCBCR_ENC_DEFAULT,
-		.quantization	= V4L2_QUANTIZATION_DEFAULT,
-		.xfer_func	= V4L2_XFER_FUNC_DEFAULT,
-	};
-
 	static const struct v4l2_subdev_krouting routing = {
 		.num_routes = ARRAY_SIZE(routes),
 		.routes = routes,
@@ -2120,13 +2155,13 @@ static irqreturn_t rcsi2_irq_thread(int irq, void *data)
 
 	state = v4l2_subdev_lock_and_get_active_state(&priv->subdev);
 
-	if (priv->stream_count == 0)
+	if (!priv->enabled_sink_streams_mask)
 		goto out;
 
 	rcsi2_stop(priv);
 
 	ret = v4l2_subdev_disable_streams(priv->remote, priv->remote_pad,
-					  BIT_ULL(0));
+					  priv->enabled_sink_streams_mask);
 	if (ret) {
 		dev_warn(priv->dev,
 			 "Error recovery: failed to disable streams: %d\n",
@@ -2145,7 +2180,7 @@ static irqreturn_t rcsi2_irq_thread(int irq, void *data)
 	}
 
 	ret = v4l2_subdev_enable_streams(priv->remote, priv->remote_pad,
-					 BIT_ULL(0));
+					 priv->enabled_sink_streams_mask);
 	if (ret) {
 		dev_warn(priv->dev,
 			 "Error recovery: failed to start streams: %d\n",
@@ -2748,8 +2783,6 @@ static int rcsi2_probe(struct platform_device *pdev)
 
 	priv->dev = &pdev->dev;
 
-	priv->stream_count = 0;
-
 	ret = rcsi2_probe_resources(priv, pdev);
 	if (ret) {
 		dev_err(priv->dev, "Failed to get resources\n");

-- 
2.43.0