stepcompress: Propagate errors back to python code

Propagate error codes back to the python code and raise an exception
on an error.

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

3 files changed, 146 insertions, 80 deletions

diff --git a/klippy/chelper.py b/klippy/chelper.py
index f979e2c7..b7baf061 100644
--- a/klippy/chelper.py
+++ b/klippy/chelper.py
@@ -16,7 +16,7 @@ defs_stepcompress = """
         , uint32_t queue_step_msgid, uint32_t set_next_step_dir_msgid
         , uint32_t invert_sdir, uint32_t oid);
     void stepcompress_free(struct stepcompress *sc);
-    void stepcompress_push(struct stepcompress *sc, double step_clock
+    int stepcompress_push(struct stepcompress *sc, double step_clock
         , int32_t sdir);
     int32_t stepcompress_push_factor(struct stepcompress *sc
         , double steps, double step_offset
@@ -32,16 +32,14 @@ defs_stepcompress = """
         , double clock_offset, double dist, double start_pos
         , double accel_multiplier, double step_dist, double height
         , double closestxy_d, double closest_height2, double movez_r);
-    void stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock);
-    void stepcompress_set_homing(struct stepcompress *sc, uint64_t homing_clock);
-    void stepcompress_queue_msg(struct stepcompress *sc
-        , uint32_t *data, int len);
-    uint32_t stepcompress_get_errors(struct stepcompress *sc);
+    int stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock);
+    int stepcompress_set_homing(struct stepcompress *sc, uint64_t homing_clock);
+    int stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len);
 
     struct steppersync *steppersync_alloc(struct serialqueue *sq
         , struct stepcompress **sc_list, int sc_num, int move_num);
     void steppersync_free(struct steppersync *ss);
-    void steppersync_flush(struct steppersync *ss, uint64_t move_clock);
+    int steppersync_flush(struct steppersync *ss, uint64_t move_clock);
 """
 
 defs_serialqueue = """
diff --git a/klippy/mcu.py b/klippy/mcu.py
index eddc56a5..4fadc13d 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -19,6 +19,8 @@ def parse_pin_extras(pin, can_pullup=False):
         pin = pin[1:].strip()
     return pin, pullup, invert
 
+STEPCOMPRESS_ERROR_RET = -989898989
+
 class MCU_stepper:
     def __init__(self, mcu, step_pin, dir_pin, min_stop_interval, max_error):
         self._mcu = mcu
@@ -59,18 +61,31 @@ class MCU_stepper:
     def get_mcu_position(self):
         return self.commanded_position + self._mcu_position_offset
     def note_homing_start(self, homing_clock):
-        self.ffi_lib.stepcompress_set_homing(self._stepqueue, homing_clock)
+        ret = self.ffi_lib.stepcompress_set_homing(self._stepqueue, homing_clock)
+        if ret:
+            raise error("Internal error in stepcompress")
     def note_homing_finalized(self):
-        self.ffi_lib.stepcompress_set_homing(self._stepqueue, 0)
-        self.ffi_lib.stepcompress_reset(self._stepqueue, 0)
+        ret = self.ffi_lib.stepcompress_set_homing(self._stepqueue, 0)
+        if ret:
+            raise error("Internal error in stepcompress")
+        ret = self.ffi_lib.stepcompress_reset(self._stepqueue, 0)
+        if ret:
+            raise error("Internal error in stepcompress")
     def reset_step_clock(self, mcu_time):
         clock = int(mcu_time * self._mcu_freq)
-        self.ffi_lib.stepcompress_reset(self._stepqueue, clock)
+        ret = self.ffi_lib.stepcompress_reset(self._stepqueue, clock)
+        if ret:
+            raise error("Internal error in stepcompress")
         data = (self._reset_cmd.msgid, self._oid, clock & 0xffffffff)
-        self.ffi_lib.stepcompress_queue_msg(self._stepqueue, data, len(data))
+        ret = self.ffi_lib.stepcompress_queue_msg(
+            self._stepqueue, data, len(data))
+        if ret:
+            raise error("Internal error in stepcompress")
     def step(self, mcu_time, sdir):
         clock = mcu_time * self._mcu_freq
-        self.ffi_lib.stepcompress_push(self._stepqueue, clock, sdir)
+        ret = self.ffi_lib.stepcompress_push(self._stepqueue, clock, sdir)
+        if ret:
+            raise error("Internal error in stepcompress")
         if sdir:
             self.commanded_position += 1
         else:
@@ -81,12 +96,16 @@ class MCU_stepper:
         count = self.ffi_lib.stepcompress_push_sqrt(
             self._stepqueue, steps, step_offset, clock
             , sqrt_offset * mcu_freq2, factor * mcu_freq2)
+        if count == STEPCOMPRESS_ERROR_RET:
+            raise error("Internal error in stepcompress")
         self.commanded_position += count
         return count
     def step_factor(self, mcu_time, steps, step_offset, factor):
         clock = mcu_time * self._mcu_freq
         count = self.ffi_lib.stepcompress_push_factor(
             self._stepqueue, steps, step_offset, clock, factor * self._mcu_freq)
+        if count == STEPCOMPRESS_ERROR_RET:
+            raise error("Internal error in stepcompress")
         self.commanded_position += count
         return count
     def step_delta_const(self, mcu_time, dist, start_pos
@@ -97,6 +116,8 @@ class MCU_stepper:
             self._stepqueue, clock, dist, start_pos
             , inv_velocity * self._mcu_freq, step_dist
             , height, closestxy_d, closest_height2, movez_r)
+        if count == STEPCOMPRESS_ERROR_RET:
+            raise error("Internal error in stepcompress")
         self.commanded_position += count
         return count
     def step_delta_accel(self, mcu_time, dist, start_pos
@@ -108,10 +129,10 @@ class MCU_stepper:
             self._stepqueue, clock, dist, start_pos
             , accel_multiplier * mcu_freq2, step_dist
             , height, closestxy_d, closest_height2, movez_r)
+        if count == STEPCOMPRESS_ERROR_RET:
+            raise error("Internal error in stepcompress")
         self.commanded_position += count
         return count
-    def get_errors(self):
-        return self.ffi_lib.stepcompress_get_errors(self._stepqueue)
 
 class MCU_endstop:
     error = error
@@ -390,15 +411,9 @@ class MCU:
             self.ffi_lib.steppersync_free(self._steppersync)
             self._steppersync = None
     def stats(self, eventtime):
-        stats = self.serial.stats(eventtime)
-        stats += " mcu_task_avg=%.06f mcu_task_stddev=%.06f" % (
+        return "%s mcu_task_avg=%.06f mcu_task_stddev=%.06f" % (
+            self.serial.stats(eventtime),
             self._mcu_tick_avg, self._mcu_tick_stddev)
-        err = 0
-        for s in self._steppers:
-            err += s.get_errors()
-        if err:
-            stats += " step_errors=%d" % (err,)
-        return stats
     def force_shutdown(self):
         self.send(self._emergency_stop_cmd.encode())
     def clear_shutdown(self):
@@ -527,7 +542,9 @@ class MCU:
             return
         mcu_time = print_time + self._print_start_time
         clock = int(mcu_time * self._mcu_freq)
-        self.ffi_lib.steppersync_flush(self._steppersync, clock)
+        ret = self.ffi_lib.steppersync_flush(self._steppersync, clock)
+        if ret:
+            raise error("Internal error in stepcompress")
     def pause(self, waketime):
         return self._printer.reactor.pause(waketime)
     def __del__(self):
diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c
index e7d09572..5b401cbe 100644
--- a/klippy/stepcompress.c
+++ b/klippy/stepcompress.c
@@ -31,8 +31,6 @@ struct stepcompress {
     uint64_t *queue, *queue_end, *queue_pos, *queue_next;
     // Internal tracking
     uint32_t max_error;
-    // Error checking
-    uint32_t errors;
     // Message generation
     uint64_t last_step_clock, homing_clock;
     struct list_head msg_queue;
@@ -224,27 +222,28 @@ compress_bisect_add(struct stepcompress *sc)
  * Step compress checking
  ****************************************************************/
 
+#define ERROR_RET -989898989
+
 // Verify that a given 'step_move' matches the actual step times
-static void
+static int
 check_line(struct stepcompress *sc, struct step_move move)
 {
     if (!CHECK_LINES)
-        return;
+        return 0;
     if (move.count == 1) {
         if (move.interval != (uint32_t)(*sc->queue_pos - sc->last_step_clock)
             || *sc->queue_pos < sc->last_step_clock) {
             errorf("Count 1 point out of range: %d %d %d"
                    , move.interval, move.count, move.add);
-            sc->errors++;
+            return ERROR_RET;
         }
-        return;
+        return 0;
     }
-    int err = 0;
     if (!move.count || (!move.interval && !move.add)
         || move.interval >= 0x80000000) {
         errorf("Point out of range: %d %d %d"
                , move.interval, move.count, move.add);
-        err++;
+        return ERROR_RET;
     }
     uint32_t interval = move.interval, p = 0;
     uint16_t i;
@@ -254,16 +253,16 @@ check_line(struct stepcompress *sc, struct step_move move)
         if (p < point.minp || p > point.maxp) {
             errorf("Point %d of %d: %d not in %d:%d"
                    , i+1, move.count, p, point.minp, point.maxp);
-            err++;
+            return ERROR_RET;
         }
         if (interval >= 0x80000000) {
             errorf("Point %d of %d: interval overflow %d"
                    , i+1, move.count, interval);
-            err++;
+            return ERROR_RET;
         }
         interval += move.add;
     }
-    sc->errors += err;
+    return 0;
 }
 
 
@@ -317,14 +316,16 @@ stepcompress_free(struct stepcompress *sc)
 }
 
 // Convert previously scheduled steps into commands for the mcu
-static void
+static int
 stepcompress_flush(struct stepcompress *sc, uint64_t move_clock)
 {
     if (sc->queue_pos >= sc->queue_next)
-        return;
+        return 0;
     while (move_clock > sc->last_step_clock) {
         struct step_move move = compress_bisect_add(sc);
-        check_line(sc, move);
+        int ret = check_line(sc, move);
+        if (ret)
+            return ret;
 
         uint32_t msg[5] = {
             sc->queue_step_msgid, sc->oid, move.interval, move.count, move.add
@@ -350,54 +351,70 @@ stepcompress_flush(struct stepcompress *sc, uint64_t move_clock)
         }
         sc->queue_pos += move.count;
     }
+    return 0;
 }
 
 // Send the set_next_step_dir command
-static void
+static int
 set_next_step_dir(struct stepcompress *sc, int sdir)
 {
     if (sc->sdir == sdir)
-        return;
+        return 0;
     sc->sdir = sdir;
-    stepcompress_flush(sc, UINT64_MAX);
+    int ret = stepcompress_flush(sc, UINT64_MAX);
+    if (ret)
+        return ret;
     uint32_t msg[3] = {
         sc->set_next_step_dir_msgid, sc->oid, sdir ^ sc->invert_sdir
     };
     struct queue_message *qm = message_alloc_and_encode(msg, 3);
     qm->req_clock = sc->homing_clock ?: sc->last_step_clock;
     list_add_tail(&qm->node, &sc->msg_queue);
+    return 0;
 }
 
 // Check if the internal queue needs to be expanded, and expand if so
-static void
+static int
 _check_expand(struct stepcompress *sc, uint64_t *qn)
 {
     sc->queue_next = qn;
-    if (qn - sc->queue_pos > 65535 + 2000)
+    if (qn - sc->queue_pos > 65535 + 2000) {
         // No point in keeping more than 64K steps in memory
-        stepcompress_flush(sc, *(qn - 65535));
+        int ret = stepcompress_flush(sc, *(qn - 65535));
+        if (ret)
+            return ret;
+    }
     expand_queue(sc, 1);
+    return 0;
 }
-static inline void
+static inline int
 check_expand(struct stepcompress *sc, uint64_t **pqn, uint64_t **pqend)
 {
     if (likely(*pqn < *pqend))
-        return;
-    _check_expand(sc, *pqn);
+        return 0;
+    int ret = _check_expand(sc, *pqn);
+    if (ret)
+        return ret;
     *pqn = sc->queue_next;
     *pqend = sc->queue_end;
+    return 0;
 }
 
 // Schedule a step event at the specified step_clock time
-void
+int
 stepcompress_push(struct stepcompress *sc, double step_clock, int32_t sdir)
 {
-    set_next_step_dir(sc, !!sdir);
+    int ret = set_next_step_dir(sc, !!sdir);
+    if (ret)
+        return ret;
     step_clock += 0.5;
     uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
-    check_expand(sc, &qn, &qend);
+    ret = check_expand(sc, &qn, &qend);
+    if (ret)
+        return ret;
     *qn++ = step_clock;
     sc->queue_next = qn;
+    return 0;
 }
 
 // Schedule 'steps' number of steps with a constant time between steps
@@ -416,12 +433,16 @@ stepcompress_push_factor(struct stepcompress *sc
     }
     int count = steps + .5 - step_offset;
     if (count <= 0 || count > 10000000) {
-        if (count && steps)
+        if (count && steps) {
             errorf("push_factor invalid count %d %f %f %f %f"
                    , sc->oid, steps, step_offset, clock_offset, factor);
+            return ERROR_RET;
+        }
         return 0;
     }
-    set_next_step_dir(sc, sdir);
+    int ret = set_next_step_dir(sc, sdir);
+    if (ret)
+        return ret;
     int res = sdir ? count : -count;
 
     // Calculate each step time
@@ -429,7 +450,9 @@ stepcompress_push_factor(struct stepcompress *sc
     double pos = step_offset + .5;
     uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
     while (count--) {
-        check_expand(sc, &qn, &qend);
+        int ret = check_expand(sc, &qn, &qend);
+        if (ret)
+            return ret;
         *qn++ = clock_offset + pos*factor;
         pos += 1.0;
     }
@@ -452,13 +475,17 @@ stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset
     }
     int count = steps + .5 - step_offset;
     if (count <= 0 || count > 10000000) {
-        if (count && steps)
+        if (count && steps) {
             errorf("push_sqrt invalid count %d %f %f %f %f %f"
                    , sc->oid, steps, step_offset, clock_offset, sqrt_offset
                    , factor);
+            return ERROR_RET;
+        }
         return 0;
     }
-    set_next_step_dir(sc, sdir);
+    int ret = set_next_step_dir(sc, sdir);
+    if (ret)
+        return ret;
     int res = sdir ? count : -count;
 
     // Calculate each step time
@@ -466,7 +493,9 @@ stepcompress_push_sqrt(struct stepcompress *sc, double steps, double step_offset
     double pos = step_offset + .5 + sqrt_offset/factor;
     uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
     while (count--) {
-        check_expand(sc, &qn, &qend);
+        int ret = check_expand(sc, &qn, &qend);
+        if (ret)
+            return ret;
         double v = safe_sqrt(pos*factor);
         *qn++ = clock_offset + (factor >= 0. ? v : -v);
         pos += 1.0;
@@ -488,13 +517,17 @@ stepcompress_push_delta_const(
     double end_height = safe_sqrt(closest_height2 - reldist*reldist);
     int count = (end_height - height + movez_r*dist) / step_dist + .5;
     if (count <= 0 || count > 10000000) {
-        if (count)
+        if (count) {
             errorf("push_delta_const invalid count %d %d %f %f %f %f %f %f %f %f"
                    , sc->oid, count, clock_offset, dist, step_dist, start_pos
                    , closest_height2, height, movez_r, inv_velocity);
+            return ERROR_RET;
+        }
         return 0;
     }
-    set_next_step_dir(sc, step_dist > 0.);
+    int ret = set_next_step_dir(sc, step_dist > 0.);
+    if (ret)
+        return ret;
     int res = step_dist > 0. ? count : -count;
 
     // Calculate each step time
@@ -505,7 +538,9 @@ stepcompress_push_delta_const(
     if (!movez_r) {
         // Optmized case for common XY only moves (no Z movement)
         while (count--) {
-            check_expand(sc, &qn, &qend);
+            int ret = check_expand(sc, &qn, &qend);
+            if (ret)
+                return ret;
             double v = safe_sqrt(closest_height2 - height*height);
             double pos = start_pos + (step_dist > 0. ? -v : v);
             *qn++ = clock_offset + pos * inv_velocity;
@@ -515,7 +550,9 @@ stepcompress_push_delta_const(
         // Optmized case for Z only moves
         double v = (step_dist > 0. ? -end_height : end_height);
         while (count--) {
-            check_expand(sc, &qn, &qend);
+            int ret = check_expand(sc, &qn, &qend);
+            if (ret)
+                return ret;
             double pos = start_pos + movez_r*height + v;
             *qn++ = clock_offset + pos * inv_velocity;
             height += step_dist;
@@ -523,7 +560,9 @@ stepcompress_push_delta_const(
     } else {
         // General case (handles XY+Z moves)
         while (count--) {
-            check_expand(sc, &qn, &qend);
+            int ret = check_expand(sc, &qn, &qend);
+            if (ret)
+                return ret;
             double relheight = movexy_r*height - movez_r*closestxy_d;
             double v = safe_sqrt(closest_height2 - relheight*relheight);
             double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v);
@@ -548,13 +587,17 @@ stepcompress_push_delta_accel(
     double end_height = safe_sqrt(closest_height2 - reldist*reldist);
     int count = (end_height - height + movez_r*dist) / step_dist + .5;
     if (count <= 0 || count > 10000000) {
-        if (count)
+        if (count) {
             errorf("push_delta_accel invalid count %d %d %f %f %f %f %f %f %f %f"
                    , sc->oid, count, clock_offset, dist, step_dist, start_pos
                    , closest_height2, height, movez_r, accel_multiplier);
+            return ERROR_RET;
+        }
         return 0;
     }
-    set_next_step_dir(sc, step_dist > 0.);
+    int ret = set_next_step_dir(sc, step_dist > 0.);
+    if (ret)
+        return ret;
     int res = step_dist > 0. ? count : -count;
 
     // Calculate each step time
@@ -563,7 +606,9 @@ stepcompress_push_delta_accel(
     height += .5 * step_dist;
     uint64_t *qn = sc->queue_next, *qend = sc->queue_end;
     while (count--) {
-        check_expand(sc, &qn, &qend);
+        int ret = check_expand(sc, &qn, &qend);
+        if (ret)
+            return ret;
         double relheight = movexy_r*height - movez_r*closestxy_d;
         double v = safe_sqrt(closest_height2 - relheight*relheight);
         double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v);
@@ -576,38 +621,40 @@ stepcompress_push_delta_accel(
 }
 
 // Reset the internal state of the stepcompress object
-void
+int
 stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock)
 {
-    stepcompress_flush(sc, UINT64_MAX);
+    int ret = stepcompress_flush(sc, UINT64_MAX);
+    if (ret)
+        return ret;
     sc->last_step_clock = last_step_clock;
     sc->sdir = -1;
+    return 0;
 }
 
 // Indicate the stepper is in homing mode (or done homing if zero)
-void
+int
 stepcompress_set_homing(struct stepcompress *sc, uint64_t homing_clock)
 {
-    stepcompress_flush(sc, UINT64_MAX);
+    int ret = stepcompress_flush(sc, UINT64_MAX);
+    if (ret)
+        return ret;
     sc->homing_clock = homing_clock;
+    return 0;
 }
 
 // Queue an mcu command to go out in order with stepper commands
-void
+int
 stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len)
 {
-    stepcompress_flush(sc, UINT64_MAX);
+    int ret = stepcompress_flush(sc, UINT64_MAX);
+    if (ret)
+        return ret;
 
     struct queue_message *qm = message_alloc_and_encode(data, len);
     qm->req_clock = sc->homing_clock ?: sc->last_step_clock;
     list_add_tail(&qm->node, &sc->msg_queue);
-}
-
-// Return the count of internal errors found
-uint32_t
-stepcompress_get_errors(struct stepcompress *sc)
-{
-    return sc->errors;
+    return 0;
 }
 
 
@@ -693,13 +740,16 @@ heap_replace(struct steppersync *ss, uint64_t req_clock)
 }
 
 // Find and transmit any scheduled steps prior to the given 'move_clock'
-void
+int
 steppersync_flush(struct steppersync *ss, uint64_t move_clock)
 {
     // Flush each stepcompress to the specified move_clock
     int i;
-    for (i=0; i<ss->sc_num; i++)
-        stepcompress_flush(ss->sc_list[i], move_clock);
+    for (i=0; i<ss->sc_num; i++) {
+        int ret = stepcompress_flush(ss->sc_list[i], move_clock);
+        if (ret)
+            return ret;
+    }
 
     // Order commands by the reqclock of each pending command
     struct list_head msgs;
@@ -739,4 +789,5 @@ steppersync_flush(struct steppersync *ss, uint64_t move_clock)
     // Transmit commands
     if (!list_empty(&msgs))
         serialqueue_send_batch(ss->sq, ss->cq, &msgs);
+    return 0;
 }