msgblock: Add clock estimation helper functions

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

1 files changed, 26 insertions, 0 deletions

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;
+}