msgblock: Add clock estimation helper functions

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

5 files changed, 61 insertions, 18 deletions

diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py
index 55f394ef..4a8728fb 100644
--- a/klippy/chelper/__init__.py
+++ b/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);
diff --git a/klippy/chelper/msgblock.c b/klippy/chelper/msgblock.c
index 3c848b13..e6cb298b 100644
--- a/klippy/chelper/msgblock.c
+++ b/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;
+}
diff --git a/klippy/chelper/msgblock.h b/klippy/chelper/msgblock.h
index 2d2967eb..43ee9532 100644
--- a/klippy/chelper/msgblock.h
+++ b/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
diff --git a/klippy/chelper/serialqueue.c b/klippy/chelper/serialqueue.c
index 517a435c..78115e09 100644
--- a/klippy/chelper/serialqueue.c
+++ b/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;
-    uint64_t last_clock;
+    struct clock_estimate ce;
     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 = ((uint64_t)(timedelta * sq->est_freq)
-                          + sq->last_clock);
+    uint64_t ack_clock = clock_from_time(&sq->ce, idletime);
     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)(
-                    (sq->idle_time - sq->last_clock_time
-                     + MIN_REQTIME_DELTA + MIN_BACKGROUND_DELTA)
-                    * sq->est_freq) + sq->last_clock;
+                req_clock = clock_from_time(&sq->ce, sq->idle_time + bgoffset);
             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->last_clock_time = last_clock_time;
-    sq->last_clock = last_clock;
+    sq->ce.est_freq = est_freq;
+    sq->ce.conv_time = conv_time;
+    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);
 }
 
diff --git a/klippy/chelper/serialqueue.h b/klippy/chelper/serialqueue.h
index 17e14316..62e70403 100644
--- a/klippy/chelper/serialqueue.h
+++ b/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);