Improve DEXTER SFN support

1 files changed, 145 insertions, 19 deletions

diff --git a/src/output/Dexter.cpp b/src/output/Dexter.cpp
index 605c61a..e4f672b 100644
--- a/src/output/Dexter.cpp
+++ b/src/output/Dexter.cpp
@@ -44,7 +44,11 @@ using namespace std;
 
 namespace Output {
 
+static constexpr uint64_t DSP_CLOCK = 2048000uLL * 80;
+
 static constexpr size_t TRANSMISSION_FRAME_LEN = (2656 + 76 * 2552) * 4;
+static constexpr size_t IIO_BUFFERS = 4;
+static constexpr size_t IIO_BUFFER_LEN = TRANSMISSION_FRAME_LEN / IIO_BUFFERS;
 
 static string get_iio_error(int err)
 {
@@ -53,6 +57,13 @@ static string get_iio_error(int err)
     return string(dst);
 }
 
+static void fill_time(struct timespec *t)
+{
+    if (clock_gettime(CLOCK_REALTIME, t) != 0) {
+        throw std::runtime_error(string("Failed to retrieve CLOCK_REALTIME") + strerror(errno));
+    }
+}
+
 Dexter::Dexter(SDRDeviceConfig& config) :
     SDRDevice(),
     m_conf(config)
@@ -93,8 +104,6 @@ Dexter::Dexter(SDRDeviceConfig& config) :
         etiLog.level(warn) << "Failed to set dexter_dsp_tx.stream0_start_clks = 0: " << get_iio_error(r);
     }
 
-    //iio_device_attr_read_longlong(const struct iio_device *dev, const char *attr, long long *val);
-
     if (m_conf.sampleRate != 2048000) {
         throw std::runtime_error("Dexter: Only 2048000 samplerate supported");
     }
@@ -109,7 +118,62 @@ Dexter::Dexter(SDRDeviceConfig& config) :
 
     // skip: antenna
 
-    // TODO: set H/W time
+    // 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;
+
+    // Reset start_clks
+    if ((r = iio_device_attr_write_longlong(m_dexter_dsp_tx, "stream0_start_clks", 0)) != 0) {
+        etiLog.level(warn) << "Failed to set dexter_dsp_tx.stream0_start_clks = " << 0 << " : " << get_iio_error(r);
+    }
 
     // Prepare streams
     constexpr int CHANNEL_INDEX = 0;
@@ -120,7 +184,7 @@ Dexter::Dexter(SDRDeviceConfig& config) :
 
     iio_channel_enable(m_tx_channel);
 
-    m_buffer = iio_device_create_buffer(m_ad9957_tx0, TRANSMISSION_FRAME_LEN/sizeof(int16_t), 0);
+    m_buffer = iio_device_create_buffer(m_ad9957_tx0, IIO_BUFFER_LEN/sizeof(int16_t), 0);
     if (!m_buffer) {
         throw std::runtime_error("Dexter: Cannot create IIO buffer.");
     }
@@ -208,8 +272,8 @@ SDRDevice::RunStatistics Dexter::get_run_statistics(void) const
 {
     RunStatistics rs;
     rs.num_underruns = underflows;
-    rs.num_overruns = overflows;
-    rs.num_late_packets = late_packets;
+    rs.num_overruns = 0;
+    rs.num_late_packets = num_late;
     rs.num_frames_modulated = num_frames_modulated;
     return rs;
 }
@@ -217,8 +281,22 @@ SDRDevice::RunStatistics Dexter::get_run_statistics(void) const
 
 double Dexter::get_real_secs(void) const
 {
-    // TODO
-    return 0;
+    struct timespec time_now;
+    fill_time(&time_now);
+    return (double)time_now.tv_sec + time_now.tv_nsec / 1000000000.0;
+
+    /* We don't use actual device time, because we only have clock counter on pps edge available, not
+     * current clock counter. */
+#if 0
+    long long pps_clks = 0;
+    int r = 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");
+    }
+
+    return (double)m_utc_seconds_at_startup + (double)(pps_clks - m_clock_count_at_startup) / (double)DSP_CLOCK;
+#endif
 }
 
 void Dexter::set_rxgain(double rxgain)
@@ -235,7 +313,7 @@ double Dexter::get_rxgain(void) const
 size_t Dexter::receive_frame(
         complexf *buf,
         size_t num_samples,
-        struct frame_timestamp& ts,
+        frame_timestamp& ts,
         double timeout_secs)
 {
     // TODO
@@ -263,34 +341,82 @@ double Dexter::get_temperature(void) const
 
 void Dexter::transmit_frame(const struct FrameData& frame)
 {
-    long long int timeNs = frame.ts.get_ns();
-    const bool has_time_spec = (m_conf.enableSync and frame.ts.timestamp_valid);
-
     if (frame.buf.size() != TRANSMISSION_FRAME_LEN) {
         etiLog.level(debug) << "Dexter::transmit_frame Expected " <<
             TRANSMISSION_FRAME_LEN << " got " << frame.buf.size();
         throw std::runtime_error("Dexter: invalid buffer size");
     }
 
+    const bool has_time_spec = (m_conf.enableSync and frame.ts.timestamp_valid);
+
+    if (has_time_spec) {
+        /*
+        uint64_t timeS = frame.ts.timestamp_sec;
+        etiLog.level(debug) << "Dexter: TS S " << timeS << " - " << m_utc_seconds_at_startup << " = " <<
+            timeS - m_utc_seconds_at_startup;
+        */
+
+        // 10 because timestamp_pps is represented in 16.384 MHz clocks
+        constexpr uint64_t TIMESTAMP_PPS_PER_DSP_CLOCKS = DSP_CLOCK / 16384000;
+        uint64_t frame_ts_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;
+
+        long long pps_clks = 0;
+        int r;
+        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);
+        }
+
+        etiLog.level(debug) << "Dexter: 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_ts_clocks << " DELTA " <<
+            frame_ts_clocks << " - " << pps_clks << " = " <<
+            (double)((int64_t)frame_ts_clocks - pps_clks) / DSP_CLOCK;
+
+        // Ensure we hand the frame over to HW at least 0.1s before timestamp
+        if (((int64_t)frame_ts_clocks - pps_clks) < (int64_t)DSP_CLOCK / 10) {
+            etiLog.level(warn) << "Skip frame short margin";
+            num_late++;
+            return;
+        }
+
+
+        if ((r = iio_device_attr_write_longlong(m_dexter_dsp_tx, "stream0_start_clks", frame_ts_clocks)) != 0) {
+            etiLog.level(warn) << "Skip frame, failed to set dexter_dsp_tx.stream0_start_clks = " << frame_ts_clocks << " : " << get_iio_error(r);
+            num_late++;
+            return;
+        }
+    }
+
     // DabMod::launch_modulator ensures we get int16_t IQ here
     //const size_t num_samples = frame.buf.size() / (2*sizeof(int16_t));
     //const int16_t *buf = reinterpret_cast<const int16_t*>(frame.buf.data());
 
-    memcpy(iio_buffer_start(m_buffer), frame.buf.data(), frame.buf.size());
-    ssize_t pushed = iio_buffer_push(m_buffer);
-    if (pushed < 0) {
-        etiLog.level(error) << "Dexter: failed to push buffer " << get_iio_error(pushed);
+    for (size_t i = 0; i < IIO_BUFFERS; i++) {
+        constexpr size_t buflen = TRANSMISSION_FRAME_LEN / IIO_BUFFERS;
+
+        memcpy(iio_buffer_start(m_buffer), frame.buf.data() + (i * buflen), buflen);
+        ssize_t pushed = iio_buffer_push(m_buffer);
+        if (pushed < 0) {
+            etiLog.level(error) << "Dexter: failed to push buffer " << get_iio_error(pushed);
+        }
     }
 
     num_frames_modulated++;
-    // TODO overflows, late_packets
 
     long long attr_value = 0;
     int r = 0;
 
     if ((r = iio_device_attr_read_longlong(m_dexter_dsp_tx, "buffer_underflows0", &attr_value)) == 0) {
-        fprintf(stderr, "buffer_underflows0 %lld\n", attr_value);
-        underflows = attr_value;
+        if ((size_t)attr_value != underflows and underflows != 0) {
+            etiLog.level(warn) << "Dexter: underflow! " << underflows << " -> " << attr_value;
+            underflows = attr_value;
+        }
     }
 }