From a9601953129df0af408b0b0eb0982df54ecee3c7 Mon Sep 17 00:00:00 2001
From: Kamil Trzcinski <ayufan@ayufan.eu>
Date: Tue, 5 Apr 2022 12:24:55 +0200
Subject: [PATCH] Implement pausing

---
 cmd/camera_srgb_isp.c |  2 ++
 cmd/main.c            |  6 ++--
 hw/device.h           |  1 +
 hw/links.c            | 66 +++++++++++++++++++++++++++++++++----------
 4 files changed, 57 insertions(+), 18 deletions(-)

diff --git a/cmd/camera_srgb_isp.c b/cmd/camera_srgb_isp.c
index 96fd1e5..784e002 100644
--- a/cmd/camera_srgb_isp.c
+++ b/cmd/camera_srgb_isp.c
@@ -22,6 +22,7 @@ int camera_configure_srgb_isp(camera_t *camera, float high_div, float low_div)
 
   camera->isp.isp_srgb = device_open("ISP", "/dev/video13");
   camera->isp.isp_yuuv = device_open("ISP-YUUV", "/dev/video14");
+  camera->isp.isp_yuuv->output_device = camera->isp.isp_srgb;
   camera->codec_jpeg = device_open("JPEG", "/dev/video31");
   camera->codec_h264 = device_open("H264", "/dev/video11");
 
@@ -32,6 +33,7 @@ int camera_configure_srgb_isp(camera_t *camera, float high_div, float low_div)
 
   if (low_div >= 1) {
     camera->isp.isp_yuuv_low = device_open("ISP-YUUV-LOW", "/dev/video15");
+    camera->isp.isp_yuuv_low->output_device = camera->isp.isp_srgb;
 
     if (device_open_buffer_list(camera->isp.isp_yuuv_low, true, src->fmt_width / low_div, src->fmt_height / low_div, V4L2_PIX_FMT_YUYV, 0, camera->nbufs) < 0) {
       return -1;
diff --git a/cmd/main.c b/cmd/main.c
index d6650b6..47b417c 100644
--- a/cmd/main.c
+++ b/cmd/main.c
@@ -33,8 +33,8 @@ int main(int argc, char *argv[])
 
   camera_init(&camera);
 
-  // camera.width = 1920; camera.height = 1080;
-  camera.width = 2328; camera.height = 1748; // 1164x874
+  camera.width = 1920; camera.height = 1080;
+  //camera.width = 2328; camera.height = 1748; // 1164x874
   //camera.width = 4656; camera.height = 3496;
   //camera.width = 3840; camera.height = 2160;
   camera.nbufs = 4;
@@ -44,7 +44,7 @@ int main(int argc, char *argv[])
   }
 
 #if 1
-  if (camera_configure_srgb_isp(&camera, 1.6, 0) < 0) {
+  if (camera_configure_srgb_isp(&camera, 1, 0) < 0) {
     goto error;
   }
 #else
diff --git a/hw/device.h b/hw/device.h
index be0563d..c7f66a8 100644
--- a/hw/device.h
+++ b/hw/device.h
@@ -13,6 +13,7 @@ typedef struct device_s {
   struct buffer_list_s *output_list;
 
   struct device_s *output_device;
+  bool paused;
 } device_t;
 
 device_t *device_open(const char *name, const char *path);
diff --git a/hw/links.c b/hw/links.c
index a0dbc91..ecb37db 100644
--- a/hw/links.c
+++ b/hw/links.c
@@ -5,6 +5,48 @@
 
 #define N_FDS 50
 
+void _update_paused(link_t *all_links)
+{
+  int n = 0;
+
+  for (n = 0; all_links[n].capture; n++);
+
+  for (int i = n; i-- > 0; ) {
+    link_t *link = &all_links[i];
+
+    bool paused = false;
+
+    if (!link->capture->capture_list->streaming) {
+      continue;
+    }
+    if (link->callbacks.check_streaming) {
+      paused = !link->callbacks.check_streaming();
+    }
+
+    for (int j = 0; link->outputs[j]; j++) {
+      device_t *output = link->outputs[j];
+
+      if (!output->output_list->streaming) {
+        continue;
+      }
+      if (output->output_list->device->paused) {
+        continue;
+      }
+
+      int count_enqueued = buffer_list_count_enqueued(output->output_list);
+      if (count_enqueued == output->output_list->nbufs) {
+        paused = true;
+      }
+    }
+
+    link->capture->paused = paused;
+
+    if (link->capture->output_device) {
+      link->capture->output_device->paused = paused;
+    }
+  }
+}
+
 int _build_fds(link_t *all_links, struct pollfd *fds, link_t **links, buffer_list_t **buf_lists, int max_n)
 {
   int n = 0;
@@ -22,12 +64,6 @@ int _build_fds(link_t *all_links, struct pollfd *fds, link_t **links, buffer_lis
       continue;
     }
 
-    bool can_consume = true;
-
-    if (link->callbacks.check_streaming) {
-      can_consume = link->callbacks.check_streaming();
-    }
-
     for (int j = 0; link->outputs[j]; j++) {
       device_t *output = link->outputs[j];
 
@@ -40,12 +76,6 @@ int _build_fds(link_t *all_links, struct pollfd *fds, link_t **links, buffer_lis
 
       int count_enqueued = buffer_list_count_enqueued(output->output_list);
 
-      if (count_enqueued == output->output_list->nbufs) {
-        E_LOG_DEBUG(link->capture->capture_list, "Cannot consume due to %s using %d of %d",
-          output->output_list->name, count_enqueued, output->output_list->nbufs);
-        can_consume = false;
-      }
-
       // Can something be dequeued?
       if (count_enqueued == 0) {
         continue;
@@ -58,7 +88,7 @@ int _build_fds(link_t *all_links, struct pollfd *fds, link_t **links, buffer_lis
       n++;
     }
 
-    if (can_consume) {
+    if (!link->capture->paused) {
       struct pollfd fd = {link->capture->fd, POLLIN};
       fds[n] = fd;
       buf_lists[n] = link->capture->capture_list;
@@ -76,6 +106,9 @@ int links_step(link_t *all_links, int timeout)
   link_t *links[N_FDS];
   buffer_list_t *buf_lists[N_FDS];
   buffer_t *buf;
+
+  _update_paused(all_links);
+
   int n = _build_fds(all_links, fds, links, buf_lists, N_FDS);
   int ret = poll(fds, n, timeout);
 
@@ -87,13 +120,16 @@ int links_step(link_t *all_links, int timeout)
     buffer_list_t *buf_list = buf_lists[i];
     link_t *link = links[i];
 
-    E_LOG_DEBUG(buf_list, "pool i=%d revents=%08x streaming=%d enqueued=%d/%d", i, fds[i].revents, buf_list->streaming,
-      buffer_list_count_enqueued(buf_list), buf_list->nbufs);
+    E_LOG_DEBUG(buf_list, "pool i=%d revents=%08x streaming=%d enqueued=%d/%d paused=%d", i, fds[i].revents, buf_list->streaming,
+      buffer_list_count_enqueued(buf_list), buf_list->nbufs, link->capture->paused);
 
     if (fds[i].revents & POLLIN) {
       E_LOG_DEBUG(buf_list, "POLLIN");
       if (buf = buffer_list_dequeue(buf_list)) {
         for (int j = 0; link->outputs[j]; j++) {
+          if (link->outputs[j]->paused) {
+            continue;
+          }
           buffer_list_enqueue(link->outputs[j]->output_list, buf);
         }