1 files changed, 274 insertions, 0 deletions
diff --git a/app/ptp.c b/app/ptp.c
new file mode 100644
index 0000000..264918f
--- /dev/null
+++ b/app/ptp.c
@@ -0,0 +1,274 @@
+#include "project.h"
+
+static volatile unsigned us_ptp_idx;
+static volatile uint64_t us_ptp_freq[2];
+static volatile uint64_t us_ptp_offset[2];
+static volatile uint64_t us_ptp_phase[2];
+
+
+static int64_t ptp_phase;
+static int64_t ptp_freq = PTP_CLOCK_HZ;
+static int64_t ptp_offset;
+
+int ptp_offset_known;
+
+
+
+static void us_put (void)
+{
+  unsigned i = us_ptp_idx;
+
+  i ^= 1;
+  us_ptp_freq[i] = ptp_freq;
+  us_ptp_offset[i] = ptp_offset;
+  us_ptp_phase[i] = ptp_phase;
+
+  compiler_mb();
+  us_ptp_idx = i;
+}
+
+static void us_get (uint64_t *f, uint64_t *o, uint64_t *p)
+{
+  unsigned i;
+
+  do {
+    i = us_ptp_idx;
+    *f = us_ptp_freq[i];
+    *o = us_ptp_offset[i];
+    *p = us_ptp_phase[i];
+    compiler_mb();
+  } while (i != us_ptp_idx);
+}
+
+static void modify_ptp_freq (uint64_t now, uint64_t new)
+{
+  int64_t pd1, pd2, te;
+
+  pd1 = now - ptp_phase;
+  te = pd1 / ptp_freq;
+  pd1 %= ptp_freq;
+
+  if (pd1 > (ptp_freq >> 1)) {
+    te++;
+    pd1 = pd1 - ptp_freq;
+  }
+
+  ptp_freq = new;
+
+  pd2 = pd1 + (te * ptp_freq);
+
+  ptp_phase = now - pd2;
+
+}
+
+
+
+
+
+uint64_t ptp_to_u64 (uint32_t l, uint32_t h)
+{
+  uint64_t ret;
+
+  ret = h;
+  ret <<= 31;
+  ret |= l;
+
+  return ret;
+}
+
+uint64_t ptp_get (void)
+{
+  uint32_t pl, ph;
+
+  ph = ETH_PTPTSHR;
+
+  do {
+    compiler_mb();
+    pl = ETH_PTPTSLR;
+  } while (ph != ETH_PTPTSHR);
+
+  return ptp_to_u64 (pl, ph);
+}
+
+
+
+#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 - ptp_phase;
+  obs_phase %= ptp_freq;
+
+  if (obs_phase > ptp_freq / 2)
+    obs_phase -= ptp_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 ptp_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;
+
+  obs_freq = edge - last_edge;
+  last_edge = edge;
+  obs_phase = edge_to_phase (edge);
+
+  printf ("PTP PLL: obs_f=%9d delta_phi=%5d f=%9d %s\r\n",
+          (int) obs_freq,
+          (int) obs_phase,
+          (int) ptp_freq, src);
+
+  /*Ignore bogus observations*/
+  if (obs_freq > (PTP_CLOCK_HZ + (PTP_CLOCK_HZ / 2)))
+    return;
+
+  if (obs_freq < (PTP_CLOCK_HZ - (PTP_CLOCK_HZ / 2)))
+    return;
+
+  if (jump_start) {
+    new_freq = obs_freq;
+    fll_init (new_freq);
+    modify_ptp_freq (edge, new_freq);
+    ptp_phase += obs_phase;
+    jump_start = 0;
+  } else {
+    new_freq = fll (obs_freq);
+    modify_ptp_freq (edge, new_freq);
+    ptp_phase += pll (obs_phase);
+  }
+
+  us_put();
+
+}
+
+void ptp_set_offset (EPOCH epoch, uint64_t abs)
+{
+  int64_t new_offset;
+  int diff;
+
+
+  /* Find nearest second to abs*/
+  abs += ptp_freq >> 2;
+  abs -= ptp_phase;
+  abs /= ptp_freq;
+
+  new_offset = epoch.s - abs;
+
+  if (new_offset != ptp_offset) {
+    diff = (int) (new_offset - ptp_offset);
+
+
+    printf ("PTP wallclock offset moved by %d\r\n", diff);
+    ptp_offset = new_offset;
+    us_put();
+  }
+
+  ptp_offset_known = 1;
+}
+
+static EPOCH _ptp_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 ptp_decompose_diff (int64_t diff)
+{
+  EPOCH ret;
+  uint64_t f, o, p;
+
+  us_get (&f, &o, &p);
+
+  if (diff >= 0)
+    return _ptp_decompose (diff, f, o);
+
+  ret = _ptp_decompose (-diff, f, o);
+  ret.s = -ret.s;
+  ret.ns = -ret.ns;
+
+  return ret;
+}
+
+
+EPOCH ptp_decompose (uint64_t abs)
+{
+  uint64_t f, o, p;
+  us_get (&f, &o, &p);
+
+  abs -= p;
+
+  return _ptp_decompose (abs, f, o);
+}
+
+void ptp_slow_tick (void)
+{
+}
+
+
+
+