From 1793c59818503be98be9c5eb32e2fa4d6e9e1a00 Mon Sep 17 00:00:00 2001 From: Kamil Trzcinski Date: Tue, 5 Apr 2022 22:25:35 +0200 Subject: [PATCH] Support subdevs --- cmd/camera.c | 2 + hw/device.c | 11 +++- hw/device.h | 4 ++ hw/device_media.c | 130 ++++++++++++++++++++++++++++++++++++++++++++++ hw/v4l2.h | 1 + 5 files changed, 146 insertions(+), 2 deletions(-) create mode 100644 hw/device_media.c diff --git a/cmd/camera.c b/cmd/camera.c index 11b5f9c..167182b 100644 --- a/cmd/camera.c +++ b/cmd/camera.c @@ -37,6 +37,8 @@ int camera_open(camera_t *camera) camera->camera->allow_dma = false; } + device_set_pad_format(camera->camera, camera->options.width, camera->options.height, 0); + if (device_open_buffer_list(camera->camera, true, camera->options.width, camera->options.height, camera->options.format, 0, camera->options.nbufs, true) < 0) { return -1; } diff --git a/hw/device.c b/hw/device.c index 6460453..4d244b2 100644 --- a/hw/device.c +++ b/hw/device.c @@ -7,6 +7,7 @@ device_t *device_open(const char *name, const char *path) { dev->name = strdup(name); dev->path = strdup(path); dev->fd = open(path, O_RDWR|O_NONBLOCK); + dev->subdev_fd = -1; dev->allow_dma = true; if(dev->fd < 0) { E_LOG_ERROR(dev, "Can't open device"); @@ -20,6 +21,8 @@ device_t *device_open(const char *name, const char *path) { } E_LOG_INFO(dev, "Device path=%s fd=%d opened", dev->path, dev->fd); + + dev->subdev_fd = device_open_v4l2_subdev(dev, 0); return dev; error: @@ -42,6 +45,10 @@ void device_close(device_t *dev) { dev->output_list = NULL; } + if (dev->subdev_fd >= 0) { + close(dev->subdev_fd); + } + if(dev->fd >= 0) { close(dev->fd); } @@ -222,8 +229,8 @@ int device_set_option(device_t *dev, const char *name, uint32_t id, int32_t valu ctl.id = id; ctl.value = value; E_LOG_DEBUG(dev, "Configuring option %s (%08x) = %d", name, id, value); - E_XIOCTL(dev, dev->fd, VIDIOC_S_CTRL, &ctl, "Can't set option %s", name); + E_XIOCTL(dev, dev->subdev_fd >= 0 ? dev->subdev_fd : dev->fd, VIDIOC_S_CTRL, &ctl, "Can't set option %s", name); return 0; error: return -1; -} \ No newline at end of file +} diff --git a/hw/device.h b/hw/device.h index 599a0fa..c88f037 100644 --- a/hw/device.h +++ b/hw/device.h @@ -6,6 +6,7 @@ typedef struct device_s { char *name; char *path; int fd; + int subdev_fd; struct v4l2_capability v4l2_cap; bool allow_dma; @@ -17,6 +18,8 @@ typedef struct device_s { } device_t; device_t *device_open(const char *name, const char *path); +int device_open_media_device(device_t *dev); +int device_open_v4l2_subdev(device_t *dev, int subdev); void device_close(device_t *device); int device_open_buffer_list(device_t *dev, bool do_capture, unsigned width, unsigned height, unsigned format, unsigned bytesperline, int nbufs, bool do_mmap); @@ -26,6 +29,7 @@ int device_consume_event(device_t *device); int device_stream(device_t *dev, bool do_on); int device_video_force_key(device_t *dev); +int device_set_pad_format(device_t *device, unsigned width, unsigned height, unsigned format); int device_set_option(device_t *dev, const char *name, uint32_t id, int32_t value); int device_set_fps(device_t *dev, int desired_fps); diff --git a/hw/device_media.c b/hw/device_media.c new file mode 100644 index 0000000..b6eae1b --- /dev/null +++ b/hw/device_media.c @@ -0,0 +1,130 @@ +#include "hw/device.h" + +#include +#include +#include +#include +#include +#include + +int device_open_media_device(device_t *dev) +{ + struct stat st; + if (fstat(dev->fd, &st) < 0) { + E_LOG_ERROR(dev, "Cannot get fstat"); + return -1; + } + if (~st.st_mode & S_IFCHR) { + E_LOG_ERROR(dev, "FD is not char"); + return -1; + } + + char path[256]; + sprintf(path, "/sys/dev/char/%d:%d/device", major(st.st_rdev), minor(st.st_rdev)); + + struct dirent **namelist; + int n = scandir(path, &namelist, NULL, NULL); + if (n < 0) { + E_LOG_ERROR(dev, "Cannot scan: %s", path); + return -1; + } + + int ret = -1; + while (n--) { + if (ret == -1 && strstr(namelist[n]->d_name, "media") == namelist[n]->d_name) { + path[0] = 0; + sprintf(path, "/dev/%s", namelist[n]->d_name); + ret = open(path, O_RDWR); + if (ret >= 0) { + E_LOG_VERBOSE(dev, "Opened '%s' (fd=%d)", path, ret); + } + } + + free(namelist[n]); + } + free(namelist); + +error: + return ret; +} + +int device_open_v4l2_subdev(device_t *dev, int subdev) +{ + int media_fd = -1; + unsigned int last_id = 0; + int ret = -1; + + media_fd = device_open_media_device(dev); + if (media_fd < 0) { + E_LOG_ERROR(dev, "Cannot find media controller"); + } + + for (;;) { + struct media_entity_desc entity = { + .id = MEDIA_ENT_ID_FLAG_NEXT | last_id, + }; + + int rc = ioctl(media_fd, MEDIA_IOC_ENUM_ENTITIES, &entity); + if (rc < 0 && errno == EINVAL) + break; + + if (rc < 0) { + goto error; + } + + last_id = entity.id; + char path[256]; + sprintf(path, "/sys/dev/char/%d:%d", entity.dev.major, entity.dev.minor); + + char link[256]; + if (readlink(path, link, sizeof(link)) < 0) { + E_LOG_ERROR(dev, "Cannot readlink '%s'", path); + goto error; + } + + char * last = strrchr(link, '/'); + if (!last) { + E_LOG_ERROR(dev, "Link '%s' for '%s' does not end with '/'", link, path); + goto error; + } + + if (strstr(last, "/v4l-subdev") != last) { + goto error; + } + + sprintf(path, "/dev%s", last); + ret = open(path, O_RDWR); + if (ret >= 0) { + E_LOG_INFO(dev, "Opened '%s' (fd=%d)", path, ret); + } + break; + } + +error: + close(media_fd); + return ret; +} + +int device_set_pad_format(device_t *dev, unsigned width, unsigned height, unsigned format) +{ + struct v4l2_subdev_format fmt = {0}; + + if (dev->subdev_fd < 0) { + return -1; + } + + fmt.pad = 0; + fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE; + fmt.format.code = format; + fmt.format.width = width; + fmt.format.height = height; + fmt.format.colorspace = V4L2_COLORSPACE_RAW; + fmt.format.field = V4L2_FIELD_ANY; + + E_LOG_DEBUG(dev, "Configuring mpad %d (subdev_fd=%d)...", fmt.pad, dev->subdev_fd); + E_XIOCTL(dev, dev->subdev_fd, VIDIOC_SUBDEV_S_FMT, &fmt, "Can't configure mpad %d (subdev_fd=%d)", fmt.pad, dev->subdev_fd); + return 0; + +error: + return -1; +} diff --git a/hw/v4l2.h b/hw/v4l2.h index fc0867a..c4b883e 100644 --- a/hw/v4l2.h +++ b/hw/v4l2.h @@ -17,6 +17,7 @@ #include #include +#include #include "opts/log.h"