Since the chip supports 4 incoming GMSL links allowing for 4 sensors to
be connected, we need to add support for streams if we are to use more
than one sensor with this deserializer.
Signed-off-by: Laurentiu Palcu <laurentiu.palcu@xxxxxxxxxxx>
---
drivers/staging/media/max96712/max96712.c | 177 +++++++++++++++++++---
1 file changed, 158 insertions(+), 19 deletions(-)
diff --git a/drivers/staging/media/max96712/max96712.c b/drivers/staging/media/max96712/max96712.c
index f68a1d241b846..a078e4c67c360 100644
--- a/drivers/staging/media/max96712/max96712.c
+++ b/drivers/staging/media/max96712/max96712.c
@@ -13,6 +13,7 @@
#include <linux/of_graph.h>
#include <linux/regmap.h>
+#include <media/mipi-csi2.h>
#include <media/v4l2-cci.h>
#include <media/v4l2-ctrls.h>
#include <media/v4l2-fwnode.h>
@@ -153,6 +154,10 @@
MAX96712_MAX_TX_PORTS + \
MAX96712_MAX_VPG_PORTS)
+#define MAX96712_FIRST_SOURCE_PAD MAX96712_MAX_RX_PORTS
+#define MAX96712_VPG_PAD (MAX96712_FIRST_SOURCE_PAD + \
+ MAX96712_MAX_TX_PORTS)
+
#define MHZ(f) ((f) * 1000000U)
enum max96712_pattern {
@@ -194,6 +199,16 @@ struct max96712_priv {
enum max96712_pattern pattern;
};
+static inline bool max96712_pad_is_sink(u32 pad)
+{
+ return pad < MAX96712_FIRST_SOURCE_PAD || pad == MAX96712_VPG_PAD;
+}
+
+static inline bool max96712_pad_is_source(u32 pad)
+{
+ return pad >= MAX96712_FIRST_SOURCE_PAD && pad < MAX96712_VPG_PAD;
+}
+
static int max96712_write(struct max96712_priv *priv, unsigned int reg, u64 val)
{
int ret;
@@ -364,27 +379,119 @@ static void max96712_pattern_enable(struct max96712_priv *priv, bool enable)
}
}
-static int max96712_s_stream(struct v4l2_subdev *sd, int enable)
+static int max96712_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
+ struct v4l2_mbus_frame_desc *fd)
{
- struct max96712_priv *priv = v4l2_get_subdevdata(sd);
+ struct max96712_priv *priv = container_of(sd, struct max96712_priv, sd);
+ struct device *dev = &priv->client->dev;
+ struct v4l2_subdev_state *state;
+ struct v4l2_subdev_route *route;
+ struct media_pad *remote_pad;
+ int ret = 0;
+ int i;
- if (enable) {
- max96712_pattern_enable(priv, true);
- max96712_mipi_enable(priv, true);
- } else {
- max96712_mipi_enable(priv, false);
- max96712_pattern_enable(priv, false);
+ if (!max96712_pad_is_source(pad))
+ return -EINVAL;
+
+ memset(fd, 0, sizeof(*fd));
+
+ fd->type = V4L2_MBUS_FRAME_DESC_TYPE_CSI2;
+
+ state = v4l2_subdev_lock_and_get_active_state(sd);
+
+ for_each_active_route(&state->routing, route) {
+ struct v4l2_mbus_frame_desc_entry *source_entry = NULL;
+ struct v4l2_mbus_frame_desc source_fd = {0};
+
+ if (route->source_pad != pad)
+ continue;
+
+ if (route->sink_pad == MAX96712_VPG_PAD) {
+ fd->entry[fd->num_entries].stream = route->source_stream;
+ fd->entry[fd->num_entries].pixelcode = MEDIA_BUS_FMT_RGB888_1X24;
+ fd->entry[fd->num_entries].bus.csi2.vc = 0;
+ fd->entry[fd->num_entries].bus.csi2.dt = MIPI_CSI2_DT_RGB888;
+ fd->num_entries++;
+ continue;
+ }
+
+ remote_pad = media_pad_remote_pad_first(&priv->pads[route->sink_pad]);
+ if (!remote_pad) {
+ dev_dbg(dev, "no remote pad found for sink pad\n");
+ ret = -EPIPE;
+ goto unlock_state;
+ }
+
+ ret = v4l2_subdev_call(priv->rx_ports[route->sink_pad].sd, pad, get_frame_desc,
+ remote_pad->index, &source_fd);
+ if (ret) {
+ dev_err(dev, "Failed to get source frame desc for pad %u\n",
+ route->sink_pad);
+
+ goto unlock_state;
+ }
+
+ 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(dev, "Failed to find stream from source frame desc\n");
+
+ ret = -EPIPE;
+ goto unlock_state;
+ }
+
+ 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++;
}
+unlock_state:
+ v4l2_subdev_unlock_state(state);
+
+ return ret;
+}
+
+static int max96712_enable_streams(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ u32 source_pad, u64 streams_mask)
+{
+ struct max96712_priv *priv = v4l2_get_subdevdata(sd);
+
+ max96712_pattern_enable(priv, true);
+ max96712_mipi_enable(priv, true);
+
+ return 0;
+}
+
+static int max96712_disable_streams(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ u32 source_pad, u64 streams_mask)
+{
+ struct max96712_priv *priv = v4l2_get_subdevdata(sd);
+
+ max96712_mipi_enable(priv, false);
+ max96712_pattern_enable(priv, false);
+
return 0;
}
static const struct v4l2_subdev_video_ops max96712_video_ops = {
- .s_stream = max96712_s_stream,
+ .s_stream = v4l2_subdev_s_stream_helper,
};
-static int max96712_init_state(struct v4l2_subdev *sd,
- struct v4l2_subdev_state *state)
+static int _max96712_set_routing(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_krouting *routing)
{
static const struct v4l2_mbus_framefmt default_fmt = {
.width = 1920,
@@ -396,15 +503,43 @@ static int max96712_init_state(struct v4l2_subdev *sd,
.quantization = V4L2_QUANTIZATION_DEFAULT,
.xfer_func = V4L2_XFER_FUNC_DEFAULT,
};
- struct v4l2_mbus_framefmt *fmt;
- int i;
+ int ret;
- for (i = 0; i < MAX96712_MAX_PORTS; i++) {
- fmt = v4l2_subdev_state_get_format(state, i);
- *fmt = default_fmt;
- }
+ ret = v4l2_subdev_routing_validate(sd, routing, V4L2_SUBDEV_ROUTING_ONLY_1_TO_1);
+ if (ret)
+ return ret;
- return 0;
+ return v4l2_subdev_set_routing_with_fmt(sd, state, routing, &default_fmt);
+}
+
+static int max96712_set_routing(struct v4l2_subdev *sd, struct v4l2_subdev_state *state,
+ enum v4l2_subdev_format_whence which,
+ struct v4l2_subdev_krouting *routing)
+{
+ if (which == V4L2_SUBDEV_FORMAT_ACTIVE && media_entity_is_streaming(&sd->entity))
+ return -EBUSY;
+
+ return _max96712_set_routing(sd, state, routing);
+}
+
+static int max96712_init_state(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state)
+{
+ struct v4l2_subdev_route routes[] = {
+ {
+ .sink_pad = MAX96712_VPG_PAD,
+ .sink_stream = 0,
+ .source_pad = MAX96712_FIRST_SOURCE_PAD,
+ .source_stream = 0,
+ .flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE,
+ },
+ };
+ struct v4l2_subdev_krouting routing = {
+ .num_routes = ARRAY_SIZE(routes),
+ .routes = routes,
+ };
+
+ return _max96712_set_routing(sd, state, &routing);
}
static const struct v4l2_subdev_internal_ops max96712_internal_ops = {
@@ -414,6 +549,10 @@ static const struct v4l2_subdev_internal_ops max96712_internal_ops = {
static const struct v4l2_subdev_pad_ops max96712_pad_ops = {
.get_fmt = v4l2_subdev_get_fmt,
.set_fmt = v4l2_subdev_get_fmt,
+ .enable_streams = max96712_enable_streams,
+ .disable_streams = max96712_disable_streams,
+ .set_routing = max96712_set_routing,
+ .get_frame_desc = max96712_get_frame_desc,
};
static const struct v4l2_subdev_ops max96712_subdev_ops = {
@@ -453,7 +592,7 @@ static int max96712_v4l2_register(struct max96712_priv *priv)
priv->sd.internal_ops = &max96712_internal_ops;
v4l2_i2c_subdev_init(&priv->sd, priv->client, &max96712_subdev_ops);
- priv->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+ priv->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_STREAMS;
priv->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
v4l2_ctrl_handler_init(&priv->ctrl_handler, 2);
--
2.44.1
