[v3,10/13] media: cadence: csi2rx: Enable multi-stream support

From: Jai Luthra <j-luthra@ti.com>

Cadence CSI-2 bridge IP supports capturing multiple virtual "streams"
of data over the same physical interface using MIPI Virtual Channels.

The V4L2 subdev APIs should reflect this capability and allow per-stream
routing and controls.

While the hardware IP supports usecases where streams coming in the sink
pad can be broadcasted to multiple source pads, the driver will need
significant re-architecture to make that possible. The two users of this
IP in mainline linux are TI Shim and StarFive JH7110 CAMSS, and both
have only integrated the first source pad i.e stream0 of this IP. So for
now keep it simple and only allow 1-to-1 mapping of streams from sink to
source, without any broadcasting.

With stream routing now supported in the driver, implement the
enable_stream and disable_stream hooks in place of the stream-unaware
s_stream hook.

This allows consumer devices like a DMA bridge or ISP, to enable
particular streams on a source pad, which in turn can be used to enable
only particular streams on the CSI-TX device connected on the sink pad.

Implement a fallback s_stream hook that internally calls enable_stream
on each source pad, for consumer drivers that don't use multi-stream
APIs to still work.

Signed-off-by: Jai Luthra <j-luthra@ti.com>
Co-developed-by: Rishikesh Donadkar <r-donadkar@ti.com>
Signed-off-by: Rishikesh Donadkar <r-donadkar@ti.com>
---
 drivers/media/platform/cadence/cdns-csi2rx.c | 378 ++++++++++++++-----
 1 file changed, 288 insertions(+), 90 deletions(-)

> From: Jai Luthra <j-luthra@ti.com>
> 
> Cadence CSI-2 bridge IP supports capturing multiple virtual "streams"
> of data over the same physical interface using MIPI Virtual Channels.
> 
> The V4L2 subdev APIs should reflect this capability and allow per-stream
> routing and controls.
> 
> While the hardware IP supports usecases where streams coming in the sink
> pad can be broadcasted to multiple source pads, the driver will need
> significant re-architecture to make that possible. The two users of this IP in
> mainline linux are TI Shim and StarFive JH7110 CAMSS, and both have only
> integrated the first source pad i.e stream0 of this IP. So for now keep it simple
> and only allow 1-to-1 mapping of streams from sink to source, without any
> broadcasting.
> 
> With stream routing now supported in the driver, implement the
> enable_stream and disable_stream hooks in place of the stream-unaware
> s_stream hook.
> 
> This allows consumer devices like a DMA bridge or ISP, to enable particular
> streams on a source pad, which in turn can be used to enable only particular
> streams on the CSI-TX device connected on the sink pad.
> 
> Implement a fallback s_stream hook that internally calls enable_stream on
> each source pad, for consumer drivers that don't use multi-stream APIs to still
> work.
> 
> Signed-off-by: Jai Luthra <j-luthra@ti.com>
> Co-developed-by: Rishikesh Donadkar <r-donadkar@ti.com>
> Signed-off-by: Rishikesh Donadkar <r-donadkar@ti.com>

Reviewed-by: Changhuang Liang <changhuang.liang@starfivetech.com>

Best Regards,
Changhuang

Hi Rishikesh,

Thank you for the patch.

On Thu, Apr 17, 2025 at 12:25:51PM +0530, Rishikesh Donadkar wrote:
> From: Jai Luthra <j-luthra@ti.com>
> 
> Cadence CSI-2 bridge IP supports capturing multiple virtual "streams"
> of data over the same physical interface using MIPI Virtual Channels.
> 
> The V4L2 subdev APIs should reflect this capability and allow per-stream
> routing and controls.
> 
> While the hardware IP supports usecases where streams coming in the sink
> pad can be broadcasted to multiple source pads, the driver will need
> significant re-architecture to make that possible. The two users of this
> IP in mainline linux are TI Shim and StarFive JH7110 CAMSS, and both
> have only integrated the first source pad i.e stream0 of this IP. So for
> now keep it simple and only allow 1-to-1 mapping of streams from sink to
> source, without any broadcasting.
> 
> With stream routing now supported in the driver, implement the
> enable_stream and disable_stream hooks in place of the stream-unaware
> s_stream hook.
> 
> This allows consumer devices like a DMA bridge or ISP, to enable
> particular streams on a source pad, which in turn can be used to enable
> only particular streams on the CSI-TX device connected on the sink pad.
> 
> Implement a fallback s_stream hook that internally calls enable_stream
> on each source pad, for consumer drivers that don't use multi-stream
> APIs to still work.

Can't you use v4l2_subdev_s_stream_helper() ? If not, please briefly
explain why in the commit message.

> Signed-off-by: Jai Luthra <j-luthra@ti.com>
> Co-developed-by: Rishikesh Donadkar <r-donadkar@ti.com>
> Signed-off-by: Rishikesh Donadkar <r-donadkar@ti.com>
> ---
>  drivers/media/platform/cadence/cdns-csi2rx.c | 378 ++++++++++++++-----
>  1 file changed, 288 insertions(+), 90 deletions(-)
> 
> diff --git a/drivers/media/platform/cadence/cdns-csi2rx.c b/drivers/media/platform/cadence/cdns-csi2rx.c
> index df05d278df379..95beb6623de8c 100644
> --- a/drivers/media/platform/cadence/cdns-csi2rx.c
> +++ b/drivers/media/platform/cadence/cdns-csi2rx.c
> @@ -90,6 +90,7 @@ struct csi2rx_priv {
>  	struct reset_control		*pixel_rst[CSI2RX_STREAMS_MAX];
>  	struct phy			*dphy;
>  
> +	u32				vc_select[CSI2RX_STREAMS_MAX];
>  	u8				lanes[CSI2RX_LANES_MAX];
>  	u8				num_lanes;
>  	u8				max_lanes;
> @@ -179,29 +180,36 @@ static void csi2rx_reset(struct csi2rx_priv *csi2rx)
>  
>  static int csi2rx_configure_ext_dphy(struct csi2rx_priv *csi2rx)
>  {
> -	struct media_pad *src_pad =
> -		&csi2rx->source_subdev->entity.pads[csi2rx->source_pad];
> +	struct v4l2_ctrl_handler *handler = csi2rx->source_subdev->ctrl_handler;
>  	union phy_configure_opts opts = { };
>  	struct phy_configure_opts_mipi_dphy *cfg = &opts.mipi_dphy;
> -	struct v4l2_subdev_format sd_fmt = {
> -		.which	= V4L2_SUBDEV_FORMAT_ACTIVE,
> -		.pad	= CSI2RX_PAD_SINK,
> -	};
> +	struct v4l2_mbus_framefmt *framefmt;
> +	struct v4l2_subdev_state *state;
>  	const struct csi2rx_fmt *fmt;
>  	s64 link_freq;
>  	int ret;
>  
> -	ret = v4l2_subdev_call_state_active(&csi2rx->subdev, pad, get_fmt,
> -					    &sd_fmt);
> -	if (ret < 0)
> -		return ret;
> -
> -	fmt = csi2rx_get_fmt_by_code(sd_fmt.format.code);
> +	if (v4l2_ctrl_find(handler, V4L2_CID_LINK_FREQ)) {
> +		link_freq = v4l2_get_link_freq(handler, 0, 0);
> +	} else {
> +		state = v4l2_subdev_get_locked_active_state(&csi2rx->subdev);
> +		framefmt = v4l2_subdev_state_get_format(state, CSI2RX_PAD_SINK,
> +							0);
> +		fmt = csi2rx_get_fmt_by_code(framefmt->code);
> +
> +		link_freq = v4l2_get_link_freq(handler, fmt->bpp,
> +					       2 * csi2rx->num_lanes);
> +
> +		dev_warn(csi2rx->dev,
> +			 "Guessing link frequency using bitdepth of stream 0.\n");
> +		dev_warn(csi2rx->dev,
> +			 "V4L2_CID_LINK_FREQ control is required for multi format sources.\n");
> +	}

Why do you need this complexity, instead of calling the
v4l2_get_link_freq() variant that takes the source pad as its first
argument ?

>  
> -	link_freq = v4l2_get_link_freq(src_pad,
> -				       fmt->bpp, 2 * csi2rx->num_lanes);
> -	if (link_freq < 0)
> +	if (link_freq < 0) {
> +		dev_err(csi2rx->dev, "Unable to calculate link frequency\n");
>  		return link_freq;
> +	}
>  
>  	ret = phy_mipi_dphy_get_default_config_for_hsclk(link_freq,
>  							 csi2rx->num_lanes, cfg);
> @@ -224,18 +232,10 @@ static int csi2rx_configure_ext_dphy(struct csi2rx_priv *csi2rx)
>  static int csi2rx_start(struct csi2rx_priv *csi2rx)
>  {
>  	unsigned int i;
> -	struct media_pad *remote_pad;
>  	unsigned long lanes_used = 0;
>  	u32 reg;
>  	int ret;
>  
> -	remote_pad = media_pad_remote_pad_first(&csi2rx->pads[CSI2RX_PAD_SINK]);
> -	if (!remote_pad) {
> -		dev_err(csi2rx->dev,
> -			"Failed to find connected source\n");
> -		return -ENODEV;
> -	}
> -
>  	ret = clk_prepare_enable(csi2rx->p_clk);
>  	if (ret)
>  		return ret;
> @@ -302,11 +302,7 @@ static int csi2rx_start(struct csi2rx_priv *csi2rx)
>  		writel(CSI2RX_STREAM_CFG_FIFO_MODE_LARGE_BUF,
>  		       csi2rx->base + CSI2RX_STREAM_CFG_REG(i));
>  
> -		/*
> -		 * Enable one virtual channel. When multiple virtual channels
> -		 * are supported this will have to be changed.
> -		 */
> -		writel(CSI2RX_STREAM_DATA_CFG_VC_SELECT(0),
> +		writel(csi2rx->vc_select[i],
>  		       csi2rx->base + CSI2RX_STREAM_DATA_CFG_REG(i));
>  
>  		writel(CSI2RX_STREAM_CTRL_START,
> @@ -319,17 +315,10 @@ static int csi2rx_start(struct csi2rx_priv *csi2rx)
>  
>  	reset_control_deassert(csi2rx->sys_rst);
>  
> -	ret = v4l2_subdev_enable_streams(csi2rx->source_subdev,
> -					 remote_pad->index, BIT(0));
> -	if (ret)
> -		goto err_disable_sysclk;
> -
>  	clk_disable_unprepare(csi2rx->p_clk);
>  
>  	return 0;
>  
> -err_disable_sysclk:
> -	clk_disable_unprepare(csi2rx->sys_clk);
>  err_disable_pixclk:
>  	for (; i > 0; i--) {
>  		reset_control_assert(csi2rx->pixel_rst[i - 1]);
> @@ -348,7 +337,6 @@ static int csi2rx_start(struct csi2rx_priv *csi2rx)
>  
>  static void csi2rx_stop(struct csi2rx_priv *csi2rx)
>  {
> -	struct media_pad *remote_pad;
>  	unsigned int i;
>  	u32 val;
>  	int ret;
> @@ -377,13 +365,6 @@ static void csi2rx_stop(struct csi2rx_priv *csi2rx)
>  	reset_control_assert(csi2rx->p_rst);
>  	clk_disable_unprepare(csi2rx->p_clk);
>  
> -	remote_pad = media_pad_remote_pad_first(&csi2rx->pads[CSI2RX_PAD_SINK]);
> -	if (!remote_pad ||
> -	    v4l2_subdev_disable_streams(csi2rx->source_subdev,
> -					remote_pad->index, BIT(0))) {
> -		dev_warn(csi2rx->dev, "Couldn't disable our subdev\n");
> -	}
> -
>  	if (csi2rx->dphy) {
>  		writel(0, csi2rx->base + CSI2RX_DPHY_LANE_CTRL_REG);
>  
> @@ -392,37 +373,153 @@ static void csi2rx_stop(struct csi2rx_priv *csi2rx)
>  	}
>  }
>  
> -static int csi2rx_s_stream(struct v4l2_subdev *subdev, int enable)
> +static void csi2rx_update_vc_select(struct csi2rx_priv *csi2rx,
> +				    struct v4l2_subdev_state *state)
> +{
> +	struct v4l2_mbus_frame_desc fd = {0};
> +	struct v4l2_subdev_route *route;
> +	unsigned int i;
> +	int ret;
> +
> +	/* Capture VC=0 by default */
> +	for (i = 0; i < CSI2RX_STREAMS_MAX; i++)
> +		csi2rx->vc_select[i] = CSI2RX_STREAM_DATA_CFG_VC_SELECT(0);
> +
> +	ret = csi2rx_get_frame_desc_from_source(csi2rx, &fd);
> +	if (ret || fd.type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2) {
> +		dev_dbg(csi2rx->dev,
> +			"Failed to get source frame desc, allowing only VC=0\n");
> +		return;
> +	}
> +
> +	/* If source provides per-stream VC info, use it to filter by VC */
> +	memset(csi2rx->vc_select, 0, sizeof(csi2rx->vc_select));
> +
> +	for_each_active_route(&state->routing, route) {
> +		u32 cdns_stream = route->source_pad - CSI2RX_PAD_SOURCE_STREAM0;
> +
> +		for (i = 0; i < fd.num_entries; i++) {
> +			if (fd.entry[i].stream != route->sink_stream)
> +				continue;
> +
> +			csi2rx->vc_select[cdns_stream] |=
> +				CSI2RX_STREAM_DATA_CFG_VC_SELECT(fd.entry[i].bus.csi2.vc);
> +		}
> +	}
> +}
> +
> +static int csi2rx_enable_streams(struct v4l2_subdev *subdev,
> +				 struct v4l2_subdev_state *state, u32 pad,
> +				 u64 streams_mask)
>  {
>  	struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev);
> -	int ret = 0;
> +	struct media_pad *remote_pad;
> +	u64 sink_streams;
> +	int ret;
> +
> +	remote_pad = media_pad_remote_pad_first(&csi2rx->pads[CSI2RX_PAD_SINK]);
> +	if (!remote_pad) {
> +		dev_err(csi2rx->dev,
> +			"Failed to find connected source\n");
> +		return -ENODEV;
> +	}
> +
> +	sink_streams = v4l2_subdev_state_xlate_streams(state, pad,
> +						       CSI2RX_PAD_SINK,
> +						       &streams_mask);
>  
>  	mutex_lock(&csi2rx->lock);

Can you use lock guards ? It will simplify error handling.

> +	/*
> +	 * If we're not the first users, there's no need to
> +	 * enable the whole controller.
> +	 */
> +	if (!csi2rx->count) {

I would have stored the bitmask of enabled streams instead of a count,
but it doesn't matter that much.

> +		ret = csi2rx_start(csi2rx);
> +		if (ret)
> +			goto err_stream_start;
> +	}
>  
> -	if (enable) {
> -		/*
> -		 * If we're not the first users, there's no need to
> -		 * enable the whole controller.
> -		 */
> -		if (!csi2rx->count) {
> -			ret = csi2rx_start(csi2rx);
> -			if (ret)
> -				goto out;
> -		}
> +	/* Start streaming on the source */
> +	ret = v4l2_subdev_enable_streams(csi2rx->source_subdev, remote_pad->index,
> +					 sink_streams);
> +	if (ret) {
> +		dev_err(csi2rx->dev,
> +			"Failed to start streams %#llx on subdev\n",
> +			sink_streams);
> +		goto err_subdev_enable;
> +	}
>  
> -		csi2rx->count++;
> -	} else {
> -		csi2rx->count--;
> +	csi2rx->count++;
> +	mutex_unlock(&csi2rx->lock);
> +
> +	return 0;
> +
> +err_subdev_enable:
> +	if (!csi2rx->count)
> +		csi2rx_stop(csi2rx);
> +err_stream_start:
> +	mutex_unlock(&csi2rx->lock);
> +	return ret;
> +}
> +
> +static int csi2rx_disable_streams(struct v4l2_subdev *subdev,
> +				  struct v4l2_subdev_state *state, u32 pad,
> +				  u64 streams_mask)
> +{
> +	struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev);
> +	struct media_pad *remote_pad;
> +	u64 sink_streams;
> +
> +	sink_streams = v4l2_subdev_state_xlate_streams(state, pad,
> +						       CSI2RX_PAD_SINK,
> +						       &streams_mask);
>  
> -		/*
> -		 * Let the last user turn off the lights.
> -		 */
> -		if (!csi2rx->count)
> -			csi2rx_stop(csi2rx);
> +	remote_pad = media_pad_remote_pad_first(&csi2rx->pads[CSI2RX_PAD_SINK]);
> +	if (!remote_pad ||
> +	    v4l2_subdev_disable_streams(csi2rx->source_subdev,
> +					remote_pad->index, sink_streams)) {
> +		dev_err(csi2rx->dev, "Couldn't disable our subdev\n");
>  	}
>  
> -out:
> +	mutex_lock(&csi2rx->lock);
> +	csi2rx->count--;
> +	/*
> +	 * Let the last user turn off the lights.
> +	 */

This can become a single line.

> +	if (!csi2rx->count)
> +		csi2rx_stop(csi2rx);
>  	mutex_unlock(&csi2rx->lock);
> +
> +	return 0;
> +}
> +
> +static int csi2rx_s_stream_fallback(struct v4l2_subdev *sd, int enable)
> +{
> +	struct v4l2_subdev_state *state;
> +	struct v4l2_subdev_route *route;
> +	u64 mask[CSI2RX_PAD_MAX] = {0};
> +	int i, ret;
> +
> +	/* Find the stream mask on all source pads */
> +	state = v4l2_subdev_lock_and_get_active_state(sd);
> +	for (i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++) {
> +		for_each_active_route(&state->routing, route) {
> +			if (route->source_pad == i)
> +				mask[i] |= BIT_ULL(route->source_stream);
> +		}
> +	}
> +	v4l2_subdev_unlock_state(state);
> +
> +	/* Start streaming on each pad */
> +	for (i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++) {
> +		if (enable)
> +			ret = v4l2_subdev_enable_streams(sd, i, mask[i]);
> +		else
> +			ret = v4l2_subdev_disable_streams(sd, i, mask[i]);
> +		if (ret)
> +			return ret;
> +	}
> +
>  	return ret;
>  }
>  
> @@ -438,12 +535,58 @@ static int csi2rx_enum_mbus_code(struct v4l2_subdev *subdev,
>  	return 0;
>  }
>  
> +static int _csi2rx_set_routing(struct v4l2_subdev *subdev,
> +			       struct v4l2_subdev_state *state,
> +			       struct v4l2_subdev_krouting *routing)
> +{
> +	static const struct v4l2_mbus_framefmt format = {
> +		.width = 640,
> +		.height = 480,
> +		.code = MEDIA_BUS_FMT_UYVY8_1X16,
> +		.field = V4L2_FIELD_NONE,
> +		.colorspace = V4L2_COLORSPACE_SRGB,
> +		.ycbcr_enc = V4L2_YCBCR_ENC_601,
> +		.quantization = V4L2_QUANTIZATION_LIM_RANGE,
> +		.xfer_func = V4L2_XFER_FUNC_SRGB,
> +	};
> +	int ret;
> +
> +	if (routing->num_routes > V4L2_FRAME_DESC_ENTRY_MAX)
> +		return -EINVAL;
> +
> +	ret = v4l2_subdev_routing_validate(subdev, routing,
> +					   V4L2_SUBDEV_ROUTING_ONLY_1_TO_1);
> +	if (ret)
> +		return ret;
> +
> +	return v4l2_subdev_set_routing_with_fmt(subdev, state, routing, &format);
> +}
> +
> +static int csi2rx_set_routing(struct v4l2_subdev *subdev,
> +			      struct v4l2_subdev_state *state,
> +			      enum v4l2_subdev_format_whence which,
> +			      struct v4l2_subdev_krouting *routing)
> +{
> +	struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev);
> +	int ret;
> +
> +	if (which == V4L2_SUBDEV_FORMAT_ACTIVE && csi2rx->count)
> +		return -EBUSY;
> +
> +	ret = _csi2rx_set_routing(subdev, state, routing);
> +	if (ret)
> +		return ret;
> +
> +	csi2rx_update_vc_select(csi2rx, state);

When setting routing on the TRY state this will update the active state,
which isn't correct. Can you call this function when starting the device
instead ?

> +
> +	return 0;
> +}
> +
>  static int csi2rx_set_fmt(struct v4l2_subdev *subdev,
>  			  struct v4l2_subdev_state *state,
>  			  struct v4l2_subdev_format *format)
>  {
>  	struct v4l2_mbus_framefmt *fmt;
> -	unsigned int i;
>  
>  	/* No transcoding, source and sink formats must match. */
>  	if (format->pad != CSI2RX_PAD_SINK)
> @@ -455,14 +598,16 @@ static int csi2rx_set_fmt(struct v4l2_subdev *subdev,
>  	format->format.field = V4L2_FIELD_NONE;
>  
>  	/* Set sink format */
> -	fmt = v4l2_subdev_state_get_format(state, format->pad);
> +	fmt = v4l2_subdev_state_get_format(state, format->pad, format->stream);
>  	*fmt = format->format;
>  
> -	/* Propagate to source formats */
> -	for (i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++) {
> -		fmt = v4l2_subdev_state_get_format(state, i);
> -		*fmt = format->format;
> -	}
> +	/* Propagate to source format */
> +	fmt = v4l2_subdev_state_get_opposite_stream_format(state, format->pad,
> +							   format->stream);
> +	if (!fmt)
> +		return -EINVAL;
> +
> +	*fmt = format->format;
>  
>  	return 0;
>  }
> @@ -470,40 +615,92 @@ static int csi2rx_set_fmt(struct v4l2_subdev *subdev,
>  static int csi2rx_init_state(struct v4l2_subdev *subdev,
>  			     struct v4l2_subdev_state *state)
>  {
> -	struct v4l2_subdev_format format = {
> -		.pad = CSI2RX_PAD_SINK,
> -		.format = {
> -			.width = 640,
> -			.height = 480,
> -			.code = MEDIA_BUS_FMT_UYVY8_1X16,
> -			.field = V4L2_FIELD_NONE,
> -			.colorspace = V4L2_COLORSPACE_SRGB,
> -			.ycbcr_enc = V4L2_YCBCR_ENC_601,
> -			.quantization = V4L2_QUANTIZATION_LIM_RANGE,
> -			.xfer_func = V4L2_XFER_FUNC_SRGB,
> +	struct v4l2_subdev_route routes[] = {
> +		{
> +			.sink_pad = CSI2RX_PAD_SINK,
> +			.sink_stream = 0,
> +			.source_pad = CSI2RX_PAD_SOURCE_STREAM0,
> +			.source_stream = 0,
> +			.flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE,
>  		},
>  	};
>  
> -	return csi2rx_set_fmt(subdev, state, &format);
> +	struct v4l2_subdev_krouting routing = {
> +		.num_routes = ARRAY_SIZE(routes),
> +		.routes = routes,
> +	};
> +
> +	return _csi2rx_set_routing(subdev, state, &routing);
>  }
>  
>  static int csi2rx_get_frame_desc(struct v4l2_subdev *subdev, unsigned int pad,
>  				 struct v4l2_mbus_frame_desc *fd)
>  {
>  	struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev);
> +	struct v4l2_mbus_frame_desc source_fd = {0};
> +	struct v4l2_subdev_route *route;
> +	struct v4l2_subdev_state *state;
> +	int ret;
>  
> -	return csi2rx_get_frame_desc_from_source(csi2rx, fd);
> +	ret = csi2rx_get_frame_desc_from_source(csi2rx, &source_fd);
> +	if (ret)
> +		return ret;
> +
> +	fd->type = V4L2_MBUS_FRAME_DESC_TYPE_CSI2;
> +
> +	state = v4l2_subdev_lock_and_get_active_state(subdev);
> +
> +	for_each_active_route(&state->routing, route) {
> +		struct v4l2_mbus_frame_desc_entry *source_entry = NULL;
> +		unsigned int i;
> +
> +		if (route->source_pad != pad)
> +			continue;
> +
> +		for (i = 0; i < source_fd.num_entries; i++) {
> +			if (source_fd.entry[i].stream == route->sink_stream) {
> +				source_entry = &source_fd.entry[i];
> +				break;
> +			}
> +		}
> +
> +		if (!source_entry) {
> +			dev_err(csi2rx->dev,
> +				"Failed to find stream from source frame desc\n");
> +			ret = -EPIPE;
> +			goto err_missing_stream;
> +		}
> +
> +		fd->entry[fd->num_entries].stream = route->source_stream;
> +		fd->entry[fd->num_entries].flags = source_entry->flags;
> +		fd->entry[fd->num_entries].length = source_entry->length;
> +		fd->entry[fd->num_entries].pixelcode = source_entry->pixelcode;
> +		fd->entry[fd->num_entries].bus.csi2.vc =
> +			source_entry->bus.csi2.vc;
> +		fd->entry[fd->num_entries].bus.csi2.dt =
> +			source_entry->bus.csi2.dt;
> +
> +		fd->num_entries++;
> +	}
> +
> +err_missing_stream:
> +	v4l2_subdev_unlock_state(state);
> +
> +	return ret;
>  }
>  
>  static const struct v4l2_subdev_pad_ops csi2rx_pad_ops = {
> -	.enum_mbus_code	= csi2rx_enum_mbus_code,
> -	.get_fmt	= v4l2_subdev_get_fmt,
> -	.set_fmt	= csi2rx_set_fmt,
> -	.get_frame_desc	= csi2rx_get_frame_desc,
> +	.enum_mbus_code		= csi2rx_enum_mbus_code,
> +	.get_fmt		= v4l2_subdev_get_fmt,
> +	.set_fmt		= csi2rx_set_fmt,
> +	.get_frame_desc		= csi2rx_get_frame_desc,
> +	.set_routing		= csi2rx_set_routing,
> +	.enable_streams		= csi2rx_enable_streams,
> +	.disable_streams	= csi2rx_disable_streams,
>  };
>  
>  static const struct v4l2_subdev_video_ops csi2rx_video_ops = {
> -	.s_stream	= csi2rx_s_stream,
> +	.s_stream	= csi2rx_s_stream_fallback,
>  };
>  
>  static const struct v4l2_subdev_ops csi2rx_subdev_ops = {
> @@ -735,7 +932,8 @@ static int csi2rx_probe(struct platform_device *pdev)
>  	csi2rx->pads[CSI2RX_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
>  	for (i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++)
>  		csi2rx->pads[i].flags = MEDIA_PAD_FL_SOURCE;
> -	csi2rx->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
> +	csi2rx->subdev.flags = V4L2_SUBDEV_FL_HAS_DEVNODE |
> +		V4L2_SUBDEV_FL_STREAMS;
>  	csi2rx->subdev.entity.ops = &csi2rx_media_ops;
>  
>  	ret = media_entity_pads_init(&csi2rx->subdev.entity, CSI2RX_PAD_MAX,

Hi Rishikesh,

On Tue, May 06, 2025 at 04:36:58PM +0530, Rishikesh Donadkar wrote:
> On 21/04/25 19:27, Laurent Pinchart wrote:
> > On Thu, Apr 17, 2025 at 12:25:51PM +0530, Rishikesh Donadkar wrote:
> >> From: Jai Luthra <j-luthra@ti.com>
> >>
> >> Cadence CSI-2 bridge IP supports capturing multiple virtual "streams"
> >> of data over the same physical interface using MIPI Virtual Channels.
> >>
> >> The V4L2 subdev APIs should reflect this capability and allow per-stream
> >> routing and controls.
> >>
> >> While the hardware IP supports usecases where streams coming in the sink
> >> pad can be broadcasted to multiple source pads, the driver will need
> >> significant re-architecture to make that possible. The two users of this
> >> IP in mainline linux are TI Shim and StarFive JH7110 CAMSS, and both
> >> have only integrated the first source pad i.e stream0 of this IP. So for
> >> now keep it simple and only allow 1-to-1 mapping of streams from sink to
> >> source, without any broadcasting.
> >>
> >> With stream routing now supported in the driver, implement the
> >> enable_stream and disable_stream hooks in place of the stream-unaware
> >> s_stream hook.
> >>
> >> This allows consumer devices like a DMA bridge or ISP, to enable
> >> particular streams on a source pad, which in turn can be used to enable
> >> only particular streams on the CSI-TX device connected on the sink pad.
> >>
> >> Implement a fallback s_stream hook that internally calls enable_stream
> >> on each source pad, for consumer drivers that don't use multi-stream
> >> APIs to still work.
> >
> > Can't you use v4l2_subdev_s_stream_helper() ? If not, please briefly
> > explain why in the commit message.
> 
> The v4l2_subdev_s_stream_helper() can only be used by subdevs that have 
> a single source pad, This IP is used by TI Shim and StarFive JH7110 
> CAMSS, currently both of them use only the first source pad (stream0) 
> but the driver can be re-architectured to use have more than one source 
> pad, this is the reason v4l2_subdev_s_stream_helper() is not used.
> 
> I will mention this in the commit message as well.
> 
> >> Signed-off-by: Jai Luthra <j-luthra@ti.com>
> >> Co-developed-by: Rishikesh Donadkar <r-donadkar@ti.com>
> >> Signed-off-by: Rishikesh Donadkar <r-donadkar@ti.com>
> >> ---
> >>   drivers/media/platform/cadence/cdns-csi2rx.c | 378 ++++++++++++++-----
> >>   1 file changed, 288 insertions(+), 90 deletions(-)
> >>
> >> diff --git a/drivers/media/platform/cadence/cdns-csi2rx.c b/drivers/media/platform/cadence/cdns-csi2rx.c
> >> index df05d278df379..95beb6623de8c 100644
> >> --- a/drivers/media/platform/cadence/cdns-csi2rx.c
> >> +++ b/drivers/media/platform/cadence/cdns-csi2rx.c
> >> @@ -90,6 +90,7 @@ struct csi2rx_priv {
> >>   	struct reset_control		*pixel_rst[CSI2RX_STREAMS_MAX];
> >>   	struct phy			*dphy;
> >>   
> >> +	u32				vc_select[CSI2RX_STREAMS_MAX];
> >>   	u8				lanes[CSI2RX_LANES_MAX];
> >>   	u8				num_lanes;
> >>   	u8				max_lanes;
> >> @@ -179,29 +180,36 @@ static void csi2rx_reset(struct csi2rx_priv *csi2rx)
> >>   
> >>   static int csi2rx_configure_ext_dphy(struct csi2rx_priv *csi2rx)
> >>   {
> >> -	struct media_pad *src_pad =
> >> -		&csi2rx->source_subdev->entity.pads[csi2rx->source_pad];
> >> +	struct v4l2_ctrl_handler *handler = csi2rx->source_subdev->ctrl_handler;
> >>   	union phy_configure_opts opts = { };
> >>   	struct phy_configure_opts_mipi_dphy *cfg = &opts.mipi_dphy;
> >> -	struct v4l2_subdev_format sd_fmt = {
> >> -		.which	= V4L2_SUBDEV_FORMAT_ACTIVE,
> >> -		.pad	= CSI2RX_PAD_SINK,
> >> -	};
> >> +	struct v4l2_mbus_framefmt *framefmt;
> >> +	struct v4l2_subdev_state *state;
> >>   	const struct csi2rx_fmt *fmt;
> >>   	s64 link_freq;
> >>   	int ret;
> >>   
> >> -	ret = v4l2_subdev_call_state_active(&csi2rx->subdev, pad, get_fmt,
> >> -					    &sd_fmt);
> >> -	if (ret < 0)
> >> -		return ret;
> >> -
> >> -	fmt = csi2rx_get_fmt_by_code(sd_fmt.format.code);
> >> +	if (v4l2_ctrl_find(handler, V4L2_CID_LINK_FREQ)) {
> >> +		link_freq = v4l2_get_link_freq(handler, 0, 0);
> >> +	} else {
> >> +		state = v4l2_subdev_get_locked_active_state(&csi2rx->subdev);
> >> +		framefmt = v4l2_subdev_state_get_format(state, CSI2RX_PAD_SINK,
> >> +							0);
> >> +		fmt = csi2rx_get_fmt_by_code(framefmt->code);
> >> +
> >> +		link_freq = v4l2_get_link_freq(handler, fmt->bpp,
> >> +					       2 * csi2rx->num_lanes);
> >> +
> >> +		dev_warn(csi2rx->dev,
> >> +			 "Guessing link frequency using bitdepth of stream 0.\n");
> >> +		dev_warn(csi2rx->dev,
> >> +			 "V4L2_CID_LINK_FREQ control is required for multi format sources.\n");
> >> +	}
> >
> > Why do you need this complexity, instead of calling the
> > v4l2_get_link_freq() variant that takes the source pad as its first
> > argument ?
> 
> This was introduced to not break support for the single-stream 
> transmitters that do not implement V4L2_CID_LINK_FREQ. But now I realize 
> that v4l2_get_link_freq() checks if V4L2_CID_LINK_FREQ is present, if 
> not falls back to V4L2_CID_PIXEL_RATE for single-stream transmitters.
> 
> For multistream transmitters, V4L2_CID_LINK_FREQ should be implemented 
> as PIXEL_RATE does not make any sense, so I will call 
> v4l2_get_link_freq(handler, 0, 0) in multi-stream case.
> 
> This will force the multi-stream transmitter to implement 
> V4L2_CID_LINK_FREQ or else give an error.

I think that's fine, we want drivers to move towards the recommended
APIs.

> >>   
> >> -	link_freq = v4l2_get_link_freq(src_pad,
> >> -				       fmt->bpp, 2 * csi2rx->num_lanes);
> >> -	if (link_freq < 0)
> >> +	if (link_freq < 0) {
> >> +		dev_err(csi2rx->dev, "Unable to calculate link frequency\n");
> >>   		return link_freq;
> >> +	}
> >>   
> >>   	ret = phy_mipi_dphy_get_default_config_for_hsclk(link_freq,
> >>   							 csi2rx->num_lanes, cfg);
> >> @@ -224,18 +232,10 @@ static int csi2rx_configure_ext_dphy(struct csi2rx_priv *csi2rx)
> >>   static int csi2rx_start(struct csi2rx_priv *csi2rx)
> >>   {
> >>   	unsigned int i;
> >> -	struct media_pad *remote_pad;
> >>   	unsigned long lanes_used = 0;
> >>   	u32 reg;
> >>   	int ret;
> >>   
> >> -	remote_pad = media_pad_remote_pad_first(&csi2rx->pads[CSI2RX_PAD_SINK]);
> >> -	if (!remote_pad) {
> >> -		dev_err(csi2rx->dev,
> >> -			"Failed to find connected source\n");
> >> -		return -ENODEV;
> >> -	}
> >> -
> >>   	ret = clk_prepare_enable(csi2rx->p_clk);
> >>   	if (ret)
> >>   		return ret;
> >> @@ -302,11 +302,7 @@ static int csi2rx_start(struct csi2rx_priv *csi2rx)
> >>   		writel(CSI2RX_STREAM_CFG_FIFO_MODE_LARGE_BUF,
> >>   		       csi2rx->base + CSI2RX_STREAM_CFG_REG(i));
> >>   
> >> -		/*
> >> -		 * Enable one virtual channel. When multiple virtual channels
> >> -		 * are supported this will have to be changed.
> >> -		 */
> >> -		writel(CSI2RX_STREAM_DATA_CFG_VC_SELECT(0),
> >> +		writel(csi2rx->vc_select[i],
> >>   		       csi2rx->base + CSI2RX_STREAM_DATA_CFG_REG(i));
> >>   
> >>   		writel(CSI2RX_STREAM_CTRL_START,
> >> @@ -319,17 +315,10 @@ static int csi2rx_start(struct csi2rx_priv *csi2rx)
> >>   
> >>   	reset_control_deassert(csi2rx->sys_rst);
> >>   
> >> -	ret = v4l2_subdev_enable_streams(csi2rx->source_subdev,
> >> -					 remote_pad->index, BIT(0));
> >> -	if (ret)
> >> -		goto err_disable_sysclk;
> >> -
> >>   	clk_disable_unprepare(csi2rx->p_clk);
> >>   
> >>   	return 0;
> >>   
> >> -err_disable_sysclk:
> >> -	clk_disable_unprepare(csi2rx->sys_clk);
> >>   err_disable_pixclk:
> >>   	for (; i > 0; i--) {
> >>   		reset_control_assert(csi2rx->pixel_rst[i - 1]);
> >> @@ -348,7 +337,6 @@ static int csi2rx_start(struct csi2rx_priv *csi2rx)
> >>   
> >>   static void csi2rx_stop(struct csi2rx_priv *csi2rx)
> >>   {
> >> -	struct media_pad *remote_pad;
> >>   	unsigned int i;
> >>   	u32 val;
> >>   	int ret;
> >> @@ -377,13 +365,6 @@ static void csi2rx_stop(struct csi2rx_priv *csi2rx)
> >>   	reset_control_assert(csi2rx->p_rst);
> >>   	clk_disable_unprepare(csi2rx->p_clk);
> >>   
> >> -	remote_pad = media_pad_remote_pad_first(&csi2rx->pads[CSI2RX_PAD_SINK]);
> >> -	if (!remote_pad ||
> >> -	    v4l2_subdev_disable_streams(csi2rx->source_subdev,
> >> -					remote_pad->index, BIT(0))) {
> >> -		dev_warn(csi2rx->dev, "Couldn't disable our subdev\n");
> >> -	}
> >> -
> >>   	if (csi2rx->dphy) {
> >>   		writel(0, csi2rx->base + CSI2RX_DPHY_LANE_CTRL_REG);
> >>   
> >> @@ -392,37 +373,153 @@ static void csi2rx_stop(struct csi2rx_priv *csi2rx)
> >>   	}
> >>   }
> >>   
> >> -static int csi2rx_s_stream(struct v4l2_subdev *subdev, int enable)
> >> +static void csi2rx_update_vc_select(struct csi2rx_priv *csi2rx,
> >> +				    struct v4l2_subdev_state *state)
> >> +{
> >> +	struct v4l2_mbus_frame_desc fd = {0};
> >> +	struct v4l2_subdev_route *route;
> >> +	unsigned int i;
> >> +	int ret;
> >> +
> >> +	/* Capture VC=0 by default */
> >> +	for (i = 0; i < CSI2RX_STREAMS_MAX; i++)
> >> +		csi2rx->vc_select[i] = CSI2RX_STREAM_DATA_CFG_VC_SELECT(0);
> >> +
> >> +	ret = csi2rx_get_frame_desc_from_source(csi2rx, &fd);
> >> +	if (ret || fd.type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2) {
> >> +		dev_dbg(csi2rx->dev,
> >> +			"Failed to get source frame desc, allowing only VC=0\n");
> >> +		return;
> >> +	}
> >> +
> >> +	/* If source provides per-stream VC info, use it to filter by VC */
> >> +	memset(csi2rx->vc_select, 0, sizeof(csi2rx->vc_select));
> >> +
> >> +	for_each_active_route(&state->routing, route) {
> >> +		u32 cdns_stream = route->source_pad - CSI2RX_PAD_SOURCE_STREAM0;
> >> +
> >> +		for (i = 0; i < fd.num_entries; i++) {
> >> +			if (fd.entry[i].stream != route->sink_stream)
> >> +				continue;
> >> +
> >> +			csi2rx->vc_select[cdns_stream] |=
> >> +				CSI2RX_STREAM_DATA_CFG_VC_SELECT(fd.entry[i].bus.csi2.vc);
> >> +		}
> >> +	}
> >> +}
> >> +
> >> +static int csi2rx_enable_streams(struct v4l2_subdev *subdev,
> >> +				 struct v4l2_subdev_state *state, u32 pad,
> >> +				 u64 streams_mask)
> >>   {
> >>   	struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev);
> >> -	int ret = 0;
> >> +	struct media_pad *remote_pad;
> >> +	u64 sink_streams;
> >> +	int ret;
> >> +
> >> +	remote_pad = media_pad_remote_pad_first(&csi2rx->pads[CSI2RX_PAD_SINK]);
> >> +	if (!remote_pad) {
> >> +		dev_err(csi2rx->dev,
> >> +			"Failed to find connected source\n");
> >> +		return -ENODEV;
> >> +	}
> >> +
> >> +	sink_streams = v4l2_subdev_state_xlate_streams(state, pad,
> >> +						       CSI2RX_PAD_SINK,
> >> +						       &streams_mask);
> >>   
> >>   	mutex_lock(&csi2rx->lock);
> >
> > Can you use lock guards ? It will simplify error handling.
> 
> Sure.
> 
> >> +	/*
> >> +	 * If we're not the first users, there's no need to
> >> +	 * enable the whole controller.
> >> +	 */
> >> +	if (!csi2rx->count) {
> >
> > I would have stored the bitmask of enabled streams instead of a count,
> > but it doesn't matter that much.
> >
> >> +		ret = csi2rx_start(csi2rx);
> >> +		if (ret)
> >> +			goto err_stream_start;
> >> +	}
> >>   
> >> -	if (enable) {
> >> -		/*
> >> -		 * If we're not the first users, there's no need to
> >> -		 * enable the whole controller.
> >> -		 */
> >> -		if (!csi2rx->count) {
> >> -			ret = csi2rx_start(csi2rx);
> >> -			if (ret)
> >> -				goto out;
> >> -		}
> >> +	/* Start streaming on the source */
> >> +	ret = v4l2_subdev_enable_streams(csi2rx->source_subdev, remote_pad->index,
> >> +					 sink_streams);
> >> +	if (ret) {
> >> +		dev_err(csi2rx->dev,
> >> +			"Failed to start streams %#llx on subdev\n",
> >> +			sink_streams);
> >> +		goto err_subdev_enable;
> >> +	}
> >>   
> >> -		csi2rx->count++;
> >> -	} else {
> >> -		csi2rx->count--;
> >> +	csi2rx->count++;
> >> +	mutex_unlock(&csi2rx->lock);
> >> +
> >> +	return 0;
> >> +
> >> +err_subdev_enable:
> >> +	if (!csi2rx->count)
> >> +		csi2rx_stop(csi2rx);
> >> +err_stream_start:
> >> +	mutex_unlock(&csi2rx->lock);
> >> +	return ret;
> >> +}
> >> +
> >> +static int csi2rx_disable_streams(struct v4l2_subdev *subdev,
> >> +				  struct v4l2_subdev_state *state, u32 pad,
> >> +				  u64 streams_mask)
> >> +{
> >> +	struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev);
> >> +	struct media_pad *remote_pad;
> >> +	u64 sink_streams;
> >> +
> >> +	sink_streams = v4l2_subdev_state_xlate_streams(state, pad,
> >> +						       CSI2RX_PAD_SINK,
> >> +						       &streams_mask);
> >>   
> >> -		/*
> >> -		 * Let the last user turn off the lights.
> >> -		 */
> >> -		if (!csi2rx->count)
> >> -			csi2rx_stop(csi2rx);
> >> +	remote_pad = media_pad_remote_pad_first(&csi2rx->pads[CSI2RX_PAD_SINK]);
> >> +	if (!remote_pad ||
> >> +	    v4l2_subdev_disable_streams(csi2rx->source_subdev,
> >> +					remote_pad->index, sink_streams)) {
> >> +		dev_err(csi2rx->dev, "Couldn't disable our subdev\n");
> >>   	}
> >>   
> >> -out:
> >> +	mutex_lock(&csi2rx->lock);
> >> +	csi2rx->count--;
> >> +	/*
> >> +	 * Let the last user turn off the lights.
> >> +	 */
> >
> > This can become a single line.
> 
> Will fix.
> 
> >> +	if (!csi2rx->count)
> >> +		csi2rx_stop(csi2rx);
> >>   	mutex_unlock(&csi2rx->lock);
> >> +
> >> +	return 0;
> >> +}
> >> +
> >> +static int csi2rx_s_stream_fallback(struct v4l2_subdev *sd, int enable)
> >> +{
> >> +	struct v4l2_subdev_state *state;
> >> +	struct v4l2_subdev_route *route;
> >> +	u64 mask[CSI2RX_PAD_MAX] = {0};
> >> +	int i, ret;
> >> +
> >> +	/* Find the stream mask on all source pads */
> >> +	state = v4l2_subdev_lock_and_get_active_state(sd);
> >> +	for (i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++) {
> >> +		for_each_active_route(&state->routing, route) {
> >> +			if (route->source_pad == i)
> >> +				mask[i] |= BIT_ULL(route->source_stream);
> >> +		}
> >> +	}
> >> +	v4l2_subdev_unlock_state(state);
> >> +
> >> +	/* Start streaming on each pad */
> >> +	for (i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++) {
> >> +		if (enable)
> >> +			ret = v4l2_subdev_enable_streams(sd, i, mask[i]);
> >> +		else
> >> +			ret = v4l2_subdev_disable_streams(sd, i, mask[i]);
> >> +		if (ret)
> >> +			return ret;
> >> +	}
> >> +
> >>   	return ret;
> >>   }
> >>   
> >> @@ -438,12 +535,58 @@ static int csi2rx_enum_mbus_code(struct v4l2_subdev *subdev,
> >>   	return 0;
> >>   }
> >>   
> >> +static int _csi2rx_set_routing(struct v4l2_subdev *subdev,
> >> +			       struct v4l2_subdev_state *state,
> >> +			       struct v4l2_subdev_krouting *routing)
> >> +{
> >> +	static const struct v4l2_mbus_framefmt format = {
> >> +		.width = 640,
> >> +		.height = 480,
> >> +		.code = MEDIA_BUS_FMT_UYVY8_1X16,
> >> +		.field = V4L2_FIELD_NONE,
> >> +		.colorspace = V4L2_COLORSPACE_SRGB,
> >> +		.ycbcr_enc = V4L2_YCBCR_ENC_601,
> >> +		.quantization = V4L2_QUANTIZATION_LIM_RANGE,
> >> +		.xfer_func = V4L2_XFER_FUNC_SRGB,
> >> +	};
> >> +	int ret;
> >> +
> >> +	if (routing->num_routes > V4L2_FRAME_DESC_ENTRY_MAX)
> >> +		return -EINVAL;
> >> +
> >> +	ret = v4l2_subdev_routing_validate(subdev, routing,
> >> +					   V4L2_SUBDEV_ROUTING_ONLY_1_TO_1);
> >> +	if (ret)
> >> +		return ret;
> >> +
> >> +	return v4l2_subdev_set_routing_with_fmt(subdev, state, routing, &format);
> >> +}
> >> +
> >> +static int csi2rx_set_routing(struct v4l2_subdev *subdev,
> >> +			      struct v4l2_subdev_state *state,
> >> +			      enum v4l2_subdev_format_whence which,
> >> +			      struct v4l2_subdev_krouting *routing)
> >> +{
> >> +	struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev);
> >> +	int ret;
> >> +
> >> +	if (which == V4L2_SUBDEV_FORMAT_ACTIVE && csi2rx->count)
> >> +		return -EBUSY;
> >> +
> >> +	ret = _csi2rx_set_routing(subdev, state, routing);
> >> +	if (ret)
> >> +		return ret;
> >> +
> >> +	csi2rx_update_vc_select(csi2rx, state);
> >
> > When setting routing on the TRY state this will update the active state,
> > which isn't correct. Can you call this function when starting the device
> > instead ?
> 
> Yes, this can be called at stream start, before configuring the VC data 
> in the registers.
> 
> Will do this in the next revision.
> 
> >> +
> >> +	return 0;
> >> +}
> >> +
> >>   static int csi2rx_set_fmt(struct v4l2_subdev *subdev,
> >>   			  struct v4l2_subdev_state *state,
> >>   			  struct v4l2_subdev_format *format)
> >>   {
> >>   	struct v4l2_mbus_framefmt *fmt;
> >> -	unsigned int i;
> >>   
> >>   	/* No transcoding, source and sink formats must match. */
> >>   	if (format->pad != CSI2RX_PAD_SINK)
> >> @@ -455,14 +598,16 @@ static int csi2rx_set_fmt(struct v4l2_subdev *subdev,
> >>   	format->format.field = V4L2_FIELD_NONE;
> >>   
> >>   	/* Set sink format */
> >> -	fmt = v4l2_subdev_state_get_format(state, format->pad);
> >> +	fmt = v4l2_subdev_state_get_format(state, format->pad, format->stream);
> >>   	*fmt = format->format;
> >>   
> >> -	/* Propagate to source formats */
> >> -	for (i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++) {
> >> -		fmt = v4l2_subdev_state_get_format(state, i);
> >> -		*fmt = format->format;
> >> -	}
> >> +	/* Propagate to source format */
> >> +	fmt = v4l2_subdev_state_get_opposite_stream_format(state, format->pad,
> >> +							   format->stream);
> >> +	if (!fmt)
> >> +		return -EINVAL;
> >> +
> >> +	*fmt = format->format;
> >>   
> >>   	return 0;
> >>   }
> >> @@ -470,40 +615,92 @@ static int csi2rx_set_fmt(struct v4l2_subdev *subdev,
> >>   static int csi2rx_init_state(struct v4l2_subdev *subdev,
> >>   			     struct v4l2_subdev_state *state)
> >>   {
> >> -	struct v4l2_subdev_format format = {
> >> -		.pad = CSI2RX_PAD_SINK,
> >> -		.format = {
> >> -			.width = 640,
> >> -			.height = 480,
> >> -			.code = MEDIA_BUS_FMT_UYVY8_1X16,
> >> -			.field = V4L2_FIELD_NONE,
> >> -			.colorspace = V4L2_COLORSPACE_SRGB,
> >> -			.ycbcr_enc = V4L2_YCBCR_ENC_601,
> >> -			.quantization = V4L2_QUANTIZATION_LIM_RANGE,
> >> -			.xfer_func = V4L2_XFER_FUNC_SRGB,
> >> +	struct v4l2_subdev_route routes[] = {
> >> +		{
> >> +			.sink_pad = CSI2RX_PAD_SINK,
> >> +			.sink_stream = 0,
> >> +			.source_pad = CSI2RX_PAD_SOURCE_STREAM0,
> >> +			.source_stream = 0,
> >> +			.flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE,
> >>   		},
> >>   	};
> >>   
> >> -	return csi2rx_set_fmt(subdev, state, &format);
> >> +	struct v4l2_subdev_krouting routing = {
> >> +		.num_routes = ARRAY_SIZE(routes),
> >> +		.routes = routes,
> >> +	};
> >> +
> >> +	return _csi2rx_set_routing(subdev, state, &routing);
> >>   }
> >>   
> >>   static int csi2rx_get_frame_desc(struct v4l2_subdev *subdev, unsigned int pad,
> >>   				 struct v4l2_mbus_frame_desc *fd)
> >>   {
> >>   	struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev);
> >> +	struct v4l2_mbus_frame_desc source_fd = {0};
> >> +	struct v4l2_subdev_route *route;
> >> +	struct v4l2_subdev_state *state;
> >> +	int ret;
> >>   
> >> -	return csi2rx_get_frame_desc_from_source(csi2rx, fd);
> >> +	ret = csi2rx_get_frame_desc_from_source(csi2rx, &source_fd);
> >> +	if (ret)
> >> +		return ret;
> >> +
> >> +	fd->type = V4L2_MBUS_FRAME_DESC_TYPE_CSI2;
> >> +
> >> +	state = v4l2_subdev_lock_and_get_active_state(subdev);
> >> +
> >> +	for_each_active_route(&state->routing, route) {
> >> +		struct v4l2_mbus_frame_desc_entry *source_entry = NULL;
> >> +		unsigned int i;
> >> +
> >> +		if (route->source_pad != pad)
> >> +			continue;
> >> +
> >> +		for (i = 0; i < source_fd.num_entries; i++) {
> >> +			if (source_fd.entry[i].stream == route->sink_stream) {
> >> +				source_entry = &source_fd.entry[i];
> >> +				break;
> >> +			}
> >> +		}
> >> +
> >> +		if (!source_entry) {
> >> +			dev_err(csi2rx->dev,
> >> +				"Failed to find stream from source frame desc\n");
> >> +			ret = -EPIPE;
> >> +			goto err_missing_stream;
> >> +		}
> >> +
> >> +		fd->entry[fd->num_entries].stream = route->source_stream;
> >> +		fd->entry[fd->num_entries].flags = source_entry->flags;
> >> +		fd->entry[fd->num_entries].length = source_entry->length;
> >> +		fd->entry[fd->num_entries].pixelcode = source_entry->pixelcode;
> >> +		fd->entry[fd->num_entries].bus.csi2.vc =
> >> +			source_entry->bus.csi2.vc;
> >> +		fd->entry[fd->num_entries].bus.csi2.dt =
> >> +			source_entry->bus.csi2.dt;
> >> +
> >> +		fd->num_entries++;
> >> +	}
> >> +
> >> +err_missing_stream:
> >> +	v4l2_subdev_unlock_state(state);
> >> +
> >> +	return ret;
> >>   }
> >>   
> >>   static const struct v4l2_subdev_pad_ops csi2rx_pad_ops = {
> >> -	.enum_mbus_code	= csi2rx_enum_mbus_code,
> >> -	.get_fmt	= v4l2_subdev_get_fmt,
> >> -	.set_fmt	= csi2rx_set_fmt,
> >> -	.get_frame_desc	= csi2rx_get_frame_desc,
> >> +	.enum_mbus_code		= csi2rx_enum_mbus_code,
> >> +	.get_fmt		= v4l2_subdev_get_fmt,
> >> +	.set_fmt		= csi2rx_set_fmt,
> >> +	.get_frame_desc		= csi2rx_get_frame_desc,
> >> +	.set_routing		= csi2rx_set_routing,
> >> +	.enable_streams		= csi2rx_enable_streams,
> >> +	.disable_streams	= csi2rx_disable_streams,
> >>   };
> >>   
> >>   static const struct v4l2_subdev_video_ops csi2rx_video_ops = {
> >> -	.s_stream	= csi2rx_s_stream,
> >> +	.s_stream	= csi2rx_s_stream_fallback,
> >>   };
> >>   
> >>   static const struct v4l2_subdev_ops csi2rx_subdev_ops = {
> >> @@ -735,7 +932,8 @@ static int csi2rx_probe(struct platform_device *pdev)
> >>   	csi2rx->pads[CSI2RX_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
> >>   	for (i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++)
> >>   		csi2rx->pads[i].flags = MEDIA_PAD_FL_SOURCE;
> >> -	csi2rx->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
> >> +	csi2rx->subdev.flags = V4L2_SUBDEV_FL_HAS_DEVNODE |
> >> +		V4L2_SUBDEV_FL_STREAMS;
> >>   	csi2rx->subdev.entity.ops = &csi2rx_media_ops;
> >>   
> >>   	ret = media_entity_pads_init(&csi2rx->subdev.entity, CSI2RX_PAD_MAX,