[PATCH v3 5/7] media: imx-mipi-csis: Add support for dynamic VC selection

[Inbaraj E <inbaraj.e@xxxxxxxxxxx>]

The existing implementation configures VC0 by default for streaming.
This patch adds support to obtain the VC dynamically from the
subdevice(Sensor) through the get_frame_desc() operation and configure
the corresponding VC when starting the stream.

If get_frame_desc() is not implemented by the subdevice, VC0 will be
selected by default for configuration and streaming.

Signed-off-by: Inbaraj E <inbaraj.e@xxxxxxxxxxx>
---
 drivers/media/platform/nxp/imx-mipi-csis.c | 61 ++++++++++++++++++----
 1 file changed, 51 insertions(+), 10 deletions(-)

diff --git a/drivers/media/platform/nxp/imx-mipi-csis.c b/drivers/media/platform/nxp/imx-mipi-csis.c
index 2443906377bd..b7ab441cc78a 100644
--- a/drivers/media/platform/nxp/imx-mipi-csis.c
+++ b/drivers/media/platform/nxp/imx-mipi-csis.c
@@ -334,6 +334,7 @@ struct mipi_csis_device {
 	struct regulator *mipi_phy_regulator;
 
 	const struct mipi_csis_info *info;
+	unsigned int vc;
 	unsigned int num_channels;
 
 	struct v4l2_subdev sd;
@@ -585,7 +586,7 @@ static void __mipi_csis_set_format(struct mipi_csis_device *csis,
 	u32 val;
 
 	/* Color format */
-	val = mipi_csis_read(csis, MIPI_CSIS_ISP_CONFIG_CH(0));
+	val = mipi_csis_read(csis, MIPI_CSIS_ISP_CONFIG_CH(csis->vc));
 	val &= ~(MIPI_CSIS_ISPCFG_PARALLEL | MIPI_CSIS_ISPCFG_PIXEL_MODE_MASK |
 		 MIPI_CSIS_ISPCFG_DATAFORMAT_MASK);
 
@@ -606,10 +607,10 @@ static void __mipi_csis_set_format(struct mipi_csis_device *csis,
 		val |= MIPI_CSIS_ISPCFG_PIXEL_MODE_DUAL;
 
 	val |= MIPI_CSIS_ISPCFG_DATAFORMAT(csis_fmt->data_type);
-	mipi_csis_write(csis, MIPI_CSIS_ISP_CONFIG_CH(0), val);
+	mipi_csis_write(csis, MIPI_CSIS_ISP_CONFIG_CH(csis->vc), val);
 
 	/* Pixel resolution */
-	mipi_csis_write(csis, MIPI_CSIS_ISP_RESOL_CH(0),
+	mipi_csis_write(csis, MIPI_CSIS_ISP_RESOL_CH(csis->vc),
 			MIPI_CSIS_ISP_RESOL_VRESOL(format->height) |
 			MIPI_CSIS_ISP_RESOL_HRESOL(format->width));
 }
@@ -683,14 +684,14 @@ static void mipi_csis_set_params(struct mipi_csis_device *csis,
 			MIPI_CSIS_DPHY_CMN_CTRL_HSSETTLE(csis->hs_settle) |
 			MIPI_CSIS_DPHY_CMN_CTRL_CLKSETTLE(csis->clk_settle));
 
-	mipi_csis_write(csis, MIPI_CSIS_ISP_SYNC_CH(0),
+	mipi_csis_write(csis, MIPI_CSIS_ISP_SYNC_CH(csis->vc),
 			MIPI_CSIS_ISP_SYNC_HSYNC_LINTV(0) |
 			MIPI_CSIS_ISP_SYNC_VSYNC_SINTV(0) |
 			MIPI_CSIS_ISP_SYNC_VSYNC_EINTV(0));
 
 	val = mipi_csis_read(csis, MIPI_CSIS_CLK_CTRL);
-	val |= MIPI_CSIS_CLK_CTRL_WCLK_SRC(0);
-	val |= MIPI_CSIS_CLK_CTRL_CLKGATE_TRAIL(0, 15);
+	val |= MIPI_CSIS_CLK_CTRL_WCLK_SRC(csis->vc);
+	val |= MIPI_CSIS_CLK_CTRL_CLKGATE_TRAIL(csis->vc, 15);
 	val &= ~MIPI_CSIS_CLK_CTRL_CLKGATE_EN_MSK;
 	mipi_csis_write(csis, MIPI_CSIS_CLK_CTRL, val);
 
@@ -707,7 +708,7 @@ static void mipi_csis_set_params(struct mipi_csis_device *csis,
 	/* Update the shadow register. */
 	val = mipi_csis_read(csis, MIPI_CSIS_CMN_CTRL);
 	mipi_csis_write(csis, MIPI_CSIS_CMN_CTRL,
-			val | MIPI_CSIS_CMN_CTRL_UPDATE_SHADOW(0) |
+			val | MIPI_CSIS_CMN_CTRL_UPDATE_SHADOW(csis->vc) |
 			MIPI_CSIS_CMN_CTRL_UPDATE_SHADOW_CTRL);
 }
 
@@ -778,7 +779,7 @@ static void mipi_csis_queue_event_sof(struct mipi_csis_device *csis)
 	};
 	u32 frame;
 
-	frame = mipi_csis_read(csis, MIPI_CSIS_FRAME_COUNTER_CH(0));
+	frame = mipi_csis_read(csis, MIPI_CSIS_FRAME_COUNTER_CH(csis->vc));
 	event.u.frame_sync.frame_sequence = frame;
 	v4l2_event_queue(csis->sd.devnode, &event);
 }
@@ -810,7 +811,7 @@ static irqreturn_t mipi_csis_irq_handler(int irq, void *dev_id)
 		}
 	}
 
-	if (status & MIPI_CSIS_INT_SRC_FRAME_START(0))
+	if (status & MIPI_CSIS_INT_SRC_FRAME_START(csis->vc))
 		mipi_csis_queue_event_sof(csis);
 
 	spin_unlock_irqrestore(&csis->slock, flags);
@@ -1191,7 +1192,7 @@ static int mipi_csis_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
 
 	entry->flags = 0;
 	entry->pixelcode = csis_fmt->code;
-	entry->bus.csi2.vc = 0;
+	entry->bus.csi2.vc = csis->vc;
 	entry->bus.csi2.dt = csis_fmt->data_type;
 
 	return 0;
@@ -1269,6 +1270,39 @@ static const struct v4l2_subdev_internal_ops mipi_csis_internal_ops = {
 	.init_state		= mipi_csis_init_state,
 };
 
+static int mipi_csis_get_vc(struct mipi_csis_device *csis)
+{
+	struct v4l2_mbus_frame_desc fd = { };
+	int ret;
+
+	ret = v4l2_subdev_call(csis->source.sd, pad, get_frame_desc, csis->source.pad->index, &fd);
+	if (ret < 0 && ret != -ENOIOCTLCMD) {
+		dev_err(csis->dev, "get_frame_desc failed on source subdev\n");
+		return ret;
+	}
+
+	/* If remote subdev does not implement .get_frame_desc default to VC0 */
+	if (ret == -ENOIOCTLCMD)
+		return 0;
+
+	if (fd.type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2) {
+		dev_err(csis->dev, "get_frame_desc returned invalid bus type %d\n", fd.type);
+		return -EINVAL;
+	}
+
+	if (!fd.num_entries) {
+		dev_err(csis->dev, "get_frame_desc returned zero enteries\n");
+		return -EINVAL;
+	}
+
+	if (fd.entry[0].bus.csi2.vc >= csis->num_channels) {
+		dev_err(csis->dev, "get_frame_desc returned invalid virtual channel\n");
+		return -EINVAL;
+	}
+
+	return fd.entry[0].bus.csi2.vc;
+}
+
 /* -----------------------------------------------------------------------------
  * Media entity operations
  */
@@ -1296,6 +1330,13 @@ static int mipi_csis_link_setup(struct media_entity *entity,
 
 		csis->source.sd = remote_sd;
 		csis->source.pad = remote_pad;
+
+		ret = mipi_csis_get_vc(csis);
+
+		if (ret < 0)
+			return -EBUSY;
+
+		csis->vc = ret;
 	} else {
 		csis->source.sd = NULL;
 		csis->source.pad = NULL;
-- 
2.49.0