From c817f730515b8a37bb1a09ed77a8a24987e38e2d Mon Sep 17 00:00:00 2001
From: Kamil Trzcinski <ayufan@ayufan.eu>
Date: Thu, 7 Apr 2022 17:46:15 +0200
Subject: [PATCH] Improve FPS handling

---
 cmd/camera_isp.c | 5 -----
 hw/buffer_lock.c | 6 +++---
 hw/buffer_lock.h | 2 ++
 hw/links.c       | 8 +++++++-
 opts/log.h       | 3 +++
 5 files changed, 15 insertions(+), 9 deletions(-)

diff --git a/cmd/camera_isp.c b/cmd/camera_isp.c
index 0855da2..bfd741a 100644
--- a/cmd/camera_isp.c
+++ b/cmd/camera_isp.c
@@ -54,11 +54,6 @@ int camera_configure_isp(camera_t *camera, float high_div, float low_div)
 
   link_t *links = camera->links;
 
-  if (camera->options.fps > 0) {
-    camera->codec_jpeg->capture_list->fmt_interval_us = 1000 * 1000 / camera->options.fps;
-    printf("buffer_lock.c: frame interval: %d\n", camera->codec_jpeg->capture_list->fmt_interval_us);
-  }
-
   *links++ = (link_t){ camera->camera->capture_list, { camera->isp_srgb->output_list } };
 
   if (camera->isp_yuuv_low) {
diff --git a/hw/buffer_lock.c b/hw/buffer_lock.c
index 42c7917..ea0c225 100644
--- a/hw/buffer_lock.c
+++ b/hw/buffer_lock.c
@@ -47,10 +47,10 @@ void buffer_lock_capture(buffer_lock_t *buf_lock, buffer_t *buf)
     buffer_consumed(buf_lock->buf, buf_lock->name);
     buf_lock->buf = NULL;
     buf_lock->buf_time_us = now;
-  } else if (now - buf_lock->buf_time_us < buf->buf_list->fmt_interval_us) {
+  } else if (now - buf_lock->buf_time_us < buf_lock->frame_interval_ms * 1000) {
     buf_lock->dropped++;
 
-    E_LOG_DEBUG(buf_lock, "Dropped buffer %s (refs=%d), frame=%d/%d, frame_us=%.1f",
+    E_LOG_DEBUG(buf_lock, "Dropped buffer %s (refs=%d), frame=%d/%d, frame_ms=%.1f",
       dev_name(buf), buf ? buf->mmap_reflinks : 0,
       buf_lock->counter, buf_lock->dropped,
       (now - captured_time_us) / 1000.0f);
@@ -59,7 +59,7 @@ void buffer_lock_capture(buffer_lock_t *buf_lock, buffer_t *buf)
     buffer_use(buf);
     buf_lock->buf = buf;
     buf_lock->counter++;
-    E_LOG_DEBUG(buf_lock, "Captured buffer %s (refs=%d), frame=%d/%d, processing_us=%.1f, frame_us=%.1f",
+    E_LOG_DEBUG(buf_lock, "Captured buffer %s (refs=%d), frame=%d/%d, processing_ms=%.1f, frame_ms=%.1f",
       dev_name(buf), buf ? buf->mmap_reflinks : 0,
       buf_lock->counter, buf_lock->dropped,
       (now - captured_time_us) / 1000.0f,
diff --git a/hw/buffer_lock.h b/hw/buffer_lock.h
index 66cd82b..91c0e57 100644
--- a/hw/buffer_lock.h
+++ b/hw/buffer_lock.h
@@ -14,6 +14,8 @@ typedef struct buffer_lock_s {
   int refs;
   int dropped;
   uint64_t timeout_us;
+
+  int frame_interval_ms;
 } buffer_lock_t;
 
 #define DEFAULT_BUFFER_LOCK_TIMEOUT 16 // ~60fps
diff --git a/hw/links.c b/hw/links.c
index 575408c..9c137cd 100644
--- a/hw/links.c
+++ b/hw/links.c
@@ -67,10 +67,16 @@ int _build_fds(link_t *all_links, struct pollfd *fds, link_t **links, buffer_lis
     }
   
     int count_enqueued = buffer_list_count_enqueued(source);
+    bool can_dequeue = count_enqueued > 0;
+
+    if (now_us - source->last_dequeued_us < source->fmt_interval_us) {
+      can_dequeue = false;
+      *max_timeout_ms = MIN(*max_timeout_ms, (source->last_dequeued_us + source->fmt_interval_us - now_us) / 1000);
+    }
 
     fds[n].fd = source->device->fd;
     fds[n].events = POLLHUP;
-    if (count_enqueued > 0)
+    if (can_dequeue)
       fds[n].events |= POLLIN;
     fds[n].revents = 0;
     buf_lists[n] = source;
diff --git a/opts/log.h b/opts/log.h
index 250bb76..c9b701f 100644
--- a/opts/log.h
+++ b/opts/log.h
@@ -11,6 +11,9 @@ typedef struct log_options_s {
 
 extern log_options_t log_options;
 
+#define MIN(a, b) ((a) < (b) ? (a) : (b))
+#define MAX(a, b) ((a) > (b) ? (a) : (b))
+
 // assumes that name is first item
 #define dev_name(dev) (dev ? *(const char**)dev : "?")
 #define E_LOG_ERROR(dev, _msg, ...)		do { fprintf(stderr, "%s: %s: " _msg "\n", __FILENAME__, dev_name(dev), ##__VA_ARGS__); goto error; } while(0)