sim: support camera framework multi-instance and dynamic mounting

Allow the SIM camera/V4L2 capture framework to manage multiple
imgdata instances with dynamic mounting. This avoids cross-talk
between camera streams when multiple devices are used.

Signed-off-by: Peter Bee <bijunda@bytedance.com>

arch/sim/Kconfig

@@ -424,12 +424,8 @@ config SIM_CAMERA_V4L2
 
 endchoice
 
-config HOST_CAMERA_DEV_PATH
-	string "Host camera device path"
-	default "/dev/video0"
-
 config SIM_CAMERA_DEV_PATH
-	string "NuttX video device path"
+	string "NuttX video device path prefix"
 	default "/dev/video"
 
 endif

arch/sim/src/sim/posix/sim_host_v4l2.c

@@ -25,12 +25,14 @@
  ****************************************************************************/
 
 #include <errno.h>
+#include <dirent.h>
 #include <fcntl.h>
 #include <stdbool.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <stdint.h>
 #include <string.h>
+#include <limits.h>
 #include <syslog.h>
 #include <unistd.h>
 #include <linux/videodev2.h>
@@ -44,7 +46,6 @@
  ****************************************************************************/
 
 #define MAX_REQBUFS 3
-
 #define WARN(fmt, ...) \
         syslog(LOG_WARNING, "sim_host_video: " fmt "\n", ##__VA_ARGS__)
 
@@ -55,6 +56,7 @@
 struct host_video_dev_s
 {
   int fd;
+  char path[PATH_MAX];
   void *addrs[MAX_REQBUFS];
   size_t buflen[MAX_REQBUFS];
 };
@@ -84,24 +86,237 @@ static int host_video_ioctl(int fd, int request, void *arg)
   return r;
 }
 
+static int host_video_parse_device_index(const char *name, int *index)
+{
+  long value;
+  char *endptr;
+
+  if (strncmp(name, "video", 5) != 0)
+    {
+      return -EINVAL;
+    }
+
+  name += 5;
+  if (*name == '\0')
+    {
+      return -EINVAL;
+    }
+
+  value = strtol(name, &endptr, 10);
+  if (*endptr != '\0' || value < 0 || value > INT_MAX)
+    {
+      return -EINVAL;
+    }
+
+  *index = value;
+  return 0;
+}
+
+static int host_video_get_next_device_path(int current_index,
+                                           char *devpath,
+                                           size_t devpathlen,
+                                           int *next_index)
+{
+  DIR *dir;
+  struct dirent *entry;
+  int candidate;
+  int found = INT_MAX;
+
+  dir = opendir("/dev");
+  if (dir == NULL)
+    {
+      return -errno;
+    }
+
+  while ((entry = readdir(dir)) != NULL)
+    {
+      if (host_video_parse_device_index(entry->d_name, &candidate) < 0 ||
+          candidate <= current_index || candidate >= found)
+        {
+          continue;
+        }
+
+      found = candidate;
+    }
+
+  closedir(dir);
+
+  if (found == INT_MAX)
+    {
+      return -ENODEV;
+    }
+
+  if (snprintf(devpath, devpathlen, "/dev/video%d", found) >=
+      (int)devpathlen)
+    {
+      return -ENAMETOOLONG;
+    }
+
+  *next_index = found;
+  return 0;
+}
+
+static bool host_video_is_capture_device(const char *host_video_dev_path)
+{
+  struct v4l2_capability cap;
+  int fd;
+  bool available = false;
+
+  fd = open(host_video_dev_path, O_RDWR | O_NONBLOCK);
+  if (fd < 0)
+    {
+      fd = open(host_video_dev_path, O_RDONLY | O_NONBLOCK);
+      if (fd < 0)
+        {
+          return false;
+        }
+    }
+
+  memset(&cap, 0, sizeof(cap));
+  if (host_video_ioctl(fd, VIDIOC_QUERYCAP, &cap) == 0)
+    {
+      uint32_t capabilities = cap.device_caps != 0 ? cap.device_caps :
+                              cap.capabilities;
+
+      if ((capabilities & V4L2_CAP_VIDEO_CAPTURE) != 0)
+        {
+          available = true;
+        }
+    }
+
+  close(fd);
+  return available;
+}
+
+static int host_video_get_device_path_by_index(int index,
+                                               char *devpath,
+                                               size_t devpathlen)
+{
+  int count = 0;
+  int current_index = -1;
+  char path[PATH_MAX];
+
+  while ((host_video_get_next_device_path(current_index, path,
+                                          sizeof(path),
+                                          &current_index)) == 0)
+    {
+      if (!host_video_is_capture_device(path))
+        {
+          continue;
+        }
+
+      if (count == index)
+        {
+          if (snprintf(devpath, devpathlen, "%s", path) >=
+              (int)devpathlen)
+            {
+              return -ENAMETOOLONG;
+            }
+
+          return 0;
+        }
+
+      count++;
+    }
+
+  return -ENODEV;
+}
+
+static int host_video_resolve_device_path(const char *host_video_dev_path,
+                                          char *resolved_path,
+                                          size_t resolved_path_len)
+{
+  const char *name;
+  char *endptr;
+  long index;
+  int ret;
+
+  name = strrchr(host_video_dev_path, '/');
+  name = name != NULL ? name + 1 : host_video_dev_path;
+
+  if (strncmp(name, "video", 5) != 0)
+    {
+      if (snprintf(resolved_path, resolved_path_len, "%s",
+                   host_video_dev_path) >= (int)resolved_path_len)
+        {
+          return -ENAMETOOLONG;
+        }
+
+      return 0;
+    }
+
+  index = strtol(name + 5, &endptr, 10);
+  if (endptr == name + 5 || *endptr != '\0' || index < 0 ||
+      index > INT_MAX)
+    {
+      return -EINVAL;
+    }
+
+  ret = host_video_get_device_path_by_index(index, resolved_path,
+                                            resolved_path_len);
+
+  if (ret < 0)
+    {
+      WARN("failed to resolve %s to host capture device: %d",
+           host_video_dev_path, ret);
+    }
+
+  return ret;
+}
+
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
 
+int host_video_get_device_count(void)
+{
+  int count = 0;
+  char devpath[PATH_MAX];
+
+  while (host_video_get_device_path_by_index(count, devpath,
+                                             sizeof(devpath)) == 0)
+    {
+      count++;
+    }
+
+  return count > 0 ? count : -ENODEV;
+}
+
 bool host_video_is_available(const char *host_video_dev_path)
 {
-  return access(host_video_dev_path, F_OK) == 0;
+  char resolved_path[PATH_MAX];
+  int ret;
+
+  ret = host_video_resolve_device_path(host_video_dev_path, resolved_path,
+                                       sizeof(resolved_path));
+  if (ret < 0)
+    {
+      return false;
+    }
+
+  return host_video_is_capture_device(resolved_path);
 }
 
 struct host_video_dev_s *host_video_init(const char *host_video_dev_path)
 {
   int fd;
+  int ret;
+  char resolved_path[PATH_MAX];
   struct host_video_dev_s *vdev;
 
-  fd = open(host_video_dev_path, O_RDWR | O_NONBLOCK);
+  ret = host_video_resolve_device_path(host_video_dev_path, resolved_path,
+                                       sizeof(resolved_path));
+  if (ret < 0)
+    {
+      errno = -ret;
+      perror(host_video_dev_path);
+      return NULL;
+    }
+
+  fd = open(resolved_path, O_RDWR | O_NONBLOCK);
   if (fd < 0)
     {
-      perror(host_video_dev_path);
+      perror(resolved_path);
       return NULL;
     }
 
@@ -114,6 +329,7 @@ struct host_video_dev_s *host_video_init(const char *host_video_dev_path)
     }
 
   vdev->fd = fd;
+  snprintf(vdev->path, sizeof(vdev->path), "%s", resolved_path);
   return vdev;
 }
 
@@ -292,19 +508,59 @@ int host_video_set_fmt(struct host_video_dev_s *vdev,
       return -errno;
     }
 
+  if (v4l2_fmt.fmt.pix.pixelformat != fmt)
+    {
+      WARN("%s fallback pixel format from %08x to %08x",
+           vdev->path, fmt, v4l2_fmt.fmt.pix.pixelformat);
+      return -EINVAL;
+    }
+
+  if (v4l2_fmt.fmt.pix.width != width || v4l2_fmt.fmt.pix.height != height)
+    {
+      WARN("%s fallback frame size from %ux%u to %ux%u",
+           vdev->path, width, height,
+           v4l2_fmt.fmt.pix.width, v4l2_fmt.fmt.pix.height);
+      return -EINVAL;
+    }
+
+  if (denom == 0 || numer == 0)
+    {
+      return 0; /* Keep default frame interval */
+    }
+
   memset(&streamparm, 0, sizeof(streamparm));
   streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
   if (-1 == host_video_ioctl(vdev->fd, VIDIOC_G_PARM, &streamparm))
     {
+      if (errno == EINVAL)
+        {
+          WARN("%s does not support VIDIOC_G_PARM, keep default "
+               "frame interval", vdev->path);
+          return 0; /* Keep default frame interval */
+        }
+
       perror("VIDIOC_G_PARM");
       return -errno;
     }
 
-  streamparm.parm.capture.capturemode |= V4L2_CAP_TIMEPERFRAME;
+  if ((streamparm.parm.capture.capability & V4L2_CAP_TIMEPERFRAME) == 0)
+    {
+      WARN("%s does not support programmable frame interval",
+           vdev->path);
+      return 0; /* Keep default frame interval */
+    }
+
   streamparm.parm.capture.timeperframe.numerator = numer;
   streamparm.parm.capture.timeperframe.denominator = denom;
   if (-1 == host_video_ioctl(vdev->fd, VIDIOC_S_PARM, &streamparm))
     {
+      if (errno == EINVAL)
+        {
+          WARN("%s rejected frame interval %u/%u, keep default",
+               vdev->path, numer, denom);
+          return 0; /* Keep default frame interval */
+        }
+
       perror("VIDIOC_S_PARM");
       return -errno;
     }
@@ -334,32 +590,52 @@ int host_video_try_fmt(struct host_video_dev_s *vdev,
 
   if (v4l2_fmt.fmt.pix.pixelformat != fmt)
     {
-      WARN("Pixel format not supported");
+      WARN("%s does not support pixel format %08x, fallback to %08x",
+           vdev->path, fmt, v4l2_fmt.fmt.pix.pixelformat);
+      return -EINVAL;
+    }
+
+  if (v4l2_fmt.fmt.pix.width != width || v4l2_fmt.fmt.pix.height != height)
+    {
+      WARN("%s does not support frame size %ux%u, fallback to %ux%u",
+           vdev->path, width, height,
+           v4l2_fmt.fmt.pix.width, v4l2_fmt.fmt.pix.height);
       return -EINVAL;
     }
 
   /* Need not check frame interval for STILL type */
 
-  if (!denom)
+  if (denom == 0 || numer == 0)
     {
-      memset(&v4l2_frmival, 0, sizeof(v4l2_frmival));
-      v4l2_frmival.width = width;
-      v4l2_frmival.height = height;
-      v4l2_frmival.pixel_format = fmt;
-      while (host_video_ioctl(vdev->fd, VIDIOC_ENUM_FRAMEINTERVALS,
-                              &v4l2_frmival) == 0)
-        {
-          if (v4l2_frmival.type == V4L2_FRMSIZE_TYPE_DISCRETE &&
-              v4l2_frmival.discrete.denominator == denom &&
-              v4l2_frmival.discrete.numerator == numer)
-            {
-              return 0;
-            }
+      return 0;
+    }
 
-          v4l2_frmival.index++;
+  memset(&v4l2_frmival, 0, sizeof(v4l2_frmival));
+  v4l2_frmival.width = width;
+  v4l2_frmival.height = height;
+  v4l2_frmival.pixel_format = fmt;
+  while (host_video_ioctl(vdev->fd, VIDIOC_ENUM_FRAMEINTERVALS,
+                          &v4l2_frmival) == 0)
+    {
+      if (v4l2_frmival.type == V4L2_FRMIVAL_TYPE_DISCRETE &&
+          v4l2_frmival.discrete.denominator == denom &&
+          v4l2_frmival.discrete.numerator == numer)
+        {
+          return 0;
         }
 
-        WARN("Invalid frame interval, fallback to default");
+      v4l2_frmival.index++;
+    }
+
+  if (errno != EINVAL)
+    {
+      WARN("%s failed to enumerate frame intervals: %d",
+           vdev->path, errno);
+    }
+  else
+    {
+      WARN("%s does not expose frame interval %u/%u, fallback to default",
+           vdev->path, numer, denom);
     }
 
   return 0;

arch/sim/src/sim/sim_camera.c

@@ -25,8 +25,15 @@
  ****************************************************************************/
 
 #include <errno.h>
-#include <string.h>
+#include <inttypes.h>
+#include <stdio.h>
+#include <debug.h>
+
+#include <nuttx/clock.h>
+#include <nuttx/kmalloc.h>
+#include <nuttx/nuttx.h>
 #include <nuttx/wdog.h>
+#include <nuttx/video/v4l2_cap.h>
 #include <nuttx/video/imgsensor.h>
 #include <nuttx/video/imgdata.h>
 #include <nuttx/video/video.h>
@@ -44,18 +51,22 @@
  * Private Types
  ****************************************************************************/
 
-typedef struct
+typedef struct sim_camera_priv_s sim_camera_priv_t;
+
+struct sim_camera_priv_s
 {
   struct imgdata_s data;
   struct imgsensor_s sensor;
   imgdata_capture_t capture_cb;
   void *capture_arg;
   uint32_t buf_size;
-  uint8_t  *next_buf;
-  struct timeval *next_ts;
+  uint8_t *next_buf;
   struct host_video_dev_s *vdev;
   struct wdog_s wdog;
-} sim_camera_priv_t;
+  bool capture_started;
+  int index;
+  char devpath[32];
+};
 
 /****************************************************************************
  * Private Function Prototypes
@@ -100,6 +111,7 @@ static int sim_camera_data_set_buf(struct imgdata_s *data,
                                    uint8_t nr_datafmts,
                                    imgdata_format_t *datafmts,
                                    uint8_t *addr, uint32_t size);
+static void sim_camera_interrupt(wdparm_t arg);
 
 /****************************************************************************
  * Private Data
@@ -138,30 +150,6 @@ static const struct v4l2_frmsizeenum g_frmsizes[] =
   }
 };
 
-static struct v4l2_fmtdesc g_fmts[] =
-{
-  {
-    .pixelformat = V4L2_PIX_FMT_YUV420,
-    .description = "YUV420",
-  }
-};
-
-static sim_camera_priv_t g_sim_camera_priv =
-{
-  .data =
-  {
-    &g_sim_camera_data_ops
-  },
-  .sensor =
-  {
-    .ops = &g_sim_camera_ops,
-    .frmsizes_num = 1,
-    .frmsizes = g_frmsizes,
-    .fmtdescs_num = 1,
-    .fmtdescs = g_fmts,
-  }
-};
-
 /****************************************************************************
  * Private Functions
  ****************************************************************************/
@@ -215,7 +203,8 @@ static uint32_t imgdata_fmt_to_v4l2(uint32_t pixelformat)
 
 static bool sim_camera_is_available(struct imgsensor_s *sensor)
 {
-  return true;
+  sim_camera_priv_t *priv = container_of(sensor, sim_camera_priv_t, sensor);
+  return host_video_is_available(priv->devpath);
 }
 
 static int sim_camera_init(struct imgsensor_s *sensor)
@@ -239,7 +228,25 @@ static int sim_camera_validate_frame_setting(struct imgsensor_s *sensor,
                                              imgsensor_format_t *fmt,
                                              imgsensor_interval_t *interval)
 {
-  return 0;
+  sim_camera_priv_t *priv = container_of(sensor, sim_camera_priv_t, sensor);
+  uint32_t v4l2_fmt;
+
+  if (nr_fmt > 1)
+    {
+      return -ENOTSUP;
+    }
+
+  v4l2_fmt = imgdata_fmt_to_v4l2(fmt[IMGSENSOR_FMT_MAIN].pixelformat);
+  if (v4l2_fmt == 0)
+    {
+      verr("sim_camera[%d]: unsupported sensor pixfmt=%" PRIu32 "\n",
+           priv->index, fmt[IMGSENSOR_FMT_MAIN].pixelformat);
+      return -EINVAL;
+    }
+
+  return host_video_try_fmt(priv->vdev, fmt[IMGSENSOR_FMT_MAIN].width,
+                            fmt[IMGSENSOR_FMT_MAIN].height, v4l2_fmt,
+                            interval->denominator, interval->numerator);
 }
 
 static int sim_camera_start_capture(struct imgsensor_s *sensor,
@@ -261,12 +268,15 @@ static int sim_camera_stop_capture(struct imgsensor_s *sensor,
 
 static int sim_camera_data_init(struct imgdata_s *data)
 {
-  sim_camera_priv_t *priv = (sim_camera_priv_t *)data;
+  sim_camera_priv_t *priv = container_of(data, sim_camera_priv_t, data);
 
-  priv->vdev = host_video_init(CONFIG_HOST_CAMERA_DEV_PATH);
   if (priv->vdev == NULL)
     {
-      return -ENODEV;
+      priv->vdev = host_video_init(priv->devpath);
+      if (priv->vdev == NULL)
+        {
+          return -ENODEV;
+        }
     }
 
   return 0;
@@ -274,9 +284,16 @@ static int sim_camera_data_init(struct imgdata_s *data)
 
 static int sim_camera_data_uninit(struct imgdata_s *data)
 {
-  sim_camera_priv_t *priv = (sim_camera_priv_t *)data;
+  sim_camera_priv_t *priv = container_of(data, sim_camera_priv_t, data);
+  int ret = 0;
 
-  return host_video_uninit(priv->vdev);
+  if (priv->vdev != NULL)
+    {
+      ret = host_video_uninit(priv->vdev);
+      priv->vdev = NULL;
+    }
+
+  return ret;
 }
 
 static int sim_camera_data_validate_buf(uint8_t *addr, uint32_t size)
@@ -294,7 +311,7 @@ static int sim_camera_data_set_buf(struct imgdata_s *data,
                                    imgdata_format_t *datafmts,
                                    uint8_t *addr, uint32_t size)
 {
-  sim_camera_priv_t *priv = (sim_camera_priv_t *)data;
+  sim_camera_priv_t *priv = container_of(data, sim_camera_priv_t, data);
 
   if (sim_camera_data_validate_buf(addr, size) < 0)
     {
@@ -311,7 +328,7 @@ static int sim_camera_data_validate_frame_setting(struct imgdata_s *data,
                                                   imgdata_format_t *datafmt,
                                                   imgdata_interval_t *interv)
 {
-  sim_camera_priv_t *priv = (sim_camera_priv_t *)data;
+  sim_camera_priv_t *priv = container_of(data, sim_camera_priv_t, data);
   uint32_t v4l2_fmt;
 
   if (nr_datafmt > 1)
@@ -320,6 +337,13 @@ static int sim_camera_data_validate_frame_setting(struct imgdata_s *data,
     }
 
   v4l2_fmt = imgdata_fmt_to_v4l2(datafmt->pixelformat);
+  if (v4l2_fmt == 0)
+    {
+      verr("sim_camera[%d]: unsupported data pixfmt=%" PRIu32 "\n",
+           priv->index, datafmt->pixelformat);
+      return -EINVAL;
+    }
+
   return host_video_try_fmt(priv->vdev, datafmt->width,
                             datafmt->height, v4l2_fmt, interv->denominator,
                             interv->numerator);
@@ -332,7 +356,7 @@ static int sim_camera_data_start_capture(struct imgdata_s *data,
                                          imgdata_capture_t callback,
                                          void *arg)
 {
-  sim_camera_priv_t *priv = (sim_camera_priv_t *)data;
+  sim_camera_priv_t *priv = container_of(data, sim_camera_priv_t, data);
   int ret;
 
   ret = host_video_set_fmt(priv->vdev,
@@ -348,14 +372,26 @@ static int sim_camera_data_start_capture(struct imgdata_s *data,
 
   priv->capture_cb = callback;
   priv->capture_arg = arg;
-  return host_video_start_capture(priv->vdev);
+
+  ret = host_video_start_capture(priv->vdev);
+  if (ret < 0)
+    {
+      return ret;
+    }
+
+  priv->capture_started = true;
+  wd_start(&priv->wdog, SIM_CAMERA_PERIOD, sim_camera_interrupt,
+           (wdparm_t)priv);
+  return 0;
 }
 
 static int sim_camera_data_stop_capture(struct imgdata_s *data)
 {
-  sim_camera_priv_t *priv = (sim_camera_priv_t *)data;
+  sim_camera_priv_t *priv = container_of(data, sim_camera_priv_t, data);
 
   priv->next_buf = NULL;
+  priv->capture_started = false;
+  wd_cancel(&priv->wdog);
   return host_video_stop_capture(priv->vdev);
 }
 
@@ -366,7 +402,12 @@ static void sim_camera_interrupt(wdparm_t arg)
   struct timeval tv;
   int ret;
 
-  if (priv->next_buf)
+  if (priv == NULL)
+    {
+      return;
+    }
+
+  if (priv->next_buf != NULL)
     {
       ret = host_video_dqbuf(priv->vdev, priv->next_buf, priv->buf_size);
       if (ret > 0)
@@ -377,7 +418,11 @@ static void sim_camera_interrupt(wdparm_t arg)
         }
     }
 
-  wd_start_next(&priv->wdog, SIM_CAMERA_PERIOD, sim_camera_interrupt, arg);
+  if (priv->capture_started && priv->next_buf != NULL)
+    {
+      wd_start(&priv->wdog, SIM_CAMERA_PERIOD, sim_camera_interrupt,
+               (wdparm_t)priv);
+    }
 }
 
 /****************************************************************************
@@ -386,11 +431,43 @@ static void sim_camera_interrupt(wdparm_t arg)
 
 int sim_camera_initialize(void)
 {
-  sim_camera_priv_t *priv = &g_sim_camera_priv;
+  sim_camera_priv_t *privs;
+  int count;
+  int first_error = 0;
 
-  imgsensor_register(&priv->sensor);
-  imgdata_register(&priv->data);
+  count = host_video_get_device_count();
+  if (count < 0)
+    {
+      return count;
+    }
 
-  wd_start(&priv->wdog, 0, sim_camera_interrupt, (wdparm_t)priv);
-  return 0;
+  privs = kmm_zalloc(sizeof(sim_camera_priv_t) * count);
+  if (privs == NULL)
+    {
+      return -ENOMEM;
+    }
+
+  for (int i = 0; i < count; i++)
+    {
+      sim_camera_priv_t *priv = &privs[i];
+      FAR struct imgsensor_s *sensor;
+      int ret;
+
+      priv->data.ops = &g_sim_camera_data_ops;
+      priv->sensor.ops = &g_sim_camera_ops;
+      priv->sensor.frmsizes_num = 1;
+      priv->sensor.frmsizes = g_frmsizes;
+      sensor = &priv->sensor;
+      priv->index = i;
+      snprintf(priv->devpath, sizeof(priv->devpath), "%s%d",
+               CONFIG_SIM_CAMERA_DEV_PATH, i);
+
+      ret = capture_register(priv->devpath, &priv->data, &sensor, 1);
+      if (ret < 0 && first_error == 0)
+        {
+          first_error = ret;
+        }
+    }
+
+  return first_error;
 }

arch/sim/src/sim/sim_hostvideo.h

@@ -40,6 +40,7 @@ struct host_video_dev_s;
  * Public Function Prototypes
  ****************************************************************************/
 
+int host_video_get_device_count(void);
 bool host_video_is_available(const char *host_video_dev_path);
 struct host_video_dev_s *host_video_init(const char *host_video_dev_path);
 int host_video_uninit(struct host_video_dev_s *vdev);

boards/sim/sim/sim/src/sim_bringup.c

@@ -315,12 +315,10 @@ int sim_bringup(void)
 #ifdef CONFIG_SIM_CAMERA
   /* Initialize and register the simulated video driver */
 
-  sim_camera_initialize();
-
-  ret = capture_initialize(CONFIG_SIM_CAMERA_DEV_PATH);
+  ret = sim_camera_initialize();
   if (ret < 0)
     {
-      syslog(LOG_ERR, "ERROR: capture_initialize() failed: %d\n", ret);
+      syslog(LOG_ERR, "ERROR: sim_camera_initialize() failed: %d\n", ret);
     }
 
 #endif