From 21ce78a277d4f65254c2467ae1cf76a29a5087e9 Mon Sep 17 00:00:00 2001
From: Kamil Trzcinski <ayufan@ayufan.eu>
Date: Thu, 23 Feb 2023 16:48:19 +0100
Subject: [PATCH] camera: improve FPS handling

---
 device/camera/camera_pipeline.c |  4 ----
 device/device.c                 | 24 +++++++++++++++++++++---
 device/libcamera/device.cc      |  2 +-
 3 files changed, 22 insertions(+), 8 deletions(-)

diff --git a/device/camera/camera_pipeline.c b/device/camera/camera_pipeline.c
index 9642bd3..8244c0f 100644
--- a/device/camera/camera_pipeline.c
+++ b/device/camera/camera_pipeline.c
@@ -47,10 +47,6 @@ int camera_configure_pipeline(camera_t *camera, buffer_list_t *capture)
   if (capture) {
     capture->do_timestamps = true;
 
-    if (camera->options.fps > 0) {
-      capture->fmt.interval_us = 1000 * 1000 / camera->options.fps;
-    }
-
     switch (capture->fmt.format) {
     case V4L2_PIX_FMT_SRGGB10P:
     case V4L2_PIX_FMT_SGRBG10P:
diff --git a/device/device.c b/device/device.c
index 3931ca7..2c7f0aa 100644
--- a/device/device.c
+++ b/device/device.c
@@ -107,6 +107,9 @@ buffer_list_t *device_open_buffer_list_output(device_t *dev, buffer_list_t *capt
   }
 
   buffer_format_t fmt = capture_list->fmt;
+
+  fmt.interval_us = 0;
+
   bool do_mmap = capture_list->dev->opts.allow_dma ? !capture_list->do_mmap : true;
 
   // If manually allocating buffers, ensure that `sizeimage` is at least `buf->length`
@@ -137,8 +140,6 @@ buffer_list_t *device_open_buffer_list_capture(device_t *dev, const char *path,
     fmt.height = output_list->fmt.height;
   if (!fmt.nbufs)
     fmt.nbufs = output_list->fmt.nbufs;
-  if (!fmt.interval_us)
-    fmt.interval_us = output_list->fmt.interval_us;
 
   return device_open_buffer_list2(dev, path, true, fmt, do_mmap);
 }
@@ -235,9 +236,26 @@ void device_dump_options(device_t *dev, FILE *stream)
 
 int device_set_fps(device_t *dev, int desired_fps)
 {
-  if (!dev || dev->hw->device_set_fps(dev, desired_fps) < 0)
+  if (!dev)
     return -1;
 
+  unsigned interval_us = 0;
+
+  if (desired_fps > 0) {
+    interval_us = 1000 * 1000 / desired_fps;
+  }
+
+  // try to use HW fps setting
+  if (dev->hw->device_set_fps && dev->hw->device_set_fps(dev, desired_fps) >= 0) {
+    interval_us = 0;
+  }
+
+  LOG_INFO(dev, "Setting frame interval_us=%d for FPS=%d", interval_us, desired_fps);
+
+  for (int i = 0; i < dev->n_capture_list; i++) {
+    dev->capture_lists[i]->fmt.interval_us = interval_us;
+  }
+
   return 0;
 }
 
diff --git a/device/libcamera/device.cc b/device/libcamera/device.cc
index 82c5b70..dbaf061 100644
--- a/device/libcamera/device.cc
+++ b/device/libcamera/device.cc
@@ -128,7 +128,7 @@ void libcamera_device_close(device_t *dev)
 
 int libcamera_device_set_fps(device_t *dev, int desired_fps)
 {
-  int64_t frame_time = 1000000 / desired_fps;
+  int64_t frame_time = desired_fps ? 1000000 / desired_fps : 0;
   dev->libcamera->controls.set(libcamera::controls::FrameDurationLimits, libcamera::Span<const int64_t, 2>({ frame_time, frame_time }));
   return 0;
 }