[PATCH 01/45] media: qcom: camss: support CSID streams on one source pad

[Gjorgji . Rosikopulos . gjorgji . rosikopulos]

From: Gjorgji Rosikopulos <gjorgji.rosikopulos@xxxxxxxxxxxxxxxx>

Allow a CSID resource to expose hardware outputs as streams on one
source pad. Keep the existing one-output-per-pad layout as the default.

Translate V4L2 stream IDs to hardware ports. Keep shared sink streams
enabled until their last source stream stops.

Signed-off-by: Gjorgji Rosikopulos <gjorgji.rosikopulos@xxxxxxxxxxxxxxxx>
Signed-off-by: Atanas Filipov <atanas.filipov@xxxxxxxxxxxxxxxx>
---
 .../media/platform/qcom/camss/camss-csid.c    | 129 +++++++++++-------
 .../media/platform/qcom/camss/camss-csid.h    |   6 +
 2 files changed, 83 insertions(+), 52 deletions(-)

diff --git a/drivers/media/platform/qcom/camss/camss-csid.c b/drivers/media/platform/qcom/camss/camss-csid.c
index 2aab9104ac5b..bdac29b7af86 100644
--- a/drivers/media/platform/qcom/camss/camss-csid.c
+++ b/drivers/media/platform/qcom/camss/camss-csid.c
@@ -1385,6 +1385,7 @@ static bool csid_get_stream_csi2_desc(struct v4l2_mbus_frame_desc *frame_desc,
  * @state: V4L2 subdevice state
  * @remote_pad: Remote pad linked on the CSID sink pad, or NULL if unlinked
  * @pad: Source pad number the caller is enabling a stream on
+ * @stream: Source stream number
  * @format_dt: Data type derived from the sink format, used as a fallback
  *	       and sanity-checked against the discovered data type
  *
@@ -1393,7 +1394,7 @@ static bool csid_get_stream_csi2_desc(struct v4l2_mbus_frame_desc *frame_desc,
  */
 static struct v4l2_mbus_frame_desc_entry_csi2
 csid_get_stream_vc_dt(struct csid_device *csid, struct v4l2_subdev_state *state,
-		      struct media_pad *remote_pad, u32 pad, u8 format_dt)
+		      struct media_pad *remote_pad, u32 pad, u32 stream, u8 format_dt)
 {
 	struct v4l2_mbus_frame_desc_entry_csi2 desc_csi2 = { .dt = format_dt };
 	struct v4l2_mbus_frame_desc fd = { };
@@ -1404,7 +1405,8 @@ csid_get_stream_vc_dt(struct csid_device *csid, struct v4l2_subdev_state *state,
 			     pad, get_frame_desc, remote_pad->index, &fd))
 		return desc_csi2;
 
-	if (v4l2_subdev_routing_find_opposite_end(&state->routing, pad, 0, NULL, &sink_stream))
+	if (v4l2_subdev_routing_find_opposite_end(&state->routing,
+						  pad, stream, NULL, &sink_stream))
 		return desc_csi2;
 
 	if (!csid_get_stream_csi2_desc(&fd, sink_stream, &desc_csi2)) {
@@ -1422,6 +1424,14 @@ csid_get_stream_vc_dt(struct csid_device *csid, struct v4l2_subdev_state *state,
 	return desc_csi2;
 }
 
+static u32 csid_get_hw_port(struct csid_device *csid, u32 pad, u32 stream)
+{
+	if (csid->res->source_streams_mode == CSID_STREAMS_SINGLE_PAD)
+		return stream;
+
+	return pad - MSM_CSID_PAD_FIRST_SRC;
+}
+
 /*
  * csid_pad_enable_streams - Enable one or more streams on a source pad
  * @sd: CSID V4L2 subdevice
@@ -1442,9 +1452,8 @@ static int csid_pad_enable_streams(struct v4l2_subdev *sd,
 	const struct csid_hw_ops *hw_ops = csid->res->hw_ops;
 	struct media_pad *remote_pad =
 		media_pad_remote_pad_first(&csid->pads[MSM_CSID_PAD_SINK]);
-	unsigned int hw_port = pad - MSM_CSID_PAD_FIRST_SRC;
-	const struct csid_format_info *format;
-	struct v4l2_mbus_frame_desc_entry_csi2 desc_csi2;
+	unsigned long streams;
+	unsigned int stream_id;
 	u64 sink_streams, propagate_mask;
 	int ret;
 
@@ -1478,12 +1487,19 @@ static int csid_pad_enable_streams(struct v4l2_subdev *sd,
 	csid->enabled_streams[MSM_CSID_PAD_SINK] |= sink_streams;
 	csid->enabled_streams[pad] |= streams_mask;
 
-	format = csid_get_fmt_entry(csid->res->formats->formats,
-				    csid->res->formats->nformats,
-				    csid->fmt[pad].code);
-	desc_csi2 = csid_get_stream_vc_dt(csid, state, remote_pad, pad, format->data_type);
-
-	hw_ops->enable_stream(csid, hw_port, desc_csi2.vc, desc_csi2.dt);
+	streams = streams_mask;
+	for_each_set_bit(stream_id, &streams, BITS_PER_TYPE(streams_mask)) {
+		const struct csid_format_info *format =
+			csid_get_fmt_entry(csid->res->formats->formats,
+					   csid->res->formats->nformats,
+					   csid->fmt[pad + stream_id].code);
+		struct v4l2_mbus_frame_desc_entry_csi2 desc_csi2 =
+			csid_get_stream_vc_dt(csid, state, remote_pad, pad, stream_id,
+					      format->data_type);
+		u32 hw_port = csid_get_hw_port(csid, pad, stream_id);
+
+		hw_ops->enable_stream(csid, hw_port, desc_csi2.vc, desc_csi2.dt);
+	}
 
 	if (propagate_mask && remote_pad) {
 		ret = v4l2_subdev_enable_streams(media_entity_to_v4l2_subdev(remote_pad->entity),
@@ -1492,7 +1508,12 @@ static int csid_pad_enable_streams(struct v4l2_subdev *sd,
 			csid->enabled_streams[MSM_CSID_PAD_SINK] &= ~propagate_mask;
 			csid->enabled_streams[pad] &= ~streams_mask;
 
-			hw_ops->disable_stream(csid, hw_port);
+			streams = streams_mask;
+			for_each_set_bit(stream_id, &streams, BITS_PER_TYPE(streams_mask)) {
+				u32 hw_port = csid_get_hw_port(csid, pad, stream_id);
+
+				hw_ops->disable_stream(csid, hw_port);
+			}
 
 			return ret;
 		}
@@ -1502,18 +1523,17 @@ static int csid_pad_enable_streams(struct v4l2_subdev *sd,
 }
 
 /*
- * csid_sink_streams_in_use - Compute the subset of sink streams still
- *			      referenced by a source pad other than @pad
+ * csid_sink_streams_in_use - Find sink streams used by another source stream
  * @csid: CSID device
  * @state: V4L2 subdevice state
- * @pad: Source pad to exclude from the check
+ * @pad: Source pad being disabled
+ * @streams_mask: Source streams being disabled
  * @sink_streams: Candidate sink streams to check
  *
- * Return the subset of @sink_streams still referenced by some other source
- * pad
+ * Return the subset of @sink_streams still in use
  */
 static u64 csid_sink_streams_in_use(struct csid_device *csid, struct v4l2_subdev_state *state,
-				    u32 pad, u64 sink_streams)
+				    u32 pad, u64 streams_mask, u64 sink_streams)
 {
 	u64 in_use = 0;
 	unsigned int i;
@@ -1523,7 +1543,7 @@ static u64 csid_sink_streams_in_use(struct csid_device *csid, struct v4l2_subdev
 		u64 other_sink_streams;
 
 		if (i == pad)
-			continue;
+			other_streams &= ~streams_mask;
 
 		other_sink_streams = v4l2_subdev_state_xlate_streams(state, i, MSM_CSID_PAD_SINK,
 								     &other_streams);
@@ -1540,12 +1560,8 @@ static u64 csid_sink_streams_in_use(struct csid_device *csid, struct v4l2_subdev
  * @pad: Pad number
  * @streams_mask: Bitmask of v4l2 streams to disable
  *
- * The v4l2 core only calls this on a source pad (v4l2_subdev_disable_streams()
- * rejects sink pads with -EOPNOTSUPP before reaching the driver), so @pad is
- * not checked here. Each source pad only ever carries stream 0.
- *
  * A sink stream is only disabled, and propagated upstream to disable it there
- * too, once no source pad references it any more.
+ * too, once no source stream references it any more.
  *
  * Return 0 on success or a negative error code otherwise
  */
@@ -1557,16 +1573,18 @@ static int csid_pad_disable_streams(struct v4l2_subdev *sd,
 	const struct csid_hw_ops *hw_ops = csid->res->hw_ops;
 	struct media_pad *remote_pad =
 		media_pad_remote_pad_first(&csid->pads[MSM_CSID_PAD_SINK]);
-	unsigned int hw_port = pad - MSM_CSID_PAD_FIRST_SRC;
+	unsigned long streams = streams_mask;
+	unsigned int stream_id;
 	u64 sink_streams, disable_sink_streams;
 	int ret = 0;
 
 	sink_streams = v4l2_subdev_state_xlate_streams(state, pad, MSM_CSID_PAD_SINK,
 						       &streams_mask);
 
-	/* Keep a sink stream active as long as any other source pad still uses it. */
+	/* Keep a sink stream active as long as another source stream uses it. */
 	disable_sink_streams = sink_streams &
-				~csid_sink_streams_in_use(csid, state, pad, sink_streams);
+				~csid_sink_streams_in_use(csid, state, pad, streams_mask,
+						  sink_streams);
 
 	if (disable_sink_streams && remote_pad) {
 		ret = v4l2_subdev_disable_streams(media_entity_to_v4l2_subdev(remote_pad->entity),
@@ -1576,7 +1594,11 @@ static int csid_pad_disable_streams(struct v4l2_subdev *sd,
 				"Failed to disable stream on remote pad: %d\n", ret);
 	}
 
-	hw_ops->disable_stream(csid, hw_port);
+	for_each_set_bit(stream_id, &streams, BITS_PER_TYPE(streams_mask)) {
+		u32 hw_port = csid_get_hw_port(csid, pad, stream_id);
+
+		hw_ops->disable_stream(csid, hw_port);
+	}
 
 	csid->enabled_streams[pad] &= ~streams_mask;
 	csid->enabled_streams[MSM_CSID_PAD_SINK] &= ~disable_sink_streams;
@@ -1606,6 +1628,8 @@ static int csid_set_routing(struct v4l2_subdev *sd,
 			    enum v4l2_subdev_format_whence which,
 			    struct v4l2_subdev_krouting *routing)
 {
+	enum v4l2_subdev_routing_restriction restrictions =
+		V4L2_SUBDEV_ROUTING_NO_SOURCE_STREAM_MIX | V4L2_SUBDEV_ROUTING_NO_N_TO_1;
 	struct csid_device *csid = v4l2_get_subdevdata(sd);
 	unsigned int i;
 	int ret;
@@ -1613,14 +1637,15 @@ static int csid_set_routing(struct v4l2_subdev *sd,
 	if (which == V4L2_SUBDEV_FORMAT_ACTIVE && csid->enabled_streams[MSM_CSID_PAD_SINK])
 		return -EBUSY;
 
-	for (i = 0; i < routing->num_routes; i++)
-		if (routing->routes[i].source_stream != 0)
-			return -EINVAL;
+	if (csid->res->source_streams_mode != CSID_STREAMS_SINGLE_PAD) {
+		for (i = 0; i < routing->num_routes; i++)
+			if (routing->routes[i].source_stream != 0)
+				return -EINVAL;
+
+		restrictions |= V4L2_SUBDEV_ROUTING_NO_SOURCE_MULTIPLEXING;
+	}
 
-	ret = v4l2_subdev_routing_validate(sd, routing,
-					   V4L2_SUBDEV_ROUTING_NO_SOURCE_STREAM_MIX |
-					   V4L2_SUBDEV_ROUTING_NO_SOURCE_MULTIPLEXING |
-					   V4L2_SUBDEV_ROUTING_NO_N_TO_1);
+	ret = v4l2_subdev_routing_validate(sd, routing, restrictions);
 	if (ret)
 		return ret;
 
@@ -1635,9 +1660,8 @@ static int csid_set_routing(struct v4l2_subdev *sd,
  * @stream: stream from which format is requested
  * @which: TRY or ACTIVE format
  *
- * Same as __csid_get_format(), but honors @stream for TRY-state lookups.
- * For ACTIVE state, csid->fmt[] is indexed by pad + stream. @stream is
- * always 0 and @pad selects the RDI channel (0-3).
+ * Same as __csid_get_format(), but honors @stream. For ACTIVE state,
+ * csid->fmt[] is indexed by pad + stream.
  *
  * Return pointer to TRY or ACTIVE format structure
  */
@@ -1750,16 +1774,8 @@ static const struct v4l2_subdev_ops csid_streams_v4l2_ops = {
  * @sd: CSID V4L2 subdevice
  * @state: V4L2 subdev state
  *
- * source_stream is always 0: each source pad MSM_CSID_PAD_FIRST_SRC + i
- * links to its own independent downstream subdev, and a link's sink side is
- * validated against the implicit stream 0 exposed by any subdev without
- * V4L2_SUBDEV_FL_STREAMS (see v4l2_link_validate_get_streams()) - every
- * downstream VFE line is such a subdev.
- *
- * All source pads route from sink_stream 0 by default, fanning the single
- * incoming stream out to every port; a multi-VC source is supported by
- * remapping each route's sink_stream via .set_routing, leaving
- * source_pad/source_stream untouched.
+ * Multi-pad mode exposes one stream on each source pad. Single-pad mode
+ * exposes all output streams on the first source pad.
  *
  * Return 0 on success or a negative error code otherwise
  */
@@ -1780,8 +1796,13 @@ static int csid_init_state(struct v4l2_subdev *sd, struct v4l2_subdev_state *sta
 	for (i = 0; i < num_routes; i++) {
 		routes[i].sink_pad = MSM_CSID_PAD_SINK;
 		routes[i].sink_stream = 0;
-		routes[i].source_pad = MSM_CSID_PAD_FIRST_SRC + i;
-		routes[i].source_stream = 0;
+		if (csid->res->source_streams_mode == CSID_STREAMS_SINGLE_PAD) {
+			routes[i].source_pad = MSM_CSID_PAD_FIRST_SRC;
+			routes[i].source_stream = i;
+		} else {
+			routes[i].source_pad = MSM_CSID_PAD_FIRST_SRC + i;
+			routes[i].source_stream = 0;
+		}
 		routes[i].flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE;
 	}
 
@@ -1821,11 +1842,15 @@ int msm_csid_register_entity(struct csid_device *csid,
 	struct media_pad *pads = csid->pads;
 	struct device *dev = csid->camss->dev;
 	bool streams_api = csid->res->streams_enable;
-	unsigned int num_pads = csid_is_lite(csid) ? MSM_CSID_PADS_NUM :
-						       MSM_CSID_PADS_NUM - 1;
+	unsigned int num_pads;
 	int i;
 	int ret;
 
+	if (csid->res->source_streams_mode == CSID_STREAMS_SINGLE_PAD)
+		num_pads = MSM_CSID_PAD_FIRST_SRC + 1;
+	else
+		num_pads = csid_is_lite(csid) ? MSM_CSID_PADS_NUM : MSM_CSID_PADS_NUM - 1;
+
 	v4l2_subdev_init(sd, streams_api ? &csid_streams_v4l2_ops : &csid_v4l2_ops);
 	sd->internal_ops = streams_api ? &csid_streams_internal_ops
 					: &csid_v4l2_internal_ops;
diff --git a/drivers/media/platform/qcom/camss/camss-csid.h b/drivers/media/platform/qcom/camss/camss-csid.h
index 30674eac51a7..f079956bae8e 100644
--- a/drivers/media/platform/qcom/camss/camss-csid.h
+++ b/drivers/media/platform/qcom/camss/camss-csid.h
@@ -29,6 +29,11 @@
 
 #define CSID_RESET_TIMEOUT_MS 500
 
+enum csid_streams_mode {
+	CSID_STREAMS_MULTI_PAD = 0,
+	CSID_STREAMS_SINGLE_PAD,
+};
+
 enum csid_testgen_mode {
 	CSID_PAYLOAD_MODE_DISABLED = 0,
 	CSID_PAYLOAD_MODE_INCREMENTING = 1,
@@ -166,6 +171,7 @@ struct csid_hw_ops {
 struct csid_subdev_resources {
 	bool is_lite;
 	bool streams_enable;
+	enum csid_streams_mode source_streams_mode;
 	const struct csid_hw_ops *hw_ops;
 	const struct parent_dev_ops *parent_dev_ops;
 	const struct csid_formats *formats;
-- 
2.34.1