diff --git a/arch/sim/Kconfig b/arch/sim/Kconfig index 9c70c1fe948..3ce6b8bd4ce 100644 --- a/arch/sim/Kconfig +++ b/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 diff --git a/arch/sim/src/sim/posix/sim_host_v4l2.c b/arch/sim/src/sim/posix/sim_host_v4l2.c index dc36b5db6b0..880b7fb5e47 100644 --- a/arch/sim/src/sim/posix/sim_host_v4l2.c +++ b/arch/sim/src/sim/posix/sim_host_v4l2.c @@ -25,12 +25,14 @@ ****************************************************************************/ #include +#include #include #include #include #include #include #include +#include #include #include #include @@ -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), + ¤t_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; diff --git a/arch/sim/src/sim/sim_camera.c b/arch/sim/src/sim/sim_camera.c index c34fb6d3552..ef12b3f4843 100644 --- a/arch/sim/src/sim/sim_camera.c +++ b/arch/sim/src/sim/sim_camera.c @@ -25,8 +25,15 @@ ****************************************************************************/ #include -#include +#include +#include +#include + +#include +#include +#include #include +#include #include #include #include @@ -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; } diff --git a/arch/sim/src/sim/sim_hostvideo.h b/arch/sim/src/sim/sim_hostvideo.h index 17ec196698d..162f32595d6 100644 --- a/arch/sim/src/sim/sim_hostvideo.h +++ b/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); diff --git a/boards/sim/sim/sim/src/sim_bringup.c b/boards/sim/sim/sim/src/sim_bringup.c index cae5664176d..c839d223332 100644 --- a/boards/sim/sim/sim/src/sim_bringup.c +++ b/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