From 8c7ee4599e01be99cef45e4a900bfd8e74c75219 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Tue, 30 Aug 2016 14:22:42 -0500
Subject: [PATCH] Optimize stepper advance_isr

---
 Marlin/stepper.cpp | 51 ++++++++++++++++++++++++++++++++--------------
 1 file changed, 36 insertions(+), 15 deletions(-)

diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index 1beeb2ce6..a604c9271 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -683,29 +683,50 @@ void Stepper::isr() {
     old_OCR0A += eISR_Rate;
     OCR0A = old_OCR0A;
 
-    #define STEP_E_ONCE(INDEX) \
-      if (e_steps[INDEX] != 0) { \
-        E## INDEX ##_STEP_WRITE(INVERT_E_STEP_PIN); \
-        if (e_steps[INDEX] < 0) { \
-          E## INDEX ##_DIR_WRITE(INVERT_E## INDEX ##_DIR); \
-          e_steps[INDEX]++; \
-        } \
-        else { \
-          E## INDEX ##_DIR_WRITE(!INVERT_E## INDEX ##_DIR); \
-          e_steps[INDEX]--; \
-        } \
+    #define SET_E_STEP_DIR(INDEX) \
+      E## INDEX ##_DIR_WRITE(e_steps[INDEX] <= 0 ? INVERT_E## INDEX ##_DIR : !INVERT_E## INDEX ##_DIR)
+
+    #define START_E_PULSE(INDEX) \
+      if (e_steps[INDEX]) E## INDEX ##_STEP_WRITE(INVERT_E_STEP_PIN)
+
+    #define STOP_E_PULSE(INDEX) \
+      if (e_steps[INDEX]) { \
+        e_steps[INDEX] < 0 ? ++e_steps[INDEX] : --e_steps[INDEX]; \
         E## INDEX ##_STEP_WRITE(!INVERT_E_STEP_PIN); \
       }
 
+    SET_E_STEP_DIR(0);
+    #if E_STEPPERS > 1
+      SET_E_STEP_DIR(1);
+      #if E_STEPPERS > 2
+        SET_E_STEP_DIR(2);
+        #if E_STEPPERS > 3
+          SET_E_STEP_DIR(3);
+        #endif
+      #endif
+    #endif
+
     // Step all E steppers that have steps
     for (uint8_t i = 0; i < step_loops; i++) {
-      STEP_E_ONCE(0);
+
+      START_E_PULSE(0);
       #if E_STEPPERS > 1
-        STEP_E_ONCE(1);
+        START_E_PULSE(1);
         #if E_STEPPERS > 2
-          STEP_E_ONCE(2);
+          START_E_PULSE(2);
           #if E_STEPPERS > 3
-            STEP_E_ONCE(3);
+            START_E_PULSE(3);
+          #endif
+        #endif
+      #endif
+
+      STOP_E_PULSE(0);
+      #if E_STEPPERS > 1
+        STOP_E_PULSE(1);
+        #if E_STEPPERS > 2
+          STOP_E_PULSE(2);
+          #if E_STEPPERS > 3
+            STOP_E_PULSE(3);
           #endif
         #endif
       #endif