[openwrt/staging/xback.git] /

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;