diff --git a/Firmware/Configuration.h b/Firmware/Configuration.h
index b1b29b58e..41590d4e3 100644
--- a/Firmware/Configuration.h
+++ b/Firmware/Configuration.h
@@ -10,7 +10,7 @@
 
 // Firmware version
 #define FW_version "3.1.0"
-#define FW_local_variant 6
+#define FW_local_variant 7
 #define FW_report_version FW_version " r" STR(FW_local_variant)
 
 #define FW_PRUSA3D_MAGIC "PRUSA3DFW"
diff --git a/Firmware/MarlinSerial.cpp b/Firmware/MarlinSerial.cpp
index ad973a692..4a48a0a56 100644
--- a/Firmware/MarlinSerial.cpp
+++ b/Firmware/MarlinSerial.cpp
@@ -46,12 +46,8 @@ FORCE_INLINE void store_char(unsigned char c)
   }
 }
 
-
-//#elif defined(SIG_USART_RECV)
 #if defined(M_USARTx_RX_vect)
-  // fixed by Mark Sproul this is on the 644/644p
-  //SIGNAL(SIG_USART_RECV)
-  SIGNAL(M_USARTx_RX_vect)
+  ISR(M_USARTx_RX_vect)
   {
       // Test for a framing error.
       if (M_UCSRxA & (1<<M_FEx)) {
@@ -65,7 +61,7 @@ FORCE_INLINE void store_char(unsigned char c)
       }
   }
 #ifndef SNMM
-  SIGNAL(USART2_RX_vect)
+  ISR(USART2_RX_vect)
   {
       if (selectedSerialPort == 1) {
         // Test for a framing error.
@@ -159,6 +155,55 @@ void MarlinSerial::end()
 }
 
 
+void MarlinSerial::checkRx(void)
+{
+
+#ifdef SNMM
+    if((M_UCSRxA & (1<<M_RXCx)) != 0) {
+        CRITICAL_SECTION_START
+        // Test for a framing error.
+        if (M_UCSRxA & (1<<M_FEx)) {
+            // Characters received with the framing errors will be ignored.
+            (void)(*(char *)M_UDRx);
+        } else {
+            unsigned char c  =  M_UDRx;
+            store_char(c);
+            selectedSerialPort = 0;
+        }
+        CRITICAL_SECTION_END
+    }
+#else
+    if (selectedSerialPort == 0) {
+        if((M_UCSRxA & (1<<M_RXCx)) != 0) {
+		CRITICAL_SECTION_START
+        // Test for a framing error.
+            if (M_UCSRxA & (1<<M_FEx)) {
+                // Characters received with the framing errors will be ignored.
+                (void)(*(char *)M_UDRx);
+            } else {
+                unsigned char c  =  M_UDRx;
+                store_char(c);
+                selectedSerialPort = 0;
+            }
+            CRITICAL_SECTION_END
+        }
+    } else if(selectedSerialPort == 1) {
+        if((UCSR2A & (1<<RXC2)) != 0) {
+            CRITICAL_SECTION_START
+            // Test for a framing error.
+            if (UCSR2A & (1<<FE2)) {
+                // Characters received with the framing errors will be ignored.
+                (void)(*(char *)UDR2);
+            } else {
+                unsigned char c  =  UDR2;
+                store_char(c);
+                selectedSerialPort = 1;
+            }
+            CRITICAL_SECTION_END
+        }
+    }
+#endif
+}
 
 int MarlinSerial::peek(void)
 {
diff --git a/Firmware/MarlinSerial.h b/Firmware/MarlinSerial.h
index e869de7bc..3e364d114 100644
--- a/Firmware/MarlinSerial.h
+++ b/Firmware/MarlinSerial.h
@@ -96,6 +96,7 @@ class MarlinSerial //: public Stream
     int peek(void);
     int read(void);
     void flush(void);
+    void checkRx(void);
     
     FORCE_INLINE int available(void)
     {
@@ -124,76 +125,7 @@ class MarlinSerial //: public Stream
 		}
 #endif
 	}
-    
-    
-    void checkRx(void)
-    {
-        
-#ifdef SNMM
-        if((M_UCSRxA & (1<<M_RXCx)) != 0) {
-                // Test for a framing error.
-                if (M_UCSRxA & (1<<M_FEx)) {
-                    // Characters received with the framing errors will be ignored.
-                    (void)(*(char *)M_UDRx);
-                } else {
-                    unsigned char c  =  M_UDRx;
-                    int i = (unsigned int)(rx_buffer.head + 1) % RX_BUFFER_SIZE;
-                    // if we should be storing the received character into the location
-                    // just before the tail (meaning that the head would advance to the
-                    // current location of the tail), we're about to overflow the buffer
-                    // and so we don't write the character or advance the head.
-                    if (i != rx_buffer.tail) {
-                        rx_buffer.buffer[rx_buffer.head] = c;
-                        rx_buffer.head = i;
-                    }
-                    selectedSerialPort = 0;
-                }
-            }
-#else
-        if (selectedSerialPort == 0) {
-            if((M_UCSRxA & (1<<M_RXCx)) != 0) {
-                // Test for a framing error.
-                if (M_UCSRxA & (1<<M_FEx)) {
-                    // Characters received with the framing errors will be ignored.
-                    (void)(*(char *)M_UDRx);
-                } else {
-                    unsigned char c  =  M_UDRx;
-                    int i = (unsigned int)(rx_buffer.head + 1) % RX_BUFFER_SIZE;
-                    // if we should be storing the received character into the location
-                    // just before the tail (meaning that the head would advance to the
-                    // current location of the tail), we're about to overflow the buffer
-                    // and so we don't write the character or advance the head.
-                    if (i != rx_buffer.tail) {
-                        rx_buffer.buffer[rx_buffer.head] = c;
-                        rx_buffer.head = i;
-                    }
-                    selectedSerialPort = 0;
-                }
-            }
-        } else if(selectedSerialPort == 1) {
-            if((UCSR2A & (1<<RXC2)) != 0) {
-                // Test for a framing error.
-                if (UCSR2A & (1<<FE2)) {
-                    // Characters received with the framing errors will be ignored.
-                    (void)(*(char *)UDR2);
-                } else {
-                    unsigned char c  =  UDR2;
-                    int i = (unsigned int)(rx_buffer.head + 1) % RX_BUFFER_SIZE;
-                    // if we should be storing the received character into the location
-                    // just before the tail (meaning that the head would advance to the
-                    // current location of the tail), we're about to overflow the buffer
-                    // and so we don't write the character or advance the head.
-                    if (i != rx_buffer.tail) {
-                        rx_buffer.buffer[rx_buffer.head] = c;
-                        rx_buffer.head = i;
-                    }
-                    selectedSerialPort = 1;
-                }
-            }
-        }
-#endif
-    }
-    
+
     
     private:
     void printNumber(unsigned long, uint8_t);
diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp
index a1d1f0c97..29ad1f598 100644
--- a/Firmware/planner.cpp
+++ b/Firmware/planner.cpp
@@ -585,7 +585,7 @@ static inline void planner_update_queue_min_counter()
 void planner_abort_hard()
 {
     // Abort the stepper routine and flush the planner queue.
-    quickStop();
+    DISABLE_STEPPER_DRIVER_INTERRUPT();
 
     // Now the front-end (the Marlin_main.cpp with its current_position) is out of sync.
     // First update the planner's current position in the physical motor steps.
@@ -604,6 +604,10 @@ void planner_abort_hard()
     if (mbl.active)
         current_position[Z_AXIS] -= mbl.get_z(current_position[X_AXIS], current_position[Y_AXIS]);
 #endif
+
+    // Clear the planner queue, reset and re-enable the stepper timer
+    quickStop();
+
     // Apply inverse world correction matrix.
     machine2world(current_position[X_AXIS], current_position[Y_AXIS]);
     memcpy(destination, current_position, sizeof(destination));
@@ -1229,7 +1233,10 @@ Having the real displacement of the head, we can calculate the total movement le
 #ifdef PLANNER_DIAGNOSTICS
   planner_update_queue_min_counter();
 #endif /* PLANNER_DIAGNOSTIC */
-  st_wake_up();
+
+  // Safe to turn on here
+  // Note: ints only disabled while higher priority thread running - no recursion
+  ENABLE_STEPPER_DRIVER_INTERRUPT();
 }
 
 #ifdef ENABLE_AUTO_BED_LEVELING
@@ -1254,6 +1261,7 @@ void plan_set_position(float x, float y, float z, const float &e)
 #endif // ENABLE_AUTO_BED_LEVELING
 
     // Apply the machine correction matrix.
+    if (world2machine_correction_mode != WORLD2MACHINE_CORRECTION_NONE)
     {
         float tmpx = x;
         float tmpy = y;
@@ -1264,11 +1272,9 @@ void plan_set_position(float x, float y, float z, const float &e)
   position[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
   position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
 #ifdef MESH_BED_LEVELING
-    if (mbl.active){
-      position[Z_AXIS] = lround((z+mbl.get_z(x, y))*axis_steps_per_unit[Z_AXIS]);
-    }else{
-        position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
-    }
+    position[Z_AXIS] = (mbl.active) ? 
+      lround((z + mbl.get_z(x, y)) * axis_steps_per_unit[Z_AXIS]) :
+      lround(z * axis_steps_per_unit[Z_AXIS]);
 #else
   position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
 #endif // ENABLE_MESH_BED_LEVELING
diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp
index ae24b7310..e255f362c 100644
--- a/Firmware/stepper.cpp
+++ b/Firmware/stepper.cpp
@@ -189,12 +189,6 @@ asm volatile ( \
 "r26" , "r27" \
 )
 
-// Some useful constants
-
-#define ENABLE_STEPPER_DRIVER_INTERRUPT()  TIMSK1 |= (1<<OCIE1A)
-#define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~(1<<OCIE1A)
-
-
 void checkHitEndstops()
 {
  if( endstop_x_hit || endstop_y_hit || endstop_z_hit) {
@@ -277,11 +271,6 @@ bool enable_z_endstop(bool check)
 //  step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
 //  The slope of acceleration is calculated with the leib ramp alghorithm.
 
-void st_wake_up() {
-  //  TCNT1 = 0;
-  ENABLE_STEPPER_DRIVER_INTERRUPT();
-}
-
 unsigned short calc_timer(unsigned short step_rate) {
   unsigned short timer;
   if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY;
@@ -348,6 +337,13 @@ ISR(TIMER1_COMPA_vect) {
 }
 
 void isr() {
+  #ifndef LIN_ADVANCE
+    // Disable Timer0 ISRs and enable global ISR again to capture UART events (incoming chars)
+    DISABLE_TEMPERATURE_INTERRUPT(); // Temperature ISR
+    DISABLE_STEPPER_DRIVER_INTERRUPT();
+    sei();
+  #endif
+
   // If there is no current block, attempt to pop one from the buffer
   if (current_block == NULL) {
     // Anything in the buffer?
@@ -366,6 +362,7 @@ void isr() {
         if(current_block->steps_z > 0) {
           enable_z();
           _NEXT_ISR(2000); //1ms wait
+          ENABLE_ISRs();
           return;
         }
       #endif
@@ -573,7 +570,8 @@ void isr() {
 
     for(uint8_t i=0; i < step_loops; i++) { // Take multiple steps per interrupt (For high speed moves)
       #ifndef AT90USB
-      MSerial.checkRx(); // Check for serial chars.
+      // Not needed - ints are enabled here
+      //MSerial.checkRx(); // Check for serial chars.
       #endif
       
       #ifdef LIN_ADVANCE
@@ -719,6 +717,10 @@ void isr() {
       plan_discard_current_block();
     }
   }
+
+  #ifndef LIN_ADVANCE
+    ENABLE_ISRs();	// Re-enable ISRs
+  #endif  
 }
 
 #ifdef LIN_ADVANCE
@@ -750,6 +752,11 @@ void isr() {
   }
 
   void advance_isr_scheduler() {
+    // Disable Timer0 ISRs and enable global ISR again to capture UART events (incoming chars)
+    DISABLE_TEMPERATURE_INTERRUPT(); // Temperature ISR
+    DISABLE_STEPPER_DRIVER_INTERRUPT();
+    sei();
+
     // Run main stepping ISR if flagged
     if (!nextMainISR) isr();
 
@@ -777,6 +784,9 @@ void isr() {
 
     // Don't run the ISR faster than possible
     if (OCR1A < TCNT1 + 16) OCR1A = TCNT1 + 16;
+
+    // Restore original ISR settings
+    ENABLE_ISRs();
   }
   
   void clear_current_adv_vars() {
diff --git a/Firmware/stepper.h b/Firmware/stepper.h
index 3427f12fa..20a1dc8f2 100644
--- a/Firmware/stepper.h
+++ b/Firmware/stepper.h
@@ -102,6 +102,11 @@ void microstep_readings();
   void babystep(const uint8_t axis,const bool direction); // perform a short step with a single stepper motor, outside of any convention
 #endif
      
+// Interrupt enable/disable macros
 
+#define ENABLE_STEPPER_DRIVER_INTERRUPT()  sbi(TIMSK1, OCIE1A)
+#define DISABLE_STEPPER_DRIVER_INTERRUPT() cbi(TIMSK1, OCIE1A)
+
+#define ENABLE_ISRs()	do { cli(); if (in_temp_isr) DISABLE_TEMPERATURE_INTERRUPT(); else ENABLE_TEMPERATURE_INTERRUPT(); ENABLE_STEPPER_DRIVER_INTERRUPT(); } while(0)
 
 #endif
diff --git a/Firmware/temperature.cpp b/Firmware/temperature.cpp
index f249f98f0..48c0cb8ce 100644
--- a/Firmware/temperature.cpp
+++ b/Firmware/temperature.cpp
@@ -991,7 +991,7 @@ void tp_init()
   // Use timer0 for temperature measurement
   // Interleave temperature interrupt with millies interrupt
   OCR0B = 128;
-  TIMSK0 |= (1<<OCIE0B);  
+  sbi(TIMSK0, OCIE0B);  
   
   // Wait for temperature measurement to settle
   delay(250);
@@ -1439,10 +1439,20 @@ int read_max6675()
 }
 #endif
 
+volatile bool in_temp_isr = false;
 
 // Timer 0 is shared with millies
 ISR(TIMER0_COMPB_vect)
 {
+  // The stepper ISR can interrupt this ISR. When it does it re-enables this ISR
+  // at the end of its run, potentially causing re-entry. This flag prevents it.
+  if (in_temp_isr) return;
+  in_temp_isr = true;
+
+  // Allow UART and stepper ISRs
+  DISABLE_TEMPERATURE_INTERRUPT(); //Disable Temperature ISR
+  sei();
+
   //these variables are only accesible from the ISR, but static, so they don't lose their value
   static unsigned char temp_count = 0;
   static unsigned long raw_temp_0_value = 0;
@@ -1759,6 +1769,12 @@ ISR(TIMER0_COMPB_vect)
   
 #endif //ifndef SLOW_PWM_HEATERS
   
+  //
+  // Update lcd buttons 488 times per second
+  //
+  static bool do_buttons;
+  if ((do_buttons ^= true)) lcd_buttons_update();
+
   switch(temp_state) {
     case 0: // Prepare TEMP_0
       #if defined(TEMP_0_PIN) && (TEMP_0_PIN > -1)
@@ -1770,7 +1786,6 @@ ISR(TIMER0_COMPB_vect)
         ADMUX = ((1 << REFS0) | (TEMP_0_PIN & 0x07));
         ADCSRA |= 1<<ADSC; // Start conversion
       #endif
-      lcd_buttons_update();
       temp_state = 1;
       break;
     case 1: // Measure TEMP_0
@@ -1792,7 +1807,6 @@ ISR(TIMER0_COMPB_vect)
         ADMUX = ((1 << REFS0) | (TEMP_BED_PIN & 0x07));
         ADCSRA |= 1<<ADSC; // Start conversion
       #endif
-      lcd_buttons_update();
       temp_state = 3;
       break;
     case 3: // Measure TEMP_BED
@@ -1811,7 +1825,6 @@ ISR(TIMER0_COMPB_vect)
         ADMUX = ((1 << REFS0) | (TEMP_1_PIN & 0x07));
         ADCSRA |= 1<<ADSC; // Start conversion
       #endif
-      lcd_buttons_update();
       temp_state = 5;
       break;
     case 5: // Measure TEMP_1
@@ -1830,7 +1843,6 @@ ISR(TIMER0_COMPB_vect)
         ADMUX = ((1 << REFS0) | (TEMP_2_PIN & 0x07));
         ADCSRA |= 1<<ADSC; // Start conversion
       #endif
-      lcd_buttons_update();
       temp_state = 7;
       break;
     case 7: // Measure TEMP_2
@@ -1850,7 +1862,6 @@ ISR(TIMER0_COMPB_vect)
       ADMUX = ((1 << REFS0) | (FILWIDTH_PIN & 0x07)); 
       ADCSRA |= 1<<ADSC; // Start conversion 
      #endif 
-     lcd_buttons_update();       
      temp_state = 9; 
      break; 
     case 9:   //Measure FILWIDTH 
@@ -1999,6 +2010,10 @@ ISR(TIMER0_COMPB_vect)
     }
   }
 #endif //BABYSTEPPING
+
+  cli();
+  in_temp_isr = false;
+  ENABLE_TEMPERATURE_INTERRUPT(); //re-enable Temperature ISR
 }
 
 #ifdef PIDTEMP
diff --git a/Firmware/temperature.h b/Firmware/temperature.h
index 5e80a0b39..7ab35a1a9 100644
--- a/Firmware/temperature.h
+++ b/Firmware/temperature.h
@@ -202,5 +202,10 @@ void PID_autotune(float temp, int extruder, int ncycles);
 void setExtruderAutoFanState(int pin, bool state);
 void checkExtruderAutoFans();
 
+extern volatile bool in_temp_isr;
+
+#define ENABLE_TEMPERATURE_INTERRUPT()  sbi(TIMSK0, OCIE0B)
+#define DISABLE_TEMPERATURE_INTERRUPT() cbi(TIMSK0, OCIE0B)
+
 #endif
 
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index ec802b913..6550a0d2e 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -6289,6 +6289,11 @@ void lcd_setcontrast(uint8_t value)
 /* Warning: This function is called from interrupt context */
 void lcd_buttons_update()
 {
+  // Avoid re-entrancy from temperature interrups
+ static bool in_buttons_update = false;
+ if (in_buttons_update) return;
+ in_buttons_update = true;
+
 #ifdef NEWPANEL
   uint8_t newbutton = 0;
   if (READ(BTN_EN1) == 0)  newbutton |= EN_A;
@@ -6413,6 +6418,7 @@ void lcd_buttons_update()
     }
   }
   lastEncoderBits = enc;
+  in_buttons_update = false;
 }
 
 bool lcd_detected(void)