diff --git a/Firmware/MarlinSerial.cpp b/Firmware/MarlinSerial.cpp
index 357de7c60..0464fe41f 100644
--- a/Firmware/MarlinSerial.cpp
+++ b/Firmware/MarlinSerial.cpp
@@ -49,11 +49,13 @@ 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)
+// The serial line receive interrupt routine for a baud rate 115200
+// ticks at maximum 11.76 kHz and blocks for 2.688 us at each tick.
+// If the serial line is fully utilized, this corresponds to 3.16%
+// loading of the CPU (the interrupt invocation overhead not taken into account).
+// As the serial line is not fully utilized, the CPU load is likely around 1%.
+ISR(M_USARTx_RX_vect)
 {
 	// Test for a framing error.
 	if (M_UCSRxA & (1<<M_FEx))
@@ -74,7 +76,7 @@ SIGNAL(M_USARTx_RX_vect)
 	}
 }
 #ifndef SNMM
-SIGNAL(USART1_RX_vect)
+ISR(USART1_RX_vect)
 {
 	// Test for a framing error.
 	if (UCSR1A & (1<<FE1))
diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 94e76298b..62e1a0506 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -628,6 +628,8 @@ void crashdet_stop_and_save_print2()
 	cmdqueue_reset(); //empty cmdqueue
 	card.sdprinting = false;
 	card.closefile();
+  // Reset and re-enable the stepper timer just before the global interrupts are enabled.
+  st_reset_timer();
 	sei();
 }
 
@@ -1421,8 +1423,12 @@ void loop()
     #endif //SDSUPPORT
 
     if (! cmdbuffer_front_already_processed && buflen)
-	  {
-		    cli();
+    {
+      // ptr points to the start of the block currently being processed.
+      // The first character in the block is the block type.      
+      char *ptr = cmdbuffer + bufindr;
+      if (*ptr == CMDBUFFER_CURRENT_TYPE_SDCARD) {
+        // To support power panic, move the lenght of the command on the SD card to a planner buffer.
         union {
           struct {
               char lo;
@@ -1431,14 +1437,28 @@ void loop()
           uint16_t value;
         } sdlen;
         sdlen.value = 0;
-		    if (CMDBUFFER_CURRENT_TYPE == CMDBUFFER_CURRENT_TYPE_SDCARD) {
-			      sdlen.lohi.lo = cmdbuffer[bufindr + 1];
-            sdlen.lohi.hi = cmdbuffer[bufindr + 2];
+        {
+          // This block locks the interrupts globally for 3.25 us,
+          // which corresponds to a maximum repeat frequency of 307.69 kHz.
+          // This blocking is safe in the context of a 10kHz stepper driver interrupt
+          // or a 115200 Bd serial line receive interrupt, which will not trigger faster than 12kHz.
+          cli();
+          // Reset the command to something, which will be ignored by the power panic routine,
+          // so this buffer length will not be counted twice.
+          *ptr ++ = CMDBUFFER_CURRENT_TYPE_TO_BE_REMOVED;
+          // Extract the current buffer length.
+          sdlen.lohi.lo = *ptr ++;
+          sdlen.lohi.hi = *ptr;
+          // and pass it to the planner queue.
+          planner_add_sd_length(sdlen.value);
+          sei();
         }
-	      cmdqueue_pop_front();
-		    planner_add_sd_length(sdlen.value);
-		    sei();
-	  }
+      }
+      // Now it is safe to release the already processed command block. If interrupted by the power panic now,
+      // this block's SD card length will not be counted twice as its command type has been replaced 
+      // by CMDBUFFER_CURRENT_TYPE_TO_BE_REMOVED.
+      cmdqueue_pop_front();
+    }
 	host_keepalive();
   }
 }
@@ -1987,11 +2007,14 @@ void force_high_power_mode(bool start_high_power_section) {
 	if (silent == 1) {
 		//we are in silent mode, set to normal mode to enable crash detection
 
-
+    // Wait for the planner queue to drain and for the stepper timer routine to reach an idle state.
 		st_synchronize();
 		cli();
 		tmc2130_mode = (start_high_power_section == true) ? TMC2130_MODE_NORMAL : TMC2130_MODE_SILENT;
 		tmc2130_init();
+    // We may have missed a stepper timer interrupt due to the time spent in the tmc2130_init() routine.
+    // Be safe than sorry, reset the stepper timer before re-enabling interrupts.
+    st_reset_timer();
 		sei();
 		digipot_init();
 	}
@@ -7555,6 +7578,8 @@ void setup_fan_interrupt() {
 	EIMSK |= (1 << 7);
 }
 
+// The fan interrupt is triggered at maximum 325Hz (may be a bit more due to component tollerances),
+// and it takes 4.24 us to process (the interrupt invocation overhead not taken into account).
 ISR(INT7_vect) {
 	//measuring speed now works for fanSpeed > 18 (approximately), which is sufficient because MIN_PRINT_FAN_SPEED is higher
 
@@ -7912,6 +7937,8 @@ void stop_and_save_print_to_ram(float z_move, float e_move)
 	card.sdprinting = false;
 //	card.closefile();
 	saved_printing = true;
+  // We may have missed a stepper timer interrupt. Be safe than sorry, reset the stepper timer before re-enabling interrupts.
+  st_reset_timer();
 	sei();
 	if ((z_move != 0) || (e_move != 0)) { // extruder or z move
 #if 1
diff --git a/Firmware/cmdqueue.cpp b/Firmware/cmdqueue.cpp
index 771daafec..bdf4c3abf 100644
--- a/Firmware/cmdqueue.cpp
+++ b/Firmware/cmdqueue.cpp
@@ -627,6 +627,10 @@ void get_command()
       sd_count.value = 0;
 
       cli();
+      // This block locks the interrupts globally for 3.56 us,
+      // which corresponds to a maximum repeat frequency of 280.70 kHz.
+      // This blocking is safe in the context of a 10kHz stepper driver interrupt
+      // or a 115200 Bd serial line receive interrupt, which will not trigger faster than 12kHz.
       ++ buflen;
       bufindw += len;
       sdpos_atomic = card.get_sdpos()+1;
diff --git a/Firmware/cmdqueue.h b/Firmware/cmdqueue.h
index 2c6f93b75..dad81c94c 100644
--- a/Firmware/cmdqueue.h
+++ b/Firmware/cmdqueue.h
@@ -18,6 +18,10 @@
 #define CMDBUFFER_CURRENT_TYPE_UI       3
 // Command in cmdbuffer was generated by another G-code.
 #define CMDBUFFER_CURRENT_TYPE_CHAINED  4
+// Command has been processed and its SD card length has been possibly pushed 
+// to the planner queue, but not yet removed from the cmdqueue. 
+// This is a temporary state to reduce stepper interrupt locking time.
+#define CMDBUFFER_CURRENT_TYPE_TO_BE_REMOVED 5
 
 // How much space to reserve for the chained commands
 // of type CMDBUFFER_CURRENT_TYPE_CHAINED,
diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp
index 999e7b18d..a08aacc05 100644
--- a/Firmware/planner.cpp
+++ b/Firmware/planner.cpp
@@ -264,6 +264,10 @@ void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit
   }
 
   CRITICAL_SECTION_START;  // Fill variables used by the stepper in a critical section
+  // This block locks the interrupts globally for 4.38 us,
+  // which corresponds to a maximum repeat frequency of 228.57 kHz.
+  // This blocking is safe in the context of a 10kHz stepper driver interrupt
+  // or a 115200 Bd serial line receive interrupt, which will not trigger faster than 12kHz.
   if (! block->busy) { // Don't update variables if block is busy.
     block->accelerate_until = accelerate_steps;
     block->decelerate_after = accelerate_steps+plateau_steps;
@@ -561,8 +565,7 @@ extern volatile uint32_t step_events_completed; // The number of step events exe
 void planner_abort_hard()
 {
     // Abort the stepper routine and flush the planner queue.
-    // DISABLE_STEPPER_DRIVER_INTERRUPT
-    TIMSK1 &= ~(1<<OCIE1A);
+    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.
@@ -608,7 +611,7 @@ void planner_abort_hard()
 #endif
     }
 #endif
-    // Clear the planner queue.
+    // Clear the planner queue, reset and re-enable the stepper timer.
     quickStop();
 
     // Apply inverse world correction matrix.
@@ -1292,7 +1295,12 @@ 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();
+
+  // The stepper timer interrupt will run continuously from now on.
+  // If there are no planner blocks to be executed by the stepper routine,
+  // the stepper interrupt ticks at 1kHz to wake up and pick a block
+  // from the planner queue if available.
+  ENABLE_STEPPER_DRIVER_INTERRUPT();
 }
 
 #ifdef ENABLE_AUTO_BED_LEVELING
diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp
index 8ea867de4..61a2f7f63 100644
--- a/Firmware/stepper.cpp
+++ b/Firmware/stepper.cpp
@@ -221,10 +221,6 @@ void MultiU24X24toH16(uint16_t& intRes, int32_t& longIn1, long& longIn2)
 
 // 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) {
@@ -307,17 +303,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();
-}
-
-void step_wait(){
-    for(int8_t i=0; i < 6; i++){
-    }
-}
-
-
 FORCE_INLINE unsigned short calc_timer(uint16_t step_rate) {
   unsigned short timer;
   if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY;
@@ -858,6 +843,11 @@ void isr() {
   if (OCR1A < TCNT1) {
     stepper_timer_overflow_state = true;
     WRITE_NC(BEEPER, HIGH);
+    SERIAL_PROTOCOLPGM("Stepper timer overflow ");
+    SERIAL_PROTOCOL(OCR1A);
+    SERIAL_PROTOCOLPGM("<");
+    SERIAL_PROTOCOL(TCNT1);
+    SERIAL_PROTOCOLLN("!");
   }
 #endif
 }
@@ -1141,6 +1131,7 @@ void st_init()
   // create_speed_lookuptable.py
   TCCR1B = (TCCR1B & ~(0x07<<CS10)) | (2<<CS10);
 
+  // Plan the first interrupt after 8ms from now.
   OCR1A = 0x4000;
   TCNT1 = 0;
   ENABLE_STEPPER_DRIVER_INTERRUPT();
@@ -1180,6 +1171,11 @@ void st_synchronize()
 void st_set_position(const long &x, const long &y, const long &z, const long &e)
 {
   CRITICAL_SECTION_START;
+  // Copy 4x4B.
+  // This block locks the interrupts globally for 4.56 us,
+  // which corresponds to a maximum repeat frequency of 219.18 kHz.
+  // This blocking is safe in the context of a 10kHz stepper driver interrupt
+  // or a 115200 Bd serial line receive interrupt, which will not trigger faster than 12kHz.
   count_position[X_AXIS] = x;
   count_position[Y_AXIS] = y;
   count_position[Z_AXIS] = z;
@@ -1234,6 +1230,7 @@ void quickStop()
   DISABLE_STEPPER_DRIVER_INTERRUPT();
   while (blocks_queued()) plan_discard_current_block(); 
   current_block = NULL;
+  st_reset_timer();
   ENABLE_STEPPER_DRIVER_INTERRUPT();
 }
 
diff --git a/Firmware/stepper.h b/Firmware/stepper.h
index 5ca68a391..1295faadd 100644
--- a/Firmware/stepper.h
+++ b/Firmware/stepper.h
@@ -23,6 +23,9 @@
 
 #include "planner.h"
 
+#define ENABLE_STEPPER_DRIVER_INTERRUPT()  TIMSK1 |= (1<<OCIE1A)
+#define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~(1<<OCIE1A)
+
 #ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED
 extern bool abort_on_endstop_hit;
 #endif
@@ -57,11 +60,18 @@ void st_get_position_xy(long &x, long &y);
 float st_get_position_mm(uint8_t axis);
 
 
-// The stepper subsystem goes to sleep when it runs out of things to execute. Call this
-// to notify the subsystem that it is time to go to work.
-void st_wake_up();
+// Call this function just before re-enabling the stepper driver interrupt and the global interrupts
+// to avoid a stepper timer overflow.
+FORCE_INLINE void st_reset_timer()
+{
+  // Clear a possible pending interrupt on OCR1A overflow.
+  TIFR1 |= 1 << OCF1A;
+  // Reset the counter.
+  TCNT1 = 0;
+  // Wake up after 1ms from now.
+  OCR1A = 2000;
+}
 
-  
 void checkHitEndstops(); //call from somewhere to create an serial error message with the locations the endstops where hit, in case they were triggered
 bool endstops_hit_on_purpose(); //avoid creation of the message, i.e. after homing and before a routine call of checkHitEndstops();
 bool endstop_z_hit_on_purpose();
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index ff0cc3373..59614a0c1 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -3367,6 +3367,8 @@ static void lcd_silent_mode_set() {
   SilentModeMenu = !SilentModeMenu;
   eeprom_update_byte((unsigned char *)EEPROM_SILENT, SilentModeMenu);
 #ifdef TMC2130
+  // Wait until the planner queue is drained and the stepper routine achieves
+  // an idle state.
   st_synchronize();
   if (tmc2130_wait_standstill_xy(1000)) {}
 //	  MYSERIAL.print("standstill OK");
@@ -3375,6 +3377,9 @@ static void lcd_silent_mode_set() {
   cli();
 	tmc2130_mode = SilentModeMenu?TMC2130_MODE_SILENT:TMC2130_MODE_NORMAL;
 	tmc2130_init();
+  // We may have missed a stepper timer interrupt due to the time spent in tmc2130_init.
+  // Be safe than sorry, reset the stepper timer before re-enabling interrupts.
+  st_reset_timer();
   sei();
 #endif //TMC2130
   digipot_init();