media: subdev: Refactor v4l2_subdev_enable/disable_streams()

Add two internal helper functions, v4l2_subdev_collect_streams() and
v4l2_subdev_set_streams_enabled(), which allows us to refactor
v4l2_subdev_enable/disable_streams() functions.

This (I think) makes the code a bit easier to read, and lets us more
easily add new functionality in the helper functions in the following
patch.

Reviewed-by: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
Tested-by: Umang Jain <umang.jain@ideasonboard.com>
Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>
Signed-off-by: Sakari Ailus <sakari.ailus@linux.intel.com>
Signed-off-by: Hans Verkuil <hverkuil-cisco@xs4all.nl>

diff --git a/drivers/media/v4l2-core/v4l2-subdev.c b/drivers/media/v4l2-core/v4l2-subdev.c
index daf3636c5aabed54ee05a53c071bedada2a2c015..cad7e2479f2aa8081928cfabd47f732be91c8db0 100644 (file)
--- a/drivers/media/v4l2-core/v4l2-subdev.c
+++ b/drivers/media/v4l2-core/v4l2-subdev.c
@@ -2155,6 +2155,42 @@ out:
 }
 EXPORT_SYMBOL_GPL(v4l2_subdev_routing_validate);
 
+static void v4l2_subdev_collect_streams(struct v4l2_subdev *sd,
+                                       struct v4l2_subdev_state *state,
+                                       u32 pad, u64 streams_mask,
+                                       u64 *found_streams,
+                                       u64 *enabled_streams)
+{
+       *found_streams = 0;
+       *enabled_streams = 0;
+
+       for (unsigned int i = 0; i < state->stream_configs.num_configs; ++i) {
+               const struct v4l2_subdev_stream_config *cfg =
+                       &state->stream_configs.configs[i];
+
+               if (cfg->pad != pad || !(streams_mask & BIT_ULL(cfg->stream)))
+                       continue;
+
+               *found_streams |= BIT_ULL(cfg->stream);
+               if (cfg->enabled)
+                       *enabled_streams |= BIT_ULL(cfg->stream);
+       }
+}
+
+static void v4l2_subdev_set_streams_enabled(struct v4l2_subdev *sd,
+                                           struct v4l2_subdev_state *state,
+                                           u32 pad, u64 streams_mask,
+                                           bool enabled)
+{
+       for (unsigned int i = 0; i < state->stream_configs.num_configs; ++i) {
+               struct v4l2_subdev_stream_config *cfg =
+                       &state->stream_configs.configs[i];
+
+               if (cfg->pad == pad && (streams_mask & BIT_ULL(cfg->stream)))
+                       cfg->enabled = enabled;
+       }
+}
+
 static int v4l2_subdev_enable_streams_fallback(struct v4l2_subdev *sd, u32 pad,
                                               u64 streams_mask)
 {
@@ -2206,8 +2242,8 @@ int v4l2_subdev_enable_streams(struct v4l2_subdev *sd, u32 pad,
        struct device *dev = sd->entity.graph_obj.mdev->dev;
        struct v4l2_subdev_state *state;
        bool already_streaming;
-       u64 found_streams = 0;
-       unsigned int i;
+       u64 enabled_streams;
+       u64 found_streams;
        int ret;
 
        /* A few basic sanity checks first. */
@@ -2228,22 +2264,9 @@ int v4l2_subdev_enable_streams(struct v4l2_subdev *sd, u32 pad,
         * Verify that the requested streams exist and that they are not
         * already enabled.
         */
-       for (i = 0; i < state->stream_configs.num_configs; ++i) {
-               struct v4l2_subdev_stream_config *cfg =
-                       &state->stream_configs.configs[i];
 
-               if (cfg->pad != pad || !(streams_mask & BIT_ULL(cfg->stream)))
-                       continue;
-
-               found_streams |= BIT_ULL(cfg->stream);
-
-               if (cfg->enabled) {
-                       dev_dbg(dev, "stream %u already enabled on %s:%u\n",
-                               cfg->stream, sd->entity.name, pad);
-                       ret = -EALREADY;
-                       goto done;
-               }
-       }
+       v4l2_subdev_collect_streams(sd, state, pad, streams_mask,
+                                   &found_streams, &enabled_streams);
 
        if (found_streams != streams_mask) {
                dev_dbg(dev, "streams 0x%llx not found on %s:%u\n",
@@ -2252,6 +2275,13 @@ int v4l2_subdev_enable_streams(struct v4l2_subdev *sd, u32 pad,
                goto done;
        }
 
+       if (enabled_streams) {
+               dev_dbg(dev, "streams 0x%llx already enabled on %s:%u\n",
+                       enabled_streams, sd->entity.name, pad);
+               ret = -EALREADY;
+               goto done;
+       }
+
        dev_dbg(dev, "enable streams %u:%#llx\n", pad, streams_mask);
 
        already_streaming = v4l2_subdev_is_streaming(sd);
@@ -2266,13 +2296,7 @@ int v4l2_subdev_enable_streams(struct v4l2_subdev *sd, u32 pad,
        }
 
        /* Mark the streams as enabled. */
-       for (i = 0; i < state->stream_configs.num_configs; ++i) {
-               struct v4l2_subdev_stream_config *cfg =
-                       &state->stream_configs.configs[i];
-
-               if (cfg->pad == pad && (streams_mask & BIT_ULL(cfg->stream)))
-                       cfg->enabled = true;
-       }
+       v4l2_subdev_set_streams_enabled(sd, state, pad, streams_mask, true);
 
        if (!already_streaming)
                v4l2_subdev_enable_privacy_led(sd);
@@ -2334,8 +2358,8 @@ int v4l2_subdev_disable_streams(struct v4l2_subdev *sd, u32 pad,
 {
        struct device *dev = sd->entity.graph_obj.mdev->dev;
        struct v4l2_subdev_state *state;
-       u64 found_streams = 0;
-       unsigned int i;
+       u64 enabled_streams;
+       u64 found_streams;
        int ret;
 
        /* A few basic sanity checks first. */
@@ -2356,22 +2380,9 @@ int v4l2_subdev_disable_streams(struct v4l2_subdev *sd, u32 pad,
         * Verify that the requested streams exist and that they are not
         * already disabled.
         */
-       for (i = 0; i < state->stream_configs.num_configs; ++i) {
-               struct v4l2_subdev_stream_config *cfg =
-                       &state->stream_configs.configs[i];
 
-               if (cfg->pad != pad || !(streams_mask & BIT_ULL(cfg->stream)))
-                       continue;
-
-               found_streams |= BIT_ULL(cfg->stream);
-
-               if (!cfg->enabled) {
-                       dev_dbg(dev, "stream %u already disabled on %s:%u\n",
-                               cfg->stream, sd->entity.name, pad);
-                       ret = -EALREADY;
-                       goto done;
-               }
-       }
+       v4l2_subdev_collect_streams(sd, state, pad, streams_mask,
+                                   &found_streams, &enabled_streams);
 
        if (found_streams != streams_mask) {
                dev_dbg(dev, "streams 0x%llx not found on %s:%u\n",
@@ -2380,6 +2391,13 @@ int v4l2_subdev_disable_streams(struct v4l2_subdev *sd, u32 pad,
                goto done;
        }
 
+       if (enabled_streams != streams_mask) {
+               dev_dbg(dev, "streams 0x%llx already disabled on %s:%u\n",
+                       streams_mask & ~enabled_streams, sd->entity.name, pad);
+               ret = -EALREADY;
+               goto done;
+       }
+
        dev_dbg(dev, "disable streams %u:%#llx\n", pad, streams_mask);
 
        /* Call the .disable_streams() operation. */
@@ -2391,14 +2409,7 @@ int v4l2_subdev_disable_streams(struct v4l2_subdev *sd, u32 pad,
                goto done;
        }
 
-       /* Mark the streams as disabled. */
-       for (i = 0; i < state->stream_configs.num_configs; ++i) {
-               struct v4l2_subdev_stream_config *cfg =
-                       &state->stream_configs.configs[i];
-
-               if (cfg->pad == pad && (streams_mask & BIT_ULL(cfg->stream)))
-                       cfg->enabled = false;
-       }
+       v4l2_subdev_set_streams_enabled(sd, state, pad, streams_mask, false);
 
 done:
        if (!v4l2_subdev_is_streaming(sd))