From 49c4d625a8477689891d68509d9f2b3ede3ed3f9 Mon Sep 17 00:00:00 2001
From: Dave Stevenson <dave.stevenson@raspberrypi.com>
Date: Fri, 29 Jul 2022 17:46:49 +0100
Subject: [PATCH] media: video-mux: Read CSI2 config from FW, and pass
 to receiver

There is no obligation for all source devices on a video-mux to
require the same bus configuration, so read the configuration
from the sink ports, and relay via get_mbus_config on the source
port.
If the sources support get_mbus_config, then call that first.

Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.com>
---
 drivers/media/platform/video-mux.c | 68 +++++++++++++++++++++++++++++-
 1 file changed, 66 insertions(+), 2 deletions(-)

--- a/drivers/media/platform/video-mux.c
+++ b/drivers/media/platform/video-mux.c
@@ -20,11 +20,28 @@
 #include <media/v4l2-mc.h>
 #include <media/v4l2-subdev.h>
 
+struct video_mux_asd {
+	struct v4l2_async_subdev base;
+	unsigned int port;
+};
+
+static inline struct video_mux_asd *to_video_mux_asd(struct v4l2_async_subdev *asd)
+{
+	return container_of(asd, struct video_mux_asd, base);
+}
+
+struct video_mux_pad_cfg {
+	unsigned int num_lanes;
+	bool non_continuous;
+	struct v4l2_subdev *source;
+};
+
 struct video_mux {
 	struct v4l2_subdev subdev;
 	struct v4l2_async_notifier notifier;
 	struct media_pad *pads;
 	struct v4l2_mbus_framefmt *format_mbus;
+	struct video_mux_pad_cfg *cfg;
 	struct mux_control *mux;
 	struct mutex lock;
 	int active;
@@ -330,10 +347,34 @@ static int video_mux_init_cfg(struct v4l
 	return 0;
 }
 
+static int video_mux_get_mbus_config(struct v4l2_subdev *sd,
+				     unsigned int pad,
+				     struct v4l2_mbus_config *cfg)
+{
+	struct video_mux *vmux = v4l2_subdev_to_video_mux(sd);
+	int ret;
+
+	ret = v4l2_subdev_call(vmux->cfg[vmux->active].source, pad, get_mbus_config,
+			       0, cfg);
+
+	if (ret != -ENOIOCTLCMD)
+		return ret;
+
+	cfg->type = V4L2_MBUS_CSI2_DPHY;
+	cfg->bus.mipi_csi2.num_data_lanes = vmux->cfg[vmux->active].num_lanes;
+
+	/* Support for non-continuous CSI-2 clock is missing in pdate mode */
+	if (vmux->cfg[vmux->active].non_continuous)
+		cfg->bus.mipi_csi2.flags |= V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK;
+
+	return 0;
+};
+
 static const struct v4l2_subdev_pad_ops video_mux_pad_ops = {
 	.init_cfg = video_mux_init_cfg,
 	.get_fmt = video_mux_get_format,
 	.set_fmt = video_mux_set_format,
+	.get_mbus_config = video_mux_get_mbus_config,
 };
 
 static const struct v4l2_subdev_ops video_mux_subdev_ops = {
@@ -346,6 +387,9 @@ static int video_mux_notify_bound(struct
 				  struct v4l2_async_subdev *asd)
 {
 	struct video_mux *vmux = notifier_to_video_mux(notifier);
+	unsigned int port = to_video_mux_asd(asd)->port;
+
+	vmux->cfg[port].source = sd;
 
 	return v4l2_create_fwnode_links(sd, &vmux->subdev);
 }
@@ -363,7 +407,7 @@ static int video_mux_async_register(stru
 	v4l2_async_nf_init(&vmux->notifier);
 
 	for (i = 0; i < num_input_pads; i++) {
-		struct v4l2_async_subdev *asd;
+		struct video_mux_asd *asd;
 		struct fwnode_handle *ep, *remote_ep;
 
 		ep = fwnode_graph_get_endpoint_by_id(
@@ -381,7 +425,8 @@ static int video_mux_async_register(stru
 		fwnode_handle_put(remote_ep);
 
 		asd = v4l2_async_nf_add_fwnode_remote(&vmux->notifier, ep,
-						      struct v4l2_async_subdev);
+						      struct video_mux_asd);
+		asd->port = i;
 
 		fwnode_handle_put(ep);
 
@@ -406,6 +451,9 @@ static int video_mux_probe(struct platfo
 {
 	struct device_node *np = pdev->dev.of_node;
 	struct device *dev = &pdev->dev;
+	struct v4l2_fwnode_endpoint fwnode_ep = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
 	struct device_node *ep;
 	struct video_mux *vmux;
 	unsigned int num_pads = 0;
@@ -458,10 +506,26 @@ static int video_mux_probe(struct platfo
 	if (!vmux->format_mbus)
 		return -ENOMEM;
 
+	vmux->cfg = devm_kcalloc(dev, num_pads, sizeof(*vmux->cfg), GFP_KERNEL);
+	if (!vmux->cfg)
+		return -ENOMEM;
+
 	for (i = 0; i < num_pads; i++) {
 		vmux->pads[i].flags = (i < num_pads - 1) ? MEDIA_PAD_FL_SINK
 							 : MEDIA_PAD_FL_SOURCE;
 		vmux->format_mbus[i] = video_mux_format_mbus_default;
+
+		ep = of_graph_get_endpoint_by_regs(pdev->dev.of_node, i, 0);
+		if (ep) {
+			ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep), &fwnode_ep);
+			if (!ret) {
+				/* Get number of data lanes */
+				vmux->cfg[i].num_lanes = fwnode_ep.bus.mipi_csi2.num_data_lanes;
+				vmux->cfg[i].non_continuous = fwnode_ep.bus.mipi_csi2.flags &
+							V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK;
+			}
+			of_node_put(ep);
+		}
 	}
 
 	vmux->subdev.entity.function = MEDIA_ENT_F_VID_MUX;