From 8c7ee88332652e7e79f6c1e4baacabe2183f7e8e Mon Sep 17 00:00:00 2001 From: root Date: Tue, 2 Mar 2021 12:54:03 +0000 Subject: working, with hybrid FLL/PLL, new refclk input and support for max7219 displays, neo 5 and neo 7 and a bazillion other fixes --- app/ref.c | 486 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 486 insertions(+) create mode 100644 app/ref.c (limited to 'app/ref.c') 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); +} -- cgit v1.2.3