use bladeRF async API

main.cpp

@@ -8,6 +8,7 @@
 #include <complex>
 #include <chrono>
 #include <format>
+#include <semaphore>
 #include <experimental/net>
 
 #include <GLFW/glfw3.h>
@@ -19,6 +20,7 @@
 #include <libbladeRF.h>
 
 namespace net = std::experimental::net;
+using namespace std::literals::chrono_literals;
 
 
 void fft_mag(std::complex<float>* in, float* out, size_t len);
@@ -26,59 +28,21 @@ void fft_mag(std::complex<float>* in, float* out, size_t len);
 
 static const size_t block_len = 16384;
 static const int wf_width = 1000;
-static const int wf_height = 2000;
+static const int wf_height = 1200;
 
+static std::atomic<bool> shutdown_threads;
 
-static std::atomic<bool> terminate_dsp;
-
-static struct bladerf* bladerf_dev;
-
-static std::mutex wf_data_mutex;
-static unsigned int wf_data[wf_width*wf_height];
-
-static std::atomic<float> freq_setpoint;
-static std::atomic<float> freq_peak;
-static std::atomic<float> tec_bias;
-static std::atomic<float> tec_p;
 static std::atomic<float> tec_current;
 
-static std::atomic<int> fps;
+static void tec_thread()
-
-static void dsp_thread()
 {
-    using namespace std::literals::chrono_literals;
-
     net::io_context io_context;
     net::ip::tcp::socket tec_socket(io_context);
     tec_socket.connect(net::ip::tcp::endpoint(net::ip::make_address("192.168.1.27"), 23));
 
-    std::vector<std::complex<int16_t>> frames(block_len);
+    while(!shutdown_threads) {
-    std::vector<std::complex<float>> frames_f(block_len);
-    std::vector<float> frames_mag(block_len);
-
-    int iterations = 0;
-    auto last_second = std::chrono::steady_clock::now();
-    auto last_tec = std::chrono::steady_clock::now();
-    while(!terminate_dsp) {
-        int status;
-        if((status = bladerf_sync_rx(bladerf_dev, frames.data(), block_len, NULL, 0)) != 0) {
-            std::cerr << "failed to receive samples from bladeRF: " << bladerf_strerror(status) << std::endl;
-            break;
-        }
-
-        fft_mag(frames_f.data(), frames_mag.data(), block_len);
-
-        // stabilize laser
-        bool tick = false;
-        if((std::chrono::steady_clock::now() - last_tec) >= 100ms) {
-            float freq_peak_local;
-            freq_peak_local = 40.0f*float(distance(frames_mag.begin(), max_element(frames_mag.begin(), frames_mag.end())))/float(block_len);
-            freq_peak = freq_peak_local;
-            float freq_error = freq_peak_local - freq_setpoint;
-            float tec_current_local = std::max(tec_bias+tec_p*freq_error, 0.0f);
-            tec_current = tec_current_local;
         // FIXME: net::write seems unimplemented as of libstdc++ 13
-            tec_socket.write_some(net::buffer(std::format("pwm 0 i_set {:.6f}\n", tec_current_local)));
+        tec_socket.write_some(net::buffer(std::format("pwm 0 i_set {:.6f}\n", (float)tec_current)));
         std::string reply;
         net::read(tec_socket, net::dynamic_buffer(reply),
             [&reply](auto ec, auto n) -> std::size_t
@@ -88,32 +52,156 @@ static void dsp_thread()
                 else
                     return 1;
             });
-            last_tec += 100ms;
+        std::this_thread::sleep_for(100ms);
-            tick = true;
     }
+}
+
+static std::atomic<bool> poh_accept_input;
+static std::binary_semaphore poh_input_ready{0};
+static std::complex<float> poh_input_block[block_len];
+static std::mutex wf_poh_mutex;
+static unsigned int wf_poh[wf_width*wf_height];
+
+static std::atomic<float> freq_setpoint;
+static std::atomic<float> freq_peak;
+static std::atomic<float> tec_bias;
+static std::atomic<float> tec_p;
+
+static void poh_thread()
+{
+    std::vector<float> block_mag(block_len);
+
+    while(!shutdown_threads) {
+        poh_accept_input = true;
+        poh_input_ready.acquire();
+
+        fft_mag(poh_input_block, block_mag.data(), block_len);
+
+        // stabilize laser
+        float freq_peak_local;
+        freq_peak_local = 40.0f*float(distance(block_mag.begin(), max_element(block_mag.begin(), block_mag.end())))/float(block_len);
+        freq_peak = freq_peak_local;
+        float freq_error = freq_peak_local - freq_setpoint;
+        tec_current = std::max(tec_bias+tec_p*freq_error, 0.0f);
 
         // update waterfall
         {
-            std::lock_guard<std::mutex> guard(wf_data_mutex);
+            std::lock_guard<std::mutex> guard(wf_poh_mutex);
-            std::memmove(&wf_data[wf_width], &wf_data[0], sizeof(int)*wf_width*(wf_height - 1));
+            std::memmove(&wf_poh[wf_width], &wf_poh[0], sizeof(int)*wf_width*(wf_height - 1));
             for(int i=0;i<wf_width;i++) {
                 int j = i*block_len/wf_width;
-                wf_data[i] = 0xff000000 | 0x010101*std::min(int(frames_mag[j]/900.0f), 255);
+                wf_poh[i] = 0xff000000 | 0x010101*std::min(int(block_mag[j]/900.0f), 255);
+            }
+            wf_poh[int(freq_setpoint*wf_width)/40] = 0xff0000ff;
+        }
     }
-            wf_data[int(freq_setpoint*wf_width)/40] = 0xff0000ff;
-            if(tick)
-                for(int i=0;i<100;i++)
-                    wf_data[i] = 0xffff0000;
 }
 
-        // FPS counter
+static std::atomic<int> frames_processed;
-        iterations++;
+static std::atomic<int> frames_dropped;
-        if((std::chrono::steady_clock::now() - last_second) >= 1s) {
+
-            fps = iterations;
+static auto brf_last_second = std::chrono::time_point<std::chrono::steady_clock>::min();
-            iterations = 0;
+static int brf_frames_processed_cnt = 0;
-            last_second += 1s;
+static int brf_frames_dropped_cnt = 0;
+
+static void* brf_dispatch(struct bladerf* dev, struct bladerf_stream* stream,
+    struct bladerf_metadata* meta, void* samples_v, size_t num_samples, void* user_data)
+{
+    if(poh_accept_input) {
+        std::complex<int16_t>* samples = (std::complex<int16_t>*)samples_v;
+
+        for(size_t i=0;i<block_len;i++) {
+            poh_input_block[i] = samples[2*i];
+        }
+
+        poh_accept_input = false;
+        poh_input_ready.release();
+        brf_frames_processed_cnt++;
+    } else
+        brf_frames_dropped_cnt++;
+
+    if((std::chrono::steady_clock::now() - brf_last_second) >= 1s) {
+        frames_processed = brf_frames_processed_cnt;
+        frames_dropped = brf_frames_dropped_cnt;
+        brf_frames_processed_cnt = 0;
+        brf_frames_dropped_cnt = 0;
+        brf_last_second += 1s;
+    }
+
+    if(shutdown_threads)
+        return BLADERF_STREAM_SHUTDOWN;
+    else
+        return samples_v;
+}
+
+static void brf_thread()
+{
+    struct bladerf_devinfo brf_dev_info;
+    struct bladerf* brf_dev;
+    struct bladerf_stream* brf_stream;
+    void** brf_buffers;
+    int status;
+
+    bladerf_init_devinfo(&brf_dev_info);
+    if((status = bladerf_open_with_devinfo(&brf_dev, &brf_dev_info)) != 0) {
+        std::cerr << "cannot open bladeRF device: " << bladerf_strerror(status) << std::endl;
+        return;
+    }
+    for(int i=0;i<2;i++) {
+        if((status = bladerf_set_frequency(brf_dev, BLADERF_CHANNEL_RX(i), 1'655'000'000)) != 0) {
+            std::cerr << "failed to set bladeRF frequency: " << bladerf_strerror(status) << std::endl;
+            bladerf_close(brf_dev);
+            return;
+        }
+        if((status = bladerf_set_sample_rate(brf_dev, BLADERF_CHANNEL_RX(i), 40'000'000, NULL)) != 0) {
+            std::cerr << "failed to set bladeRF sample rate: " << bladerf_strerror(status) << std::endl;
+            bladerf_close(brf_dev);
+            return;
+        }
+        if((status = bladerf_set_bandwidth(brf_dev, BLADERF_CHANNEL_RX(i), 35'000'000, NULL)) != 0) {
+            std::cerr << "failed to set bladeRF bandwidth: " << bladerf_strerror(status) << std::endl;
+            bladerf_close(brf_dev);
+            return;
+        }
+        if((status = bladerf_set_gain(brf_dev, BLADERF_CHANNEL_RX(i), 20)) != 0) {
+            std::cerr << "failed to set bladeRF gain: " << bladerf_strerror(status) << std::endl;
+            bladerf_close(brf_dev);
+            return;
         }
     }
+    if((status = bladerf_init_stream(&brf_stream, brf_dev, brf_dispatch, &brf_buffers,
+            16, BLADERF_FORMAT_SC16_Q11, 2*block_len, 8, NULL)) != 0) {
+        std::cerr << "failed to init bladeRF stream: " << bladerf_strerror(status) << std::endl;
+        return;
+    }
+    if((status = bladerf_enable_module(brf_dev, BLADERF_CHANNEL_RX(0), true) != 0)) {
+        std::cerr << "failed to enable bladeRF RX: " << bladerf_strerror(status) << std::endl;
+        bladerf_deinit_stream(brf_stream);
+        bladerf_close(brf_dev);
+        return;
+    }
+    if((status = bladerf_enable_module(brf_dev, BLADERF_CHANNEL_RX(1), true) != 0)) {
+        std::cerr << "failed to enable bladeRF RX: " << bladerf_strerror(status) << std::endl;
+        bladerf_enable_module(brf_dev, BLADERF_CHANNEL_RX(0), false);
+        bladerf_deinit_stream(brf_stream);
+        bladerf_close(brf_dev);
+        return;
+    }
+
+    brf_last_second = std::chrono::steady_clock::now();
+    if((status = bladerf_stream(brf_stream, BLADERF_RX_X2)) != 0) {
+        std::cerr << "bladeRF stream failed: " << bladerf_strerror(status) << std::endl;
+        bladerf_enable_module(brf_dev, BLADERF_CHANNEL_RX(1), false);
+        bladerf_enable_module(brf_dev, BLADERF_CHANNEL_RX(0), false);
+        bladerf_deinit_stream(brf_stream);
+        bladerf_close(brf_dev);
+        return;
+    };
+
+    bladerf_enable_module(brf_dev, BLADERF_CHANNEL_RX(1), false);
+    bladerf_enable_module(brf_dev, BLADERF_CHANNEL_RX(0), false);
+    bladerf_deinit_stream(brf_stream);
+    bladerf_close(brf_dev);
 }
 
 int main(int argc, char* argv[])
@@ -161,57 +249,29 @@ int main(int argc, char* argv[])
         glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
     }
 
-    int status;
+    static std::thread brf_thread_h = std::thread(brf_thread);
-    struct bladerf_devinfo dev_info;
+    static auto JoinBRF = []() {
-    bladerf_init_devinfo(&dev_info);
+        brf_thread_h.join();
-    if((status = bladerf_open_with_devinfo(&bladerf_dev, &dev_info)) != 0) {
-        std::cerr << "cannot open bladeRF device: " << bladerf_strerror(status) << std::endl;
-        return 1;
-    }
-    static auto BladeRFClose = []() {
-        bladerf_close(bladerf_dev);
     };
-    std::atexit(BladeRFClose);
+    std::atexit(JoinBRF);
-    if((status = bladerf_set_frequency(bladerf_dev, BLADERF_CHANNEL_RX(0), 1'655'000'000)) != 0) {
-        std::cerr << "failed to set bladeRF frequency: " << bladerf_strerror(status) << std::endl;
-        return 1;
-    }
-    if((status = bladerf_set_sample_rate(bladerf_dev, BLADERF_CHANNEL_RX(0), 40'000'000, NULL)) != 0) {
-        std::cerr << "failed to set bladeRF sample rate: " << bladerf_strerror(status) << std::endl;
-        return 1;
-    }
-    if((status = bladerf_set_bandwidth(bladerf_dev, BLADERF_CHANNEL_RX(0), 35'000'000, NULL)) != 0) {
-        std::cerr << "failed to set bladeRF bandwidth: " << bladerf_strerror(status) << std::endl;
-        return 1;
-    }
-    if((status = bladerf_set_gain(bladerf_dev, BLADERF_CHANNEL_RX(0), 20)) != 0) {
-        std::cerr << "failed to set bladeRF gain: " << bladerf_strerror(status) << std::endl;
-        return 1;
-    }
-    if((status = bladerf_sync_config(bladerf_dev, BLADERF_RX_X1, BLADERF_FORMAT_SC16_Q11,
-            16,   /* num_buffers */
-            8192, /* buffer_size */
-            8,    /* num_transfers */
-            3500  /* timeout_ms */)) != 0) {
-        std::cerr << "failed to set bladeRF sync settings: " << bladerf_strerror(status) << std::endl;
-        return 1;
-    }
-    if((status = bladerf_enable_module(bladerf_dev, BLADERF_RX, true) != 0)) {
-        std::cerr << "failed to enable bladeRF RX: " << bladerf_strerror(status) << std::endl;
-        return 1;
-    }
-    static auto BladeRFDisableRX = []() {
-        bladerf_enable_module(bladerf_dev, BLADERF_RX, false);
-    };
-    std::atexit(BladeRFDisableRX);
 
-    terminate_dsp = false;
+    static std::thread poh_thread_h = std::thread(poh_thread);
-    static std::thread dsp_thread_h = std::thread(dsp_thread);
+    static auto JoinPOH = []() {
-    static auto TerminateDSP = []() {
+        poh_thread_h.join();
-        terminate_dsp = true;
-        dsp_thread_h.join();
     };
-    std::atexit(TerminateDSP);
+    std::atexit(JoinPOH);
+
+    static std::thread tec_thread_h = std::thread(tec_thread);
+    static auto JoinTEC = []() {
+        tec_thread_h.join();
+    };
+    std::atexit(JoinTEC);
+
+    shutdown_threads = false;
+    static auto SetShutdown = []() {
+        shutdown_threads = true;
+    };
+    std::atexit(SetShutdown);
 
     bool exit = false;
     bool update_wf = true;
@@ -229,13 +289,13 @@ int main(int argc, char* argv[])
             #endif
             glBindTexture(GL_TEXTURE_2D, wftex[0]);
             {
-                std::lock_guard<std::mutex> guard(wf_data_mutex);
+                std::lock_guard<std::mutex> guard(wf_poh_mutex);
-                glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, wf_width, wf_height, 0, GL_RGBA, GL_UNSIGNED_BYTE, wf_data);
+                glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, wf_width, wf_height, 0, GL_RGBA, GL_UNSIGNED_BYTE, wf_poh);
             }
             glBindTexture(GL_TEXTURE_2D, wftex[1]);
             {
-                std::lock_guard<std::mutex> guard(wf_data_mutex);
+                std::lock_guard<std::mutex> guard(wf_poh_mutex);
-                glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, wf_width, wf_height, 0, GL_RGBA, GL_UNSIGNED_BYTE, wf_data);
+                glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, wf_width, wf_height, 0, GL_RGBA, GL_UNSIGNED_BYTE, wf_poh);
             }
         }
 
@@ -251,7 +311,7 @@ int main(int argc, char* argv[])
         ImGui::BeginTable("fastsa", 3, ImGuiTableFlags_SizingStretchSame | ImGuiTableFlags_Resizable);
         ImGui::TableSetupColumn("SBS laser control", 0, 280.0f);
         ImGui::TableSetupColumn("Pump/output heterodyne", 0, 1000.0f);
-        ImGui::TableSetupColumn("Linewidth", 0, 1000.0f);
+        ImGui::TableSetupColumn("Lineshape", 0, 1000.0f);
         ImGui::TableHeadersRow();
         ImGui::TableNextColumn();
         ImGui::Checkbox("Update waterfall plots", &update_wf);
@@ -265,7 +325,8 @@ int main(int argc, char* argv[])
         tec_p = 0.05f*tec_p_local;
         if(ImGui::Button("Exit"))
             exit = true;
-        ImGui::Text("FPS: %d", (int)fps);
+        ImGui::Text("Frames processed: %d", (int)frames_processed);
+        ImGui::Text("Frames dropped: %d", (int)frames_dropped);
         ImGui::TableNextColumn();
         ImGui::Image((void*)(intptr_t)wftex[0], ImVec2(wf_width, wf_height));
         ImGui::TableNextColumn();