[PATCH] imx-mipi-csis: Get the number of active lanes from mbus_config

[Isaac Scott <isaac.scott@ideasonboard.com>]

Although 4 lanes may be physically available, we may not be using all of
them. Get the number of configured lanes in the case a driver has
implemented the get_mbus_config op.

Signed-off-by: Isaac Scott <isaac.scott@ideasonboard.com>

---

Currently, the imx-mipi-csis driver parses the device tree to determine
the number of configured lanes for the CSI receiver. This may be
incorrect in the case that the connected device only uses a subset of
lanes, for example. Allow the drivers for these cameras to create a
mbus_config to configure the number of lanes that are actually being
used.

If the driver does not support the get_mbus_config op, this patch will
have no functional change.

Compile tested against media-master (v6.17-rc1)
---
 drivers/media/platform/nxp/imx-mipi-csis.c | 41 ++++++++++++++++++++++
 1 file changed, 41 insertions(+)

diff --git a/drivers/media/platform/nxp/imx-mipi-csis.c b/drivers/media/platform/nxp/imx-mipi-csis.c
index 2beb5f43c2c0..efe4e2ad0382 100644
--- a/drivers/media/platform/nxp/imx-mipi-csis.c
+++ b/drivers/media/platform/nxp/imx-mipi-csis.c
@@ -939,6 +939,43 @@ static struct mipi_csis_device *sd_to_mipi_csis_device(struct v4l2_subdev *sdev)
 	return container_of(sdev, struct mipi_csis_device, sd);
 }
 
+static int mipi_csis_get_active_lanes(struct v4l2_subdev *sd)
+{
+	struct mipi_csis_device *csis = sd_to_mipi_csis_device(sd);
+	struct v4l2_mbus_config mbus_config = { 0 };
+	int ret;
+
+	ret = v4l2_subdev_call(csis->source.sd, pad, get_mbus_config,
+			       0, &mbus_config);
+	if (ret == -ENOIOCTLCMD) {
+		dev_dbg(csis->dev, "No remote mbus configuration available\n");
+		return 0;
+	}
+
+	if (ret) {
+		dev_err(csis->dev, "Failed to get remote mbus configuration\n");
+		return ret;
+	}
+
+	if (mbus_config.type != V4L2_MBUS_CSI2_DPHY) {
+		dev_err(csis->dev, "Unsupported media bus type %u\n",
+			mbus_config.type);
+		return -EINVAL;
+	}
+
+	if (mbus_config.bus.mipi_csi2.num_data_lanes > csis->bus.num_data_lanes) {
+		dev_err(csis->dev,
+			"Unsupported mbus config: too many data lanes %u\n",
+			mbus_config.bus.mipi_csi2.num_data_lanes);
+		return -EINVAL;
+	}
+
+	csis->bus.num_data_lanes = mbus_config.bus.mipi_csi2.num_data_lanes;
+	dev_dbg(csis->dev, "Number of lanes: %d\n", csis->bus.num_data_lanes);
+
+	return 0;
+}
+
 static int mipi_csis_s_stream(struct v4l2_subdev *sd, int enable)
 {
 	struct mipi_csis_device *csis = sd_to_mipi_csis_device(sd);
@@ -965,6 +1002,10 @@ static int mipi_csis_s_stream(struct v4l2_subdev *sd, int enable)
 	format = v4l2_subdev_state_get_format(state, CSIS_PAD_SINK);
 	csis_fmt = find_csis_format(format->code);
 
+	ret = mipi_csis_get_active_lanes(sd);
+	if (ret < 0)
+		dev_dbg(csis->dev, "Failed to get active lanes: %d", ret);
+
 	ret = mipi_csis_calculate_params(csis, csis_fmt);
 	if (ret < 0)
 		goto err_unlock;
-- 
2.43.0