summaryrefslogtreecommitdiffstats
path: root/app/ref.c
diff options
context:
space:
mode:
Diffstat (limited to 'app/ref.c')
-rw-r--r--app/ref.c486
1 files changed, 486 insertions, 0 deletions
diff --git a/app/ref.c b/app/ref.c
new file mode 100644
index 0000000..a131fda
--- /dev/null
+++ b/app/ref.c
@@ -0,0 +1,486 @@
+#include "project.h"
+
+static volatile unsigned us_ref_idx;
+static volatile uint64_t us_ref_freq[2];
+static volatile uint64_t us_ref_offset[2];
+static volatile uint64_t us_ref_phase[2];
+
+static volatile unsigned us_ro_idx;
+static volatile uint32_t us_ro_high_tick[2];
+static volatile int us_ro_mm[2];
+
+
+static int64_t ref_offset;
+static int64_t ref_phase;
+static int64_t ref_freq = HW_CLOCK_HZ;
+
+uint64_t ref_last_update;
+int ref_valid = 0;
+int ref_offset_known = 0;
+
+
+static void us_put (void)
+{
+ unsigned i = us_ref_idx;
+
+ i ^= 1;
+ us_ref_freq[i] = ref_freq;
+ us_ref_offset[i] = ref_offset;
+ us_ref_phase[i] = ref_phase;
+
+ compiler_mb();
+ us_ref_idx = i;
+}
+
+static void us_get (uint64_t *f, uint64_t *o, uint64_t *p)
+{
+ unsigned i;
+
+ do {
+ i = us_ref_idx;
+ *f = us_ref_freq[i];
+ *o = us_ref_offset[i];
+ *p = us_ref_phase[i];
+ compiler_mb();
+ } while (i != us_ref_idx);
+}
+
+#if HW_CLOCK_LEN == 32
+
+#define QUARTER (1UL << 29)
+#define HALF (1UL << 30)
+#define THREE_QUARTERS (HALF+QUARTER)
+#define ONE (~(uint32_t)0)
+
+uint64_t ref_extend_irq (uint32_t now)
+{
+ uint64_t ret;
+ uint32_t ht;
+ int m;
+
+ ht = us_ro_high_tick[us_ro_idx];
+ m = us_ro_mm[us_ro_idx];
+
+ if (!m) {
+ ret = ht;
+ ret <<= 32;
+ ret |= now;
+ } else {
+ if (now < HALF) {
+ ret = ht;
+ ret <<= 32;
+ ret |= now;
+ } else {
+ ret = ht - 1;
+ ret <<= 32;
+ ret |= now;
+ }
+ }
+
+ return ret;
+}
+
+uint64_t ref_extend (uint32_t now)
+{
+ uint64_t ret;
+
+ uint32_t ht, oht;
+ int m, om;
+ unsigned i = us_ro_idx;
+
+ oht = ht = us_ro_high_tick[i];
+ om = m = us_ro_mm[i];
+
+
+ if (!m) {
+ ret = ht;
+ ret <<= 32;
+ ret |= now;
+
+
+ if ((now > THREE_QUARTERS) && (now <= ONE)) {
+ ht++;
+ m = 1;
+ }
+
+ } else {
+ if (now < HALF) {
+ ret = ht;
+ ret <<= 32;
+ ret |= now;
+ } else {
+ ret = ht - 1;
+ ret <<= 32;
+ ret |= now;
+ }
+
+ if ((now > QUARTER) && (now < HALF))
+ m = 0;
+ }
+
+
+ if ((ht != oht) || (m != om)) {
+ i ^= 1;
+ us_ro_high_tick[i] = ht;
+ us_ro_mm[i] = m;
+ compiler_mb();
+ us_ro_idx = i;
+ }
+
+ return ret;
+}
+#elif HW_CLOCK_LEN == 31
+
+#define QUARTER (1UL << 28)
+#define HALF (1UL << 29)
+#define THREE_QUARTERS (HALF+QUARTER)
+#define ONE (0x7fffffff)
+
+
+uint64_t ref_extend_irq (uint32_t now)
+{
+ uint64_t ret;
+
+
+ uint32_t ht;
+ int m;
+
+ ht = us_ro_high_tick[us_ro_idx];
+ m = us_ro_mm[us_ro_idx];
+
+
+ if (!m) {
+ ret = ht;
+ ret <<= 31;
+ ret |= now;
+ } else {
+ if (now < HALF) {
+ ret = ht;
+ ret <<= 31;
+ ret |= now;
+ } else {
+ ret = ht - 1;
+ ret <<= 31;
+ ret |= now;
+ }
+ }
+
+ return ret;
+}
+
+
+uint64_t ref_extend (uint32_t now)
+{
+ uint64_t ret;
+
+
+ uint32_t ht, oht;
+ int m, om;
+ unsigned i = us_ro_idx;
+
+ oht = ht = us_ro_high_tick[i];
+ om = m = us_ro_mm[i];
+
+
+
+
+ if (!m) {
+ ret = ht;
+ ret <<= 31;
+ ret |= now;
+
+ if ((now > THREE_QUARTERS) && (now <= ONE)) {
+ ht++;
+ m = 1;
+ }
+
+ } else {
+ if (now < HALF) {
+ ret = ht;
+ ret <<= 31;
+ ret |= now;
+ } else {
+ ret = ht - 1;
+ ret <<= 31;
+ ret |= now;
+ }
+
+ if ((now > QUARTER) && (now < HALF))
+ m = 0;
+ }
+
+
+
+ if ((ht != oht) || (m != om)) {
+ i ^= 1;
+ us_ro_high_tick[i] = ht;
+ us_ro_mm[i] = m;
+ compiler_mb();
+ us_ro_idx = i;
+ }
+
+ return ret;
+}
+
+
+
+
+
+#else
+#error unknown hardware clock length
+#endif
+
+
+uint64_t ref_get_irq (void)
+{
+ uint32_t now = HW_CLOCK_REG;
+ return ref_extend_irq (now);
+}
+
+
+uint64_t ref_get (void)
+{
+ uint32_t now = HW_CLOCK_REG;
+ return ref_extend (now);
+}
+
+
+void ref_slow_tick()
+{
+ ref_get();
+}
+
+
+
+static void modify_ref_freq (uint64_t now, uint64_t new)
+{
+ int64_t pd1, pd2, te;
+
+ pd1 = now - ref_phase;
+ te = pd1 / ref_freq;
+ pd1 %= ref_freq;
+
+ if (pd1 > (ref_freq >> 1)) {
+ te++;
+ pd1 = pd1 - ref_freq;
+ }
+
+ ref_freq = new;
+
+ pd2 = pd1 + (te * ref_freq);
+
+ ref_phase = now - pd2;
+
+}
+
+uint64_t make_happy (uint64_t abs, int64_t shift)
+{
+ shift *= HW_CLOCK_HZ;
+
+ if (shift < 0) {
+ shift = -shift;
+
+ if (abs < (uint64_t) shift) return 0;
+ else
+ return abs - shift;
+ }
+
+ return abs + shift;
+
+
+}
+
+#define FF_B 16
+#define FF_A ((FF_B)-1)
+#define FF_C 16
+
+static uint64_t fff;
+
+static void fll_init (uint64_t start_freq)
+{
+ fff = start_freq * FF_C;
+}
+
+static uint64_t fll (uint64_t obs_freq)
+{
+ uint64_t new_freq;
+
+
+
+ fff *= FF_A;
+ fff += obs_freq * FF_C;
+ fff += FF_B / 2;
+ fff /= FF_B;
+
+ new_freq = fff;
+ new_freq += FF_C / 2;
+ new_freq /= FF_C;
+
+
+ return new_freq;
+}
+
+
+static int64_t edge_to_phase (uint64_t edge)
+{
+ int64_t obs_phase;
+ obs_phase = edge - ref_phase;
+ obs_phase %= ref_freq;
+
+ if (obs_phase > ref_freq / 2)
+ obs_phase -= ref_freq;
+
+ return obs_phase;
+}
+
+#define PF_A 16
+static uint64_t pll (int64_t obs_phase)
+{
+ int64_t pd ;
+
+
+ pd = obs_phase / PF_A;
+
+ if (!pd) {
+ if (obs_phase < 0) pd--;
+
+ if (obs_phase > 0) pd++;
+ }
+
+
+ return pd;
+}
+
+
+
+void ref_dispatch (uint64_t edge, const char *src)
+{
+ static uint64_t last_edge;
+ uint64_t obs_freq, obs_phase, new_freq;
+ static int jump_start = 1;
+
+ if (!last_edge) {
+ last_edge = edge;
+ return;
+ }
+
+
+ obs_freq = edge - last_edge;
+ last_edge = edge;
+ obs_phase = edge_to_phase (edge);
+
+
+
+ // delta_f = obs_freq - ref_freq;
+
+
+
+ printf ("REF PLL: obs_f=%9d delta_phi=%5d f=%9d %s\r\n",
+ (int) obs_freq,
+ (int) obs_phase,
+ (int) ref_freq, src);
+
+
+ /*Ignore bogus observations*/
+ if (obs_freq > (HW_CLOCK_HZ + (HW_CLOCK_HZ / 2)))
+ return;
+
+ if (obs_freq < (HW_CLOCK_HZ - (HW_CLOCK_HZ / 2)))
+ return;
+
+ if (jump_start) {
+ new_freq = obs_freq;
+ fll_init (new_freq);
+ modify_ref_freq (edge, new_freq);
+ ref_phase += obs_phase;
+
+ jump_start = 0;
+ } else {
+ new_freq = fll (obs_freq);
+ modify_ref_freq (edge, new_freq);
+ ref_phase += pll (obs_phase);
+ }
+
+ if (ref_offset_known)
+ ref_valid = 1;
+
+ us_put();
+ ref_last_update = edge;
+}
+
+
+
+void ref_set_offset (EPOCH epoch, uint64_t abs)
+{
+ int64_t new_offset;
+ int diff;
+
+
+ /* Find nearest second to abs*/
+ abs += ref_freq >> 2;
+ abs -= ref_phase;
+ abs /= ref_freq;
+
+ new_offset = epoch.s - abs;
+
+ if (new_offset != ref_offset) {
+ diff = (int) (new_offset - ref_offset);
+
+
+ printf ("REF wallclock offset moved by %d\r\n", diff);
+ ref_offset = new_offset;
+ }
+
+ us_put();
+ ref_offset_known = 1;
+ time_known = 1;
+}
+
+
+
+
+static EPOCH _ref_decompose (uint64_t abs, uint64_t f, uint64_t o)
+{
+ EPOCH ret;
+
+
+ ret.s = abs / f;
+ abs -= f * ret.s;
+
+ ret.s += o;
+
+ abs *= (uint64_t) 1000000000;
+ abs = abs / f;
+
+ ret.ns = abs;
+
+ return ret;
+}
+
+EPOCH ref_decompose_diff (int64_t diff)
+{
+ EPOCH ret;
+ uint64_t f, o, p;
+
+ us_get (&f, &o, &p);
+
+ if (diff >= 0)
+ return _ref_decompose (diff, f, o);
+
+ ret = _ref_decompose (-diff, f, o);
+ ret.s = -ret.s;
+ ret.ns = -ret.ns;
+
+ return ret;
+}
+
+
+EPOCH ref_decompose (uint64_t abs)
+{
+ uint64_t f, o, p;
+ us_get (&f, &o, &p);
+
+ abs -= p;
+
+ return _ref_decompose (abs, f, o);
+}
generated by cgit v1.2.3 (git 2.25.1) at 2024-04-30 07:48:40 +0000