[PATCH v2 05/10] media: adv748x: Implement set_routing

[Jacopo Mondi <jacopo+renesas@jmondi.org>]

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 | 66 ++++++++++++++++++++++++
 1 file changed, 66 insertions(+)

diff --git a/drivers/media/i2c/adv748x/adv748x-csi2.c b/drivers/media/i2c/adv748x/adv748x-csi2.c
index 91cd70739e9d..af1c2469900c 100644
--- a/drivers/media/i2c/adv748x/adv748x-csi2.c
+++ b/drivers/media/i2c/adv748x/adv748x-csi2.c
@@ -314,12 +314,78 @@ 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;
+
+	ret = adv748x_csi2_routing_validate(tx, routing);
+	if (ret)
+		return ret;
+
+	ret = v4l2_subdev_set_routing(sd, state, routing);
+	if (ret)
+		return ret;
+
+	if (which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
+
+	return adv748x_csi2_set_vc_with_routing(tx, routing);
+}
+
 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,
 };
 
 /* -----------------------------------------------------------------------------
-- 
2.35.1