From d95f7806746ff3b58769eee09965c29df1622cab Mon Sep 17 00:00:00 2001
From: Sebastien Bourdeauducq <sb@m-labs.hk>
Date: Mon, 28 Aug 2023 22:24:58 +0800
Subject: [PATCH] add RPLL port

---
 dsplib.cpp | 103 +++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 103 insertions(+)
 create mode 100644 dsplib.cpp

diff --git a/dsplib.cpp b/dsplib.cpp
new file mode 100644
index 0000000..37e9188
--- /dev/null
+++ b/dsplib.cpp
@@ -0,0 +1,103 @@
+#include <cstdint>
+#include <tuple>
+#include <optional>
+#include <cassert>
+
+// C++ port of RPLL from https://github.com/quartiq/idsp
+// MIT or Apache-2.0 license
+
+/// Reciprocal PLL.
+///
+/// Consumes noisy, quantized timestamps of a reference signal and reconstructs
+/// the phase and frequency of the update() invocations with respect to (and in units of
+/// 1 << 32 of) that reference.
+/// In other words, `update()` rate ralative to reference frequency,
+/// `u32::MAX` corresponding to both being equal.
+class RPLL {
+    private:
+        uint32_t dt2;  // 1 << dt2 is the counter rate to update() rate ratio
+        int32_t x;     // previous timestamp
+        uint32_t ff;   // current frequency estimate from frequency loop
+        uint32_t f;    // current frequency estimate from both frequency and phase loop
+        int32_t y;     // current phase estimate
+    public:
+        RPLL(uint32_t dt2);
+        std::tuple<int32_t, uint32_t> update(std::optional<int32_t>, uint32_t, uint32_t);
+        int32_t phase();
+        uint32_t frequency();
+};
+
+/// Create a new RPLL instance.
+///
+/// Args:
+/// * dt2: inverse update() rate. 1 << dt2 is the counter rate to update() rate ratio.
+///
+/// Returns:
+/// Initialized RPLL instance.
+RPLL::RPLL(uint32_t dt2):
+    dt2(dt2),
+    x(0),
+    ff(0),
+    f(0),
+    y(0)
+{}
+
+/// Advance the RPLL and optionally supply a new timestamp.
+///
+/// Args:
+/// * input: Optional new timestamp (wrapping around at the i32 boundary).
+///   There can be at most one timestamp per `update()` cycle (1 << dt2 counter cycles).
+/// * shift_frequency: Frequency lock settling time. 1 << shift_frequency is
+///   frequency lock settling time in counter periods. The settling time must be larger
+///   than the signal period to lock to.
+/// * shift_phase: Phase lock settling time. Usually one less than
+///   `shift_frequency` (see there).
+///
+/// Returns:
+/// A tuple containing the current phase (wrapping at the i32 boundary, pi) and
+/// frequency.
+std::tuple<int32_t, uint32_t> RPLL::update(std::optional<int32_t> input,
+                                           uint32_t shift_frequency,
+                                           uint32_t shift_phase)
+{
+    assert(shift_frequency >= dt2);
+    assert(shift_phase >= dt2);
+    // Advance phase
+    y += f;
+    if(input.has_value()) {
+        // Reference period in counter cycles
+        int32_t dx = *input - x;
+        // Store timestamp for next time
+        x = *input;
+        // Phase using the current frequency estimate
+        uint64_t p_sig_64 = (uint64_t)ff * (uint64_t)dx;
+        // Add half-up rounding bias and apply gain/attenuation
+        uint32_t p_sig =
+            (p_sig_64 + (1 << (shift_frequency - 1))) >> shift_frequency;
+        // Reference phase (1 << dt2 full turns) with gain/attenuation applied
+        uint32_t p_ref = 1 << (32 + dt2 - shift_frequency);
+        // Update frequency lock
+        ff += p_ref - p_sig;
+        // Time in counter cycles between timestamp and "now"
+        uint32_t dt = -x & ((1 << dt2) - 1);
+        // Reference phase estimate "now"
+        int32_t y_ref = (f >> dt2)*dt;
+        // Phase error with gain
+        int32_t dy = (y_ref - y) >> (shift_phase - dt2);
+        // Current frequency estimate from frequency lock and phase error
+        f = ff + dy;
+    }
+    return { y, f };
+}
+
+/// Return the current phase estimate
+int32_t RPLL::phase()
+{
+    return y;
+}
+
+/// Return the current frequency estimate
+uint32_t RPLL::frequency()
+{
+    return f;
+}