stepcompress: Support extracting stepcompress history

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

4 files changed, 59 insertions, 4 deletions

diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py
index c2ad802a..6c579ab7 100644
--- a/klippy/chelper/__init__.py
+++ b/klippy/chelper/__init__.py
@@ -30,6 +30,12 @@ OTHER_FILES = [
 ]
 
 defs_stepcompress = """
+    struct pull_history_steps {
+        uint64_t first_clock, last_clock;
+        int64_t start_position;
+        int step_count, interval, add;
+    };
+
     struct stepcompress *stepcompress_alloc(uint32_t oid);
     void stepcompress_fill(struct stepcompress *sc, uint32_t max_error
         , uint32_t invert_sdir, int32_t queue_step_msgtag
@@ -42,6 +48,9 @@ defs_stepcompress = """
         , uint64_t clock);
     int stepcompress_queue_msg(struct stepcompress *sc
         , uint32_t *data, int len);
+    int stepcompress_extract_old(struct stepcompress *sc
+        , struct pull_history_steps *p, int max
+        , uint64_t start_clock, uint64_t end_clock);
 
     struct steppersync *steppersync_alloc(struct serialqueue *sq
         , struct stepcompress **sc_list, int sc_num, int move_num);
diff --git a/klippy/chelper/stepcompress.c b/klippy/chelper/stepcompress.c
index 50a4d7c4..fab15a2c 100644
--- a/klippy/chelper/stepcompress.c
+++ b/klippy/chelper/stepcompress.c
@@ -60,8 +60,7 @@ struct history_steps {
     struct list_node node;
     uint64_t first_clock, last_clock;
     int64_t start_position;
-    int step_count;
-    struct step_move sm;
+    int step_count, interval, add;
 };
 
 
@@ -355,9 +354,10 @@ add_move(struct stepcompress *sc, uint64_t first_clock, struct step_move *move)
     hs->first_clock = first_clock;
     hs->last_clock = last_clock;
     hs->start_position = sc->last_position;
+    hs->interval = move->interval;
+    hs->add = move->add;
     hs->step_count = sc->sdir ? move->count : -move->count;
     sc->last_position += hs->step_count;
-    memcpy(&hs->sm, move, sizeof(hs->sm));
     list_add_head(&hs->node, &sc->history_list);
 }
 
@@ -559,6 +559,13 @@ stepcompress_set_last_position(struct stepcompress *sc, int64_t last_position)
     if (ret)
         return ret;
     sc->last_position = last_position;
+
+    // Add a marker to the history list
+    struct history_steps *hs = malloc(sizeof(*hs));
+    memset(hs, 0, sizeof(*hs));
+    hs->first_clock = hs->last_clock = sc->last_step_clock;
+    hs->start_position = last_position;
+    list_add_head(&hs->node, &sc->history_list);
     return 0;
 }
 
@@ -575,7 +582,7 @@ stepcompress_find_past_position(struct stepcompress *sc, uint64_t clock)
         }
         if (clock >= hs->last_clock)
             return hs->start_position + hs->step_count;
-        int32_t interval = hs->sm.interval, add = hs->sm.add;
+        int32_t interval = hs->interval, add = hs->add;
         int32_t ticks = (int32_t)(clock - hs->first_clock) + interval, offset;
         if (!add) {
             offset = ticks / interval;
@@ -605,6 +612,30 @@ stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len)
     return 0;
 }
 
+// Return history of queue_step commands
+int __visible
+stepcompress_extract_old(struct stepcompress *sc, struct pull_history_steps *p
+                         , int max, uint64_t start_clock, uint64_t end_clock)
+{
+    int res = 0;
+    struct history_steps *hs;
+    list_for_each_entry(hs, &sc->history_list, node) {
+        if (start_clock >= hs->last_clock || res >= max)
+            break;
+        if (end_clock <= hs->first_clock)
+            continue;
+        p->first_clock = hs->first_clock;
+        p->last_clock = hs->last_clock;
+        p->start_position = hs->start_position;
+        p->step_count = hs->step_count;
+        p->interval = hs->interval;
+        p->add = hs->add;
+        p++;
+        res++;
+    }
+    return res;
+}
+
 
 /****************************************************************
  * Step compress synchronization
diff --git a/klippy/chelper/stepcompress.h b/klippy/chelper/stepcompress.h
index 4c708541..d10cccf9 100644
--- a/klippy/chelper/stepcompress.h
+++ b/klippy/chelper/stepcompress.h
@@ -5,6 +5,12 @@
 
 #define ERROR_RET -989898989
 
+struct pull_history_steps {
+    uint64_t first_clock, last_clock;
+    int64_t start_position;
+    int step_count, interval, add;
+};
+
 struct stepcompress *stepcompress_alloc(uint32_t oid);
 void stepcompress_fill(struct stepcompress *sc, uint32_t max_error
                        , uint32_t invert_sdir, int32_t queue_step_msgtag
@@ -21,6 +27,9 @@ int stepcompress_set_last_position(struct stepcompress *sc
 int64_t stepcompress_find_past_position(struct stepcompress *sc
                                         , uint64_t clock);
 int stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len);
+int stepcompress_extract_old(struct stepcompress *sc
+                             , struct pull_history_steps *p, int max
+                             , uint64_t start_clock, uint64_t end_clock);
 
 struct serialqueue;
 struct steppersync *steppersync_alloc(
diff --git a/klippy/stepper.py b/klippy/stepper.py
index 5c717ca9..6609fca2 100644
--- a/klippy/stepper.py
+++ b/klippy/stepper.py
@@ -119,6 +119,12 @@ class MCU_stepper:
     def get_past_commanded_position(self, print_time):
         mcu_pos = self.get_past_mcu_position(print_time)
         return mcu_pos * self._step_dist - self._mcu_position_offset
+    def dump_steps(self, count, start_clock, end_clock):
+        ffi_main, ffi_lib = chelper.get_ffi()
+        data = ffi_main.new('struct pull_history_steps[]', count)
+        count = ffi_lib.stepcompress_extract_old(self._stepqueue, data, count,
+                                                 start_clock, end_clock)
+        return (data, count)
     def set_stepper_kinematics(self, sk):
         old_sk = self._stepper_kinematics
         mcu_pos = 0