[PATCH v2 4/5] media: qcom: camss: Take the link frequency from the CSI-2 transmitter

[Hitesh Patel]

camss_get_link_freq() walks the pipeline up to an entity with the
MEDIA_ENT_F_CAM_SENSOR function and reads the link frequency there.
The CSIPHY settle count and the CSID clock are then derived from it.

The frequency the receiver has to be programmed for is the one on the
CSI-2 bus, which belongs to whatever drives that bus. When the sensor
is wired straight to the CSIPHY that is the sensor, and the walk gives
the right answer. When a CSI-2 to CSI-2 bridge sits in between, such
as a GMSL or FPD-Link deserializer, the bridge re-times the data onto
its own output at its own rate: it may aggregate several sensors onto
one link, forward one sensor at a different rate, or generate a test
pattern with no sensor at all. The sensor's rate is then not what
arrives at the SoC, and the CSIPHY does not lock.

The walk can also fail before reaching a sensor. A deserializer has
one sink pad per serial link and the walk always follows pad 0; a
sensor attached to any other link is never found and streaming is
refused with "Cannot get CSI2 transmitter's link frequency".

Stop the walk at the first entity that is not a CAMSS receiver, i.e.
at the external subdev feeding the CSIPHY, and ask that pad with
v4l2_get_link_freq(). The helper queries the transmitter through
.get_mbus_config first and falls back to its V4L2_CID_LINK_FREQ and
V4L2_CID_PIXEL_RATE controls, so a bridge and a bare sensor are both
handled by the standard mechanism.

For a sensor connected directly to a CSIPHY the transmitter is the
sensor, so the pad found and the value returned do not change.
camss_find_sensor_pad() is kept for camss_get_pixel_clock() and the
frame skip query, which do want the sensor.

Signed-off-by: Hitesh Patel <hitesh@xxxxxxxxxxxxxx>
---
 drivers/media/platform/qcom/camss/camss.c | 70 +++++++++++++++++++++--
 1 file changed, 64 insertions(+), 6 deletions(-)

diff --git a/drivers/media/platform/qcom/camss/camss.c b/drivers/media/platform/qcom/camss/camss.c
index 2123f6388..61e98b017 100644
--- a/drivers/media/platform/qcom/camss/camss.c
+++ b/drivers/media/platform/qcom/camss/camss.c
@@ -4619,24 +4619,82 @@ struct media_pad *camss_find_sensor_pad(struct media_entity *entity)
 	}
 }
 
+/*
+ * camss_is_receiver_subdev - Test whether a subdev is a CAMSS CSI-2 receiver
+ * @camss: CAMSS device
+ * @sd: Subdevice to test
+ *
+ * Return true for a CSIPHY or CSID belonging to @camss, false for anything
+ * else, in particular for the external subdev transmitting to them.
+ */
+static bool camss_is_receiver_subdev(struct camss *camss,
+				     struct v4l2_subdev *sd)
+{
+	unsigned int i;
+
+	for (i = 0; i < camss->res->csiphy_num; i++)
+		if (sd == &camss->csiphy[i].subdev)
+			return true;
+
+	for (i = 0; i < camss->res->csid_num; i++)
+		if (sd == &camss->csid[i].subdev)
+			return true;
+
+	return false;
+}
+
+/*
+ * camss_find_transmitter_pad - Find the pad of the CSI-2 transmitter
+ * @entity: Media entity in the current pipeline
+ *
+ * Walk the pipeline upstream through the CAMSS receiver subdevs and return the
+ * source pad of the first entity that is not one of them: the CSI-2
+ * transmitter driving the SoC.
+ *
+ * Return a pointer to the transmitter media pad or NULL if not found
+ */
+static struct media_pad *camss_find_transmitter_pad(struct media_entity *entity)
+{
+	struct camss *camss = container_of(entity->graph_obj.mdev,
+					   struct camss, media_dev);
+	struct v4l2_subdev *sd;
+	struct media_pad *pad;
+
+	while (1) {
+		pad = &entity->pads[0];
+		if (!(pad->flags & MEDIA_PAD_FL_SINK))
+			return NULL;
+
+		pad = media_pad_remote_pad_first(pad);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			return NULL;
+
+		entity = pad->entity;
+		sd = media_entity_to_v4l2_subdev(entity);
+
+		if (!camss_is_receiver_subdev(camss, sd))
+			return pad;
+	}
+}
+
 /**
- * camss_get_link_freq - Get link frequency from sensor
+ * camss_get_link_freq - Get link frequency from the CSI-2 transmitter
  * @entity: Media entity in the current pipeline
  * @bpp: Number of bits per pixel for the current format
- * @lanes: Number of lanes in the link to the sensor
+ * @lanes: Number of lanes in the link to the transmitter
  *
  * Return link frequency on success or a negative error code otherwise
  */
 s64 camss_get_link_freq(struct media_entity *entity, unsigned int bpp,
 			unsigned int lanes)
 {
-	struct media_pad *sensor_pad;
+	struct media_pad *tx_pad;
 
-	sensor_pad = camss_find_sensor_pad(entity);
-	if (!sensor_pad)
+	tx_pad = camss_find_transmitter_pad(entity);
+	if (!tx_pad)
 		return -ENODEV;
 
-	return v4l2_get_link_freq(sensor_pad, bpp, 2 * lanes);
+	return v4l2_get_link_freq(tx_pad, bpp, 2 * lanes);
 }
 
 /*
-- 
2.43.0