Dexter: Add three clock states and handling

2 files changed, 208 insertions, 120 deletions

diff --git a/src/output/Dexter.cpp b/src/output/Dexter.cpp
index 4cb9cd8..59baf7e 100644
--- a/src/output/Dexter.cpp
+++ b/src/output/Dexter.cpp
@@ -120,58 +120,6 @@ Dexter::Dexter(SDRDeviceConfig& config) :
 
     // skip: antenna
 
-    // get H/W time
-    /* Procedure:
-     * Wait 200ms after second change, fetch pps_clks attribute
-     * idem at the next second, and check that pps_clks incremented by DSP_CLOCK
-     * If ok, store the correspondence between current second change (measured in UTC clock time)
-     * and the counter value at pps rising edge. */
-
-    etiLog.level(info) << "Dexter: Waiting for second change...";
-
-    struct timespec time_at_startup;
-    fill_time(&time_at_startup);
-    time_at_startup.tv_nsec = 0;
-
-    struct timespec time_now;
-    do {
-        fill_time(&time_now);
-        this_thread::sleep_for(chrono::milliseconds(1));
-    } while (time_at_startup.tv_sec == time_now.tv_sec);
-    this_thread::sleep_for(chrono::milliseconds(200));
-
-    long long pps_clks = 0;
-    if ((r = iio_device_attr_read_longlong(m_dexter_dsp_tx, "pps_clks", &pps_clks)) != 0) {
-        etiLog.level(error) << "Failed to get dexter_dsp_tx.pps_clks: " << get_iio_error(r);
-        throw std::runtime_error("Dexter: Cannot read IIO attribute");
-    }
-
-    time_t tnow = time_now.tv_sec;
-    etiLog.level(info) << "Dexter: pps_clks " << pps_clks << " at UTC " <<
-        put_time(std::gmtime(&tnow), "%Y-%m-%d %H:%M:%S");
-
-    time_at_startup.tv_sec = time_now.tv_sec;
-    do {
-        fill_time(&time_now);
-        this_thread::sleep_for(chrono::milliseconds(1));
-    } while (time_at_startup.tv_sec == time_now.tv_sec);
-    this_thread::sleep_for(chrono::milliseconds(200));
-
-    long long pps_clks2 = 0;
-    if ((r = iio_device_attr_read_longlong(m_dexter_dsp_tx, "pps_clks", &pps_clks2)) != 0) {
-        etiLog.level(error) << "Failed to get dexter_dsp_tx.pps_clks: " << get_iio_error(r);
-        throw std::runtime_error("Dexter: Cannot read IIO attribute");
-    }
-    tnow = time_now.tv_sec;
-    etiLog.level(info) << "Dexter: pps_clks increased by " << pps_clks2 - pps_clks << " at UTC " <<
-        put_time(std::gmtime(&tnow), "%Y-%m-%d %H:%M:%S");
-
-    if ((uint64_t)pps_clks + DSP_CLOCK != (uint64_t)pps_clks2) {
-        throw std::runtime_error("Dexter: Wrong increase of pps_clks, expected " + to_string(DSP_CLOCK));
-    }
-    m_utc_seconds_at_startup = time_now.tv_sec;
-    m_clock_count_at_startup = pps_clks2;
-
     // The FIFO should not contain data, but setting gain=0 before setting start_clks to zero is an additional security
     if ((r = iio_device_attr_write_longlong(m_dexter_dsp_tx, "gain0", 0)) != 0) {
         throw std::runtime_error("Failed to set dexter_dsp_tx.gain0 = 0 : " + get_iio_error(r));
@@ -248,35 +196,118 @@ void Dexter::channel_down()
     etiLog.level(debug) << "DEXTER CHANNEL_DOWN";
 }
 
-
-Dexter::~Dexter()
+void Dexter::handle_hw_time()
 {
-    m_running = false;
-    if (m_underflow_read_thread.joinable()) {
-        m_underflow_read_thread.join();
-    }
-
-    if (m_ctx) {
-        if (m_dexter_dsp_tx) {
-            iio_device_attr_write_longlong(m_dexter_dsp_tx, "gain0", 0);
-        }
-
-        if (m_buffer) {
-            iio_buffer_destroy(m_buffer);
-            m_buffer = nullptr;
-        }
-
-        if (m_tx_channel) {
-            iio_channel_disable(m_tx_channel);
-        }
-
-        iio_context_destroy(m_ctx);
-        m_ctx = nullptr;
-    }
+    /*
+     * On startup, wait until `gpsdo_locked==1` and `pps_loss_of_signal==0`,
+     * then do the clocks alignment and go to normal state.
+     *
+     * In normal state, if `pps_loss_of_signal==1`, go to holdover state.
+     *
+     * If we've been in holdover state for longer than the configured time, or
+     * if `pps_loss_of_signal==0` stop the mod and restart.
+     */
+    int r;
 
-    if (m_underflow_ctx) {
-        iio_context_destroy(m_underflow_ctx);
-        m_underflow_ctx = nullptr;
+    switch (m_clock_state) {
+        case DexterClockState::Startup:
+            {
+                long long gpsdo_locked = 0;
+                if ((r = iio_device_attr_read_longlong(m_dexter_dsp_tx, "gpsdo_locked", &gpsdo_locked)) != 0) {
+                    etiLog.level(error) << "Failed to get dexter_dsp_tx.gpsdo_locked: " << get_iio_error(r);
+                    throw std::runtime_error("Dexter: Cannot read IIO attribute");
+                }
+
+                long long pps_loss_of_signal = 0;
+                if ((r = iio_device_attr_read_longlong(m_dexter_dsp_tx, "pps_loss_of_signal", &pps_loss_of_signal)) != 0) {
+                    etiLog.level(error) << "Failed to get dexter_dsp_tx.pps_loss_of_signal: " << get_iio_error(r);
+                    throw std::runtime_error("Dexter: Cannot read IIO attribute");
+                }
+
+                if (gpsdo_locked == 1 and pps_loss_of_signal == 0) {
+                    /* Procedure:
+                     * Wait 200ms after second change, fetch pps_clks attribute
+                     * idem at the next second, and check that pps_clks incremented by DSP_CLOCK
+                     * If ok, store the correspondence between current second change (measured in UTC clock time)
+                     * and the counter value at pps rising edge. */
+
+                    etiLog.level(info) << "Dexter: Waiting for second change...";
+
+                    struct timespec time_at_startup;
+                    fill_time(&time_at_startup);
+                    time_at_startup.tv_nsec = 0;
+
+                    struct timespec time_now;
+                    do {
+                        fill_time(&time_now);
+                        this_thread::sleep_for(chrono::milliseconds(1));
+                    } while (time_at_startup.tv_sec == time_now.tv_sec);
+                    this_thread::sleep_for(chrono::milliseconds(200));
+
+                    long long pps_clks = 0;
+                    if ((r = iio_device_attr_read_longlong(m_dexter_dsp_tx, "pps_clks", &pps_clks)) != 0) {
+                        etiLog.level(error) << "Failed to get dexter_dsp_tx.pps_clks: " << get_iio_error(r);
+                        throw std::runtime_error("Dexter: Cannot read IIO attribute");
+                    }
+
+                    time_t tnow = time_now.tv_sec;
+                    etiLog.level(info) << "Dexter: pps_clks " << pps_clks << " at UTC " <<
+                        put_time(std::gmtime(&tnow), "%Y-%m-%d %H:%M:%S");
+
+                    time_at_startup.tv_sec = time_now.tv_sec;
+                    do {
+                        fill_time(&time_now);
+                        this_thread::sleep_for(chrono::milliseconds(1));
+                    } while (time_at_startup.tv_sec == time_now.tv_sec);
+                    this_thread::sleep_for(chrono::milliseconds(200));
+
+                    long long pps_clks2 = 0;
+                    if ((r = iio_device_attr_read_longlong(m_dexter_dsp_tx, "pps_clks", &pps_clks2)) != 0) {
+                        etiLog.level(error) << "Failed to get dexter_dsp_tx.pps_clks: " << get_iio_error(r);
+                        throw std::runtime_error("Dexter: Cannot read IIO attribute");
+                    }
+                    tnow = time_now.tv_sec;
+                    etiLog.level(info) << "Dexter: pps_clks increased by " << pps_clks2 - pps_clks << " at UTC " <<
+                        put_time(std::gmtime(&tnow), "%Y-%m-%d %H:%M:%S");
+
+                    if ((uint64_t)pps_clks + DSP_CLOCK != (uint64_t)pps_clks2) {
+                        throw std::runtime_error("Dexter: Wrong increase of pps_clks, expected " + to_string(DSP_CLOCK));
+                    }
+
+                    m_utc_seconds_at_startup = time_now.tv_sec;
+                    m_clock_count_at_startup = pps_clks2;
+                    m_holdover_since = chrono::steady_clock::time_point::min();
+                    m_clock_state = DexterClockState::Normal;
+                }
+            }
+            break;
+        case DexterClockState::Normal:
+            {
+                long long pps_loss_of_signal = 0;
+                if ((r = iio_device_attr_read_longlong(m_dexter_dsp_tx, "pps_loss_of_signal", &pps_loss_of_signal)) != 0) {
+                    etiLog.level(error) << "Failed to get dexter_dsp_tx.pps_loss_of_signal: " << get_iio_error(r);
+                    throw std::runtime_error("Dexter: Cannot read IIO attribute");
+                }
+
+                if (pps_loss_of_signal == 1) {
+                    m_holdover_since = chrono::steady_clock::now();
+                    m_clock_state = DexterClockState::Holdover;
+                }
+            }
+            break;
+        case DexterClockState::Holdover:
+            {
+                using namespace chrono;
+                const duration<double> d = steady_clock::now() - m_holdover_since;
+                const auto max_holdover_duration = seconds(m_conf.maxGPSHoldoverTime);
+                if (d > max_holdover_duration) {
+                    m_clock_state = DexterClockState::Startup;
+                    m_utc_seconds_at_startup = 0;
+                    m_clock_count_at_startup = 0;
+                    m_holdover_since = chrono::steady_clock::time_point::min();
+                }
+            }
+            break;
     }
 }
 
@@ -526,7 +557,14 @@ double Dexter::get_real_secs(void) const
         throw std::runtime_error("Dexter: Cannot read IIO attribute");
     }
 
-    return (double)m_utc_seconds_at_startup + (double)(clks - m_clock_count_at_startup) / (double)DSP_CLOCK;
+    switch (m_clock_state) {
+        case DexterClockState::Startup:
+            return 0;
+        case DexterClockState::Normal:
+        case DexterClockState::Holdover:
+            return (double)m_utc_seconds_at_startup + (double)(clks - m_clock_count_at_startup) / (double)DSP_CLOCK;
+    }
+    throw std::logic_error("Unhandled switch");
 }
 
 void Dexter::set_rxgain(double rxgain)
@@ -585,46 +623,53 @@ void Dexter::transmit_frame(struct FrameData&& frame)
 
     const bool require_timestamped_tx = (m_conf.enableSync and frame.ts.timestamp_valid);
 
+    handle_hw_time();
+
     if (not m_channel_is_up) {
         if (require_timestamped_tx) {
-            constexpr uint64_t TIMESTAMP_PPS_PER_DSP_CLOCKS = DSP_CLOCK / 16384000;
-            // TIMESTAMP_PPS_PER_DSP_CLOCKS=10 because timestamp_pps is represented in 16.384 MHz clocks
-            uint64_t frame_start_clocks =
-                // at second level
-                ((int64_t)frame.ts.timestamp_sec - (int64_t)m_utc_seconds_at_startup) * DSP_CLOCK + m_clock_count_at_startup +
-                // at subsecond level
-                (uint64_t)frame.ts.timestamp_pps * TIMESTAMP_PPS_PER_DSP_CLOCKS;
-
-            const double margin_s = frame.ts.offset_to_system_time();
-
-            long long clks = 0;
-            int r = 0;
-            if ((r = iio_device_attr_read_longlong(m_dexter_dsp_tx, "clks", &clks)) != 0) {
-                etiLog.level(error) << "Failed to get dexter_dsp_tx.clks: " << get_iio_error(r);
-                throw std::runtime_error("Dexter: Cannot read IIO attribute");
+            if (m_clock_state == DexterClockState::Startup) {
+                return; // not ready
             }
-
-            const double margin_device_s = (double)(frame_start_clocks - clks) / DSP_CLOCK;
-
-            etiLog.level(debug) << "DEXTER FCT " << frame.ts.fct << " TS CLK " <<
-                ((int64_t)frame.ts.timestamp_sec - (int64_t)m_utc_seconds_at_startup) * DSP_CLOCK << " + " <<
-                m_clock_count_at_startup << " + " <<
-                (uint64_t)frame.ts.timestamp_pps * TIMESTAMP_PPS_PER_DSP_CLOCKS << " = " <<
-                frame_start_clocks << " DELTA " << margin_s << " " << margin_device_s;
-
-            // Ensure we hand the frame over to HW with a bit of margin
-            if (margin_s < 0.2) {
-                etiLog.level(warn) << "Skip frame short margin " << margin_s;
-                num_late++;
-                return;
-            }
-
-            if ((r = iio_device_attr_write_longlong(m_dexter_dsp_tx, "stream0_start_clks", frame_start_clocks)) != 0) {
-                etiLog.level(warn) << "Skip frame, failed to set dexter_dsp_tx.stream0_start_clks = " << frame_start_clocks << " : " << get_iio_error(r);
-                num_late++;
-                return;
+            else {
+                constexpr uint64_t TIMESTAMP_PPS_PER_DSP_CLOCKS = DSP_CLOCK / 16384000;
+                // TIMESTAMP_PPS_PER_DSP_CLOCKS=10 because timestamp_pps is represented in 16.384 MHz clocks
+                uint64_t frame_start_clocks =
+                    // at second level
+                    ((int64_t)frame.ts.timestamp_sec - (int64_t)m_utc_seconds_at_startup) * DSP_CLOCK + m_clock_count_at_startup +
+                    // at subsecond level
+                    (uint64_t)frame.ts.timestamp_pps * TIMESTAMP_PPS_PER_DSP_CLOCKS;
+
+                const double margin_s = frame.ts.offset_to_system_time();
+
+                long long clks = 0;
+                int r = 0;
+                if ((r = iio_device_attr_read_longlong(m_dexter_dsp_tx, "clks", &clks)) != 0) {
+                    etiLog.level(error) << "Failed to get dexter_dsp_tx.clks: " << get_iio_error(r);
+                    throw std::runtime_error("Dexter: Cannot read IIO attribute");
+                }
+
+                const double margin_device_s = (double)(frame_start_clocks - clks) / DSP_CLOCK;
+
+                etiLog.level(debug) << "DEXTER FCT " << frame.ts.fct << " TS CLK " <<
+                    ((int64_t)frame.ts.timestamp_sec - (int64_t)m_utc_seconds_at_startup) * DSP_CLOCK << " + " <<
+                    m_clock_count_at_startup << " + " <<
+                    (uint64_t)frame.ts.timestamp_pps * TIMESTAMP_PPS_PER_DSP_CLOCKS << " = " <<
+                    frame_start_clocks << " DELTA " << margin_s << " " << margin_device_s;
+
+                // Ensure we hand the frame over to HW with a bit of margin
+                if (margin_s < 0.2) {
+                    etiLog.level(warn) << "Skip frame short margin " << margin_s;
+                    num_late++;
+                    return;
+                }
+
+                if ((r = iio_device_attr_write_longlong(m_dexter_dsp_tx, "stream0_start_clks", frame_start_clocks)) != 0) {
+                    etiLog.level(warn) << "Skip frame, failed to set dexter_dsp_tx.stream0_start_clks = " << frame_start_clocks << " : " << get_iio_error(r);
+                    num_late++;
+                    return;
+                }
+                m_require_timestamp_refresh = false;
             }
-            m_require_timestamp_refresh = false;
         }
 
         channel_up();
@@ -704,8 +749,37 @@ void Dexter::underflow_read_process()
     m_running = false;
 }
 
-} // namespace Output
+Dexter::~Dexter()
+{
+    m_running = false;
+    if (m_underflow_read_thread.joinable()) {
+        m_underflow_read_thread.join();
+    }
 
-#endif // HAVE_DEXTER
+    if (m_ctx) {
+        if (m_dexter_dsp_tx) {
+            iio_device_attr_write_longlong(m_dexter_dsp_tx, "gain0", 0);
+        }
 
+        if (m_buffer) {
+            iio_buffer_destroy(m_buffer);
+            m_buffer = nullptr;
+        }
 
+        if (m_tx_channel) {
+            iio_channel_disable(m_tx_channel);
+        }
+
+        iio_context_destroy(m_ctx);
+        m_ctx = nullptr;
+    }
+
+    if (m_underflow_ctx) {
+        iio_context_destroy(m_underflow_ctx);
+        m_underflow_ctx = nullptr;
+    }
+}
+
+} // namespace Output
+
+#endif // HAVE_DEXTER
diff --git a/src/output/Dexter.h b/src/output/Dexter.h
index 3d47f87..f70bb14 100644
--- a/src/output/Dexter.h
+++ b/src/output/Dexter.h
@@ -42,6 +42,7 @@ DESCRIPTION:
 #include <ctime>
 #include <mutex>
 #include <thread>
+#include <variant>
 
 #include "output/SDR.h"
 #include "ModPlugin.h"
@@ -50,6 +51,12 @@ DESCRIPTION:
 
 namespace Output {
 
+enum class DexterClockState {
+   Startup,
+   Normal,
+   Holdover
+};
+
 class Dexter : public Output::SDRDevice
 {
     public:
@@ -85,6 +92,7 @@ class Dexter : public Output::SDRDevice
     private:
         void channel_up();
         void channel_down();
+        void handle_hw_time();
 
         bool m_channel_is_up = false;
 
@@ -112,9 +120,15 @@ class Dexter : public Output::SDRDevice
 
         size_t num_buffers_pushed = 0;
 
-        uint64_t m_utc_seconds_at_startup;
+        DexterClockState m_clock_state = DexterClockState::Startup;
+
+        // Only valid when m_clock_state is not Startup
+        uint64_t m_utc_seconds_at_startup = 0;
         uint64_t m_clock_count_at_startup = 0;
-        uint64_t m_clock_count_frame = 0;
+
+        // Only valid when m_clock_state Holdover
+        std::chrono::steady_clock::time_point m_holdover_since =
+            std::chrono::steady_clock::time_point::min();
 };
 
 } // namespace Output