stepcompress: Fix proactive queue flushing on move with 64K+ steps

Commit e05c6354 changed the internal step compress queue from 64bit
integers to 32bit integers.  However, that commit broke the proactive
flushing of moves that could produce more than 64K steps.  This could
lead to large memory allocations and cpu slow downs on printers that
had a very large Z axis - possibly leading to a "Timer too close" mcu
shutdown.  Correct the code so that it properly generates a 64bit
flush clock.

Also, be sure to only expand / memmove the queue when there is no room
for a new element at the end.

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

1 files changed, 21 insertions, 16 deletions

diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c
index eef07fdb..6c5f766f 100644
--- a/klippy/stepcompress.c
+++ b/klippy/stepcompress.c
@@ -423,27 +423,32 @@ queue_append_slow(struct stepcompress *sc, double rel_sc)
 
     if (sc->queue_next - sc->queue_pos > 65535 + 2000) {
         // No point in keeping more than 64K steps in memory
-        int ret = stepcompress_flush(sc, *(sc->queue_next - 65535));
+        uint32_t flush = *(sc->queue_next-65535) - (uint32_t)sc->last_step_clock;
+        int ret = stepcompress_flush(sc, sc->last_step_clock + flush);
         if (ret)
             return ret;
     }
 
-    int in_use = sc->queue_next - sc->queue_pos;
-    if (sc->queue_pos > sc->queue) {
-        // Shuffle the internal queue to avoid having to allocate more ram
-        memmove(sc->queue, sc->queue_pos, in_use * sizeof(*sc->queue));
-    } else {
-        // Expand the internal queue of step times
-        int alloc = sc->queue_end - sc->queue;
-        if (!alloc)
-            alloc = QUEUE_START_SIZE;
-        while (in_use >= alloc)
-            alloc *= 2;
-        sc->queue = realloc(sc->queue, alloc * sizeof(*sc->queue));
-        sc->queue_end = sc->queue + alloc;
+    if (sc->queue_next >= sc->queue_end) {
+        // Make room in the queue
+        int in_use = sc->queue_next - sc->queue_pos;
+        if (sc->queue_pos > sc->queue) {
+            // Shuffle the internal queue to avoid having to allocate more ram
+            memmove(sc->queue, sc->queue_pos, in_use * sizeof(*sc->queue));
+        } else {
+            // Expand the internal queue of step times
+            int alloc = sc->queue_end - sc->queue;
+            if (!alloc)
+                alloc = QUEUE_START_SIZE;
+            while (in_use >= alloc)
+                alloc *= 2;
+            sc->queue = realloc(sc->queue, alloc * sizeof(*sc->queue));
+            sc->queue_end = sc->queue + alloc;
+        }
+        sc->queue_pos = sc->queue;
+        sc->queue_next = sc->queue + in_use;
     }
-    sc->queue_pos = sc->queue;
-    sc->queue_next = sc->queue + in_use;
+
     *sc->queue_next++ = abs_step_clock;
     return 0;
 }