From a129078927c8968022569175cec7fea35c920fd2 Mon Sep 17 00:00:00 2001
From: AnHardt <github@kitelab.de>
Date: Mon, 4 Jul 2016 23:23:22 +0200
Subject: [PATCH 1/6] Add an emergency-command parser to MarlinSerial
 (supporting M108)

Add an emergency-command parser to MarlinSerial's RX interrupt.

The parser tries to find and execute M108,M112,M410 before the commands disappear in the RX-buffer.

To avoid false positives for M117, comments and commands followed by filenames (M23, M28, M30, M32, M33) are filtered.

This enables Marlin to receive and react on the Emergency command at all times - regardless of whether the buffers are full or not. It remains to convince hosts to send the commands. To inform the hosts about the new feature a new entry in the M115-report was made. "`EMERGENCY_CODES:M112,M108,M410;`".

The parser is fast. It only ever needs two switch decisions and one assignment of the new state for every character.

One problem remains. If the host has sent an incomplete line before sending an emergency command the emergency command could be omitted when the parser is in `state_IGNORE`.
In that case the host should send "\ncommand\n"

Also introduces M108 to break the waiting for the heaters in M109, M190 and M303.

Rename `cancel_heatup` to `wait_for_heatup` to better see the purpose.
---
 Marlin/Conditionals.h                         |   6 +
 Marlin/Configuration_adv.h                    |   6 +
 Marlin/Marlin.h                               |   1 +
 Marlin/MarlinSerial.cpp                       | 158 ++++++++++++++++++
 Marlin/MarlinSerial.h                         |   9 +
 Marlin/Marlin_main.cpp                        |  65 ++++---
 Marlin/SanityCheck.h                          |   7 +
 .../Cartesio/Configuration_adv.h              |   6 +
 .../Felix/Configuration_adv.h                 |   6 +
 .../Hephestos/Configuration_adv.h             |   6 +
 .../Hephestos_2/Configuration_adv.h           |   6 +
 .../K8200/Configuration_adv.h                 |   6 +
 .../K8400/Configuration_adv.h                 |   6 +
 .../RigidBot/Configuration_adv.h              |   6 +
 .../SCARA/Configuration_adv.h                 |   6 +
 .../TAZ4/Configuration_adv.h                  |   6 +
 .../WITBOX/Configuration_adv.h                |   6 +
 .../delta/biv2.5/Configuration_adv.h          |   6 +
 .../delta/generic/Configuration_adv.h         |   6 +
 .../delta/kossel_mini/Configuration_adv.h     |   6 +
 .../delta/kossel_pro/Configuration_adv.h      |   6 +
 .../delta/kossel_xl/Configuration_adv.h       |   6 +
 .../makibox/Configuration_adv.h               |   6 +
 .../tvrrug/Round2/Configuration_adv.h         |   6 +
 Marlin/language.h                             |   2 +-
 Marlin/temperature.cpp                        |   5 +-
 26 files changed, 335 insertions(+), 26 deletions(-)

diff --git a/Marlin/Conditionals.h b/Marlin/Conditionals.h
index 1c6aa9abcd02..6b8a682ac62e 100644
--- a/Marlin/Conditionals.h
+++ b/Marlin/Conditionals.h
@@ -284,6 +284,12 @@
     #define HardwareSerial_h // trick to disable the standard HWserial
   #endif
 
+  #if ENABLED(EMERGENCY_PARSER)
+    #define EMERGENCY_PARSER_CAPABILITIES " EMERGENCY_CODES:M108,M112,M410"
+  #else
+    #define EMERGENCY_PARSER_CAPABILITIES ""
+  #endif
+
   #include "Arduino.h"
 
   /**
diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
index 3a673a673493..93ca69391887 100644
--- a/Marlin/Configuration_adv.h
+++ b/Marlin/Configuration_adv.h
@@ -520,6 +520,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.
diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index 616a8bf357bb..9d40ce1615cb 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -288,6 +288,7 @@ extern float sw_endstop_min[3]; // axis[n].sw_endstop_min
 extern float sw_endstop_max[3]; // axis[n].sw_endstop_max
 extern bool axis_known_position[3]; // axis[n].is_known
 extern bool axis_homed[3]; // axis[n].is_homed
+extern bool wait_for_heatup;
 
 // GCode support for external objects
 bool code_seen(char);
diff --git a/Marlin/MarlinSerial.cpp b/Marlin/MarlinSerial.cpp
index e6362fc7c78c..7f1920528acc 100644
--- a/Marlin/MarlinSerial.cpp
+++ b/Marlin/MarlinSerial.cpp
@@ -30,6 +30,7 @@
 
 #include "Marlin.h"
 #include "MarlinSerial.h"
+#include "stepper.h"
 
 #ifndef USBCON
 // this next line disables the entire HardwareSerial.cpp,
@@ -54,6 +55,10 @@ FORCE_INLINE void store_char(unsigned char c) {
       rx_buffer.head = i;
     }
   CRITICAL_SECTION_END;
+
+  #if ENABLED(EMERGENCY_PARSER)
+    emergency_parser(c);
+  #endif
 }
 
 
@@ -310,3 +315,156 @@ MarlinSerial customizedSerial;
 #if defined(USBCON) && ENABLED(BLUETOOTH)
   HardwareSerial bluetoothSerial;
 #endif
+
+#if ENABLED(EMERGENCY_PARSER)
+
+  // Currently looking for: M108, M112, M410
+  // If you alter the parser please don't forget to update the capabilities in Conditionals.h
+
+  void emergency_parser(unsigned char c) {
+
+    enum e_parser_state {
+      state_RESET,
+      state_M,
+      state_M1,
+      state_M10,
+      state_M11,
+      state_M2,
+      state_M3,
+      state_M4,
+      state_M41,
+      state_IGNORE // to '\n'
+    };
+
+    static e_parser_state state = state_RESET;
+
+    switch (state) {
+      case state_RESET:
+        switch (c) {
+          case 'M':
+            state = state_M;
+            break;
+          case ';':
+            state = state_IGNORE;
+            break;
+          default: state = state_RESET;
+        }
+      break;
+
+      case state_M:
+        switch (c) {
+          case '1':
+            state = state_M1;
+            break;
+          case '2':
+            state = state_M2;
+            break;
+          case '3':
+            state = state_M3;
+            break;
+          case '4':
+            state = state_M4;
+            break;
+          case ';':
+            state = state_IGNORE;
+            break;
+          default: state = state_RESET;
+        }
+      break;
+
+      case state_M1:
+        switch (c) {
+          case '0':
+            state = state_M10;
+            break;
+          case '1':
+            state = state_M11;
+            break;
+          case ';':
+            state = state_IGNORE;
+            break;
+          default: state = state_RESET;
+        }
+      break;
+
+      case state_M2:
+        switch (c) {
+          case '3': // M23
+          case '8': // M28
+          case ';':
+            state = state_IGNORE;
+            break;
+          default: state = state_RESET;
+        }
+      break;
+
+      case state_M3:
+        switch (c) {
+          case '0': // M30
+          case '2': // M32
+          case '3': // M33
+          case ';':
+            state = state_IGNORE;
+            break;
+          default: state = state_RESET;
+        }
+      break;
+
+      case state_M10:
+        switch (c) {
+          case '8': // M108
+            { state = state_RESET; wait_for_heatup = false; }
+            break;
+          case ';':
+            state = state_IGNORE;
+            break;
+          default: state = state_RESET;
+        }
+      break;
+
+      case state_M11:
+        switch (c) {
+          case '2': // M112
+            state = state_RESET; kill(PSTR(MSG_KILLED));
+            break;
+          case '7': // M117
+          case ';':
+            state = state_IGNORE;
+            break;
+          default: state = state_RESET;
+        }
+      break;
+
+      case state_M4:
+        switch (c) {
+          case '1':
+            state = state_M41;
+            break;
+          case ';':
+            state = state_IGNORE;
+            break;
+          default: state = state_RESET;
+        }
+      break;
+
+      case state_M41:
+        switch (c) {
+          case '0':
+            { state = state_RESET; stepper.quick_stop(); }
+            break;
+          case ';':
+            state = state_IGNORE;
+            break;
+          default: state = state_RESET;
+        }
+      break;
+
+      case state_IGNORE:
+        if (c == '\n') state = state_RESET;
+      break;
+
+      default:
+        state = state_RESET;
+    }
+  }
+#endif
diff --git a/Marlin/MarlinSerial.h b/Marlin/MarlinSerial.h
index 34d234615640..b27b98169ac7 100644
--- a/Marlin/MarlinSerial.h
+++ b/Marlin/MarlinSerial.h
@@ -101,6 +101,11 @@ struct ring_buffer {
   extern ring_buffer rx_buffer;
 #endif
 
+#if ENABLED(EMERGENCY_PARSER)
+  #include "language.h"
+  void emergency_parser(unsigned char c);
+#endif
+
 class MarlinSerial { //: public Stream
 
   public:
@@ -141,6 +146,10 @@ class MarlinSerial { //: public Stream
             rx_buffer.head = i;
           }
         CRITICAL_SECTION_END;
+
+        #if ENABLED(EMERGENCY_PARSER)
+          emergency_parser(c);
+        #endif
       }
     }
 
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 836b5431130d..ab7ef8f05d8e 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -160,7 +160,7 @@
  * M105 - Read current temp
  * M106 - Fan on
  * M107 - Fan off
- * M108 - Cancel heatup and wait for the hotend and bed, this G-code is asynchronously handled in the get_serial_commands() parser
+ * M108 - Stop the waiting for heaters in M109, M190, M303. Does not affect the target temperature.
  * M109 - Sxxx Wait for extruder current temp to reach target temp. Waits only when heating
  *        Rxxx Wait for extruder current temp to reach target temp. Waits when heating and cooling
  *        IF AUTOTEMP is enabled, S<mintemp> B<maxtemp> F<factor>. Exit autotemp by any M109 without F
@@ -1105,9 +1105,12 @@ inline void get_serial_commands() {
         }
       }
 
-      // If command was e-stop process now
-      if (strcmp(command, "M112") == 0) kill(PSTR(MSG_KILLED));
-      if (strcmp(command, "M108") == 0) wait_for_heatup = false;
+      #if DISABLED(EMERGENCY_PARSER)
+        // If command was e-stop process now
+        if (strcmp(command, "M108") == 0) wait_for_heatup = false;
+        if (strcmp(command, "M112") == 0) kill(PSTR(MSG_KILLED));
+        if (strcmp(command, "M410") == 0) stepper.quick_stop();
+      #endif
 
       #if defined(NO_TIMEOUTS) && NO_TIMEOUTS > 0
         last_command_time = ms;
@@ -4533,10 +4536,14 @@ inline void gcode_M105() {
 
 #endif // FAN_COUNT > 0
 
-/**
- * M108: Cancel heatup and wait for the hotend and bed, this G-code is asynchronously handled in the get_serial_commands() parser
- */
-inline void gcode_M108() { wait_for_heatup = false; }
+#if DISABLED(EMERGENCY_PARSER)
+
+  /**
+   * M108: Stop the waiting for heaters in M109, M190, M303. Does not affect the target temperature.
+   */
+  inline void gcode_M108() { wait_for_heatup = false; }
+
+#endif
 
 /**
  * M109: Sxxx Wait for extruder(s) to reach temperature. Waits only when heating.
@@ -4811,7 +4818,9 @@ inline void gcode_M111() {
 /**
  * M112: Emergency Stop
  */
-inline void gcode_M112() { kill(PSTR(MSG_KILLED)); }
+#if DISABLED(EMERGENCY_PARSER)
+  inline void gcode_M112() { kill(PSTR(MSG_KILLED)); }
+#endif
 
 #if ENABLED(HOST_KEEPALIVE_FEATURE)
 
@@ -5991,13 +6000,15 @@ inline void gcode_M400() { stepper.synchronize(); }
  * This will stop the carriages mid-move, so most likely they
  * will be out of sync with the stepper position after this.
  */
-inline void gcode_M410() {
-  stepper.quick_stop();
-  #if DISABLED(DELTA) && DISABLED(SCARA)
-    set_current_position_from_planner();
-  #endif
-}
 
+#if DISABLED(EMERGENCY_PARSER)
+  inline void gcode_M410() {
+    stepper.quick_stop();
+    #if DISABLED(DELTA) && DISABLED(SCARA)
+      set_current_position_from_planner();
+    #endif
+  }
+#endif
 
 #if ENABLED(MESH_BED_LEVELING)
 
@@ -6953,9 +6964,11 @@ void process_next_command() {
         gcode_M111();
         break;
 
-      case 112: // M112: Emergency Stop
-        gcode_M112();
-        break;
+      #if DISABLED(EMERGENCY_PARSER)
+        case 112: // M112: Emergency Stop
+          gcode_M112();
+          break;
+      #endif
 
       #if ENABLED(HOST_KEEPALIVE_FEATURE)
 
@@ -6974,9 +6987,11 @@ void process_next_command() {
         KEEPALIVE_STATE(NOT_BUSY);
         return; // "ok" already printed
 
-      case 108:
-        gcode_M108();
-        break;
+      #if DISABLED(EMERGENCY_PARSER)
+        case 108:
+          gcode_M108();
+          break;
+      #endif
 
       case 109: // M109: Wait for temperature
         gcode_M109();
@@ -7261,9 +7276,11 @@ void process_next_command() {
           break;
       #endif // ENABLED(FILAMENT_WIDTH_SENSOR)
 
-      case 410: // M410 quickstop - Abort all the planned moves.
-        gcode_M410();
-        break;
+      #if DISABLED(EMERGENCY_PARSER)
+        case 410: // M410 quickstop - Abort all the planned moves.
+          gcode_M410();
+          break;
+      #endif
 
       #if ENABLED(MESH_BED_LEVELING)
         case 420: // M420 Enable/Disable Mesh Bed Leveling
diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h
index b075cd4c7b95..3200982a0e88 100644
--- a/Marlin/SanityCheck.h
+++ b/Marlin/SanityCheck.h
@@ -579,6 +579,13 @@
 #endif
 
 /**
+ * emergency-command parser
+ */
+#if ENABLED(EMERGENCY_PARSER) && ENABLED(USBCON)
+  #error "EMERGENCY_PARSER does not work on boards with AT90USB processors (USBCON)."
+#endif
+
+ /**
  * Warnings for old configurations
  */
 #if WATCH_TEMP_PERIOD > 500
diff --git a/Marlin/example_configurations/Cartesio/Configuration_adv.h b/Marlin/example_configurations/Cartesio/Configuration_adv.h
index d17635221200..e1848a3ed880 100644
--- a/Marlin/example_configurations/Cartesio/Configuration_adv.h
+++ b/Marlin/example_configurations/Cartesio/Configuration_adv.h
@@ -520,6 +520,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.
diff --git a/Marlin/example_configurations/Felix/Configuration_adv.h b/Marlin/example_configurations/Felix/Configuration_adv.h
index ffad119958a7..b46c497787df 100644
--- a/Marlin/example_configurations/Felix/Configuration_adv.h
+++ b/Marlin/example_configurations/Felix/Configuration_adv.h
@@ -520,6 +520,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.
diff --git a/Marlin/example_configurations/Hephestos/Configuration_adv.h b/Marlin/example_configurations/Hephestos/Configuration_adv.h
index 8956c41cd1eb..48c0c4b5a462 100644
--- a/Marlin/example_configurations/Hephestos/Configuration_adv.h
+++ b/Marlin/example_configurations/Hephestos/Configuration_adv.h
@@ -520,6 +520,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.
diff --git a/Marlin/example_configurations/Hephestos_2/Configuration_adv.h b/Marlin/example_configurations/Hephestos_2/Configuration_adv.h
index 704b20ce4066..bfd8b9a921fa 100644
--- a/Marlin/example_configurations/Hephestos_2/Configuration_adv.h
+++ b/Marlin/example_configurations/Hephestos_2/Configuration_adv.h
@@ -520,6 +520,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.
diff --git a/Marlin/example_configurations/K8200/Configuration_adv.h b/Marlin/example_configurations/K8200/Configuration_adv.h
index 1a00bb37615e..9476e5b3bd4d 100644
--- a/Marlin/example_configurations/K8200/Configuration_adv.h
+++ b/Marlin/example_configurations/K8200/Configuration_adv.h
@@ -526,6 +526,12 @@ const unsigned int dropsegments = 2; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.
diff --git a/Marlin/example_configurations/K8400/Configuration_adv.h b/Marlin/example_configurations/K8400/Configuration_adv.h
index 29b759d17b23..1c73ae074ce7 100644
--- a/Marlin/example_configurations/K8400/Configuration_adv.h
+++ b/Marlin/example_configurations/K8400/Configuration_adv.h
@@ -520,6 +520,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 26
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.
diff --git a/Marlin/example_configurations/RigidBot/Configuration_adv.h b/Marlin/example_configurations/RigidBot/Configuration_adv.h
index e1280f2d2ef9..bf798bb8f17d 100644
--- a/Marlin/example_configurations/RigidBot/Configuration_adv.h
+++ b/Marlin/example_configurations/RigidBot/Configuration_adv.h
@@ -520,6 +520,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 8
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.
diff --git a/Marlin/example_configurations/SCARA/Configuration_adv.h b/Marlin/example_configurations/SCARA/Configuration_adv.h
index 475e0335a44a..f743b660896e 100644
--- a/Marlin/example_configurations/SCARA/Configuration_adv.h
+++ b/Marlin/example_configurations/SCARA/Configuration_adv.h
@@ -520,6 +520,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.
diff --git a/Marlin/example_configurations/TAZ4/Configuration_adv.h b/Marlin/example_configurations/TAZ4/Configuration_adv.h
index 1a6ab806ac57..a4309db767a7 100644
--- a/Marlin/example_configurations/TAZ4/Configuration_adv.h
+++ b/Marlin/example_configurations/TAZ4/Configuration_adv.h
@@ -528,6 +528,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.
diff --git a/Marlin/example_configurations/WITBOX/Configuration_adv.h b/Marlin/example_configurations/WITBOX/Configuration_adv.h
index 8956c41cd1eb..48c0c4b5a462 100644
--- a/Marlin/example_configurations/WITBOX/Configuration_adv.h
+++ b/Marlin/example_configurations/WITBOX/Configuration_adv.h
@@ -520,6 +520,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.
diff --git a/Marlin/example_configurations/delta/biv2.5/Configuration_adv.h b/Marlin/example_configurations/delta/biv2.5/Configuration_adv.h
index 894e861633e6..a19e75beda9d 100644
--- a/Marlin/example_configurations/delta/biv2.5/Configuration_adv.h
+++ b/Marlin/example_configurations/delta/biv2.5/Configuration_adv.h
@@ -522,6 +522,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.
diff --git a/Marlin/example_configurations/delta/generic/Configuration_adv.h b/Marlin/example_configurations/delta/generic/Configuration_adv.h
index fba00960a25e..fcd0d9f1cc6d 100644
--- a/Marlin/example_configurations/delta/generic/Configuration_adv.h
+++ b/Marlin/example_configurations/delta/generic/Configuration_adv.h
@@ -522,6 +522,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.
diff --git a/Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h b/Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h
index 7265b76284a6..d634ce5ab1ed 100644
--- a/Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h
+++ b/Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h
@@ -521,6 +521,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.
diff --git a/Marlin/example_configurations/delta/kossel_pro/Configuration_adv.h b/Marlin/example_configurations/delta/kossel_pro/Configuration_adv.h
index 69eb9ce95659..f508233ef95c 100644
--- a/Marlin/example_configurations/delta/kossel_pro/Configuration_adv.h
+++ b/Marlin/example_configurations/delta/kossel_pro/Configuration_adv.h
@@ -526,6 +526,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.
diff --git a/Marlin/example_configurations/delta/kossel_xl/Configuration_adv.h b/Marlin/example_configurations/delta/kossel_xl/Configuration_adv.h
index f224fd60e7a4..796966872536 100644
--- a/Marlin/example_configurations/delta/kossel_xl/Configuration_adv.h
+++ b/Marlin/example_configurations/delta/kossel_xl/Configuration_adv.h
@@ -522,6 +522,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.
diff --git a/Marlin/example_configurations/makibox/Configuration_adv.h b/Marlin/example_configurations/makibox/Configuration_adv.h
index 857f320e4666..f266d270ee65 100644
--- a/Marlin/example_configurations/makibox/Configuration_adv.h
+++ b/Marlin/example_configurations/makibox/Configuration_adv.h
@@ -520,6 +520,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.
diff --git a/Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h b/Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h
index 69ee6e04b5c2..ade737a739c5 100644
--- a/Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h
+++ b/Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h
@@ -520,6 +520,12 @@ const unsigned int dropsegments = 5; //everything with less than this number of
 #define MAX_CMD_SIZE 96
 #define BUFSIZE 4
 
+// Enable an emergency-command parser to intercept certain commands as they
+// enter the serial receive buffer, so they cannot be blocked.
+// Currently handles M108, M112, M410
+// Does not work on boards using AT90USB (USBCON) processors!
+//#define EMERGENCY_PARSER
+
 // Bad Serial-connections can miss a received command by sending an 'ok'
 // Therefore some clients abort after 30 seconds in a timeout.
 // Some other clients start sending commands while receiving a 'wait'.
diff --git a/Marlin/language.h b/Marlin/language.h
index eb378feea084..03c063d4fb7c 100644
--- a/Marlin/language.h
+++ b/Marlin/language.h
@@ -128,7 +128,7 @@
 #define MSG_INVALID_EXTRUDER                "Invalid extruder"
 #define MSG_INVALID_SOLENOID                "Invalid solenoid"
 #define MSG_ERR_NO_THERMISTORS              "No thermistors - no temperature"
-#define MSG_M115_REPORT                     "FIRMWARE_NAME:Marlin " DETAILED_BUILD_VERSION " SOURCE_CODE_URL:" SOURCE_CODE_URL " PROTOCOL_VERSION:" PROTOCOL_VERSION " MACHINE_TYPE:" MACHINE_NAME " EXTRUDER_COUNT:" STRINGIFY(EXTRUDERS) " UUID:" MACHINE_UUID "\n"
+#define MSG_M115_REPORT                     "FIRMWARE_NAME:Marlin " DETAILED_BUILD_VERSION " SOURCE_CODE_URL:" SOURCE_CODE_URL " PROTOCOL_VERSION:" PROTOCOL_VERSION " MACHINE_TYPE:" MACHINE_NAME " EXTRUDER_COUNT:" STRINGIFY(EXTRUDERS) " UUID:" MACHINE_UUID EMERGENCY_PARSER_CAPABILITIES "\n"
 #define MSG_COUNT_X                         " Count X: "
 #define MSG_COUNT_A                         " Count A: "
 #define MSG_ERR_KILLED                      "Printer halted. kill() called!"
diff --git a/Marlin/temperature.cpp b/Marlin/temperature.cpp
index a011b77d2da9..fb9b40cf0ffe 100644
--- a/Marlin/temperature.cpp
+++ b/Marlin/temperature.cpp
@@ -238,8 +238,10 @@ unsigned char Temperature::soft_pwm[HOTENDS];
       soft_pwm_bed = bias = d = (MAX_BED_POWER) / 2;
     #endif
 
+    wait_for_heatup = true;
+
     // PID Tuning loop
-    for (;;) {
+    while (wait_for_heatup) {
 
       millis_t ms = millis();
 
@@ -421,6 +423,7 @@ unsigned char Temperature::soft_pwm[HOTENDS];
       }
       lcd_update();
     }
+    if (!wait_for_heatup) disable_all_heaters();
   }
 
 #endif // HAS_PID_HEATING

From ecffe92e2021c75b725d2403a5f59d4c874fb80d Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Wed, 6 Jul 2016 11:52:36 -0700
Subject: [PATCH 2/6] Group EMERGENCY_PARSER commands

---
 Marlin/Marlin_main.cpp | 65 +++++++++++++++++++-----------------------
 1 file changed, 30 insertions(+), 35 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index ab7ef8f05d8e..c4cdbb73155e 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -4543,6 +4543,26 @@ inline void gcode_M105() {
    */
   inline void gcode_M108() { wait_for_heatup = false; }
 
+
+  /**
+   * M112: Emergency Stop
+   */
+  inline void gcode_M112() { kill(PSTR(MSG_KILLED)); }
+
+
+  /**
+   * M410: Quickstop - Abort all planned moves
+   *
+   * This will stop the carriages mid-move, so most likely they
+   * will be out of sync with the stepper position after this.
+   */
+  inline void gcode_M410() {
+    stepper.quick_stop();
+    #if DISABLED(DELTA) && DISABLED(SCARA)
+      set_current_position_from_planner();
+    #endif
+  }
+
 #endif
 
 /**
@@ -4815,13 +4835,6 @@ inline void gcode_M111() {
   SERIAL_EOL;
 }
 
-/**
- * M112: Emergency Stop
- */
-#if DISABLED(EMERGENCY_PARSER)
-  inline void gcode_M112() { kill(PSTR(MSG_KILLED)); }
-#endif
-
 #if ENABLED(HOST_KEEPALIVE_FEATURE)
 
   /**
@@ -5994,22 +6007,6 @@ inline void gcode_M400() { stepper.synchronize(); }
   }
 #endif
 
-/**
- * M410: Quickstop - Abort all planned moves
- *
- * This will stop the carriages mid-move, so most likely they
- * will be out of sync with the stepper position after this.
- */
-
-#if DISABLED(EMERGENCY_PARSER)
-  inline void gcode_M410() {
-    stepper.quick_stop();
-    #if DISABLED(DELTA) && DISABLED(SCARA)
-      set_current_position_from_planner();
-    #endif
-  }
-#endif
-
 #if ENABLED(MESH_BED_LEVELING)
 
   /**
@@ -6965,9 +6962,19 @@ void process_next_command() {
         break;
 
       #if DISABLED(EMERGENCY_PARSER)
+
+        case 108: // M108: Cancel Waiting
+          gcode_M108();
+          break;
+
         case 112: // M112: Emergency Stop
           gcode_M112();
           break;
+
+        case 410: // M410 quickstop - Abort all the planned moves.
+          gcode_M410();
+          break;
+
       #endif
 
       #if ENABLED(HOST_KEEPALIVE_FEATURE)
@@ -6987,12 +6994,6 @@ void process_next_command() {
         KEEPALIVE_STATE(NOT_BUSY);
         return; // "ok" already printed
 
-      #if DISABLED(EMERGENCY_PARSER)
-        case 108:
-          gcode_M108();
-          break;
-      #endif
-
       case 109: // M109: Wait for temperature
         gcode_M109();
         break;
@@ -7276,12 +7277,6 @@ void process_next_command() {
           break;
       #endif // ENABLED(FILAMENT_WIDTH_SENSOR)
 
-      #if DISABLED(EMERGENCY_PARSER)
-        case 410: // M410 quickstop - Abort all the planned moves.
-          gcode_M410();
-          break;
-      #endif
-
       #if ENABLED(MESH_BED_LEVELING)
         case 420: // M420 Enable/Disable Mesh Bed Leveling
           gcode_M420();

From 834ad14c8d43dd3496280717fcd428c88f870942 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Wed, 6 Jul 2016 12:00:28 -0700
Subject: [PATCH 3/6] Add quickstop_stepper to update current position with
 stepper.quick_stop()

---
 Marlin/Marlin.h         |  4 +---
 Marlin/MarlinSerial.cpp |  3 ++-
 Marlin/Marlin_main.cpp  | 18 +++++++-----------
 Marlin/endstops.cpp     |  5 +----
 Marlin/ultralcd.cpp     |  5 +----
 5 files changed, 12 insertions(+), 23 deletions(-)

diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index 9d40ce1615cb..2159fb019242 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -230,9 +230,7 @@ void ok_to_send();
 void reset_bed_level();
 void kill(const char*);
 
-#if DISABLED(DELTA) && DISABLED(SCARA)
-  void set_current_position_from_planner();
-#endif
+void quickstop_stepper();
 
 #if ENABLED(FILAMENT_RUNOUT_SENSOR)
   void handle_filament_runout();
diff --git a/Marlin/MarlinSerial.cpp b/Marlin/MarlinSerial.cpp
index 7f1920528acc..82812986b4f8 100644
--- a/Marlin/MarlinSerial.cpp
+++ b/Marlin/MarlinSerial.cpp
@@ -450,7 +450,8 @@ MarlinSerial customizedSerial;
       case state_M41:
         switch (c) {
           case '0':
-            { state = state_RESET; stepper.quick_stop(); }
+            state = state_RESET;
+            quickstop_stepper();
             break;
           case ';':
             state = state_IGNORE;
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index c4cdbb73155e..3d8ba8443230 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -1109,7 +1109,7 @@ inline void get_serial_commands() {
         // If command was e-stop process now
         if (strcmp(command, "M108") == 0) wait_for_heatup = false;
         if (strcmp(command, "M112") == 0) kill(PSTR(MSG_KILLED));
-        if (strcmp(command, "M410") == 0) stepper.quick_stop();
+        if (strcmp(command, "M410") == 0) { quickstop_stepper(); }
       #endif
 
       #if defined(NO_TIMEOUTS) && NO_TIMEOUTS > 0
@@ -4556,12 +4556,7 @@ inline void gcode_M105() {
    * This will stop the carriages mid-move, so most likely they
    * will be out of sync with the stepper position after this.
    */
-  inline void gcode_M410() {
-    stepper.quick_stop();
-    #if DISABLED(DELTA) && DISABLED(SCARA)
-      set_current_position_from_planner();
-    #endif
-  }
+  inline void gcode_M410() { quickstop_stepper(); }
 
 #endif
 
@@ -5990,8 +5985,9 @@ inline void gcode_M400() { stepper.synchronize(); }
 
 #endif // FILAMENT_WIDTH_SENSOR
 
-#if DISABLED(DELTA) && DISABLED(SCARA)
-  void set_current_position_from_planner() {
+void quickstop_stepper() {
+  stepper.quick_stop();
+  #if DISABLED(DELTA) && DISABLED(SCARA)
     stepper.synchronize();
     #if ENABLED(AUTO_BED_LEVELING_FEATURE)
       vector_3 pos = planner.adjusted_position(); // values directly from steppers...
@@ -6004,8 +6000,8 @@ inline void gcode_M400() { stepper.synchronize(); }
       current_position[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
     #endif
     sync_plan_position();                       // ...re-apply to planner position
-  }
-#endif
+  #endif
+}
 
 #if ENABLED(MESH_BED_LEVELING)
 
diff --git a/Marlin/endstops.cpp b/Marlin/endstops.cpp
index 81a080ebf06b..3bfb67a11a4c 100644
--- a/Marlin/endstops.cpp
+++ b/Marlin/endstops.cpp
@@ -186,10 +186,7 @@ void Endstops::report_state() {
       if (stepper.abort_on_endstop_hit) {
         card.sdprinting = false;
         card.closefile();
-        stepper.quick_stop();
-        #if DISABLED(DELTA) && DISABLED(SCARA)
-          set_current_position_from_planner();
-        #endif
+        quickstop_stepper();
         thermalManager.disable_all_heaters(); // switch off all heaters.
       }
     #endif
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index ca0db6d67145..87104d87ecf1 100755
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -556,14 +556,11 @@ static void lcd_status_screen() {
     static void lcd_sdcard_stop() {
       card.stopSDPrint();
       clear_command_queue();
-      stepper.quick_stop();
+      quickstop_stepper();
       print_job_timer.stop();
       thermalManager.autotempShutdown();
       wait_for_heatup = false;
       lcd_setstatus(MSG_PRINT_ABORTED, true);
-      #if DISABLED(DELTA) && DISABLED(SCARA)
-        set_current_position_from_planner();
-      #endif // !DELTA && !SCARA
     }
 
   #endif //SDSUPPORT

From ea47803ae2f9249bf5311a71c03647bac8e50b9b Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Wed, 6 Jul 2016 12:48:21 -0700
Subject: [PATCH 4/6] Simplify emergency parser (only parse initial command)

---
 Marlin/MarlinSerial.cpp | 142 +++++++++++-----------------------------
 1 file changed, 39 insertions(+), 103 deletions(-)

diff --git a/Marlin/MarlinSerial.cpp b/Marlin/MarlinSerial.cpp
index 82812986b4f8..69fce40558f5 100644
--- a/Marlin/MarlinSerial.cpp
+++ b/Marlin/MarlinSerial.cpp
@@ -325,12 +325,11 @@ MarlinSerial customizedSerial;
 
     enum e_parser_state {
       state_RESET,
+      state_N,
       state_M,
       state_M1,
       state_M10,
       state_M11,
-      state_M2,
-      state_M3,
       state_M4,
       state_M41,
       state_IGNORE // to '\n'
@@ -338,131 +337,68 @@ MarlinSerial customizedSerial;
 
     static e_parser_state state = state_RESET;
 
+    if (c == '\n') state = state_IGNORE;
+
     switch (state) {
       case state_RESET:
         switch (c) {
-          case 'M':
-            state = state_M;
-            break;
-          case ';':
-            state = state_IGNORE;
-            break;
-          default: state = state_RESET;
-        }
-      break;
-
-      case state_M:
-        switch (c) {
-          case '1':
-            state = state_M1;
-            break;
-          case '2':
-            state = state_M2;
-            break;
-          case '3':
-            state = state_M3;
-            break;
-          case '4':
-            state = state_M4;
-            break;
-          case ';':
-            state = state_IGNORE;
-            break;
-          default: state = state_RESET;
+          case ' ': break;
+          case 'N': state = state_N;      break;
+          case 'M': state = state_M;      break;
+          default: state = state_IGNORE;
         }
-      break;
+        break;
 
-      case state_M1:
+      case state_N:
         switch (c) {
-          case '0':
-            state = state_M10;
-            break;
-          case '1':
-            state = state_M11;
-            break;
-          case ';':
-            state = state_IGNORE;
-            break;
-          default: state = state_RESET;
+          case '0': case '1': case '2':
+          case '3': case '4': case '5':
+          case '6': case '7': case '8':
+          case '9': case '-': case ' ':   break;
+          case 'M': state = state_M;      break;
+          default:  state = state_IGNORE;
         }
-      break;
+        break;
 
-      case state_M2:
+      case state_M:
         switch (c) {
-          case '3': // M23
-          case '8': // M28
-          case ';':
-            state = state_IGNORE;
-            break;
-          default: state = state_RESET;
+          case ' ': break;
+          case '1': state = state_M1;     break;
+          case '4': state = state_M4;     break;
+          default: state = state_IGNORE;
         }
-      break;
+        break;
 
-      case state_M3:
+      case state_M1:
         switch (c) {
-          case '0': // M30
-          case '2': // M32
-          case '3': // M33
-          case ';':
-            state = state_IGNORE;
-            break;
-          default: state = state_RESET;
+          case '0': state = state_M10;    break;
+          case '1': state = state_M11;    break;
+          default: state = state_IGNORE;
         }
-      break;
+        break;
 
       case state_M10:
-        switch (c) {
-          case '8': // M108
-            { state = state_RESET; wait_for_heatup = false; }
-            break;
-          case ';':
-            state = state_IGNORE;
-            break;
-          default: state = state_RESET;
-        }
-      break;
+        if (c == '8') wait_for_heatup = false; // M108
+        state = state_IGNORE;
+        break;
 
       case state_M11:
-        switch (c) {
-          case '2': // M112
-            state = state_RESET; kill(PSTR(MSG_KILLED));
-            break;
-          case '7': // M117
-          case ';':
-            state = state_IGNORE;
-            break;
-          default: state = state_RESET;
-        }
-      break;
+        if (c == '2') kill(PSTR(MSG_KILLED));  // M112
+        state = state_IGNORE;
+        break;
 
       case state_M4:
-        switch (c) {
-          case '1':
-            state = state_M41;
-            break;
-          case ';':
-            state = state_IGNORE;
-            break;
-          default: state = state_RESET;
-        }
-      break;
+        state = (c == '1') ? state_M41 : state_IGNORE;
+        break;
 
       case state_M41:
-        switch (c) {
-          case '0':
-            state = state_RESET;
-            quickstop_stepper();
-            break;
-          case ';':
-            state = state_IGNORE;
-            break;
-          default: state = state_RESET;
-        }
-      break;
+        if (c == '0') quickstop_stepper();     // M410
+        state = state_IGNORE;
+        break;
 
       case state_IGNORE:
         if (c == '\n') state = state_RESET;
-      break;
+        break;
 
       default:
         state = state_RESET;

From bd5a825b8ba270bb17206c30a4e160b571705328 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Wed, 6 Jul 2016 16:03:33 -0700
Subject: [PATCH 5/6] Wait for end of line before invoking action

---
 Marlin/MarlinSerial.cpp | 29 ++++++++++++++++++++---------
 1 file changed, 20 insertions(+), 9 deletions(-)

diff --git a/Marlin/MarlinSerial.cpp b/Marlin/MarlinSerial.cpp
index 69fce40558f5..c87f31b315d5 100644
--- a/Marlin/MarlinSerial.cpp
+++ b/Marlin/MarlinSerial.cpp
@@ -329,16 +329,17 @@ MarlinSerial customizedSerial;
       state_M,
       state_M1,
       state_M10,
+      state_M108,
       state_M11,
+      state_M112,
       state_M4,
       state_M41,
+      state_M410,
       state_IGNORE // to '\n'
     };
 
     static e_parser_state state = state_RESET;
 
-    if (c == '\n') state = state_IGNORE;
-
     switch (state) {
       case state_RESET:
         switch (c) {
@@ -378,13 +379,11 @@ MarlinSerial customizedSerial;
         break;
 
       case state_M10:
-        if (c == '8') wait_for_heatup = false; // M108
-        state = state_IGNORE;
+        state = (c == '8') ? state_M108 : state_IGNORE;
         break;
 
       case state_M11:
-        if (c == '2') kill(PSTR(MSG_KILLED));  // M112
-        state = state_IGNORE;
+        state = (c == '2') ? state_M112 : state_IGNORE;
         break;
 
       case state_M4:
@@ -392,8 +391,7 @@ MarlinSerial customizedSerial;
         break;
 
       case state_M41:
-        if (c == '0') quickstop_stepper();     // M410
-        state = state_IGNORE;
+        state = (c == '0') ? state_M410 : state_IGNORE;
         break;
 
       case state_IGNORE:
@@ -401,7 +399,20 @@ MarlinSerial customizedSerial;
         break;
 
       default:
-        state = state_RESET;
+        if (c == '\n') {
+          switch (state) {
+            case state_M108:
+              wait_for_heatup = false;
+              break;
+            case state_M112:
+              kill(PSTR(MSG_KILLED));
+              break;
+            case state_M410:
+              quickstop_stepper();
+              break;
+          }
+          state = state_RESET;
+        }
     }
   }
 #endif

From 2ee4e4f79120e5f530a70650704cdec94b6b0376 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Wed, 6 Jul 2016 19:59:19 -0700
Subject: [PATCH 6/6] Make wait_for_heatup volatile

---
 Marlin/Marlin.h        | 2 +-
 Marlin/Marlin_main.cpp | 2 +-
 Marlin/ultralcd.h      | 2 --
 3 files changed, 2 insertions(+), 4 deletions(-)

diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index 2159fb019242..5c698834b7ad 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -286,7 +286,7 @@ extern float sw_endstop_min[3]; // axis[n].sw_endstop_min
 extern float sw_endstop_max[3]; // axis[n].sw_endstop_max
 extern bool axis_known_position[3]; // axis[n].is_known
 extern bool axis_homed[3]; // axis[n].is_homed
-extern bool wait_for_heatup;
+extern volatile bool wait_for_heatup;
 
 // GCode support for external objects
 bool code_seen(char);
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 3d8ba8443230..fffff449bc5f 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -332,7 +332,7 @@ uint8_t active_extruder = 0;
 // Relative Mode. Enable with G91, disable with G90.
 static bool relative_mode = false;
 
-bool wait_for_heatup = true;
+volatile bool wait_for_heatup = true;
 
 const char errormagic[] PROGMEM = "Error:";
 const char echomagic[] PROGMEM = "echo:";
diff --git a/Marlin/ultralcd.h b/Marlin/ultralcd.h
index cc0a10fd3be3..40b09606ae27 100644
--- a/Marlin/ultralcd.h
+++ b/Marlin/ultralcd.h
@@ -95,8 +95,6 @@
   extern int absPreheatHPBTemp;
   extern int absPreheatFanSpeed;
 
-  extern bool wait_for_heatup;
-
   #if ENABLED(FILAMENT_LCD_DISPLAY)
     extern millis_t previous_lcd_status_ms;
   #endif