libv4l: Adjust src fmt for fps when app does a S_PARM with new fps

Signed-off-by: Hans de Goede <hdegoede@redhat.com>

diff --git a/lib/libv4l2/libv4l2-priv.h b/lib/libv4l2/libv4l2-priv.h
index 46d610343790dbb9f9b5e9780701e5b7b60fda48..ad5240e83efc46ef0e7d25fc8d311ad9e3038191 100644 (file)
--- a/lib/libv4l2/libv4l2-priv.h
+++ b/lib/libv4l2/libv4l2-priv.h
@@ -33,6 +33,7 @@
 #define V4L2_DEFAULT_NREADBUFFERS 4
 #define V4L2_FRAME_BUF_SIZE (4096 * 4096)
 #define V4L2_IGNORE_FIRST_FRAME_ERRORS 3
+#define V4L2_DEFAULT_FPS 30
 
 #define V4L2_LOG_ERR(...)                      \
        do {                                    \
@@ -73,6 +74,7 @@ struct v4l2_dev_info {
        pthread_mutex_t stream_lock;
        unsigned int no_frames;
        unsigned int nreadbuffers;
+       int fps;
        int first_frame;
        struct v4lconvert_data *convert;
        unsigned char *convert_mmap_buf;

diff --git a/lib/libv4l2/libv4l2.c b/lib/libv4l2/libv4l2.c
index 95d62e78ed110352a3cb80121b7d830c995cdda2..5fb1c38eedda445c0d7f0eb6c40fdbc19c2daa4e 100644 (file)
--- a/lib/libv4l2/libv4l2.c
+++ b/lib/libv4l2/libv4l2.c
@@ -78,9 +78,12 @@
 #define V4L2_IS_UVC                    0x1000
 #define V4L2_STREAM_TOUCHED            0x2000
 #define V4L2_USE_READ_FOR_READ         0x4000
+#define V4L2_SUPPORTS_TIMEPERFRAME     0x8000
 
 #define V4L2_MMAP_OFFSET_MAGIC      0xABCDEF00u
 
+static void v4l2_adjust_src_fmt_to_fps(int index, int fps);
+
 static pthread_mutex_t v4l2_open_mutex = PTHREAD_MUTEX_INITIALIZER;
 static struct v4l2_dev_info devices[V4L2_MAX_DEVICES] = {
        { .fd = -1 },
@@ -519,6 +522,17 @@ static int v4l2_buffers_mapped(int index)
        return i != devices[index].no_frames;
 }
 
+static void v4l2_update_fps(int index, struct v4l2_streamparm *parm)
+{
+       if ((devices[index].flags & V4L2_SUPPORTS_TIMEPERFRAME) &&
+           parm->parm.capture.timeperframe.numerator != 0) {
+               int fps = parm->parm.capture.timeperframe.denominator;
+               fps += parm->parm.capture.timeperframe.numerator - 1;
+               fps /= parm->parm.capture.timeperframe.numerator;
+               devices[index].fps = fps;
+       } else
+               devices[index].fps = 0;
+}
 
 int v4l2_open(const char *file, int oflag, ...)
 {
@@ -560,6 +574,7 @@ int v4l2_fd_open(int fd, int v4l2_flags)
        char *lfname;
        struct v4l2_capability cap;
        struct v4l2_format fmt;
+       struct v4l2_streamparm parm;
        struct v4lconvert_data *convert;
 
        /* If no log file was set by the app, see if one was specified through the
@@ -594,6 +609,11 @@ int v4l2_fd_open(int fd, int v4l2_flags)
                return -1;
        }
 
+       /* Check for framerate setting support */
+       parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+       if (SYS_IOCTL(fd, VIDIOC_G_PARM, &parm))
+               parm.type = 0;
+
        /* init libv4lconvert */
        convert = v4lconvert_create(fd);
        if (!convert)
@@ -626,6 +646,9 @@ int v4l2_fd_open(int fd, int v4l2_flags)
        }
        if (!strcmp((char *)cap.driver, "uvcvideo"))
                devices[index].flags |= V4L2_IS_UVC;
+       if ((parm.type == V4L2_BUF_TYPE_VIDEO_CAPTURE) &&
+           (parm.parm.capture.capability & V4L2_CAP_TIMEPERFRAME))
+               devices[index].flags |= V4L2_SUPPORTS_TIMEPERFRAME;
        devices[index].open_count = 1;
        devices[index].src_fmt = fmt;
        devices[index].dest_fmt = fmt;
@@ -657,6 +680,12 @@ int v4l2_fd_open(int fd, int v4l2_flags)
        if (index >= devices_used)
                devices_used = index + 1;
 
+       /* Note we always tell v4lconvert to optimize src fmt selection for
+          our default fps, the only exception is the app explictly selecting
+          a framerate using the S_PARM ioctl after a S_FMT */
+       v4lconvert_set_fps(devices[index].convert, V4L2_DEFAULT_FPS);
+       v4l2_update_fps(index, &parm);
+
        V4L2_LOG("open: %d\n", fd);
 
        return fd;
@@ -783,6 +812,16 @@ static int v4l2_pix_fmt_compat(struct v4l2_format *a, struct v4l2_format *b)
        return 0;
 }
 
+static int v4l2_pix_fmt_identical(struct v4l2_format *a, struct v4l2_format *b)
+{
+       if (v4l2_pix_fmt_compat(a, b) &&
+           a->fmt.pix.bytesperline == b->fmt.pix.bytesperline &&
+           a->fmt.pix.sizeimage == b->fmt.pix.sizeimage)
+               return 1;
+
+       return 0;
+}
+
 static void v4l2_set_src_and_dest_format(int index,
                struct v4l2_format *src_fmt, struct v4l2_format *dest_fmt)
 {
@@ -876,6 +915,14 @@ int v4l2_ioctl(int fd, unsigned long int request, ...)
                        is_capture_request = 1;
                        stream_needs_locking = 1;
                }
+               break;
+       case VIDIOC_S_PARM:
+               if (((struct v4l2_streamparm *)arg)->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+                       is_capture_request = 1;
+                       if (devices[index].flags & V4L2_SUPPORTS_TIMEPERFRAME)
+                               stream_needs_locking = 1;
+               }
+               break;
        }
 
        if (!is_capture_request) {
@@ -1044,6 +1091,15 @@ int v4l2_ioctl(int fd, unsigned long int request, ...)
                }
 
                v4l2_set_src_and_dest_format(index, &src_fmt, dest_fmt);
+
+               if (devices[index].flags & V4L2_SUPPORTS_TIMEPERFRAME) {
+                       struct v4l2_streamparm parm = {
+                               .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
+                       };
+                       if (SYS_IOCTL(fd, VIDIOC_G_PARM, &parm) == 0)
+                               v4l2_update_fps(index, &parm);
+               }
+
                break;
        }
 
@@ -1192,6 +1248,27 @@ int v4l2_ioctl(int fd, unsigned long int request, ...)
                        result = v4l2_streamoff(index);
                break;
 
+       case VIDIOC_S_PARM: {
+               struct v4l2_streamparm *parm = arg;
+
+               /* See if libv4lconvert wishes to use a different src_fmt
+                  for the new framerate and set that first */
+               if ((devices[index].flags & V4L2_SUPPORTS_TIMEPERFRAME) &&
+                   parm->parm.capture.timeperframe.numerator != 0) {
+                       int fps = parm->parm.capture.timeperframe.denominator;
+                       fps += parm->parm.capture.timeperframe.numerator - 1;
+                       fps /= parm->parm.capture.timeperframe.numerator;
+                       v4l2_adjust_src_fmt_to_fps(index, fps);
+               }
+
+               result = SYS_IOCTL(devices[index].fd, VIDIOC_S_PARM, parm);
+               if (result)
+                       break;
+
+               v4l2_update_fps(index, parm);
+               break;
+       }
+
        default:
                result = SYS_IOCTL(fd, request, arg);
                break;
@@ -1207,6 +1284,70 @@ int v4l2_ioctl(int fd, unsigned long int request, ...)
        return result;
 }
 
+static void v4l2_adjust_src_fmt_to_fps(int index, int fps)
+{
+       struct v4l2_pix_format req_pix_fmt;
+       struct v4l2_format src_fmt;
+       struct v4l2_format dest_fmt = devices[index].dest_fmt;
+       struct v4l2_format orig_src_fmt = devices[index].src_fmt;
+       struct v4l2_format orig_dest_fmt = devices[index].dest_fmt;
+       int r;
+
+       if (fps == devices[index].fps)
+               return;
+
+       if (v4l2_check_buffer_change_ok(index))
+               return;
+
+       v4lconvert_set_fps(devices[index].convert, fps);
+       r = v4lconvert_try_format(devices[index].convert, &dest_fmt, &src_fmt);
+       v4lconvert_set_fps(devices[index].convert, V4L2_DEFAULT_FPS);
+       if (r)
+               return;
+
+       if (orig_src_fmt.fmt.pix.pixelformat == src_fmt.fmt.pix.pixelformat ||
+           !v4l2_pix_fmt_compat(&orig_dest_fmt, &dest_fmt))
+               return;
+
+       req_pix_fmt = src_fmt.fmt.pix;
+       if (SYS_IOCTL(devices[index].fd, VIDIOC_S_FMT, &src_fmt))
+               return;
+
+       v4l2_set_src_and_dest_format(index, &src_fmt, &dest_fmt);
+
+       /* Check we've gotten what try_fmt promised us and that the
+          new dest fmt matches the original, if this is true we're done. */
+       if (src_fmt.fmt.pix.width == req_pix_fmt.width &&
+           src_fmt.fmt.pix.height == req_pix_fmt.height &&
+           src_fmt.fmt.pix.pixelformat == req_pix_fmt.pixelformat &&
+           v4l2_pix_fmt_identical(&orig_dest_fmt, &dest_fmt)) {
+               if (v4l2_log_file) {
+                       int pixfmt = src_fmt.fmt.pix.pixelformat;
+                       fprintf(v4l2_log_file,
+                               "new src fmt for fps change: %c%c%c%c\n",
+                               pixfmt & 0xff, (pixfmt >> 8) & 0xff,
+                               (pixfmt >> 16) & 0xff, pixfmt >> 24);
+               }
+               return;
+       }
+
+       /* Not identical!! */
+       V4L2_LOG_WARN("dest fmt changed after adjusting src fmt for fps "
+                     "change, restoring original src fmt");
+       src_fmt = orig_src_fmt;
+       dest_fmt = orig_dest_fmt;
+       req_pix_fmt = src_fmt.fmt.pix;
+       if (SYS_IOCTL(devices[index].fd, VIDIOC_S_FMT, &src_fmt)) {
+               V4L2_LOG_ERR("restoring src fmt: %s\n", strerror(errno));
+               return;
+       }
+       v4l2_set_src_and_dest_format(index, &src_fmt, &dest_fmt);
+       if (src_fmt.fmt.pix.width != req_pix_fmt.width ||
+           src_fmt.fmt.pix.height != req_pix_fmt.height ||
+           src_fmt.fmt.pix.pixelformat != req_pix_fmt.pixelformat ||
+           !v4l2_pix_fmt_identical(&orig_dest_fmt, &dest_fmt))
+               V4L2_LOG_ERR("dest fmt different after restoring src fmt");
+}
 
 ssize_t v4l2_read(int fd, void *dest, size_t n)
 {

diff --git a/lib/libv4l2/log.c b/lib/libv4l2/log.c
index 4c3a52e43511d0135902668d122dd55f5f493a5a..31888116930d676b29dd0604a214128c8ffc5b03 100644 (file)
--- a/lib/libv4l2/log.c
+++ b/lib/libv4l2/log.c
@@ -216,6 +216,19 @@ void v4l2_log_ioctl(unsigned long int request, void *arg, int result)
                }
                break;
        }
+       case VIDIOC_G_PARM:
+       case VIDIOC_S_PARM: {
+               struct v4l2_streamparm *parm = arg;
+
+               if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+                       break;
+
+               if (parm->parm.capture.capability & V4L2_CAP_TIMEPERFRAME)
+                       fprintf(v4l2_log_file, "timeperframe: %u/%u\n",
+                               parm->parm.capture.timeperframe.numerator,
+                               parm->parm.capture.timeperframe.denominator);
+               break;
+       }
        }
 
        if (result < 0)