diff mbox series

[05/10] media: adv748x: Implement set_routing

Message ID 20220216210447.481006-6-jacopo+renesas@jmondi.org
State Superseded
Headers show
Series media: Multiplexed streams for R-Car CSI-2 and ADV748x | expand

Commit Message

Jacopo Mondi Feb. 16, 2022, 9:04 p.m. UTC 
Add the set_routing() subdev operation to allow userspace to configure
the CSI-2 virtual channel.

Signed-off-by: Jacopo Mondi <jacopo+renesas@jmondi.org>
---
 drivers/media/i2c/adv748x/adv748x-csi2.c | 73 ++++++++++++++++++++++++
 1 file changed, 73 insertions(+)
diff mbox series

Patch

diff --git a/drivers/media/i2c/adv748x/adv748x-csi2.c b/drivers/media/i2c/adv748x/adv748x-csi2.c
index 9824ebe44eb1..bf927898b918 100644
--- a/drivers/media/i2c/adv748x/adv748x-csi2.c
+++ b/drivers/media/i2c/adv748x/adv748x-csi2.c
@@ -340,12 +340,85 @@  static int adv748x_csi2_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
 	return ret;
 }
 
+static int adv748x_csi2_routing_validate(struct adv748x_csi2 *tx,
+					 struct v4l2_subdev_krouting *routing)
+{
+	struct v4l2_subdev_route *route;
+
+	if (routing->num_routes != 1) {
+		dev_err(tx->state->dev, "Unsupported number of routes %u",
+			routing->num_routes);
+		return -EINVAL;
+	}
+
+	route = &routing->routes[0];
+
+	if (route->sink_pad != ADV748X_CSI2_SINK ||
+	    route->source_pad != ADV748X_CSI2_SOURCE) {
+		dev_err(tx->state->dev,
+			"Routes should go from the sink to the source pads.\n");
+		return -EINVAL;
+	}
+
+	if (route->source_stream > 4) {
+		dev_err(tx->state->dev, "Unsupported source stream %u\n",
+			route->source_stream);
+		return -EINVAL;
+	}
+
+	if (route->sink_stream != 0) {
+		dev_err(tx->state->dev, "Unsupported sink stream %u\n",
+			route->sink_stream);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int adv748x_csi2_set_vc_with_routing(struct adv748x_csi2 *tx,
+					    struct v4l2_subdev_krouting *routing)
+{
+	struct v4l2_subdev_route *route = &routing->routes[0];
+
+	return adv748x_csi2_set_virtual_channel(tx, route->source_stream);
+}
+
+static int adv748x_csi2_set_routing(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *state,
+				    enum v4l2_subdev_format_whence which,
+				    struct v4l2_subdev_krouting *routing)
+{
+	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
+	int ret;
+
+	v4l2_subdev_lock_state(state);
+
+	ret = adv748x_csi2_routing_validate(tx, routing);
+	if (ret)
+		goto out;
+
+	ret = v4l2_subdev_set_routing(sd, state, routing);
+	if (ret)
+		goto out;
+
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		goto out;
+
+	ret = adv748x_csi2_set_vc_with_routing(tx, routing);
+
+out:
+	v4l2_subdev_unlock_state(state);
+
+	return ret;
+}
+
 static const struct v4l2_subdev_pad_ops adv748x_csi2_pad_ops = {
 	.init_cfg = adv748x_csi2_init_cfg,
 	.get_fmt = v4l2_subdev_get_fmt,
 	.set_fmt = adv748x_csi2_set_format,
 	.get_mbus_config = adv748x_csi2_get_mbus_config,
 	.get_frame_desc = adv748x_csi2_get_frame_desc,
+	.set_routing = adv748x_csi2_set_routing,
 };
 
 /* -----------------------------------------------------------------------------