1 files changed, 123 insertions, 0 deletions
diff --git a/app/pll.c b/app/pll.c
new file mode 100644
index 0000000..4b89143
--- /dev/null
+++ b/app/pll.c
@@ -0,0 +1,123 @@
+#include "project.h"
+
+#define JUMP_THRESH 0.1
+#define JUMP_TICKS  30
+#define FEEDBACK    0.001
+
+
+static int64_t offset;
+static int64_t phase;
+static int64_t freq = HZ;
+
+static int out_of_lock = JUMP_TICKS + 1;
+
+uint64_t pll_last_update;
+int pll_valid = 0;
+
+
+
+static void modify_freq (uint64_t now, int d)
+{
+  int64_t pd1, pd2, te;
+
+  pd1 = now - phase;
+
+  te = pd1 / freq;
+  pd1 %= freq;
+
+  if (d > 0)
+    freq += d;
+  else
+    freq -= -d;
+
+
+  pd2 = pd1 + (te * freq);
+
+  phase = now - pd2;
+}
+
+void pll_dispatch (uint64_t edge)
+{
+  double f;
+  int64_t pd;
+
+  {
+    int diff, hf;
+
+    pd = edge - phase;
+    pd %= freq;
+
+    hf = (int) (freq >> 1);
+
+    diff = (int) pd;
+
+    if (diff > hf)
+      diff = diff - (int) freq;
+
+
+    f = (double) diff;
+    f /= (double) freq;
+  }
+
+  if ((f > (JUMP_THRESH)) || (f < - (JUMP_THRESH)))
+    out_of_lock++;
+  else
+    out_of_lock = 0;
+
+#if 0
+  printf ("PLL pd %.3f freq %d phase %d\r\n", (float) f, (int) freq, (int) phase);
+#endif
+
+  if (out_of_lock > JUMP_TICKS) {
+    phase += pd;
+    out_of_lock = 0;
+    printf ("PLL - jumping\r\n");
+    freq = HZ;
+  } else {
+
+    f *= FEEDBACK;
+    f *= (double) freq;
+
+    modify_freq (edge, (int) f);
+
+  }
+
+  pll_last_update = edge;
+}
+
+
+
+void pll_set_offset (EPOCH epoch, uint64_t abs)
+{
+  /* Find nearest second to abs*/
+  abs += freq >> 2;
+  abs -= phase;
+  abs /= freq;
+
+  offset = epoch.s - abs;
+
+  pll_valid = 1;
+}
+
+
+
+
+EPOCH pll_decompose (uint64_t abs)
+{
+  EPOCH ret;
+
+  abs -= phase;
+
+  ret.s = abs / freq;
+  abs -= freq * ret.s;
+
+
+  ret.s += offset;
+
+  abs *= (uint64_t) 1000000000;
+  abs = abs / freq;
+
+  ret.ns = abs;
+
+  return ret;
+}