From 9e534c19905b47868ec10cb9037e7ed8d970857e Mon Sep 17 00:00:00 2001
From: bubnikv <bubnikv@gmail.com>
Date: Sat, 20 Jan 2018 14:37:22 +0100
Subject: [PATCH] Minimize risk of stepper routine interrupt blocking by
 reorganizing the routine to move the G-code line length from the command
 queue to the planner queue.

---
 Firmware/Marlin_main.cpp | 36 +++++++++++++++++++++++++++---------
 Firmware/cmdqueue.h      |  4 ++++
 2 files changed, 31 insertions(+), 9 deletions(-)

diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index c2a27fbb2..91cabb901 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -1409,8 +1409,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;
@@ -1419,14 +1423,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();
   }
 }
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,