Handle Soapy TX and RX timestamps for DPD

5 files changed, 78 insertions, 19 deletions

diff --git a/TODO b/TODO
index 276dc1d..b736a7a 100644
--- a/TODO
+++ b/TODO
@@ -26,6 +26,7 @@ Clean up and separate GPS and refclk checks.
  * Add refclk stuff and timestamps to Soapy.
  * Ensure muting is set properly at startup.
  * Ensure synchronous is taken in account.
+ * Verify resync after underflow and muting
 
 *done* Add antenna selection to config.
 
diff --git a/doc/example.ini b/doc/example.ini
index a1e7d3a..098765e 100644
--- a/doc/example.ini
+++ b/doc/example.ini
@@ -35,7 +35,7 @@ loop=0
 ;transport=zeromq
 ;source=tcp://localhost:8080
 ; The option max_frames_queued defines the maximum number of ETI frames
-; that can be in the input queue
+; (frame duration: 24ms) that can be in the input queue
 ;max_frames_queued=100
 
 ; EDI input.
diff --git a/src/TimestampDecoder.h b/src/TimestampDecoder.h
index 33d9992..6eedf9e 100644
--- a/src/TimestampDecoder.h
+++ b/src/TimestampDecoder.h
@@ -81,6 +81,12 @@ struct frame_timestamp
         return ns;
     }
 
+    void set_ns(long long int time_ns) {
+        timestamp_sec = time_ns / 1000000000ull;
+        const long long int subsecond = time_ns % 1000000000ull;
+        timestamp_pps = lrint(subsecond * 16384000.0);
+    }
+
     void print(const char* t) const {
         fprintf(stderr,
                 "%s <frame_timestamp(%s, %d, %.9f, %d)>\n",
diff --git a/src/output/Soapy.cpp b/src/output/Soapy.cpp
index 3093b65..43b057b 100644
--- a/src/output/Soapy.cpp
+++ b/src/output/Soapy.cpp
@@ -106,8 +106,6 @@ Soapy::Soapy(SDRDeviceConfig& config) :
 
     const std::vector<size_t> channels({0});
     m_tx_stream = m_device->setupStream(SOAPY_SDR_TX, "CF32", channels);
-    m_device->activateStream(m_tx_stream);
-
     m_rx_stream = m_device->setupStream(SOAPY_SDR_RX, "CF32", channels);
 }
 
@@ -117,6 +115,10 @@ Soapy::~Soapy()
         if (m_tx_stream != nullptr) {
             m_device->closeStream(m_tx_stream);
         }
+
+        if (m_rx_stream != nullptr) {
+            m_device->closeStream(m_rx_stream);
+        }
         SoapySDR::Device::unmake(m_device);
     }
 }
@@ -197,19 +199,31 @@ size_t Soapy::receive_frame(
     void *buffs[1];
     buffs[0] = buf;
 
-    m_device->activateStream(m_rx_stream, flags, timeNs, numElems);
-
-    auto ret = m_device->readStream(m_tx_stream, buffs, num_samples, flags, timeNs);
+    int ret = m_device->activateStream(m_rx_stream, flags, timeNs, numElems);
+    if (ret != 0) {
+        throw std::runtime_error(string("Soapy activate RX stream failed: ") +
+                SoapySDR::errToStr(ret));
+    }
+    m_rx_stream_active = true;
 
-    m_device->deactivateStream(m_rx_stream);
+    int n_read = m_device->readStream(
+            m_rx_stream, buffs, num_samples, flags, timeNs);
 
-    // TODO update effective receive ts
+    ret = m_device->deactivateStream(m_rx_stream);
+    if (ret != 0) {
+        throw std::runtime_error(string("Soapy deactivate RX stream failed: ") +
+                SoapySDR::errToStr(ret));
+    }
+    m_rx_stream_active = false;
 
-    if (ret < 0) {
-        throw runtime_error("Soapy readStream error: " + to_string(ret));
+    if (n_read < 0) {
+        throw std::runtime_error(string("Soapy failed to read from RX stream : ") +
+                SoapySDR::errToStr(ret));
     }
 
-    return ret;
+    ts.set_ns(timeNs);
+
+    return n_read;
 }
 
 
@@ -228,7 +242,19 @@ void Soapy::transmit_frame(const struct FrameData& frame)
 {
     if (not m_device) throw runtime_error("Soapy device not set up");
 
-    // TODO timestamps
+    long long int timeNs = frame.ts.get_ns();
+    // muting and mutenotimestamp is handled by SDR
+    const bool has_time_spec = (m_conf.enableSync and frame.ts.timestamp_valid);
+
+    if (not m_tx_stream_active) {
+        int flags = has_time_spec ? SOAPY_SDR_HAS_TIME : 0;
+        int ret = m_device->activateStream(m_tx_stream, flags, timeNs);
+        if (ret != 0) {
+            throw std::runtime_error(string("Soapy activate TX stream failed: ") +
+                    SoapySDR::errToStr(ret));
+        }
+        m_tx_stream_active = true;
+    }
 
     // The frame buffer contains bytes representing FC32 samples
     const complexf *buf = reinterpret_cast<const complexf*>(frame.buf.data());
@@ -242,34 +268,58 @@ void Soapy::transmit_frame(const struct FrameData& frame)
 
     size_t num_acc_samps = 0;
     while (num_acc_samps < numSamples) {
+
         const void *buffs[1];
         buffs[0] = buf + num_acc_samps;
 
         const size_t samps_to_send = std::min(numSamples - num_acc_samps, mtu);
 
+        const bool eob_because_muting = m_conf.muting;
+        const bool end_of_burst = eob_because_muting or (
+                frame.ts.timestamp_valid and
+                frame.ts.timestamp_refresh and
+                samps_to_send <= mtu );
+
         int flags = 0;
 
-        auto ret = m_device->writeStream(m_tx_stream, buffs, samps_to_send, flags);
+        auto num_sent = m_device->writeStream(
+                m_tx_stream, buffs, samps_to_send, flags, timeNs);
 
-        if (ret == SOAPY_SDR_TIMEOUT) {
+        if (num_sent == SOAPY_SDR_TIMEOUT) {
             continue;
         }
-        else if (ret == SOAPY_SDR_OVERFLOW) {
+        else if (num_sent == SOAPY_SDR_OVERFLOW) {
             overflows++;
             continue;
         }
-        else if (ret == SOAPY_SDR_UNDERFLOW) {
+        else if (num_sent == SOAPY_SDR_UNDERFLOW) {
             underflows++;
             continue;
         }
 
-        if (ret < 0) {
+        if (num_sent < 0) {
             etiLog.level(error) << "Unexpected stream error " <<
-                SoapySDR::errToStr(ret);
+                SoapySDR::errToStr(num_sent);
             throw std::runtime_error("Fault in Soapy");
         }
 
-        num_acc_samps += ret;
+        timeNs += 1e9 * num_sent/m_conf.sampleRate;
+
+        num_acc_samps += num_sent;
+
+        if (end_of_burst) {
+            int ret_deact = m_device->deactivateStream(m_tx_stream);
+            if (ret_deact != 0) {
+                throw std::runtime_error(
+                        string("Soapy activate TX stream failed: ") +
+                        SoapySDR::errToStr(ret_deact));
+            }
+            m_tx_stream_active = false;
+        }
+
+        if (eob_because_muting) {
+            break;
+        }
     }
     num_frames_modulated++;
 }
diff --git a/src/output/Soapy.h b/src/output/Soapy.h
index 67b280d..5c20156 100644
--- a/src/output/Soapy.h
+++ b/src/output/Soapy.h
@@ -84,7 +84,9 @@ class Soapy : public Output::SDRDevice
         SDRDeviceConfig& m_conf;
         SoapySDR::Device *m_device = nullptr;
         SoapySDR::Stream *m_tx_stream = nullptr;
+        bool m_tx_stream_active = false;
         SoapySDR::Stream *m_rx_stream = nullptr;
+        bool m_rx_stream_active = false;
 
         size_t underflows = 0;
         size_t overflows = 0;