works

1 files changed, 81 insertions, 0 deletions

diff --git a/app/sysclk.c b/app/sysclk.c
new file mode 100644
index 0000000..1847c28
--- /dev/null
+++ b/app/sysclk.c
@@ -0,0 +1,81 @@
+#include "project.h"
+
+
+
+static Event_ring sysclk_ring;
+
+void sysclk_event (void)
+{
+  uint32_t refclk_now = HW_CLOCK_REG;
+  uint32_t sysclk_now = SCS_DWT_CYCCNT;
+
+
+  sysclk_ring.events[sysclk_ring.tx_ptr].when = refclk_now;
+  sysclk_ring.events[sysclk_ring.tx_ptr].value = sysclk_now;
+  sysclk_ring.tx_ptr = (sysclk_ring.tx_ptr + 1) & ERING_MASK;
+
+}
+
+static uint32_t high_tick;
+
+#define QUARTER (1UL << 29)
+#define HALF (1UL << 30)
+#define THREE_QUARTERS (HALF+QUARTER)
+#define ONE (~(uint32_t)0)
+
+uint64_t sysclk_extend (uint32_t now)
+{
+  static int m;
+  uint64_t ret;
+
+
+  if (!m) {
+    ret = high_tick;
+    ret <<= 32;
+    ret |= now;
+
+
+    if ((now > THREE_QUARTERS) && (now <= ONE)) {
+      high_tick++;
+      m = 1;
+    }
+
+  } else {
+    if (now < HALF) {
+      ret = high_tick;
+      ret <<= 32;
+      ret |= now;
+    } else {
+      ret = high_tick - 1;
+      ret <<= 32;
+      ret |= now;
+    }
+
+    if ((now > QUARTER) && (now < HALF))
+      m = 0;
+  }
+
+  return ret;
+}
+void sysclk_dispatch (void)
+{
+  //char buf[80];
+  uint64_t refclk_abs;
+  uint32_t refclk_now, sysclk_now;
+  EPOCH e;
+  //UTC u;
+
+  if (sysclk_ring.rx_ptr == sysclk_ring.tx_ptr) return;
+
+  sysclk_now = sysclk_ring.events[sysclk_ring.rx_ptr].value;
+  refclk_now = sysclk_ring.events[sysclk_ring.rx_ptr].when;
+  sysclk_ring.rx_ptr = (sysclk_ring.rx_ptr + 1) & ERING_MASK;
+
+  refclk_abs = abs_extend (refclk_now);
+  e.s = sysclk_extend (sysclk_now);
+  e.ns = 0;
+
+  report_time ("SYS", e, refclk_abs, "");
+}
+
+