From a7b81dc05cebc593ac4be2012142fbca9180e199 Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Mon, 19 Sep 2016 13:18:55 -0400
Subject: [PATCH] toolhead: Force a firmware shutdown on an unhandled exception

Check for unhandled exceptions and force the MCU to shutdown in that
case.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
---
 klippy/gcode.py    | 11 +++++++++--
 klippy/mcu.py      |  6 ++++++
 klippy/toolhead.py | 48 ++++++++++++++++++++++++++++------------------
 3 files changed, 44 insertions(+), 21 deletions(-)

diff --git a/klippy/gcode.py b/klippy/gcode.py
index ad05b0d5..f6f64d1e 100644
--- a/klippy/gcode.py
+++ b/klippy/gcode.py
@@ -101,7 +101,8 @@ class GCodeParser:
                 handler(params)
             except:
                 logging.exception("Exception in command handler")
-                self.respond('echo:Internal error on command:"%s"' % (cmd,))
+                self.toolhead.force_shutdown()
+                self.respond('Error: Internal error on command:"%s"' % (cmd,))
             # Check if machine can process next command or must stall input
             if self.busy_state is not None:
                 break
@@ -142,7 +143,13 @@ class GCodeParser:
         self.busy_state = busy_handler
         self.reactor.update_timer(self.busy_timer, self.reactor.NOW)
     def busy_handler(self, eventtime):
-        busy = self.busy_state.check_busy(eventtime)
+        try:
+            busy = self.busy_state.check_busy(eventtime)
+        except:
+            logging.exception("Exception in busy handler")
+            self.toolhead.force_shutdown()
+            self.respond('Error: Internal error in busy handler')
+            busy = False
         if busy:
             self.toolhead.reset_motor_off_time(eventtime)
             return eventtime + self.RETRY_TIME
diff --git a/klippy/mcu.py b/klippy/mcu.py
index 143b4042..884adbe5 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -252,6 +252,7 @@ class MCU:
         self.is_shutdown = False
         self.output_file_mode = False
         # Config building
+        self._emergency_stop_cmd = None
         self._num_oids = 0
         self._config_cmds = []
         self._config_crc = None
@@ -328,6 +329,8 @@ class MCU:
         if err:
             stats += " step_errors=%d" % (err,)
         return stats
+    def force_shutdown(self):
+        self.send(self._emergency_stop_cmd.encode())
     # Configuration phase
     def _add_custom(self):
         data = self._config.get('custom', '')
@@ -340,6 +343,7 @@ class MCU:
                 continue
             self.add_config_cmd(line)
     def build_config(self):
+        self._emergency_stop_cmd = self.lookup_command("emergency_stop")
         # Build config commands
         self._add_custom()
         self._config_cmds.insert(0, "allocate_oids count=%d" % (
@@ -504,6 +508,8 @@ class DummyMCU:
         pass
     def stats(self, eventtime):
         return ""
+    def force_shutdown(self):
+        pass
     def build_config(self):
         pass
     def create_stepper(self, step_pin, dir_pin, min_stop_interval, max_error):
diff --git a/klippy/toolhead.py b/klippy/toolhead.py
index 8c389d19..54dae025 100644
--- a/klippy/toolhead.py
+++ b/klippy/toolhead.py
@@ -85,6 +85,9 @@ class MoveQueue:
         self.queue = []
         self.prev_junction_max = 0.
         self.junction_flush = 0.
+    def reset(self):
+        del self.queue[:]
+        self.prev_junction_max = self.junction_flush = 0.
     def flush(self, lazy=False):
         can_flush = not lazy
         flush_count = len(self.queue)
@@ -186,26 +189,30 @@ class ToolHead:
             eventtime, self.print_time)
         return buffer_time > self.buffer_time_high
     def flush_handler(self, eventtime):
-        if not self.print_time:
-            self.move_queue.flush()
+        try:
             if not self.print_time:
-                if eventtime >= self.motor_off_time:
-                    self.motor_off()
-                    self.reset_print_time()
-                    self.motor_off_time = self.reactor.NEVER
-                return self.motor_off_time
-        print_time = self.print_time
-        buffer_time = self.printer.mcu.get_print_buffer_time(
-            eventtime, print_time)
-        if buffer_time > self.buffer_time_low:
-            return eventtime + buffer_time - self.buffer_time_low
-        self.move_queue.flush()
-        if print_time != self.print_time:
-            self.print_time_stall += 1
-            self.dwell(self.buffer_time_low + STALL_TIME)
-            return self.reactor.NOW
-        self.reset_print_time()
-        return self.motor_off_time
+                self.move_queue.flush()
+                if not self.print_time:
+                    if eventtime >= self.motor_off_time:
+                        self.motor_off()
+                        self.reset_print_time()
+                        self.motor_off_time = self.reactor.NEVER
+                    return self.motor_off_time
+            print_time = self.print_time
+            buffer_time = self.printer.mcu.get_print_buffer_time(
+                eventtime, print_time)
+            if buffer_time > self.buffer_time_low:
+                return eventtime + buffer_time - self.buffer_time_low
+            self.move_queue.flush()
+            if print_time != self.print_time:
+                self.print_time_stall += 1
+                self.dwell(self.buffer_time_low + STALL_TIME)
+                return self.reactor.NOW
+            self.reset_print_time()
+            return self.motor_off_time
+        except:
+            logging.exception("Exception in flush_handler")
+            self.force_shutdown()
     def stats(self, eventtime):
         buffer_time = 0.
         if self.print_time:
@@ -273,3 +280,6 @@ class ToolHead:
         self.extruder.motor_off(last_move_time)
         self.dwell(STALL_TIME)
         logging.debug('; Max time of %f' % (last_move_time,))
+    def force_shutdown(self):
+        self.printer.mcu.force_shutdown()
+        self.move_queue.reset()