Improve DEXTER SFN support

16 files changed, 184 insertions, 43 deletions

diff --git a/src/output/BladeRF.cpp b/src/output/BladeRF.cpp
index a6ad0cc..dd48736 100755
--- a/src/output/BladeRF.cpp
+++ b/src/output/BladeRF.cpp
@@ -269,7 +269,7 @@ double BladeRF::get_rxgain(void) const
 size_t BladeRF::receive_frame(
     complexf *buf,
     size_t num_samples,
-    struct frame_timestamp &ts,
+    frame_timestamp &ts,
     double timeout_secs)
 {
     // TODO
diff --git a/src/output/BladeRF.h b/src/output/BladeRF.h
index bc6db38..e048daa 100755
--- a/src/output/BladeRF.h
+++ b/src/output/BladeRF.h
@@ -83,7 +83,7 @@ class BladeRF : public Output::SDRDevice
        virtual size_t receive_frame(
                complexf *buf,
                size_t num_samples,
-               struct frame_timestamp& ts,
+               frame_timestamp& ts,
                double timeout_secs) override;
 
        // Return true if GPS and reference clock inputs are ok
@@ -109,4 +109,4 @@ class BladeRF : public Output::SDRDevice
 
 } // namespace Output
 
-#endif // HAVE_BLADERF
\ No newline at end of file
+#endif // HAVE_BLADERF
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;
+        }
     }
 }
 
diff --git a/src/output/Dexter.h b/src/output/Dexter.h
index 7a7f6c1..5418b73 100644
--- a/src/output/Dexter.h
+++ b/src/output/Dexter.h
@@ -39,6 +39,7 @@ DESCRIPTION:
 
 #include <string>
 #include <memory>
+#include <ctime>
 
 #include "output/SDR.h"
 #include "ModPlugin.h"
@@ -70,7 +71,7 @@ class Dexter : public Output::SDRDevice
         virtual size_t receive_frame(
                 complexf *buf,
                 size_t num_samples,
-                struct frame_timestamp& ts,
+                frame_timestamp& ts,
                 double timeout_secs) override;
 
         // Return true if GPS and reference clock inputs are ok
@@ -90,9 +91,12 @@ class Dexter : public Output::SDRDevice
         struct iio_buffer *m_buffer = nullptr;
 
         size_t underflows = 0;
-        size_t overflows = 0;
-        size_t late_packets = 0;
+        size_t num_late = 0;
         size_t num_frames_modulated = 0;
+
+        uint64_t m_utc_seconds_at_startup;
+        uint64_t m_clock_count_at_startup = 0;
+        uint64_t m_clock_count_frame = 0;
 };
 
 } // namespace Output
diff --git a/src/output/Feedback.cpp b/src/output/Feedback.cpp
index 88d8319..d112b5a 100644
--- a/src/output/Feedback.cpp
+++ b/src/output/Feedback.cpp
@@ -84,7 +84,7 @@ DPDFeedbackServer::~DPDFeedbackServer()
 
 void DPDFeedbackServer::set_tx_frame(
         const std::vector<uint8_t> &buf,
-        const struct frame_timestamp &buf_ts)
+        const frame_timestamp &buf_ts)
 {
     if (not m_running) {
         throw runtime_error("DPDFeedbackServer not running");
diff --git a/src/output/Feedback.h b/src/output/Feedback.h
index aef86b0..b31347f 100644
--- a/src/output/Feedback.h
+++ b/src/output/Feedback.h
@@ -94,7 +94,7 @@ class DPDFeedbackServer {
         ~DPDFeedbackServer();
 
         void set_tx_frame(const std::vector<uint8_t> &buf,
-                const struct frame_timestamp& ts);
+                const frame_timestamp& ts);
 
     private:
         // Thread that reacts to burstRequests and receives from the SDR device
diff --git a/src/output/Lime.cpp b/src/output/Lime.cpp
index 6f7eed5..d3e4640 100644
--- a/src/output/Lime.cpp
+++ b/src/output/Lime.cpp
@@ -353,7 +353,7 @@ double Lime::get_rxgain(void) const
 size_t Lime::receive_frame(
     complexf *buf,
     size_t num_samples,
-    struct frame_timestamp &ts,
+    frame_timestamp &ts,
     double timeout_secs)
 {
     // TODO
diff --git a/src/output/Lime.h b/src/output/Lime.h
index 72a018e..a4603c0 100644
--- a/src/output/Lime.h
+++ b/src/output/Lime.h
@@ -75,7 +75,7 @@ class Lime : public Output::SDRDevice
     virtual size_t receive_frame(
         complexf *buf,
         size_t num_samples,
-        struct frame_timestamp &ts,
+        frame_timestamp &ts,
         double timeout_secs) override;
 
     // Return true if GPS and reference clock inputs are ok
diff --git a/src/output/SDR.cpp b/src/output/SDR.cpp
index 6078fc7..f1ed2b0 100644
--- a/src/output/SDR.cpp
+++ b/src/output/SDR.cpp
@@ -104,6 +104,12 @@ SDR::~SDR()
     }
 }
 
+void SDR::set_sample_size(size_t size)
+{
+    etiLog.level(debug) << "Setting sample size to " << size;
+    m_size = size;
+}
+
 int SDR::process(Buffer *dataIn)
 {
     if (not m_running) {
@@ -125,6 +131,7 @@ meta_vec_t SDR::process_metadata(const meta_vec_t& metadataIn)
     if (m_device and m_running) {
         FrameData frame;
         frame.buf = std::move(m_frame);
+        frame.sampleSize = m_size;
 
         if (metadataIn.empty()) {
             etiLog.level(info) <<
@@ -138,7 +145,7 @@ meta_vec_t SDR::process_metadata(const meta_vec_t& metadataIn)
              * This behaviour is different to earlier versions of ODR-DabMod,
              * which took the timestamp from the latest ETI frame.
              */
-            frame.ts = *(metadataIn[0].ts);
+            frame.ts = metadataIn[0].ts;
 
             // TODO check device running
 
@@ -261,8 +268,6 @@ void SDR::handle_frame(struct FrameData& frame)
 {
     // Assumes m_device is valid
 
-    constexpr double tx_timeout = 20.0;
-
     if (not m_device->is_clk_source_ok()) {
         sleep_through_frame();
         return;
@@ -298,7 +303,7 @@ void SDR::handle_frame(struct FrameData& frame)
         }
 
         if (last_tx_time_initialised) {
-            const size_t sizeIn = frame.buf.size() / sizeof(complexf);
+            const size_t sizeIn = frame.buf.size() / frame.sampleSize;
 
             // Checking units for the increment calculation:
             // samps  * ticks/s  / (samps/s)
@@ -337,7 +342,7 @@ void SDR::handle_frame(struct FrameData& frame)
 
         etiLog.log(trace, "SDR,tist %f", time_spec.get_real_secs());
 
-        if (time_spec.get_real_secs() + tx_timeout < device_time) {
+        if (time_spec.get_real_secs() < device_time) {
             etiLog.level(warn) <<
                 "OutputSDR: Timestamp in the past at FCT=" << frame.ts.fct << " offset: " <<
                 std::fixed <<
diff --git a/src/output/SDR.h b/src/output/SDR.h
index ee89243..4dfde73 100644
--- a/src/output/SDR.h
+++ b/src/output/SDR.h
@@ -51,6 +51,7 @@ class SDR : public ModOutput, public ModMetadata, public RemoteControllable {
         SDR operator=(const SDR& other) = delete;
         virtual ~SDR();
 
+        virtual void set_sample_size(size_t size);
         virtual int process(Buffer *dataIn) override;
         virtual meta_vec_t process_metadata(const meta_vec_t& metadataIn) override;
 
@@ -75,6 +76,7 @@ class SDR : public ModOutput, public ModMetadata, public RemoteControllable {
 
         std::atomic<bool> m_running = ATOMIC_VAR_INIT(false);
         std::thread m_device_thread;
+        size_t m_size = sizeof(complexf);
         std::vector<uint8_t> m_frame;
         ThreadsafeQueue<FrameData> m_queue;
 
diff --git a/src/output/SDRDevice.h b/src/output/SDRDevice.h
index bb63f60..0bba74a 100644
--- a/src/output/SDRDevice.h
+++ b/src/output/SDRDevice.h
@@ -98,10 +98,11 @@ struct SDRDeviceConfig {
 struct FrameData {
     // Buffer holding frame data
     std::vector<uint8_t> buf;
+    size_t sampleSize = sizeof(complexf);
 
     // A full timestamp contains a TIST according to standard
     // and time information within MNSC with tx_second.
-    struct frame_timestamp ts;
+    frame_timestamp ts;
 };
 
 
@@ -132,7 +133,7 @@ class SDRDevice {
         virtual size_t receive_frame(
                 complexf *buf,
                 size_t num_samples,
-                struct frame_timestamp& ts,
+                frame_timestamp& ts,
                 double timeout_secs) = 0;
 
         // Returns device temperature in degrees C or NaN if not available
diff --git a/src/output/Soapy.cpp b/src/output/Soapy.cpp
index f138e9a..50f91a4 100644
--- a/src/output/Soapy.cpp
+++ b/src/output/Soapy.cpp
@@ -216,7 +216,7 @@ double Soapy::get_rxgain(void) const
 size_t Soapy::receive_frame(
         complexf *buf,
         size_t num_samples,
-        struct frame_timestamp& ts,
+        frame_timestamp& ts,
         double timeout_secs)
 {
     int flags = 0;
diff --git a/src/output/Soapy.h b/src/output/Soapy.h
index 4ee53ca..ca2618b 100644
--- a/src/output/Soapy.h
+++ b/src/output/Soapy.h
@@ -74,7 +74,7 @@ class Soapy : public Output::SDRDevice
         virtual size_t receive_frame(
                 complexf *buf,
                 size_t num_samples,
-                struct frame_timestamp& ts,
+                frame_timestamp& ts,
                 double timeout_secs) override;
 
         // Return true if GPS and reference clock inputs are ok
diff --git a/src/output/UHD.cpp b/src/output/UHD.cpp
index 3cf5aef..9b22dde 100644
--- a/src/output/UHD.cpp
+++ b/src/output/UHD.cpp
@@ -411,7 +411,7 @@ double UHD::get_rxgain() const
 size_t UHD::receive_frame(
         complexf *buf,
         size_t num_samples,
-        struct frame_timestamp& ts,
+        frame_timestamp& ts,
         double timeout_secs)
 {
     uhd::stream_cmd_t cmd(
diff --git a/src/output/UHD.h b/src/output/UHD.h
index 29867fb..4c1a4f0 100644
--- a/src/output/UHD.h
+++ b/src/output/UHD.h
@@ -88,7 +88,7 @@ class UHD : public Output::SDRDevice
         virtual size_t receive_frame(
                 complexf *buf,
                 size_t num_samples,
-                struct frame_timestamp& ts,
+                frame_timestamp& ts,
                 double timeout_secs) override;
 
         // Return true if GPS and reference clock inputs are ok
diff --git a/src/output/USRPTime.cpp b/src/output/USRPTime.cpp
index d1197ec..5a11851 100644
--- a/src/output/USRPTime.cpp
+++ b/src/output/USRPTime.cpp
@@ -46,11 +46,14 @@ USRPTime::USRPTime(
     m_conf(conf),
     time_last_check(timepoint_t::clock::now())
 {
-    if (m_conf.pps_src == "none") {
+    if (m_conf.refclk_src == "internal" and m_conf.pps_src != "none") {
+        etiLog.level(warn) << "OutputUHD: Unusal refclk and pps source settings. Setting time once, no monitoring.";
+        set_usrp_time_from_pps();
+    }
+    else if (m_conf.pps_src == "none") {
         if (m_conf.enableSync) {
             etiLog.level(warn) <<
-                "OutputUHD: WARNING:"
-                " you are using synchronous transmission without PPS input!";
+                "OutputUHD: you are using synchronous transmission without PPS input!";
         }
 
         set_usrp_time_from_localtime();