From 92ae6ae23ce8c2b0b2af6ad4c0e39caaf6e1297e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?S=C3=A9bastien=20Bourdeauducq?= <sb@m-labs.hk>
Date: Tue, 31 Dec 2024 23:39:07 +0800
Subject: [PATCH] sync DDS and lockin FTWs

---
 fifo.h      | 30 ++++++++++++++++++++++++++++++
 sndlock.cpp | 30 +++++++++++++++++++++++++-----
 2 files changed, 55 insertions(+), 5 deletions(-)
 create mode 100644 fifo.h

diff --git a/fifo.h b/fifo.h
new file mode 100644
index 0000000..442530d
--- /dev/null
+++ b/fifo.h
@@ -0,0 +1,30 @@
+#pragma once
+
+#include <optional>
+
+template<typename T, size_t N>
+class FIFO {
+    private:
+        size_t produce = 0;
+        size_t consume = 0;
+        bool empty = true;
+        T storage[N];
+    public:
+        bool push(T x) {
+            if(produce == consume && !empty)
+                return false;
+            storage[produce] = x;
+            produce = (produce + 1) % N;
+            empty = false;
+            return true;
+        }
+        std::optional<T> pull() {
+            if(empty)
+                return std::nullopt;
+            T ret = storage[consume];
+            consume = (consume + 1) % N;
+            if(produce == consume)
+                empty = true;
+            return ret;
+        }
+};
diff --git a/sndlock.cpp b/sndlock.cpp
index 46d434f..4c1ccf0 100644
--- a/sndlock.cpp
+++ b/sndlock.cpp
@@ -2,6 +2,7 @@
 #include <thread>
 #include <cstdint>
 #include <complex>
+#include <optional>
 
 #include <GLFW/glfw3.h>
 #include <sndio.h>
@@ -13,6 +14,7 @@
 #include "imgui_impl_opengl3.h"
 
 #include "dsp_lib.h"
+#include "fifo.h"
 
 #define SND_BITS 24
 #define SND_PCHAN 2
@@ -66,6 +68,11 @@ static void dsp_thread()
     int32_t buf_out[SND_BUFLEN*SND_PCHAN];
     size_t buf_out_offset = sizeof(buf_out);
 
+    int32_t buf_in[SND_BUFLEN*SND_RCHAN];
+    size_t buf_in_offset = sizeof(buf_in);
+
+    FIFO<phase_t, 8> ftw_fifo[SND_PCHAN];
+
     Lockin<4> lockin[SND_PCHAN];
     for(int i=0;i<SND_PCHAN;i++)
         // input channels are averaged together to reduce uncorrelated noise
@@ -79,15 +86,18 @@ static void dsp_thread()
         poll(pfd, nfds, INFTIM);
         int revents = sio_revents(hdl, pfd);
 
-        double lpf_k[SND_PCHAN];
-        for(int i=0;i<SND_PCHAN;i++) {
-            dds[i].ftw = lockin[i].ftw = frequency_to_ftw(frequency[i]/SND_RATE);
+        for(int i=0;i<SND_PCHAN;i++)
             lockin[i].set_bandwidth(lpf_bandwidth[i]/SND_RATE);
-        }
 
         if(revents & POLLOUT) {
             double scale = pow(2.0, SND_BITS-1) - 1.0;
             if(buf_out_offset == sizeof(buf_out)) {
+                for(int i=0;i<SND_PCHAN;i++) {
+                    phase_t ftw = frequency_to_ftw(frequency[i]/SND_RATE);
+                    dds[i].ftw = ftw;
+                    if(!ftw_fifo[i].push(ftw))
+                        std::cerr << "FTW FIFO overflow" << std::endl;
+                }
                 for(int i=0;i<SND_PCHAN;i++)
                     for(int j=0;j<SND_BUFLEN;j++)
                         buf_out[SND_PCHAN*j+i] = scale*dds[i].get();
@@ -98,8 +108,18 @@ static void dsp_thread()
         }
 
         if(revents & POLLIN) {
-            int32_t buf_in[SND_BUFLEN*SND_RCHAN];
             size_t read = sio_read(hdl, buf_in, sizeof(buf_in));
+            buf_in_offset += read;
+            if(buf_in_offset >= sizeof(buf_in)) {
+                for(int i=0;i<SND_PCHAN;i++) {
+                    std::optional<phase_t> ftw = ftw_fifo[i].pull();
+                    if(ftw.has_value())
+                        lockin[i].ftw = ftw.value();
+                    else
+                        std::cerr << "FTW FIFO underflow" << std::endl;
+                }
+                buf_in_offset -= sizeof(buf_in);
+            }
             for(int i=0;i<SND_PCHAN;i++)
                 for(int j=0;j<read/(SND_RCHAN*sizeof(buf_in[0]));j++) {
                     double sample = 0.0;