Support subdevs

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;
   }

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,7 +229,7 @@ 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;

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);
 

hw/device_media.c

@@ -0,0 +1,130 @@
+#include "hw/device.h"
+
+#include <unistd.h>
+#include <dirent.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <sys/sysmacros.h>
+#include <linux/media.h>
+
+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;
+}

hw/v4l2.h

@@ -17,6 +17,7 @@
 #include <sys/mman.h>
 
 #include <linux/videodev2.h>
+#include <linux/v4l2-subdev.h>
 
 #include "opts/log.h"