[PATCH 8/8] media: qcom: camss: enable only the stream of the pipeline's virtual channel

[Hitesh Patel]

When a streams-aware transmitter is found at the head of the
pipeline, every stream routed to the source pad the pipeline arrived
through is enabled at start and disabled at stop.

That is right for a transmitter driving one camera per output pad,
but not when it aggregates several cameras onto one output. A GMSL
deserializer can send two cameras on a single CSI-2 port as two
virtual channels, which the CSID demultiplexes to two RDIs and thus
two pipelines. Both pipelines then reach the same source pad, and
each of them enables and, worse, disables both cameras' streams:
stopping one video node stops the other camera as well, and
starting the second one has nothing left to enable.

Identify the stream a pipeline owns from the CSID it went through:
CSID source pad MSM_CSID_PAD_FIRST_SRC + n carries virtual channel n.
Remember that virtual channel while walking upstream, ask the
transmitter for the frame descriptor of its source pad, and only
enable or disable the stream(s) it reports on that virtual channel.

A pipeline that did not pass through a CSID source pad, or a
transmitter without a CSI-2 frame descriptor, keeps enabling every
stream of the pad as before.

Signed-off-by: Hitesh Patel <hitesh@xxxxxxxxxxxxxx>
---
 .../media/platform/qcom/camss/camss-video.c   | 45 +++++++++++++++++--
 drivers/media/platform/qcom/camss/camss.c     | 30 +++++++++++++
 drivers/media/platform/qcom/camss/camss.h     |  2 +
 3 files changed, 74 insertions(+), 3 deletions(-)

diff --git a/drivers/media/platform/qcom/camss/camss-video.c b/drivers/media/platform/qcom/camss/camss-video.c
index 90f22ce76..a261f7692 100644
--- a/drivers/media/platform/qcom/camss/camss-video.c
+++ b/drivers/media/platform/qcom/camss/camss-video.c
@@ -253,14 +253,39 @@ static int video_prepare_streaming(struct vb2_queue *q)
  * video_source_pad_streams - Streams routed to a subdev source pad
  * @sd: Streams-aware subdevice
  * @pad: Source pad index on @sd
+ * @vc: Virtual channel of the pipeline, or -1 if unknown
+ *
+ * When a transmitter aggregates several cameras onto one output, that pad
+ * carries one stream per camera and each of them is a separate pipeline here.
+ * Enabling or disabling the whole pad would start or stop every camera at
+ * once, so pick out the single stream this pipeline owns: the one the frame
+ * descriptor reports on the virtual channel the CSID demultiplexed it from.
+ * With @vc unknown, or without a frame descriptor to map it, the whole pad is
+ * returned, which is the case for a transmitter driving one camera per output
+ * pad.
  *
  * Return the mask of streams of the active routes ending on @pad.
  */
-static u64 video_source_pad_streams(struct v4l2_subdev *sd, u32 pad)
+static u64 video_source_pad_streams(struct v4l2_subdev *sd, u32 pad, int vc)
 {
 	struct v4l2_subdev_state *state;
 	struct v4l2_subdev_route *route;
+	struct v4l2_mbus_frame_desc fd;
+	u64 vc_mask = ~0ULL;
 	u64 mask = 0;
+	int ret;
+
+	if (vc >= 0) {
+		ret = v4l2_subdev_call(sd, pad, get_frame_desc, pad, &fd);
+		if (!ret && fd.type == V4L2_MBUS_FRAME_DESC_TYPE_CSI2) {
+			unsigned int i;
+
+			vc_mask = 0;
+			for (i = 0; i < fd.num_entries; i++)
+				if (fd.entry[i].bus.csi2.vc == vc)
+					vc_mask |= BIT_ULL(fd.entry[i].stream);
+		}
+	}
 
 	state = v4l2_subdev_lock_and_get_active_state(sd);
 	if (!state)
@@ -270,6 +295,8 @@ static u64 video_source_pad_streams(struct v4l2_subdev *sd, u32 pad)
 		if (route->source_pad == pad)
 			mask |= BIT_ULL(route->source_stream);
 
+	mask &= vc_mask;
+
 	v4l2_subdev_unlock_state(state);
 
 	return mask;
@@ -325,6 +352,7 @@ static int video_start_streaming(struct vb2_queue *q, unsigned int count)
 	struct media_entity *entity;
 	struct media_pad *pad;
 	struct v4l2_subdev *subdev;
+	int vc = -1;
 	int ret;
 
 	ret = video_device_pipeline_alloc_start(vdev);
@@ -350,8 +378,13 @@ static int video_start_streaming(struct vb2_queue *q, unsigned int count)
 		entity = pad->entity;
 		subdev = media_entity_to_v4l2_subdev(entity);
 
+		if (vc < 0)
+			vc = camss_csid_source_vc(video->camss, subdev,
+						  pad->index);
+
 		if (subdev->flags & V4L2_SUBDEV_FL_STREAMS) {
-			u64 mask = video_source_pad_streams(subdev, pad->index);
+			u64 mask = video_source_pad_streams(subdev, pad->index,
+							    vc);
 
 			if (!mask)
 				break;
@@ -387,6 +420,7 @@ static void video_stop_streaming(struct vb2_queue *q)
 	struct media_entity *entity;
 	struct media_pad *pad;
 	struct v4l2_subdev *subdev;
+	int vc = -1;
 	int ret;
 
 	entity = &vdev->entity;
@@ -402,8 +436,13 @@ static void video_stop_streaming(struct vb2_queue *q)
 		entity = pad->entity;
 		subdev = media_entity_to_v4l2_subdev(entity);
 
+		if (vc < 0)
+			vc = camss_csid_source_vc(video->camss, subdev,
+						  pad->index);
+
 		if (subdev->flags & V4L2_SUBDEV_FL_STREAMS) {
-			u64 mask = video_source_pad_streams(subdev, pad->index);
+			u64 mask = video_source_pad_streams(subdev, pad->index,
+							    vc);
 
 			if (!mask)
 				break;
diff --git a/drivers/media/platform/qcom/camss/camss.c b/drivers/media/platform/qcom/camss/camss.c
index ca8101c2d..63cc81570 100644
--- a/drivers/media/platform/qcom/camss/camss.c
+++ b/drivers/media/platform/qcom/camss/camss.c
@@ -4655,6 +4655,36 @@ static bool camss_is_receiver_subdev(struct camss *camss,
 	return camss_subdev_stream_users(camss, sd);
 }
 
+/*
+ * camss_csid_source_vc - Virtual channel behind a CSID source pad
+ * @camss: CAMSS device
+ * @sd: Subdevice to test
+ * @pad: Source pad index on @sd
+ *
+ * The CSID demultiplexes virtual channels to its source pads, source pad
+ * MSM_CSID_PAD_FIRST_SRC + n carrying virtual channel n (see the en_vc mask
+ * maintained by csid_link_setup()). Walking a pipeline upstream, this tells
+ * which virtual channel, and so which stream of a shared transmitter, the
+ * pipeline belongs to.
+ *
+ * Return the virtual channel, or -1 if @sd is not a CSID of @camss or @pad is
+ * not one of its source pads.
+ */
+int camss_csid_source_vc(struct camss *camss, struct v4l2_subdev *sd,
+			 unsigned int pad)
+{
+	unsigned int i;
+
+	if (pad < MSM_CSID_PAD_FIRST_SRC)
+		return -1;
+
+	for (i = 0; i < camss->res->csid_num; i++)
+		if (sd == &camss->csid[i].subdev)
+			return pad - MSM_CSID_PAD_FIRST_SRC;
+
+	return -1;
+}
+
 /*
  * camss_find_transmitter_pad - Find the pad of the CSI-2 transmitter
  * @camss: CAMSS device
diff --git a/drivers/media/platform/qcom/camss/camss.h b/drivers/media/platform/qcom/camss/camss.h
index 00b8d5304..14a7c737e 100644
--- a/drivers/media/platform/qcom/camss/camss.h
+++ b/drivers/media/platform/qcom/camss/camss.h
@@ -169,6 +169,8 @@ int camss_enable_clocks(int nclocks, struct camss_clock *clock,
 void camss_disable_clocks(int nclocks, struct camss_clock *clock);
 unsigned int *camss_subdev_stream_users(struct camss *camss,
 					struct v4l2_subdev *sd);
+int camss_csid_source_vc(struct camss *camss, struct v4l2_subdev *sd,
+			 unsigned int pad);
 struct media_pad *camss_find_sensor_pad(struct media_entity *entity);
 s64 camss_get_link_freq(struct camss *camss, struct media_entity *entity,
 			unsigned int bpp, unsigned int lanes);
-- 
2.43.0