msgblock: Add clock estimation helper functions

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

klippy/chelper/__init__.py

@@ -162,7 +162,7 @@ defs_serialqueue = """
     void serialqueue_set_receive_window(struct serialqueue *sq
         , int receive_window);
     void serialqueue_set_clock_est(struct serialqueue *sq, double est_freq
-        , double last_clock_time, uint64_t last_clock);
+        , double conv_time, uint64_t conv_clock, uint64_t last_clock);
     void serialqueue_get_stats(struct serialqueue *sq, char *buf, int len);
     int serialqueue_extract_old(struct serialqueue *sq, int sentq
         , struct pull_queue_message *q, int max);

klippy/chelper/msgblock.c

@@ -181,3 +181,29 @@ message_queue_free(struct list_head *root)
         message_free(qm);
     }
 }
+
+
+/****************************************************************
+ * Clock estimation
+ ****************************************************************/
+
+// Extend a 32bit clock value to its full 64bit value
+uint64_t
+clock_from_clock32(struct clock_estimate *ce, uint32_t clock32)
+{
+    return ce->last_clock + (int32_t)(clock32 - ce->last_clock);
+}
+
+// Convert a clock to its estimated time
+double
+clock_to_time(struct clock_estimate *ce, uint64_t clock)
+{
+    return ce->conv_time + (int64_t)(clock - ce->conv_clock) / ce->est_freq;
+}
+
+// Convert a time to the nearest clock value
+uint64_t
+clock_from_time(struct clock_estimate *ce, double time)
+{
+    return (int64_t)((time - ce->conv_time)*ce->est_freq + .5) + ce->conv_clock;
+}

klippy/chelper/msgblock.h

@@ -34,6 +34,11 @@ struct queue_message {
     struct list_node node;
 };
 
+struct clock_estimate {
+    uint64_t last_clock, conv_clock;
+    double conv_time, est_freq;
+};
+
 uint16_t msgblock_crc16_ccitt(uint8_t *buf, uint8_t len);
 int msgblock_check(uint8_t *need_sync, uint8_t *buf, int buf_len);
 int msgblock_decode(uint32_t *data, int data_len, uint8_t *msg, int msg_len);
@@ -42,5 +47,8 @@ struct queue_message *message_fill(uint8_t *data, int len);
 struct queue_message *message_alloc_and_encode(uint32_t *data, int len);
 void message_free(struct queue_message *qm);
 void message_queue_free(struct list_head *root);
+uint64_t clock_from_clock32(struct clock_estimate *ce, uint32_t clock32);
+double clock_to_time(struct clock_estimate *ce, uint64_t clock);
+uint64_t clock_from_time(struct clock_estimate *ce, double time);
 
 #endif // msgblock.h

klippy/chelper/serialqueue.c

@@ -50,8 +50,7 @@ struct serialqueue {
     // Baud / clock tracking
     int receive_window;
     double baud_adjust, idle_time;
-    double est_freq, last_clock_time;
+    struct clock_estimate ce;
-    uint64_t last_clock;
     double last_receive_sent_time;
     // Retransmit support
     uint64_t send_seq, receive_seq;
@@ -483,9 +482,7 @@ check_send_command(struct serialqueue *sq, double eventtime)
     // Check for stalled messages now ready
     double idletime = eventtime > sq->idle_time ? eventtime : sq->idle_time;
     idletime += MESSAGE_MIN * sq->baud_adjust;
-    double timedelta = idletime - sq->last_clock_time;
+    uint64_t ack_clock = clock_from_time(&sq->ce, idletime);
-    uint64_t ack_clock = ((uint64_t)(timedelta * sq->est_freq)
-                          + sq->last_clock);
     uint64_t min_stalled_clock = MAX_CLOCK, min_ready_clock = MAX_CLOCK;
     struct command_queue *cq;
     list_for_each_entry(cq, &sq->pending_queues, node) {
@@ -508,11 +505,9 @@ check_send_command(struct serialqueue *sq, double eventtime)
             struct queue_message *qm = list_first_entry(
                 &cq->ready_queue, struct queue_message, node);
             uint64_t req_clock = qm->req_clock;
+            double bgoffset = MIN_REQTIME_DELTA + MIN_BACKGROUND_DELTA;
             if (req_clock == BACKGROUND_PRIORITY_CLOCK)
-                req_clock = (uint64_t)(
+                req_clock = clock_from_time(&sq->ce, sq->idle_time + bgoffset);
-                    (sq->idle_time - sq->last_clock_time
-                     + MIN_REQTIME_DELTA + MIN_BACKGROUND_DELTA)
-                    * sq->est_freq) + sq->last_clock;
             if (req_clock < min_ready_clock)
                 min_ready_clock = req_clock;
         }
@@ -521,20 +516,20 @@ check_send_command(struct serialqueue *sq, double eventtime)
     // Check for messages to send
     if (sq->ready_bytes >= MESSAGE_PAYLOAD_MAX)
         return PR_NOW;
-    if (! sq->est_freq) {
+    if (! sq->ce.est_freq) {
         if (sq->ready_bytes)
             return PR_NOW;
         sq->need_kick_clock = MAX_CLOCK;
         return PR_NEVER;
     }
-    uint64_t reqclock_delta = MIN_REQTIME_DELTA * sq->est_freq;
+    uint64_t reqclock_delta = MIN_REQTIME_DELTA * sq->ce.est_freq;
     if (min_ready_clock <= ack_clock + reqclock_delta)
         return PR_NOW;
     uint64_t wantclock = min_ready_clock - reqclock_delta;
     if (min_stalled_clock < wantclock)
         wantclock = min_stalled_clock;
     sq->need_kick_clock = wantclock;
-    return idletime + (wantclock - ack_clock) / sq->est_freq;
+    return idletime + (wantclock - ack_clock) / sq->ce.est_freq;
 }
 
 // Callback timer to send data to the serial port
@@ -819,12 +814,23 @@ serialqueue_set_receive_window(struct serialqueue *sq, int receive_window)
 // serial port
 void __visible
 serialqueue_set_clock_est(struct serialqueue *sq, double est_freq
-                          , double last_clock_time, uint64_t last_clock)
+                          , double conv_time, uint64_t conv_clock
+                          , uint64_t last_clock)
 {
     pthread_mutex_lock(&sq->lock);
-    sq->est_freq = est_freq;
+    sq->ce.est_freq = est_freq;
-    sq->last_clock_time = last_clock_time;
+    sq->ce.conv_time = conv_time;
-    sq->last_clock = last_clock;
+    sq->ce.conv_clock = conv_clock;
+    sq->ce.last_clock = last_clock;
+    pthread_mutex_unlock(&sq->lock);
+}
+
+// Return the latest clock estimate
+void
+serialqueue_get_clock_est(struct serialqueue *sq, struct clock_estimate *ce)
+{
+    pthread_mutex_lock(&sq->lock);
+    memcpy(ce, &sq->ce, sizeof(sq->ce));
     pthread_mutex_unlock(&sq->lock);
 }
 

klippy/chelper/serialqueue.h

@@ -30,7 +30,10 @@ void serialqueue_pull(struct serialqueue *sq, struct pull_queue_message *pqm);
 void serialqueue_set_baud_adjust(struct serialqueue *sq, double baud_adjust);
 void serialqueue_set_receive_window(struct serialqueue *sq, int receive_window);
 void serialqueue_set_clock_est(struct serialqueue *sq, double est_freq
-                               , double last_clock_time, uint64_t last_clock);
+                               , double conv_time, uint64_t conv_clock
+                               , uint64_t last_clock);
+void serialqueue_get_clock_est(struct serialqueue *sq
+                               , struct clock_estimate *ce);
 void serialqueue_get_stats(struct serialqueue *sq, char *buf, int len);
 int serialqueue_extract_old(struct serialqueue *sq, int sentq
                             , struct pull_queue_message *q, int max);

klippy/clocksync.py

@@ -54,7 +54,7 @@ class ClockSync:
         freq = 1000000000000.
         if pace:
             freq = self.mcu_freq
-        serial.set_clock_est(freq, self.reactor.monotonic(), 0)
+        serial.set_clock_est(freq, self.reactor.monotonic(), 0, 0)
     # MCU clock querying (_handle_clock is invoked from background thread)
     def _get_clock_event(self, eventtime):
         self.serial.raw_send(self.get_clock_cmd, 0, 0, self.cmd_queue)
@@ -116,7 +116,7 @@ class ClockSync:
         new_freq = self.clock_covariance / self.time_variance
         pred_stddev = math.sqrt(self.prediction_variance)
         self.serial.set_clock_est(new_freq, self.time_avg + TRANSMIT_EXTRA,
-                                  int(self.clock_avg - 3. * pred_stddev))
+                                  int(self.clock_avg - 3. * pred_stddev), clock)
         self.clock_est = (self.time_avg + self.min_half_rtt,
                           self.clock_avg, new_freq)
         #logging.debug("regr %.3f: freq=%.3f d=%d(%.3f)",

klippy/serialhdl.py

@@ -200,9 +200,9 @@ class SerialReader:
         self.serialqueue = self.ffi_main.gc(
             self.ffi_lib.serialqueue_alloc(self.serial_dev.fileno(), 'f', 0),
             self.ffi_lib.serialqueue_free)
-    def set_clock_est(self, freq, last_time, last_clock):
+    def set_clock_est(self, freq, conv_time, conv_clock, last_clock):
         self.ffi_lib.serialqueue_set_clock_est(
-            self.serialqueue, freq, last_time, last_clock)
+            self.serialqueue, freq, conv_time, conv_clock, last_clock)
     def disconnect(self):
         if self.serialqueue is not None:
             self.ffi_lib.serialqueue_exit(self.serialqueue)