From c19af4fb2b811dddaf314550307e7bd312aeca94 Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Fri, 3 Mar 2017 22:02:27 -0500
Subject: [PATCH] serialhdl: Load the mcu's 64bit clock at start of connection

Store a full 64bit uptime in the mcu and query it at the start of each
connection.  This ensures the host's 64bit clock is always in synch
with the mcu's clock.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
---
 klippy/serialhdl.py | 14 +++++++++-----
 src/basecmd.c       | 20 ++++++++++++++++----
 2 files changed, 25 insertions(+), 9 deletions(-)

diff --git a/klippy/serialhdl.py b/klippy/serialhdl.py
index b943f2da..408e85da 100644
--- a/klippy/serialhdl.py
+++ b/klippy/serialhdl.py
@@ -30,8 +30,7 @@ class SerialReader:
         self.lock = threading.Lock()
         self.background_thread = None
         # Message handlers
-        self.status_timer = self.reactor.register_timer(
-            self._status_event, self.reactor.NOW)
+        self.status_timer = self.reactor.register_timer(self._status_event)
         self.status_cmd = None
         handlers = {
             '#unknown': self.handle_unknown,
@@ -90,14 +89,21 @@ class SerialReader:
             len(msgparser.messages_by_id), msgparser.version))
         logging.info("MCU config: %s" % (" ".join(
             ["%s=%s" % (k, v) for k, v in msgparser.config.items()])))
-        # Setup for runtime
+        # Setup baud adjust
         mcu_baud = float(msgparser.config.get('SERIAL_BAUD', 0.))
         if mcu_baud:
             baud_adjust = self.BITS_PER_BYTE / mcu_baud
             self.ffi_lib.serialqueue_set_baud_adjust(
                 self.serialqueue, baud_adjust)
+        # Load initial last_ack_clock/last_ack_time
+        uptime_msg = msgparser.create_command('get_uptime')
+        params = self.send_with_response(uptime_msg, 'uptime')
+        self.last_ack_clock = (params['high'] << 32) | params['clock']
+        self.last_ack_time = params['#receive_time']
+        # Enable periodic get_status timer
         get_status = msgparser.lookup_command('get_status')
         self.status_cmd = get_status.encode()
+        self.reactor.update_timer(self.status_timer, self.reactor.NOW)
     def connect_file(self, debugoutput, dictionary, pace=False):
         self.ser = debugoutput
         self.msgparser.process_identify(dictionary, decompress=False)
@@ -129,8 +135,6 @@ class SerialReader:
             self.est_clock, self.last_ack_time, self.last_ack_clock)
         return sqstats + tstats
     def _status_event(self, eventtime):
-        if self.status_cmd is None:
-            return eventtime + 0.1
         self.send(self.status_cmd)
         return eventtime + 1.0
     # Serial response callbacks
diff --git a/src/basecmd.c b/src/basecmd.c
index fd80abfd..4b4eca45 100644
--- a/src/basecmd.c
+++ b/src/basecmd.c
@@ -191,6 +191,17 @@ command_get_status(uint32_t *args)
 }
 DECL_COMMAND_FLAGS(command_get_status, HF_IN_SHUTDOWN, "get_status");
 
+static uint32_t stats_send_time, stats_send_time_high;
+
+void
+command_get_uptime(uint32_t *args)
+{
+    uint32_t cur = sched_read_time();
+    uint32_t high = stats_send_time_high + (cur < stats_send_time);
+    sendf("uptime high=%u clock=%u", high, cur);
+}
+DECL_COMMAND_FLAGS(command_get_uptime, HF_IN_SHUTDOWN, "get_uptime");
+
 #define SUMSQ_BASE 256
 DECL_CONSTANT(STATS_SUMSQ_BASE, SUMSQ_BASE);
 
@@ -215,11 +226,12 @@ stats_task(void)
         nextsumsq = 0xffffffff;
     sumsq = nextsumsq;
 
-    static uint32_t prev;
-    if (sched_is_before(cur, prev + sched_from_us(5000000)))
+    if (sched_is_before(cur, stats_send_time + sched_from_us(5000000)))
         return;
-    sendf("stats count=%u sum=%u sumsq=%u", count, cur - prev, sumsq);
-    prev = cur;
+    sendf("stats count=%u sum=%u sumsq=%u", count, cur - stats_send_time, sumsq);
+    if (cur < stats_send_time)
+        stats_send_time_high++;
+    stats_send_time = cur;
     count = sumsq = 0;
 }
 DECL_TASK(stats_task);