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 1c6aa9abc..6b8a682ac 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 3a673a673..93ca69391 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 616a8bf35..9d40ce161 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 e6362fc7c..7f1920528 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 34d234615..b27b98169 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 836b54311..ab7ef8f05 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 b075cd4c7..3200982a0 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 d17635221..e1848a3ed 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 ffad11995..b46c49778 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 8956c41cd..48c0c4b5a 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 704b20ce4..bfd8b9a92 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 1a00bb376..9476e5b3b 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 29b759d17..1c73ae074 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 e1280f2d2..bf798bb8f 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 475e0335a..f743b6608 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 1a6ab806a..a4309db76 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 8956c41cd..48c0c4b5a 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 894e86163..a19e75bed 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 fba00960a..fcd0d9f1c 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 7265b7628..d634ce5ab 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 69eb9ce95..f508233ef 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 f224fd60e..796966872 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 857f320e4..f266d270e 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 69ee6e04b..ade737a73 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 eb378feea..03c063d4f 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 a011b77d2..fb9b40cf0 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 ab7ef8f05..c4cdbb731 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 9d40ce161..2159fb019 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 7f1920528..82812986b 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 c4cdbb731..3d8ba8443 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 81a080ebf..3bfb67a11 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 ca0db6d67..87104d87e 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 82812986b..69fce4055 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 69fce4055..c87f31b31 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 2159fb019..5c698834b 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 3d8ba8443..fffff449b 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 cc0a10fd3..40b09606a 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