[PATCH 1/8] media: qcom: camss: take the link frequency from the CSI-2 transmitter

[Hitesh Patel]

camss_get_link_freq() walks the pipeline to an entity whose function
is MEDIA_ENT_F_CAM_SENSOR and reads the link frequency from there, to
derive the CSIPHY settle count and the CSID clock.

The rate the receiver has to be programmed for is the rate on the
CSI-2 bus, which is a property of whatever drives that bus, not of
the sensor at the far end of the pipeline. The two coincide only when
the sensor is wired directly to the CSIPHY. With a CSI-2 to CSI-2
bridge in between, a GMSL or FPD-Link deserializer for instance, the
bridge re-times the stream onto its own output: it may aggregate
several sensors onto one link, forward a single sensor at a different
rate, or generate a test pattern with no sensor involved at all. The
sensor's rate is then simply not what arrives at the SoC, and the
PHY does not lock.

Walking to a MEDIA_ENT_F_CAM_SENSOR also fails outright on a
deserializer that has one sink pad per serial link: the walk follows
pad 0, while the sensor may be attached to any of the other sink
pads, 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 query that pad with
v4l2_get_link_freq(). This is what the helper is for: it asks the
transmitter through .get_mbus_config first and falls back to its
V4L2_CID_LINK_FREQ, then V4L2_CID_PIXEL_RATE controls.

For a sensor connected straight to a CSIPHY the transmitter is the
sensor itself, so the pad found and the value returned are the same
as before.

camss_find_sensor_pad() keeps walking to the sensor: its other users,
camss_get_pixel_clock() and the frame skip query, do want the sensor.

Signed-off-by: Hitesh Patel <hitesh@xxxxxxxxxxxxxx>
---
 .../media/platform/qcom/camss/camss-csid.c    |  2 +-
 .../media/platform/qcom/camss/camss-csiphy.c  |  6 +-
 drivers/media/platform/qcom/camss/camss.c     | 74 +++++++++++++++++--
 drivers/media/platform/qcom/camss/camss.h     |  4 +-
 4 files changed, 73 insertions(+), 13 deletions(-)

diff --git a/drivers/media/platform/qcom/camss/camss-csid.c b/drivers/media/platform/qcom/camss/camss-csid.c
index 48459b46a..c631119e2 100644
--- a/drivers/media/platform/qcom/camss/camss-csid.c
+++ b/drivers/media/platform/qcom/camss/camss-csid.c
@@ -546,7 +546,7 @@ static int csid_set_clock_rates(struct csid_device *csid)
 
 	fmt = csid_get_fmt_entry(csid->res->formats->formats, csid->res->formats->nformats,
 				 csid->fmt[MSM_CSIPHY_PAD_SINK].code);
-	link_freq = camss_get_link_freq(&csid->subdev.entity, fmt->bpp,
+	link_freq = camss_get_link_freq(csid->camss, &csid->subdev.entity, fmt->bpp,
 					csid->phy.lane_cnt);
 	if (link_freq < 0)
 		link_freq = 0;
diff --git a/drivers/media/platform/qcom/camss/camss-csiphy.c b/drivers/media/platform/qcom/camss/camss-csiphy.c
index 539ac4888..000fde129 100644
--- a/drivers/media/platform/qcom/camss/camss-csiphy.c
+++ b/drivers/media/platform/qcom/camss/camss-csiphy.c
@@ -145,7 +145,8 @@ static int csiphy_set_clock_rates(struct csiphy_device *csiphy)
 				csiphy->fmt[MSM_CSIPHY_PAD_SINK].code);
 	u8 num_lanes = csiphy->cfg.csi2->lane_cfg.num_data;
 
-	link_freq = camss_get_link_freq(&csiphy->subdev.entity, bpp, num_lanes);
+	link_freq = camss_get_link_freq(csiphy->camss, &csiphy->subdev.entity,
+					bpp, num_lanes);
 	if (link_freq < 0)
 		link_freq  = 0;
 
@@ -272,7 +273,8 @@ static int csiphy_stream_on(struct csiphy_device *csiphy)
 	u8 num_lanes = csiphy->cfg.csi2->lane_cfg.num_data;
 	u8 val;
 
-	link_freq = camss_get_link_freq(&csiphy->subdev.entity, bpp, num_lanes);
+	link_freq = camss_get_link_freq(csiphy->camss, &csiphy->subdev.entity,
+					bpp, num_lanes);
 
 	if (link_freq < 0) {
 		dev_err(csiphy->camss->dev,
diff --git a/drivers/media/platform/qcom/camss/camss.c b/drivers/media/platform/qcom/camss/camss.c
index 2123f6388..16ad1c26c 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
+ * @camss: CAMSS device
+ * @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 camss *camss,
+						    struct media_entity *entity)
+{
+	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;
+
+		if (!camss_is_receiver_subdev(camss,
+					      media_entity_to_v4l2_subdev(entity)))
+			return pad;
+	}
+}
+
 /**
- * camss_get_link_freq - Get link frequency from sensor
+ * camss_get_link_freq - Get link frequency from the CSI-2 transmitter
+ * @camss: CAMSS device
  * @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)
+s64 camss_get_link_freq(struct camss *camss, 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(camss, 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);
 }
 
 /*
diff --git a/drivers/media/platform/qcom/camss/camss.h b/drivers/media/platform/qcom/camss/camss.h
index 93d691c8a..39ea33e61 100644
--- a/drivers/media/platform/qcom/camss/camss.h
+++ b/drivers/media/platform/qcom/camss/camss.h
@@ -168,8 +168,8 @@ int camss_enable_clocks(int nclocks, struct camss_clock *clock,
 			struct device *dev);
 void camss_disable_clocks(int nclocks, struct camss_clock *clock);
 struct media_pad *camss_find_sensor_pad(struct media_entity *entity);
-s64 camss_get_link_freq(struct media_entity *entity, unsigned int bpp,
-			unsigned int lanes);
+s64 camss_get_link_freq(struct camss *camss, struct media_entity *entity,
+			unsigned int bpp, unsigned int lanes);
 int camss_get_pixel_clock(struct media_entity *entity, u64 *pixel_clock);
 int camss_pm_domain_on(struct camss *camss, int id);
 void camss_pm_domain_off(struct camss *camss, int id);
-- 
2.43.0