Merge pull request #10688 from ejtagle/bugfix-2.0.x

[2.0.x] Refactor, optimization of core planner/stepper/endstops logic

Marlin/src/HAL/HAL_AVR/HAL.h

@@ -162,24 +162,148 @@ extern "C" {
  * (otherwise, characters will be lost due to UART overflow).
  * Then: Stepper, Endstops, Temperature, and -finally- all others.
  */
-#define HAL_timer_isr_prologue_0 do{ DISABLE_TEMPERATURE_INTERRUPT(); sei(); }while(0)
+#define HAL_timer_isr_prologue(TIMER_NUM)
-#define HAL_timer_isr_epilogue_0 do{ cli(); ENABLE_TEMPERATURE_INTERRUPT(); }while(0)
+#define HAL_timer_isr_epilogue(TIMER_NUM)
 
-#define HAL_timer_isr_prologue_1 \
+/* 18 cycles maximum latency */
-  const bool temp_isr_was_enabled = TEMPERATURE_ISR_ENABLED(); \
+#define HAL_STEP_TIMER_ISR \
-  do{ \
+extern "C" void TIMER1_COMPA_vect (void) __attribute__ ((signal, naked, used, externally_visible)); \
-    DISABLE_TEMPERATURE_INTERRUPT(); \
+extern "C" void TIMER1_COMPA_vect_bottom (void) asm ("TIMER1_COMPA_vect_bottom") __attribute__ ((used, externally_visible, noinline)); \
-    DISABLE_STEPPER_DRIVER_INTERRUPT(); \
+void TIMER1_COMPA_vect (void) { \
-    sei(); \
+  __asm__ __volatile__ ( \
-  }while(0)
+    A("push r16")                      /* 2 Save R16 */ \
+    A("in r16, __SREG__")              /* 1 Get SREG */ \
+    A("push r16")                      /* 2 Save SREG into stack */ \
+    A("lds r16, %[timsk0]")            /* 2 Load into R0 the Temperature timer Interrupt mask register */ \
+    A("push r16")                      /* 2 Save TIMSK0 into the stack */ \
+    A("andi r16,~%[msk0]")             /* 1 Disable the temperature ISR */ \
+    A("sts %[timsk0], r16")            /* 2 And set the new value */ \
+    A("lds r16, %[timsk1]")            /* 2 Load into R0 the stepper timer Interrupt mask register [TIMSK1] */ \
+    A("andi r16,~%[msk1]")             /* 1 Disable the stepper ISR */ \
+    A("sts %[timsk1], r16")            /* 2 And set the new value */ \
+    A("sei")                           /* 1 Enable global interrupts - stepper and temperature ISRs are disabled, so no risk of reentry or being preempted by the temperature ISR */    \
+    A("push r16")                      /* 2 Save TIMSK1 into stack */ \
+    A("in r16, 0x3B")                  /* 1 Get RAMPZ register */ \
+    A("push r16")                      /* 2 Save RAMPZ into stack */ \
+    A("in r16, 0x3C")                  /* 1 Get EIND register */ \
+    A("push r0")                       /* C runtime can modify all the following registers without restoring them */ \
+    A("push r1")                       \
+    A("push r18")                      \
+    A("push r19")                      \
+    A("push r20")                      \
+    A("push r21")                      \
+    A("push r22")                      \
+    A("push r23")                      \
+    A("push r24")                      \
+    A("push r25")                      \
+    A("push r26")                      \
+    A("push r27")                      \
+    A("push r30")                      \
+    A("push r31")                      \
+    A("clr r1")                        /* C runtime expects this register to be 0 */ \
+    A("call TIMER1_COMPA_vect_bottom") /* Call the bottom handler - No inlining allowed, otherwise registers used are not saved */   \
+    A("pop r31")                       \
+    A("pop r30")                       \
+    A("pop r27")                       \
+    A("pop r26")                       \
+    A("pop r25")                       \
+    A("pop r24")                       \
+    A("pop r23")                       \
+    A("pop r22")                       \
+    A("pop r21")                       \
+    A("pop r20")                       \
+    A("pop r19")                       \
+    A("pop r18")                       \
+    A("pop r1")                        \
+    A("pop r0")                        \
+    A("out 0x3C, r16")                 /* 1 Restore EIND register */ \
+    A("pop r16")                       /* 2 Get the original RAMPZ register value */ \
+    A("out 0x3B, r16")                 /* 1 Restore RAMPZ register to its original value */ \
+    A("pop r16")                       /* 2 Get the original TIMSK1 value but with stepper ISR disabled */ \
+    A("ori r16,%[msk1]")               /* 1 Reenable the stepper ISR */ \
+    A("cli")                           /* 1 Disable global interrupts - Reenabling Stepper ISR can reenter amd temperature can reenter, and we want that, if it happens, after this ISR has ended */ \
+    A("sts %[timsk1], r16")            /* 2 And restore the old value - This reenables the stepper ISR */ \
+    A("pop r16")                       /* 2 Get the temperature timer Interrupt mask register [TIMSK0] */ \
+    A("sts %[timsk0], r16")            /* 2 And restore the old value - This reenables the temperature ISR */ \
+    A("pop r16")                       /* 2 Get the old SREG value */ \
+    A("out __SREG__, r16")             /* 1 And restore the SREG value */ \
+    A("pop r16")                       /* 2 Restore R16 value */ \
+    A("reti")                          /* 4 Return from interrupt */ \
+    :                                   \
+    : [timsk0] "i" ((uint16_t)&TIMSK0), \
+      [timsk1] "i" ((uint16_t)&TIMSK1), \
+      [msk0] "M" ((uint8_t)(1<<OCIE0B)),\
+      [msk1] "M" ((uint8_t)(1<<OCIE1A)) \
+    : \
+  ); \
+} \
+void TIMER1_COMPA_vect_bottom(void)
 
-#define HAL_timer_isr_epilogue_1 do{ cli(); ENABLE_STEPPER_DRIVER_INTERRUPT(); if (temp_isr_was_enabled) ENABLE_TEMPERATURE_INTERRUPT(); }while(0)
+/* 14 cycles maximum latency */
-
+#define HAL_TEMP_TIMER_ISR \
-#define HAL_timer_isr_prologue(TIMER_NUM) _CAT(HAL_timer_isr_prologue_, TIMER_NUM)
+extern "C" void TIMER0_COMPB_vect (void) __attribute__ ((signal, naked, used, externally_visible)); \
-#define HAL_timer_isr_epilogue(TIMER_NUM) _CAT(HAL_timer_isr_epilogue_, TIMER_NUM)
+extern "C" void TIMER0_COMPB_vect_bottom(void)  asm ("TIMER0_COMPB_vect_bottom") __attribute__ ((used, externally_visible, noinline)); \
-
+void TIMER0_COMPB_vect (void) { \
-#define HAL_STEP_TIMER_ISR ISR(TIMER1_COMPA_vect)
+  __asm__ __volatile__ ( \
-#define HAL_TEMP_TIMER_ISR ISR(TIMER0_COMPB_vect)
+    A("push r16")                       /* 2 Save R16 */ \
+    A("in r16, __SREG__")               /* 1 Get SREG */ \
+    A("push r16")                       /* 2 Save SREG into stack */ \
+    A("lds r16, %[timsk0]")             /* 2 Load into R0 the Temperature timer Interrupt mask register */ \
+    A("andi r16,~%[msk0]")              /* 1 Disable the temperature ISR */ \
+    A("sts %[timsk0], r16")             /* 2 And set the new value */ \
+    A("sei")                            /* 1 Enable global interrupts - It is safe, as the temperature ISR is disabled, so we cannot reenter it */    \
+    A("push r16")                       /* 2 Save TIMSK0 into stack */ \
+    A("in r16, 0x3B")                   /* 1 Get RAMPZ register */ \
+    A("push r16")                       /* 2 Save RAMPZ into stack */ \
+    A("in r16, 0x3C")                   /* 1 Get EIND register */ \
+    A("push r0")                        /* C runtime can modify all the following registers without restoring them */ \
+    A("push r1")                        \
+    A("push r18")                       \
+    A("push r19")                       \
+    A("push r20")                       \
+    A("push r21")                       \
+    A("push r22")                       \
+    A("push r23")                       \
+    A("push r24")                       \
+    A("push r25")                       \
+    A("push r26")                       \
+    A("push r27")                       \
+    A("push r30")                       \
+    A("push r31")                       \
+    A("clr r1")                         /* C runtime expects this register to be 0 */ \
+    A("call TIMER0_COMPB_vect_bottom")  /* Call the bottom handler - No inlining allowed, otherwise registers used are not saved */   \
+    A("pop r31")                        \
+    A("pop r30")                        \
+    A("pop r27")                        \
+    A("pop r26")                        \
+    A("pop r25")                        \
+    A("pop r24")                        \
+    A("pop r23")                        \
+    A("pop r22")                        \
+    A("pop r21")                        \
+    A("pop r20")                        \
+    A("pop r19")                        \
+    A("pop r18")                        \
+    A("pop r1")                         \
+    A("pop r0")                         \
+    A("out 0x3C, r16")                  /* 1 Restore EIND register */ \
+    A("pop r16")                        /* 2 Get the original RAMPZ register value */ \
+    A("out 0x3B, r16")                  /* 1 Restore RAMPZ register to its original value */ \
+    A("pop r16")                        /* 2 Get the original TIMSK0 value but with temperature ISR disabled */ \
+    A("ori r16,%[msk0]")                /* 1 Enable temperature ISR */ \
+    A("cli")                            /* 1 Disable global interrupts - We must do this, as we will reenable the temperature ISR, and we don´t want to reenter this handler until the current one is done */ \
+    A("sts %[timsk0], r16")             /* 2 And restore the old value */ \
+    A("pop r16")                        /* 2 Get the old SREG */ \
+    A("out __SREG__, r16")              /* 1 And restore the SREG value */ \
+    A("pop r16")                        /* 2 Restore R16 */ \
+    A("reti")                           /* 4 Return from interrupt */ \
+    :                                   \
+    : [timsk0] "i"((uint16_t)&TIMSK0),  \
+      [msk0] "M" ((uint8_t)(1<<OCIE0B)) \
+    : \
+  ); \
+} \
+void TIMER0_COMPB_vect_bottom(void)
 
 // ADC
 #ifdef DIDR2

Marlin/src/HAL/HAL_AVR/endstop_interrupts.h

@@ -24,7 +24,7 @@
  * Endstop Interrupts
  *
  * Without endstop interrupts the endstop pins must be polled continually in
- * the stepper-ISR via endstops.update(), most of the time finding no change.
+ * the temperature-ISR via endstops.update(), most of the time finding no change.
  * With this feature endstops.update() is called only when we know that at
  * least one endstop has changed state, saving valuable CPU cycles.
  *
@@ -40,17 +40,10 @@
 
 #include "../../core/macros.h"
 #include <stdint.h>
-
+#include "../../module/endstops.h"
-volatile uint8_t e_hit = 0; // Different from 0 when the endstops should be tested in detail.
-                            // Must be reset to 0 by the test function when finished.
-
-// This is what is really done inside the interrupts.
-FORCE_INLINE void endstop_ISR_worker( void ) {
-  e_hit = 2; // Because the detection of a e-stop hit has a 1 step debouncer it has to be called at least twice.
-}
 
 // One ISR for all EXT-Interrupts
-void endstop_ISR(void) { endstop_ISR_worker(); }
+void endstop_ISR(void) { endstops.check_possible_change(); }
 
 /**
  * Patch for pins_arduino.h (...\Arduino\hardware\arduino\avr\variants\mega\pins_arduino.h)
@@ -95,19 +88,19 @@ void pciSetup(const int8_t pin) {
 
 // Handlers for pin change interrupts
 #ifdef PCINT0_vect
-  ISR(PCINT0_vect) { endstop_ISR_worker(); }
+  ISR(PCINT0_vect) { endstop_ISR(); }
 #endif
 
 #ifdef PCINT1_vect
-  ISR(PCINT1_vect) { endstop_ISR_worker(); }
+  ISR(PCINT1_vect) { endstop_ISR(); }
 #endif
 
 #ifdef PCINT2_vect
-  ISR(PCINT2_vect) { endstop_ISR_worker(); }
+  ISR(PCINT2_vect) { endstop_ISR(); }
 #endif
 
 #ifdef PCINT3_vect
-  ISR(PCINT3_vect) { endstop_ISR_worker(); }
+  ISR(PCINT3_vect) { endstop_ISR(); }
 #endif
 
 void setup_endstop_interrupts( void ) {

Marlin/src/HAL/HAL_DUE/DebugMonitor_Due.cpp

@@ -46,6 +46,11 @@ static void TXBegin(void) {
   // Disable UART interrupt in NVIC
   NVIC_DisableIRQ( UART_IRQn );
 
+  // We NEED memory barriers to ensure Interrupts are actually disabled!
+  // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
+  __DSB();
+  __ISB();
+
   // Disable clock
   pmc_disable_periph_clk( ID_UART );
 

Marlin/src/HAL/HAL_DUE/HAL_timers_Due.cpp

@@ -99,6 +99,11 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) {
   // Disable interrupt, just in case it was already enabled
   NVIC_DisableIRQ(irq);
 
+  // We NEED memory barriers to ensure Interrupts are actually disabled!
+  // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
+  __DSB();
+  __ISB();
+
   // Disable timer interrupt
   tc->TC_CHANNEL[channel].TC_IDR = TC_IDR_CPCS;
 
@@ -126,18 +131,28 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) {
 }
 
 void HAL_timer_enable_interrupt(const uint8_t timer_num) {
-  const tTimerConfig * const pConfig = &TimerConfig[timer_num];
+  IRQn_Type irq = TimerConfig[timer_num].IRQ_Id;
-  pConfig->pTimerRegs->TC_CHANNEL[pConfig->channel].TC_IER = TC_IER_CPCS;
+  NVIC_EnableIRQ(irq);
 }
 
 void HAL_timer_disable_interrupt(const uint8_t timer_num) {
-  const tTimerConfig * const pConfig = &TimerConfig[timer_num];
+  IRQn_Type irq = TimerConfig[timer_num].IRQ_Id;
-  pConfig->pTimerRegs->TC_CHANNEL[pConfig->channel].TC_IDR = TC_IDR_CPCS;
+  NVIC_DisableIRQ(irq);
+
+  // We NEED memory barriers to ensure Interrupts are actually disabled!
+  // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
+  __DSB();
+  __ISB();
+}
+
+// missing from CMSIS: Check if interrupt is enabled or not
+static bool NVIC_GetEnabledIRQ(IRQn_Type IRQn) {
+  return (NVIC->ISER[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F))) != 0;
 }
 
 bool HAL_timer_interrupt_enabled(const uint8_t timer_num) {
-  const tTimerConfig * const pConfig = &TimerConfig[timer_num];
+  IRQn_Type irq = TimerConfig[timer_num].IRQ_Id;
-  return (pConfig->pTimerRegs->TC_CHANNEL[pConfig->channel].TC_IMR & TC_IMR_CPCS) != 0;
+  return NVIC_GetEnabledIRQ(irq);
 }
 
 #endif // ARDUINO_ARCH_SAM

Marlin/src/HAL/HAL_DUE/HAL_timers_Due.h

@@ -118,8 +118,6 @@ void HAL_timer_enable_interrupt(const uint8_t timer_num);
 void HAL_timer_disable_interrupt(const uint8_t timer_num);
 bool HAL_timer_interrupt_enabled(const uint8_t timer_num);
 
-//void HAL_timer_isr_prologue(const uint8_t timer_num);
-
 FORCE_INLINE static void HAL_timer_isr_prologue(const uint8_t timer_num) {
   const tTimerConfig * const pConfig = &TimerConfig[timer_num];
   // Reading the status register clears the interrupt flag

Marlin/src/HAL/HAL_DUE/MarlinSerial_Due.cpp

@@ -245,6 +245,11 @@
     // Disable UART interrupt in NVIC
     NVIC_DisableIRQ( HWUART_IRQ );
 
+    // We NEED memory barriers to ensure Interrupts are actually disabled!
+    // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
+    __DSB();
+    __ISB();
+
     // Disable clock
     pmc_disable_periph_clk( HWUART_IRQ_ID );
 
@@ -290,6 +295,11 @@
     // Disable UART interrupt in NVIC
     NVIC_DisableIRQ( HWUART_IRQ );
 
+    // We NEED memory barriers to ensure Interrupts are actually disabled!
+    // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
+    __DSB();
+    __ISB();
+
     pmc_disable_periph_clk( HWUART_IRQ_ID );
   }
 

Marlin/src/HAL/HAL_DUE/endstop_interrupts.h

@@ -24,7 +24,7 @@
  * Endstop Interrupts
  *
  * Without endstop interrupts the endstop pins must be polled continually in
- * the stepper-ISR via endstops.update(), most of the time finding no change.
+ * the temperature-ISR via endstops.update(), most of the time finding no change.
  * With this feature endstops.update() is called only when we know that at
  * least one endstop has changed state, saving valuable CPU cycles.
  *
@@ -37,16 +37,10 @@
 #ifndef _ENDSTOP_INTERRUPTS_H_
 #define _ENDSTOP_INTERRUPTS_H_
 
-volatile uint8_t e_hit = 0; // Different from 0 when the endstops should be tested in detail.
+#include "../../module/endstops.h"
-                            // Must be reset to 0 by the test function when finished.
-
-// This is what is really done inside the interrupts.
-FORCE_INLINE void endstop_ISR_worker( void ) {
-  e_hit = 2; // Because the detection of a e-stop hit has a 1 step debouncer it has to be called at least twice.
-}
 
 // One ISR for all EXT-Interrupts
-void endstop_ISR(void) { endstop_ISR_worker(); }
+void endstop_ISR(void) { endstops.check_possible_change(); }
 
 /**
  *  Endstop interrupts for Due based targets.

Marlin/src/HAL/HAL_DUE/watchdog_Due.cpp

@@ -68,6 +68,11 @@ void watchdogSetup(void) {
       // Disable WDT interrupt (just in case, to avoid triggering it!)
       NVIC_DisableIRQ(WDT_IRQn);
 
+      // We NEED memory barriers to ensure Interrupts are actually disabled!
+      // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
+      __DSB();
+      __ISB();
+
       // Initialize WDT with the given parameters
       WDT_Enable(WDT, value);
 

Marlin/src/HAL/HAL_LPC1768/HAL_timers.cpp

@@ -23,7 +23,7 @@
 /**
  * Description:
  *
- * For TARGET_LPC1768
+ * Timers for LPC1768
  */
 
 #ifdef TARGET_LPC1768
@@ -32,62 +32,35 @@
 #include "HAL_timers.h"
 
 void HAL_timer_init(void) {
-  SBI(LPC_SC->PCONP, 1);  // power on timer0
+  SBI(LPC_SC->PCONP, SBIT_TIMER0);  // Power ON Timer 0
   LPC_TIM0->PR = (HAL_TIMER_RATE) / (HAL_STEPPER_TIMER_RATE) - 1; // Use prescaler to set frequency if needed
 
-  SBI(LPC_SC->PCONP, 2);  // power on timer1
+  SBI(LPC_SC->PCONP, SBIT_TIMER1);  // Power ON Timer 1
   LPC_TIM1->PR = (HAL_TIMER_RATE) / 1000000 - 1;
 }
 
 void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) {
   switch (timer_num) {
     case 0:
-      LPC_TIM0->MCR = 3;              // Match on MR0, reset on MR0
+      LPC_TIM0->MCR = _BV(SBIT_MR0I) | _BV(SBIT_MR0R); // Match on MR0, reset on MR0, interrupts when NVIC enables them
       LPC_TIM0->MR0 = uint32_t(HAL_STEPPER_TIMER_RATE) / frequency; // Match value (period) to set frequency
-      LPC_TIM0->TCR = _BV(0);       // enable
+      LPC_TIM0->TCR = _BV(SBIT_CNTEN); // Counter Enable
+
+      NVIC_SetPriority(TIMER0_IRQn, NVIC_EncodePriority(0, 1, 0));
+      NVIC_EnableIRQ(TIMER0_IRQn);
       break;
+
     case 1:
-      LPC_TIM1->MCR = 3;
+      LPC_TIM1->MCR = _BV(SBIT_MR0I) | _BV(SBIT_MR0R); // Match on MR0, reset on MR0, interrupts when NVIC enables them
       LPC_TIM1->MR0 = uint32_t(HAL_TEMP_TIMER_RATE) / frequency;
-      LPC_TIM1->TCR = _BV(0);
+      LPC_TIM1->TCR = _BV(SBIT_CNTEN); // Counter Enable
+
+      NVIC_SetPriority(TIMER1_IRQn, NVIC_EncodePriority(0, 2, 0));
+      NVIC_EnableIRQ(TIMER1_IRQn);
       break;
+
     default: break;
   }
 }
 
-void HAL_timer_enable_interrupt(const uint8_t timer_num) {
-  switch (timer_num) {
-    case 0:
-      NVIC_EnableIRQ(TIMER0_IRQn); // Enable interrupt handler
-      NVIC_SetPriority(TIMER0_IRQn, NVIC_EncodePriority(0, 1, 0));
-      break;
-    case 1:
-      NVIC_EnableIRQ(TIMER1_IRQn);
-      NVIC_SetPriority(TIMER1_IRQn, NVIC_EncodePriority(0, 2, 0));
-      break;
-  }
-}
-
-void HAL_timer_disable_interrupt(const uint8_t timer_num) {
-  switch (timer_num) {
-    case 0: NVIC_DisableIRQ(TIMER0_IRQn); break; // disable interrupt handler
-    case 1: NVIC_DisableIRQ(TIMER1_IRQn); break;
-  }
-}
-
-bool HAL_timer_interrupt_enabled(const uint8_t timer_num) {
-  switch (timer_num) {
-    case 0: return NVIC_GetActive(TIMER0_IRQn);
-    case 1: return NVIC_GetActive(TIMER1_IRQn);
-  }
-  return false;
-}
-
-void HAL_timer_isr_prologue(const uint8_t timer_num) {
-  switch (timer_num) {
-    case 0: SBI(LPC_TIM0->IR, 0); break; // Clear the Interrupt
-    case 1: SBI(LPC_TIM1->IR, 0); break;
-  }
-}
-
 #endif // TARGET_LPC1768

Marlin/src/HAL/HAL_LPC1768/HAL_timers.h

@@ -34,18 +34,42 @@
 
 #include <stdint.h>
 
+#include "../../core/macros.h"
+
+#define SBIT_TIMER0 1
+#define SBIT_TIMER1 2
+
+#define SBIT_CNTEN 0
+
+#define SBIT_MR0I  0 // Timer 0 Interrupt when TC matches MR0
+#define SBIT_MR0R  1 // Timer 0 Reset TC on Match
+#define SBIT_MR0S  2 // Timer 0 Stop TC and PC on Match
+#define SBIT_MR1I  3
+#define SBIT_MR1R  4
+#define SBIT_MR1S  5
+#define SBIT_MR2I  6
+#define SBIT_MR2R  7
+#define SBIT_MR2S  8
+#define SBIT_MR3I  9
+#define SBIT_MR3R 10
+#define SBIT_MR3S 11
+
 // --------------------------------------------------------------------------
 // Defines
 // --------------------------------------------------------------------------
 
-#define FORCE_INLINE __attribute__((always_inline)) inline
+#define _HAL_TIMER(T) _CAT(LPC_TIM, T)
+#define _HAL_TIMER_IRQ(T) TIMER##T##_IRQn
+#define __HAL_TIMER_ISR(T) extern "C" void TIMER##T##_IRQHandler(void)
+#define _HAL_TIMER_ISR(T)  __HAL_TIMER_ISR(T)
 
 typedef uint32_t hal_timer_t;
 #define HAL_TIMER_TYPE_MAX 0xFFFFFFFF
 
-#define STEP_TIMER_NUM 0  // index of timer to use for stepper
+#define STEP_TIMER_NUM 0  // Timer Index for Stepper
-#define TEMP_TIMER_NUM 1  // index of timer to use for temperature
+#define TEMP_TIMER_NUM 1  // Timer Index for Temperature
 #define PULSE_TIMER_NUM STEP_TIMER_NUM
+#define PWM_TIMER_NUM 3   // Timer Index for PWM
 
 #define HAL_TIMER_RATE         ((SystemCoreClock) / 4)  // frequency of timers peripherals
 #define HAL_STEPPER_TIMER_RATE HAL_TIMER_RATE   // frequency of stepper timer (HAL_TIMER_RATE / STEPPER_TIMER_PRESCALE)
@@ -66,21 +90,12 @@ typedef uint32_t hal_timer_t;
 #define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(TEMP_TIMER_NUM)
 #define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(TEMP_TIMER_NUM)
 
-#define HAL_STEP_TIMER_ISR  extern "C" void TIMER0_IRQHandler(void)
+#define HAL_STEP_TIMER_ISR _HAL_TIMER_ISR(STEP_TIMER_NUM)
-#define HAL_TEMP_TIMER_ISR  extern "C" void TIMER1_IRQHandler(void)
+#define HAL_TEMP_TIMER_ISR _HAL_TIMER_ISR(TEMP_TIMER_NUM)
 
-// PWM timer
+// Timer references by index
-#define HAL_PWM_TIMER      LPC_TIM3
+#define STEP_TIMER _HAL_TIMER(STEP_TIMER_NUM)
-#define HAL_PWM_TIMER_ISR  extern "C" void TIMER3_IRQHandler(void)
+#define TEMP_TIMER _HAL_TIMER(TEMP_TIMER_NUM)
-#define HAL_PWM_TIMER_IRQn TIMER3_IRQn
-
-// --------------------------------------------------------------------------
-// Types
-// --------------------------------------------------------------------------
-
-// --------------------------------------------------------------------------
-// Public Variables
-// --------------------------------------------------------------------------
 
 // --------------------------------------------------------------------------
 // Public functions
@@ -90,31 +105,23 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency);
 
 FORCE_INLINE static void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t compare) {
   switch (timer_num) {
-    case 0:
+    case 0: STEP_TIMER->MR0 = compare; break; // Stepper Timer Match Register 0
-      LPC_TIM0->MR0 = compare;
+    case 1: TEMP_TIMER->MR0 = compare; break; //    Temp Timer Match Register 0
-      if (LPC_TIM0->TC > compare)
-        LPC_TIM0->TC = compare - 5; // generate an immediate stepper ISR
-      break;
-    case 1:
-      LPC_TIM1->MR0 = compare;
-      if (LPC_TIM1->TC > compare)
-        LPC_TIM1->TC = compare - 5; // make sure we don't have one extra long period
-      break;
   }
 }
 
 FORCE_INLINE static hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) {
   switch (timer_num) {
-    case 0: return LPC_TIM0->MR0;
+    case 0: return STEP_TIMER->MR0; // Stepper Timer Match Register 0
-    case 1: return LPC_TIM1->MR0;
+    case 1: return TEMP_TIMER->MR0; //    Temp Timer Match Register 0
   }
   return 0;
 }
 
 FORCE_INLINE static hal_timer_t HAL_timer_get_count(const uint8_t timer_num) {
   switch (timer_num) {
-    case 0: return LPC_TIM0->TC;
+    case 0: return STEP_TIMER->TC; // Stepper Timer Count
-    case 1: return LPC_TIM1->TC;
+    case 1: return TEMP_TIMER->TC; //    Temp Timer Count
   }
   return 0;
 }
@@ -124,10 +131,45 @@ FORCE_INLINE static void HAL_timer_restrain(const uint8_t timer_num, const uint1
   if (HAL_timer_get_compare(timer_num) < mincmp) HAL_timer_set_compare(timer_num, mincmp);
 }
 
-void HAL_timer_enable_interrupt(const uint8_t timer_num);
+FORCE_INLINE static void HAL_timer_enable_interrupt(const uint8_t timer_num) {
-void HAL_timer_disable_interrupt(const uint8_t timer_num);
+  switch (timer_num) {
-bool HAL_timer_interrupt_enabled(const uint8_t timer_num);
+    case 0: NVIC_EnableIRQ(TIMER0_IRQn); // Enable interrupt handler
-void HAL_timer_isr_prologue(const uint8_t timer_num);
+    case 1: NVIC_EnableIRQ(TIMER1_IRQn); // Enable interrupt handler
+  }
+}
+
+FORCE_INLINE static void HAL_timer_disable_interrupt(const uint8_t timer_num) {
+  switch (timer_num) {
+    case 0: NVIC_DisableIRQ(TIMER0_IRQn); // Disable interrupt handler
+    case 1: NVIC_DisableIRQ(TIMER1_IRQn); // Disable interrupt handler
+  }
+
+  // We NEED memory barriers to ensure Interrupts are actually disabled!
+  // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
+  __DSB();
+  __ISB();
+}
+
+// This function is missing from CMSIS
+FORCE_INLINE static bool NVIC_GetEnableIRQ(IRQn_Type IRQn) {
+  return (NVIC->ISER[((uint32_t)IRQn) >> 5] & (1 << ((uint32_t)IRQn) & 0x1F)) != 0;
+}
+
+FORCE_INLINE static bool HAL_timer_interrupt_enabled(const uint8_t timer_num) {
+  switch (timer_num) {
+    case 0: return NVIC_GetEnableIRQ(TIMER0_IRQn); // Check if interrupt is enabled or not
+    case 1: return NVIC_GetEnableIRQ(TIMER1_IRQn); // Check if interrupt is enabled or not
+  }
+  return false;
+}
+
+FORCE_INLINE static void HAL_timer_isr_prologue(const uint8_t timer_num) {
+  switch (timer_num) {
+    case 0: SBI(STEP_TIMER->IR, SBIT_CNTEN); break;
+    case 1: SBI(TEMP_TIMER->IR, SBIT_CNTEN); break;
+  }
+}
+
 #define HAL_timer_isr_epilogue(TIMER_NUM)
 
-#endif // _HAL_TIMERS_DUE_H
+#endif // _HAL_TIMERS_H

Marlin/src/HAL/HAL_LPC1768/LPC1768_PWM.cpp

@@ -78,12 +78,14 @@
 
 #define NUM_ISR_PWMS 20
 
+#define HAL_PWM_TIMER      LPC_TIM3
+#define HAL_PWM_TIMER_ISR  extern "C" void TIMER3_IRQHandler(void)
+#define HAL_PWM_TIMER_IRQn TIMER3_IRQn
 
 #define LPC_PORT_OFFSET         (0x0020)
 #define LPC_PIN(pin)            (1UL << pin)
 #define LPC_GPIO(port)          ((volatile LPC_GPIO_TypeDef *)(LPC_GPIO0_BASE + LPC_PORT_OFFSET * port))
 
-
 typedef struct {            // holds all data needed to control/init one of the PWM channels
   bool                active_flag;    // THIS TABLE ENTRY IS ACTIVELY TOGGLING A PIN
   pin_t               pin;
@@ -256,6 +258,11 @@ bool LPC1768_PWM_attach_pin(pin_t pin, uint32_t min /* = 1 */, uint32_t max /* =
                                  // OK to update the active table because the
                                  // ISR doesn't use any of the changed items
 
+  // We NEED memory barriers to ensure Interrupts are actually disabled!
+  // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
+  __DSB();
+  __ISB();
+
   if (ISR_table_update) //use work table if that's the newest
     temp_table = work_table;
   else
@@ -340,6 +347,11 @@ bool LPC1768_PWM_detach_pin(pin_t pin) {
 ////  interrupt controlled PWM code
   NVIC_DisableIRQ(HAL_PWM_TIMER_IRQn);
 
+  // We NEED memory barriers to ensure Interrupts are actually disabled!
+  // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
+  __DSB();
+  __ISB();
+
   if (ISR_table_update) {
     ISR_table_update = false;    // don't update yet - have another update to do
     NVIC_EnableIRQ(HAL_PWM_TIMER_IRQn);  // re-enable PWM interrupts
@@ -426,6 +438,12 @@ bool LPC1768_PWM_write(pin_t pin, uint32_t value) {
 
 ////  interrupt controlled PWM code
   NVIC_DisableIRQ(HAL_PWM_TIMER_IRQn);
+
+  // We NEED memory barriers to ensure Interrupts are actually disabled!
+  // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
+  __DSB();
+  __ISB();
+
   if (!ISR_table_update)   // use the most up to date table
     COPY_ACTIVE_TABLE;  // copy active table into work table
 
@@ -454,6 +472,11 @@ bool useable_hardware_PWM(pin_t pin) {
 
   NVIC_DisableIRQ(HAL_PWM_TIMER_IRQn);
 
+  // We NEED memory barriers to ensure Interrupts are actually disabled!
+  // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
+  __DSB();
+  __ISB();
+
   bool return_flag = false;
   for (uint8_t i = 0; i < NUM_ISR_PWMS; i++)         // see if it's already setup
     if (active_table[i].pin == pin) return_flag = true;

Marlin/src/HAL/HAL_LPC1768/endstop_interrupts.h

@@ -24,7 +24,7 @@
  * Endstop Interrupts
  *
  * Without endstop interrupts the endstop pins must be polled continually in
- * the stepper-ISR via endstops.update(), most of the time finding no change.
+ * the temperature-ISR via endstops.update(), most of the time finding no change.
  * With this feature endstops.update() is called only when we know that at
  * least one endstop has changed state, saving valuable CPU cycles.
  *
@@ -40,16 +40,10 @@
 //Currently this is untested and broken
 #error "Please disable Endstop Interrupts LPC176x is currently an unsupported platform"
 
-volatile uint8_t e_hit = 0; // Different from 0 when the endstops should be tested in detail.
+#include "../../module/endstops.h"
-                            // Must be reset to 0 by the test function when finished.
-
-// This is what is really done inside the interrupts.
-FORCE_INLINE void endstop_ISR_worker( void ) {
-  e_hit = 2; // Because the detection of a e-stop hit has a 1 step debouncer it has to be called at least twice.
-}
 
 // One ISR for all EXT-Interrupts
-void endstop_ISR(void) { endstop_ISR_worker(); }
+void endstop_ISR(void) { endstops.check_possible_change(); }
 
 void setup_endstop_interrupts(void) {
   #if HAS_X_MAX

Marlin/src/HAL/HAL_STM32F1/endstop_interrupts.h

@@ -36,7 +36,7 @@
  * Endstop Interrupts
  *
  * Without endstop interrupts the endstop pins must be polled continually in
- * the stepper-ISR via endstops.update(), most of the time finding no change.
+ * the temperature-ISR via endstops.update(), most of the time finding no change.
  * With this feature endstops.update() is called only when we know that at
  * least one endstop has changed state, saving valuable CPU cycles.
  *
@@ -49,16 +49,10 @@
 #ifndef _ENDSTOP_INTERRUPTS_H_
 #define _ENDSTOP_INTERRUPTS_H_
 
-volatile uint8_t e_hit = 0; // Different from 0 when the endstops should be tested in detail.
+#include "../../module/endstops.h"
-                            // Must be reset to 0 by the test function when finished.
-
-// This is what is really done inside the interrupts.
-FORCE_INLINE void endstop_ISR_worker( void ) {
-  e_hit = 2; // Because the detection of a e-stop hit has a 1 step debouncer it has to be called at least twice.
-}
 
 // One ISR for all EXT-Interrupts
-void endstop_ISR(void) { endstop_ISR_worker(); }
+void endstop_ISR(void) { endstops.check_possible_change(); }
 
 void setup_endstop_interrupts(void) {
   #if HAS_X_MAX

Marlin/src/HAL/HAL_STM32F4/HAL_timers_STM32F4.cpp

@@ -123,6 +123,11 @@ void HAL_timer_enable_interrupt(const uint8_t timer_num) {
 
 void HAL_timer_disable_interrupt(const uint8_t timer_num) {
   HAL_NVIC_DisableIRQ(timerConfig[timer_num].IRQ_Id);
+
+  // We NEED memory barriers to ensure Interrupts are actually disabled!
+  // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
+  __DSB();
+  __ISB();
 }
 
 hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) {

Marlin/src/HAL/HAL_STM32F4/endstop_interrupts.h

@@ -24,16 +24,10 @@
 #ifndef _ENDSTOP_INTERRUPTS_H_
 #define _ENDSTOP_INTERRUPTS_H_
 
-volatile uint8_t e_hit = 0; // Different from 0 when the endstops should be tested in detail.
+#include "../../module/endstops.h"
-                            // Must be reset to 0 by the test function when finished.
-
-// This is what is really done inside the interrupts.
-FORCE_INLINE void endstop_ISR_worker( void ) {
-  e_hit = 2; // Because the detection of a e-stop hit has a 1 step debouncer it has to be called at least twice.
-}
 
 // One ISR for all EXT-Interrupts
-void endstop_ISR(void) { endstop_ISR_worker(); }
+void endstop_ISR(void) { endstops.check_possible_change(); }
 
 void setup_endstop_interrupts(void) {
   #if HAS_X_MAX

Marlin/src/HAL/HAL_STM32F7/HAL_timers_STM32F7.cpp

@@ -127,6 +127,11 @@ void HAL_timer_enable_interrupt(const uint8_t timer_num) {
 
 void HAL_timer_disable_interrupt(const uint8_t timer_num) {
   HAL_NVIC_DisableIRQ(timerConfig[timer_num].IRQ_Id);
+
+  // We NEED memory barriers to ensure Interrupts are actually disabled!
+  // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
+  __DSB();
+  __ISB();
 }
 
 hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) {

Marlin/src/HAL/HAL_STM32F7/endstop_interrupts.h

@@ -26,16 +26,10 @@
 #ifndef _ENDSTOP_INTERRUPTS_H_
 #define _ENDSTOP_INTERRUPTS_H_
 
-volatile uint8_t e_hit = 0; // Different from 0 when the endstops should be tested in detail.
+#include "../../module/endstops.h"
-                            // Must be reset to 0 by the test function when finished.
-
-// This is what is really done inside the interrupts.
-FORCE_INLINE void endstop_ISR_worker( void ) {
-  e_hit = 2; // Because the detection of a e-stop hit has a 1 step debouncer it has to be called at least twice.
-}
 
 // One ISR for all EXT-Interrupts
-void endstop_ISR(void) { endstop_ISR_worker(); }
+void endstop_ISR(void) { endstops.check_possible_change(); }
 
 void setup_endstop_interrupts(void) {
   #if HAS_X_MAX

Marlin/src/HAL/HAL_TEENSY35_36/HAL_timers_Teensy.cpp

@@ -29,6 +29,22 @@
 #include "HAL.h"
 #include "HAL_timers_Teensy.h"
 
+/** \brief Instruction Synchronization Barrier
+  Instruction Synchronization Barrier flushes the pipeline in the processor,
+  so that all instructions following the ISB are fetched from cache or
+  memory, after the instruction has been completed.
+*/
+FORCE_INLINE static void __ISB(void) {
+  __asm__ __volatile__("isb 0xF":::"memory");
+}
+
+/** \brief Data Synchronization Barrier
+  This function acts as a special kind of Data Memory Barrier.
+  It completes when all explicit memory accesses before this instruction complete.
+*/
+FORCE_INLINE static void __DSB(void) {
+  __asm__ __volatile__("dsb 0xF":::"memory");
+}
 
 void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) {
   switch (timer_num) {
@@ -65,6 +81,11 @@ void HAL_timer_disable_interrupt(const uint8_t timer_num) {
     case 0: NVIC_DISABLE_IRQ(IRQ_FTM0); break;
     case 1: NVIC_DISABLE_IRQ(IRQ_FTM1); break;
   }
+
+  // We NEED memory barriers to ensure Interrupts are actually disabled!
+  // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
+  __DSB();
+  __ISB();
 }
 
 bool HAL_timer_interrupt_enabled(const uint8_t timer_num) {

Marlin/src/HAL/HAL_TEENSY35_36/endstop_interrupts.h

@@ -24,7 +24,7 @@
  * Endstop Interrupts
  *
  * Without endstop interrupts the endstop pins must be polled continually in
- * the stepper-ISR via endstops.update(), most of the time finding no change.
+ * the temperature-ISR via endstops.update(), most of the time finding no change.
  * With this feature endstops.update() is called only when we know that at
  * least one endstop has changed state, saving valuable CPU cycles.
  *
@@ -37,16 +37,10 @@
 #ifndef _ENDSTOP_INTERRUPTS_H_
 #define _ENDSTOP_INTERRUPTS_H_
 
-volatile uint8_t e_hit = 0; // Different from 0 when the endstops should be tested in detail.
+#include "../../module/endstops.h"
-                            // Must be reset to 0 by the test function when finished.
-
-// This is what is really done inside the interrupts.
-FORCE_INLINE void endstop_ISR_worker( void ) {
-  e_hit = 2; // Because the detection of a e-stop hit has a 1 step debouncer it has to be called at least twice.
-}
 
 // One ISR for all EXT-Interrupts
-void endstop_ISR(void) { endstop_ISR_worker(); }
+void endstop_ISR(void) { endstops.check_possible_change(); }
 
 /**
  *  Endstop interrupts for Due based targets.

Marlin/src/Marlin.cpp

@@ -95,10 +95,6 @@
   #include "feature/I2CPositionEncoder.h"
 #endif
 
-#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
-  #include HAL_PATH(HAL, endstop_interrupts.h)
-#endif
-
 #if HAS_TRINAMIC
   #include "feature/tmc_util.h"
 #endif
@@ -269,7 +265,7 @@ bool pin_is_protected(const pin_t pin) {
 }
 
 void quickstop_stepper() {
-  stepper.quick_stop();
+  planner.quick_stop();
   planner.synchronize();
   set_current_from_steppers_for_axis(ALL_AXES);
   SYNC_PLAN_POSITION_KINEMATIC();
@@ -748,7 +744,9 @@ void setup() {
 
   print_job_timer.init();   // Initial setup of print job timer
 
-  stepper.init();    // Initialize stepper, this enables interrupts!
+  endstops.init();          // Init endstops and pullups
+
+  stepper.init();           // Init stepper. This enables interrupts!
 
   #if HAS_SERVOS
     servo_init();
@@ -860,10 +858,6 @@ void setup() {
     i2c.onRequest(i2c_on_request);
   #endif
 
-  #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
-    setup_endstop_interrupts();
-  #endif
-
   #if DO_SWITCH_EXTRUDER
     move_extruder_servo(0);  // Initialize extruder servo
   #endif

Marlin/src/feature/Max7219_Debug_LEDs.cpp

@@ -73,7 +73,6 @@ static uint8_t LEDs[8] = { 0 };
 #endif
 
 void Max7219_PutByte(uint8_t data) {
-  CRITICAL_SECTION_START;
   for (uint8_t i = 8; i--;) {
     SIG_DELAY();
     WRITE(MAX7219_CLK_PIN, LOW);       // tick
@@ -84,12 +83,10 @@ void Max7219_PutByte(uint8_t data) {
     SIG_DELAY();
     data <<= 1;
   }
-  CRITICAL_SECTION_END;
 }
 
 void Max7219(const uint8_t reg, const uint8_t data) {
   SIG_DELAY();
-  CRITICAL_SECTION_START;
   WRITE(MAX7219_LOAD_PIN, LOW);  // begin
   SIG_DELAY();
   Max7219_PutByte(reg);          // specify register
@@ -99,7 +96,6 @@ void Max7219(const uint8_t reg, const uint8_t data) {
   WRITE(MAX7219_LOAD_PIN, LOW);  // and tell the chip to load the data
   SIG_DELAY();
   WRITE(MAX7219_LOAD_PIN, HIGH);
-  CRITICAL_SECTION_END;
   SIG_DELAY();
 }
 

Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp

@@ -262,7 +262,8 @@
             z_position = end[Z_AXIS];
           }
 
-          planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder);
+          if (!planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder))
+            break;
         } //else printf("FIRST MOVE PRUNED  ");
       }
 
@@ -319,7 +320,8 @@
           e_position = end[E_AXIS];
           z_position = end[Z_AXIS];
         }
-        planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder);
+        if (!planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder))
+          break;
         current_yi += dyi;
         yi_cnt--;
       }
@@ -342,7 +344,8 @@
           z_position = end[Z_AXIS];
         }
 
-        planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder);
+        if (!planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder))
+          break;
         current_xi += dxi;
         xi_cnt--;
       }

Marlin/src/gcode/config/M540.cpp

@@ -33,7 +33,7 @@
 void GcodeSuite::M540() {
 
   if (parser.seen('S'))
-    stepper.abort_on_endstop_hit = parser.value_bool();
+    planner.abort_on_endstop_hit = parser.value_bool();
 
 }
 

Marlin/src/gcode/control/M17_M18_M84.cpp

@@ -47,7 +47,7 @@ void GcodeSuite::M18_M84() {
   else {
     bool all_axis = !(parser.seen('X') || parser.seen('Y') || parser.seen('Z') || parser.seen('E'));
     if (all_axis) {
-      stepper.finish_and_disable();
+      planner.finish_and_disable();
     }
     else {
       planner.synchronize();

Marlin/src/gcode/control/M80_M81.cpp

@@ -95,7 +95,7 @@
  */
 void GcodeSuite::M81() {
   thermalManager.disable_all_heaters();
-  stepper.finish_and_disable();
+  planner.finish_and_disable();
 
   #if FAN_COUNT > 0
     for (uint8_t i = 0; i < FAN_COUNT; i++) fanSpeeds[i] = 0;

Marlin/src/gcode/motion/G2_G3.cpp

@@ -197,14 +197,17 @@ void plan_arc(
       // i.e., Complete the angular vector in the given time.
       inverse_kinematics(raw);
       ADJUST_DELTA(raw);
-      planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder);
+      if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder))
+        break;
       oldA = delta[A_AXIS]; oldB = delta[B_AXIS];
     #elif HAS_UBL_AND_CURVES
       float pos[XYZ] = { raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS] };
       planner.apply_leveling(pos);
-      planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], raw[E_AXIS], fr_mm_s, active_extruder);
+      if (!planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], raw[E_AXIS], fr_mm_s, active_extruder))
+        break;
     #else
-      planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder);
+      if (!planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder))
+        break;
     #endif
   }
 

Marlin/src/lcd/ultralcd.cpp

@@ -2421,12 +2421,10 @@ void lcd_quick_feedback(const bool clear_buttons) {
 
     void _lcd_do_nothing() {}
     void _lcd_hard_stop() {
-      stepper.quick_stop();
       const screenFunc_t old_screen = currentScreen;
       currentScreen = _lcd_do_nothing;
-      while (planner.movesplanned()) idle();
+      planner.quick_stop();
       currentScreen = old_screen;
-      stepper.cleaning_buffer_counter = 0;
       set_current_from_steppers_for_axis(ALL_AXES);
       sync_plan_position();
     }
@@ -3856,7 +3854,7 @@ void lcd_quick_feedback(const bool clear_buttons) {
 
     // M540 S - Abort on endstop hit when SD printing
     #if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
-      MENU_ITEM_EDIT(bool, MSG_ENDSTOP_ABORT, &stepper.abort_on_endstop_hit);
+      MENU_ITEM_EDIT(bool, MSG_ENDSTOP_ABORT, &planner.abort_on_endstop_hit);
     #endif
 
     END_MENU();

Marlin/src/module/endstops.cpp

@@ -32,18 +32,27 @@
 #include "../module/temperature.h"
 #include "../lcd/ultralcd.h"
 
-// TEST_ENDSTOP: test the old and the current status of an endstop
+#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
-#define TEST_ENDSTOP(ENDSTOP) (TEST(current_endstop_bits & old_endstop_bits, ENDSTOP))
+  #include HAL_PATH(../HAL, endstop_interrupts.h)
+#endif
+
+// TEST_ENDSTOP: test the current status of an endstop
+#define TEST_ENDSTOP(ENDSTOP) (TEST(current_endstop_bits, ENDSTOP))
+
+#if HAS_BED_PROBE
+  #define ENDSTOPS_ENABLED  (endstops.enabled || endstops.z_probe_enabled)
+#else
+  #define ENDSTOPS_ENABLED  endstops.enabled
+#endif
 
 Endstops endstops;
 
 // public:
 
 bool Endstops::enabled, Endstops::enabled_globally; // Initialized by settings.load()
-volatile char Endstops::endstop_hit_bits; // use X_MIN, Y_MIN, Z_MIN and Z_MIN_PROBE as BIT value
+volatile uint8_t Endstops::endstop_hit_bits; // use X_MIN, Y_MIN, Z_MIN and Z_MIN_PROBE as BIT value
 
-Endstops::esbits_t Endstops::current_endstop_bits = 0,
+Endstops::esbits_t Endstops::current_endstop_bits = 0;
-                   Endstops::old_endstop_bits = 0;
 
 #if HAS_BED_PROBE
   volatile bool Endstops::z_probe_enabled = false;
@@ -196,8 +205,93 @@ void Endstops::init() {
     #endif
   #endif
 
+  #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
+    setup_endstop_interrupts();
+  #endif
+
+  // Enable endstops
+  enable_globally(
+    #if ENABLED(ENDSTOPS_ALWAYS_ON_DEFAULT)
+      true
+    #else
+      false
+    #endif
+  );
+
 } // Endstops::init
 
+// Called from ISR. A change was detected. Find out what happened!
+void Endstops::check_possible_change() { if (ENDSTOPS_ENABLED) endstops.update(); }
+
+// Called from ISR: Poll endstop state if required
+void Endstops::poll() {
+
+  #if ENABLED(PINS_DEBUGGING)
+    endstops.run_monitor();  // report changes in endstop status
+  #endif
+
+  #if DISABLED(ENDSTOP_INTERRUPTS_FEATURE)
+    if (ENDSTOPS_ENABLED) endstops.update();
+  #endif
+}
+
+void Endstops::enable_globally(const bool onoff) {
+  enabled_globally = enabled = onoff;
+
+  #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
+    if (onoff) endstops.update(); // If enabling, update state now
+  #endif
+}
+
+// Enable / disable endstop checking
+void Endstops::enable(const bool onoff) {
+  enabled = onoff;
+
+  #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
+    if (onoff) endstops.update(); // If enabling, update state now
+  #endif
+}
+
+
+// Disable / Enable endstops based on ENSTOPS_ONLY_FOR_HOMING and global enable
+void Endstops::not_homing() {
+  enabled = enabled_globally;
+
+  #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
+    if (enabled) endstops.update(); // If enabling, update state now
+  #endif
+}
+
+// Clear endstops (i.e., they were hit intentionally) to suppress the report
+void Endstops::hit_on_purpose() {
+  endstop_hit_bits = 0;
+
+  #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
+    if (enabled) endstops.update(); // If enabling, update state now
+  #endif
+}
+
+// Enable / disable endstop z-probe checking
+#if HAS_BED_PROBE
+  void Endstops::enable_z_probe(bool onoff) {
+    z_probe_enabled = onoff;
+
+    #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
+      if (enabled) endstops.update(); // If enabling, update state now
+    #endif
+  }
+#endif
+
+#if ENABLED(PINS_DEBUGGING)
+  void Endstops::run_monitor() {
+    if (!monitor_flag) return;
+    static uint8_t monitor_count = 16;  // offset this check from the others
+    monitor_count += _BV(1);            //  15 Hz
+    monitor_count &= 0x7F;
+    if (!monitor_count) monitor();      // report changes in endstop status
+  }
+#endif
+
 void Endstops::report_state() {
   if (endstop_hit_bits) {
     #if ENABLED(ULTRA_LCD)
@@ -208,7 +302,7 @@ void Endstops::report_state() {
     #endif
 
     #define _ENDSTOP_HIT_ECHO(A,C) do{ \
-      SERIAL_ECHOPAIR(" " STRINGIFY(A) ":", stepper.triggered_position_mm(_AXIS(A))); \
+      SERIAL_ECHOPAIR(" " STRINGIFY(A) ":", planner.triggered_position_mm(_AXIS(A))); \
       _SET_STOP_CHAR(A,C); }while(0)
 
     #define _ENDSTOP_HIT_TEST(A,C) \
@@ -238,7 +332,7 @@ void Endstops::report_state() {
     hit_on_purpose();
 
     #if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED) && ENABLED(SDSUPPORT)
-      if (stepper.abort_on_endstop_hit) {
+      if (planner.abort_on_endstop_hit) {
         card.sdprinting = false;
         card.closefile();
         quickstop_stepper();
@@ -300,38 +394,41 @@ void Endstops::M119() {
   #endif
 } // Endstops::M119
 
+// The following routines are called from an ISR context. It could be the temperature ISR, the
+// endstop ISR or the Stepper ISR.
+
 #if ENABLED(X_DUAL_ENDSTOPS)
   void Endstops::test_dual_x_endstops(const EndstopEnum es1, const EndstopEnum es2) {
     const byte x_test = TEST_ENDSTOP(es1) | (TEST_ENDSTOP(es2) << 1); // bit 0 for X, bit 1 for X2
-    if (x_test && stepper.current_block->steps[X_AXIS] > 0) {
+    if (x_test && stepper.movement_non_null(X_AXIS)) {
       SBI(endstop_hit_bits, X_MIN);
       if (!stepper.performing_homing || (x_test == 0x3))  //if not performing home or if both endstops were trigged during homing...
-        stepper.kill_current_block();
+        stepper.quick_stop();
     }
   }
 #endif
 #if ENABLED(Y_DUAL_ENDSTOPS)
   void Endstops::test_dual_y_endstops(const EndstopEnum es1, const EndstopEnum es2) {
     const byte y_test = TEST_ENDSTOP(es1) | (TEST_ENDSTOP(es2) << 1); // bit 0 for Y, bit 1 for Y2
-    if (y_test && stepper.current_block->steps[Y_AXIS] > 0) {
+    if (y_test && stepper.movement_non_null(Y_AXIS)) {
       SBI(endstop_hit_bits, Y_MIN);
       if (!stepper.performing_homing || (y_test == 0x3))  //if not performing home or if both endstops were trigged during homing...
-        stepper.kill_current_block();
+        stepper.quick_stop();
     }
   }
 #endif
 #if ENABLED(Z_DUAL_ENDSTOPS)
   void Endstops::test_dual_z_endstops(const EndstopEnum es1, const EndstopEnum es2) {
     const byte z_test = TEST_ENDSTOP(es1) | (TEST_ENDSTOP(es2) << 1); // bit 0 for Z, bit 1 for Z2
-    if (z_test && stepper.current_block->steps[Z_AXIS] > 0) {
+    if (z_test && stepper.movement_non_null(Z_AXIS)) {
       SBI(endstop_hit_bits, Z_MIN);
       if (!stepper.performing_homing || (z_test == 0x3))  //if not performing home or if both endstops were trigged during homing...
-        stepper.kill_current_block();
+        stepper.quick_stop();
     }
   }
 #endif
 
-// Check endstops - Called from ISR!
+// Check endstops - Could be called from ISR!
 void Endstops::update() {
 
   #define _ENDSTOP(AXIS, MINMAX) AXIS ##_## MINMAX
@@ -349,7 +446,7 @@ void Endstops::update() {
       UPDATE_ENDSTOP_BIT(AXIS, MINMAX); \
       if (TEST_ENDSTOP(_ENDSTOP(AXIS, MINMAX))) { \
         _ENDSTOP_HIT(AXIS, MINMAX); \
-        stepper.endstop_triggered(_AXIS(AXIS)); \
+        planner.endstop_triggered(_AXIS(AXIS)); \
       } \
     }while(0)
 
@@ -358,9 +455,9 @@ void Endstops::update() {
     if (G38_move) {
       UPDATE_ENDSTOP_BIT(Z, MIN_PROBE);
       if (TEST_ENDSTOP(_ENDSTOP(Z, MIN_PROBE))) {
-        if      (stepper.current_block->steps[_AXIS(X)] > 0) { _ENDSTOP_HIT(X, MIN); stepper.endstop_triggered(_AXIS(X)); }
+        if      (stepper.movement_non_null(_AXIS(X))) { _ENDSTOP_HIT(X, MIN); planner.endstop_triggered(_AXIS(X)); }
-        else if (stepper.current_block->steps[_AXIS(Y)] > 0) { _ENDSTOP_HIT(Y, MIN); stepper.endstop_triggered(_AXIS(Y)); }
+        else if (stepper.movement_non_null(_AXIS(Y))) { _ENDSTOP_HIT(Y, MIN); planner.endstop_triggered(_AXIS(Y)); }
-        else if (stepper.current_block->steps[_AXIS(Z)] > 0) { _ENDSTOP_HIT(Z, MIN); stepper.endstop_triggered(_AXIS(Z)); }
+        else if (stepper.movement_non_null(_AXIS(Z))) { _ENDSTOP_HIT(Z, MIN); planner.endstop_triggered(_AXIS(Z)); }
         G38_endstop_hit = true;
       }
     }
@@ -371,7 +468,7 @@ void Endstops::update() {
    */
 
   #if IS_CORE
-    #define S_(N) stepper.current_block->steps[CORE_AXIS_##N]
+    #define S_(N) stepper.movement_non_null(CORE_AXIS_##N)
     #define D_(N) stepper.motor_direction(CORE_AXIS_##N)
   #endif
 
@@ -391,7 +488,7 @@ void Endstops::update() {
     #define X_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && D_(1) X_CMP D_(2)) )
     #define X_AXIS_HEAD X_HEAD
   #else
-    #define X_MOVE_TEST stepper.current_block->steps[X_AXIS] > 0
+    #define X_MOVE_TEST stepper.movement_non_null(X_AXIS)
     #define X_AXIS_HEAD X_AXIS
   #endif
 
@@ -411,7 +508,7 @@ void Endstops::update() {
     #define Y_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && D_(1) Y_CMP D_(2)) )
     #define Y_AXIS_HEAD Y_HEAD
   #else
-    #define Y_MOVE_TEST stepper.current_block->steps[Y_AXIS] > 0
+    #define Y_MOVE_TEST stepper.movement_non_null(Y_AXIS)
     #define Y_AXIS_HEAD Y_AXIS
   #endif
 
@@ -431,13 +528,13 @@ void Endstops::update() {
     #define Z_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && D_(1) Z_CMP D_(2)) )
     #define Z_AXIS_HEAD Z_HEAD
   #else
-    #define Z_MOVE_TEST stepper.current_block->steps[Z_AXIS] > 0
+    #define Z_MOVE_TEST stepper.movement_non_null(Z_AXIS)
     #define Z_AXIS_HEAD Z_AXIS
   #endif
 
   // With Dual X, endstops are only checked in the homing direction for the active extruder
   #if ENABLED(DUAL_X_CARRIAGE)
-    #define E0_ACTIVE stepper.current_block->active_extruder == 0
+    #define E0_ACTIVE stepper.movement_extruder() == 0
     #define X_MIN_TEST ((X_HOME_DIR < 0 && E0_ACTIVE) || (X2_HOME_DIR < 0 && !E0_ACTIVE))
     #define X_MAX_TEST ((X_HOME_DIR > 0 && E0_ACTIVE) || (X2_HOME_DIR > 0 && !E0_ACTIVE))
   #else
@@ -448,8 +545,6 @@ void Endstops::update() {
   /**
    * Check and update endstops according to conditions
    */
-  if (stepper.current_block) {
-
   if (X_MOVE_TEST) {
     if (stepper.motor_direction(X_AXIS_HEAD)) { // -direction
       #if HAS_X_MIN
@@ -563,11 +658,6 @@ void Endstops::update() {
       #endif
     }
   }
-
-  } // stepper.current_block
-
-  old_endstop_bits = current_endstop_bits;
-
 } // Endstops::update()
 
 #if ENABLED(PINS_DEBUGGING)

Marlin/src/module/endstops.h

@@ -51,7 +51,7 @@ class Endstops {
   public:
 
     static bool enabled, enabled_globally;
-    static volatile char endstop_hit_bits; // use X_MIN, Y_MIN, Z_MIN and Z_MIN_PROBE as BIT value
+    static volatile uint8_t endstop_hit_bits; // use X_MIN, Y_MIN, Z_MIN and Z_MIN_PROBE as BIT value
 
     #if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
       typedef uint16_t esbits_t;
@@ -68,23 +68,26 @@ class Endstops {
       typedef byte esbits_t;
     #endif
 
-    static esbits_t current_endstop_bits, old_endstop_bits;
+    static esbits_t current_endstop_bits;
 
-    Endstops() {
+    Endstops() {};
-      enable_globally(
-        #if ENABLED(ENDSTOPS_ALWAYS_ON_DEFAULT)
-          true
-        #else
-          false
-        #endif
-      );
-    };
 
     /**
      * Initialize the endstop pins
      */
     static void init();
 
+    /**
+     * A change was detected or presumed to be in endstops pins. Find out what
+     * changed, if anything. Called from ISR contexts
+     */
+    static void check_possible_change();
+
+    /**
+     * Periodic call to poll endstops if required. Called from temperature ISR
+     */
+    static void poll();
+
     /**
      * Update the endstops bits from the pins
      */
@@ -101,34 +104,28 @@ class Endstops {
     static void M119();
 
     // Enable / disable endstop checking globally
-    static void enable_globally(bool onoff=true) { enabled_globally = enabled = onoff; }
+    static void enable_globally(const bool onoff=true);
 
     // Enable / disable endstop checking
-    static void enable(bool onoff=true) { enabled = onoff; }
+    static void enable(const bool onoff=true);
 
     // Disable / Enable endstops based on ENSTOPS_ONLY_FOR_HOMING and global enable
-    static void not_homing() { enabled = enabled_globally; }
+    static void not_homing();
 
     // Clear endstops (i.e., they were hit intentionally) to suppress the report
-    static void hit_on_purpose() { endstop_hit_bits = 0; }
+    static void hit_on_purpose();
 
     // Enable / disable endstop z-probe checking
     #if HAS_BED_PROBE
       static volatile bool z_probe_enabled;
-      static void enable_z_probe(bool onoff=true) { z_probe_enabled = onoff; }
+      static void enable_z_probe(bool onoff=true);
     #endif
 
     // Debugging of endstops
     #if ENABLED(PINS_DEBUGGING)
       static bool monitor_flag;
       static void monitor();
-      FORCE_INLINE static void run_monitor() {
+      static void run_monitor();
-        if (!monitor_flag) return;
-        static uint8_t monitor_count = 16;  // offset this check from the others
-        monitor_count += _BV(1);            //  15 Hz
-        monitor_count &= 0x7F;
-        if (!monitor_count) monitor();      // report changes in endstop status
-      }
     #endif
 
   private:
@@ -146,10 +143,4 @@ class Endstops {
 
 extern Endstops endstops;
 
-#if HAS_BED_PROBE
-  #define ENDSTOPS_ENABLED  (endstops.enabled || endstops.z_probe_enabled)
-#else
-  #define ENDSTOPS_ENABLED  endstops.enabled
-#endif
-
 #endif // __ENDSTOPS_H__

Marlin/src/module/motion.cpp

@@ -644,7 +644,8 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
       #if ENABLED(SCARA_FEEDRATE_SCALING)
         // For SCARA scale the feed rate from mm/s to degrees/s
         // i.e., Complete the angular vector in the given time.
-        planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder);
+        if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder))
+          break;
         /*
         SERIAL_ECHO(segments);
         SERIAL_ECHOPAIR(": X=", raw[X_AXIS]); SERIAL_ECHOPAIR(" Y=", raw[Y_AXIS]);
@@ -654,7 +655,8 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
         //*/
         oldA = delta[A_AXIS]; oldB = delta[B_AXIS];
       #else
-        planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder, cartesian_segment_mm);
+        if (!planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder, cartesian_segment_mm))
+          break;
       #endif
     }
 
@@ -746,7 +748,8 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
           idle();
         }
         LOOP_XYZE(i) raw[i] += segment_distance[i];
-        planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder, cartesian_segment_mm);
+        if (!planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder, cartesian_segment_mm))
+          break;
       }
 
       // Since segment_distance is only approximate,
@@ -848,14 +851,14 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
           }
           // unpark extruder: 1) raise, 2) move into starting XY position, 3) lower
           for (uint8_t i = 0; i < 3; i++)
-            planner.buffer_line(
+            if (!planner.buffer_line(
               i == 0 ? raised_parked_position[X_AXIS] : current_position[X_AXIS],
               i == 0 ? raised_parked_position[Y_AXIS] : current_position[Y_AXIS],
               i == 2 ? current_position[Z_AXIS] : raised_parked_position[Z_AXIS],
               current_position[E_AXIS],
               i == 1 ? PLANNER_XY_FEEDRATE() : planner.max_feedrate_mm_s[Z_AXIS],
-              active_extruder
+              active_extruder)
-            );
+            ) break;
           delayed_move_time = 0;
           active_extruder_parked = false;
           #if ENABLED(DEBUG_LEVELING_FEATURE)
@@ -872,11 +875,11 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
             #endif
             // move duplicate extruder into correct duplication position.
             planner.set_position_mm(inactive_extruder_x_pos, current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
-            planner.buffer_line(
+            if (!planner.buffer_line(
               current_position[X_AXIS] + duplicate_extruder_x_offset,
               current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS],
-              planner.max_feedrate_mm_s[X_AXIS], 1
+              planner.max_feedrate_mm_s[X_AXIS], 1)
-            );
+            ) break;
             planner.synchronize();
             SYNC_PLAN_POSITION_KINEMATIC();
             extruder_duplication_enabled = true;

Marlin/src/module/planner.h

@@ -35,6 +35,7 @@
 #include "../Marlin.h"
 
 #include "motion.h"
+#include "../gcode/queue.h"
 
 #if ENABLED(DELTA)
   #include "delta.h"
@@ -53,7 +54,7 @@ enum BlockFlagBit : char {
   // from a safe speed (in consideration of jerking from zero speed).
   BLOCK_BIT_NOMINAL_LENGTH,
 
-  // The block is busy
+  // The block is busy, being interpreted by the stepper ISR
   BLOCK_BIT_BUSY,
 
   // The block is segment 2+ of a longer move
@@ -84,18 +85,34 @@ typedef struct {
 
   uint8_t flag;                             // Block flags (See BlockFlag enum above)
 
-  unsigned char active_extruder;            // The extruder to move (if E move)
+  // Fields used by the motion planner to manage acceleration
+  float nominal_speed_sqr,                  // The nominal speed for this block in (mm/sec)^2
+        entry_speed_sqr,                    // Entry speed at previous-current junction in (mm/sec)^2
+        max_entry_speed_sqr,                // Maximum allowable junction entry speed in (mm/sec)^2
+        millimeters,                        // The total travel of this block in mm
+        acceleration;                       // acceleration mm/sec^2
 
+  union {
+    // Data used by all move blocks
+    struct {
       // Fields used by the Bresenham algorithm for tracing the line
-  int32_t steps[NUM_AXIS];                  // Step count along each axis
+      uint32_t steps[NUM_AXIS];             // Step count along each axis
+    };
+    // Data used by all sync blocks
+    struct {
+      int32_t position[NUM_AXIS];           // New position to force when this sync block is executed
+    };
+  };
   uint32_t step_event_count;                // The number of step events required to complete this block
 
+  uint8_t active_extruder;                  // The extruder to move (if E move)
+
   #if ENABLED(MIXING_EXTRUDER)
     uint32_t mix_event_count[MIXING_STEPPERS]; // Scaled step_event_count for the mixing steppers
   #endif
 
   // Settings for the trapezoid generator
-  int32_t accelerate_until,                 // The index of the step event on which to stop acceleration
+  uint32_t accelerate_until,                // The index of the step event on which to stop acceleration
            decelerate_after;                // The index of the step event on which to start decelerating
 
   #if ENABLED(BEZIER_JERK_CONTROL)
@@ -105,7 +122,7 @@ typedef struct {
     uint32_t acceleration_time_inverse,     // Inverse of acceleration and deceleration periods, expressed as integer. Scale depends on CPU being used
              deceleration_time_inverse;
   #else
-    int32_t acceleration_rate;              // The acceleration rate used for acceleration calculation
+    uint32_t acceleration_rate;             // The acceleration rate used for acceleration calculation
   #endif
 
   uint8_t direction_bits;                   // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
@@ -119,13 +136,6 @@ typedef struct {
     float e_D_ratio;
   #endif
 
-  // Fields used by the motion planner to manage acceleration
-  float nominal_speed,                      // The nominal speed for this block in mm/sec
-        entry_speed,                        // Entry speed at previous-current junction in mm/sec
-        max_entry_speed,                    // Maximum allowable junction entry speed in mm/sec
-        millimeters,                        // The total travel of this block in mm
-        acceleration;                       // acceleration mm/sec^2
-
   uint32_t nominal_rate,                    // The nominal step rate for this block in step_events/sec
            initial_rate,                    // The jerk-adjusted step rate at start of block
            final_rate,                      // The minimal rate at exit
@@ -166,6 +176,10 @@ class Planner {
     static block_t block_buffer[BLOCK_BUFFER_SIZE];
     static volatile uint8_t block_buffer_head,      // Index of the next block to be pushed
                             block_buffer_tail;      // Index of the busy block, if any
+    static uint16_t cleaning_buffer_counter;        // A counter to disable queuing of blocks
+    static uint8_t delay_before_delivering,         // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
+                   block_buffer_planned;            // Index of the optimally planned block
+                   
 
     #if ENABLED(DISTINCT_E_FACTORS)
       static uint8_t last_extruder;                 // Respond to extruder change
@@ -233,6 +247,10 @@ class Planner {
       #endif
     #endif
 
+    #if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
+      static bool abort_on_endstop_hit;
+    #endif
+
   private:
 
     /**
@@ -247,9 +265,9 @@ class Planner {
     static float previous_speed[NUM_AXIS];
 
     /**
-     * Nominal speed of previous path line segment
+     * Nominal speed of previous path line segment (mm/s)^2
      */
-    static float previous_nominal_speed;
+    static float previous_nominal_speed_sqr;
 
     /**
      * Limit where 64bit math is necessary for acceleration calculation
@@ -308,15 +326,6 @@ class Planner {
     // Manage fans, paste pressure, etc.
     static void check_axes_activity();
 
-    /**
-     * Number of moves currently in the planner
-     */
-    FORCE_INLINE static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail + BLOCK_BUFFER_SIZE); }
-
-    FORCE_INLINE static void clear_block_buffer() { block_buffer_head = block_buffer_tail = 0; }
-
-    FORCE_INLINE static bool is_full() { return block_buffer_tail == next_block_index(block_buffer_head); }
-
     // Update multipliers based on new diameter measurements
     static void calculate_volumetric_multipliers();
 
@@ -424,16 +433,32 @@ class Planner {
       #define ARG_Z const float &rz
     #endif
 
+    // Number of moves currently in the planner
+    FORCE_INLINE static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail); }
+
+    // Remove all blocks from the buffer
+    FORCE_INLINE static void clear_block_buffer() { block_buffer_head = block_buffer_tail = 0; }
+
+    // Check if movement queue is full
+    FORCE_INLINE static bool is_full() { return block_buffer_tail == next_block_index(block_buffer_head); }
+
+    // Get count of movement slots free
+    FORCE_INLINE static uint8_t moves_free() { return BLOCK_BUFFER_SIZE - 1 - movesplanned(); }
+
     /**
      * Planner::get_next_free_block
      *
-     * - Get the next head index (passed by reference)
+     * - Get the next head indices (passed by reference)
-     * - Wait for a space to open up in the planner
+     * - Wait for the number of spaces to open up in the planner
-     * - Return the head block
+     * - Return the first head block
      */
-    FORCE_INLINE static block_t* get_next_free_block(uint8_t &next_buffer_head) {
+    FORCE_INLINE static block_t* get_next_free_block(uint8_t &next_buffer_head, uint8_t count = 1) {
+
+      // Wait until there are enough slots free
+      while (moves_free() < count) { idle(); }
+
+      // Return the first available block
       next_buffer_head = next_block_index(block_buffer_head);
-      while (block_buffer_tail == next_buffer_head) idle(); // while (is_full)
       return &block_buffer[block_buffer_head];
     }
 
@@ -446,8 +471,30 @@ class Planner {
      *  fr_mm_s     - (target) speed of the move
      *  extruder    - target extruder
      *  millimeters - the length of the movement, if known
+     *
+     * Returns true if movement was buffered, false otherwise
      */
-    static void _buffer_steps(const int32_t (&target)[XYZE]
+    static bool _buffer_steps(const int32_t (&target)[XYZE]
+      #if HAS_POSITION_FLOAT
+        , const float (&target_float)[XYZE]
+      #endif
+      , float fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
+    );
+
+    /**
+     * Planner::_populate_block
+     *
+     * Fills a new linear movement in the block (in terms of steps).
+     *
+     *  target      - target position in steps units
+     *  fr_mm_s     - (target) speed of the move
+     *  extruder    - target extruder
+     *  millimeters - the length of the movement, if known
+     *
+     * Returns true is movement is acceptable, false otherwise
+     */
+    static bool _populate_block(block_t * const block, bool split_move,
+        const int32_t (&target)[XYZE]
       #if HAS_POSITION_FLOAT
         , const float (&target_float)[XYZE]
       #endif
@@ -472,7 +519,7 @@ class Planner {
      *  extruder    - target extruder
      *  millimeters - the length of the movement, if known
      */
-    static void buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0);
+    static bool buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0);
 
     static void _set_position_mm(const float &a, const float &b, const float &c, const float &e);
 
@@ -489,11 +536,11 @@ class Planner {
      *  extruder     - target extruder
      *  millimeters  - the length of the movement, if known
      */
-    FORCE_INLINE static void buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {
+    FORCE_INLINE static bool buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {
       #if PLANNER_LEVELING && IS_CARTESIAN
         apply_leveling(rx, ry, rz);
       #endif
-      buffer_segment(rx, ry, rz, e, fr_mm_s, extruder, millimeters);
+      return buffer_segment(rx, ry, rz, e, fr_mm_s, extruder, millimeters);
     }
 
     /**
@@ -506,7 +553,7 @@ class Planner {
      *  extruder     - target extruder
      *  millimeters  - the length of the movement, if known
      */
-    FORCE_INLINE static void buffer_line_kinematic(const float (&cart)[XYZE], const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {
+    FORCE_INLINE static bool buffer_line_kinematic(const float (&cart)[XYZE], const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {
       #if PLANNER_LEVELING
         float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
         apply_leveling(raw);
@@ -515,9 +562,9 @@ class Planner {
       #endif
       #if IS_KINEMATIC
         inverse_kinematics(raw);
-        buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters);
+        return buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters);
       #else
-        buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters);
+        return buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters);
       #endif
     }
 
@@ -541,11 +588,6 @@ class Planner {
     FORCE_INLINE static void set_z_position_mm(const float &z) { set_position_mm(Z_AXIS, z); }
     FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(E_AXIS, e); }
 
-    /**
-     * Sync from the stepper positions. (e.g., after an interrupted move)
-     */
-    static void sync_from_steppers();
-
     /**
      * Get an axis position according to stepper position(s)
      * For CORE machines apply translation from ABC to XYZ.
@@ -557,73 +599,116 @@ class Planner {
       FORCE_INLINE static float get_axis_position_degrees(const AxisEnum axis) { return get_axis_position_mm(axis); }
     #endif
 
+    // Called to force a quick stop of the machine (for example, when an emergency
+    // stop is required, or when endstops are hit)
+    static void quick_stop();
+
+    // Called when an endstop is triggered. Causes the machine to stop inmediately
+    static void endstop_triggered(const AxisEnum axis);
+
+    // Triggered position of an axis in mm (not core-savvy)
+    static float triggered_position_mm(const AxisEnum axis);
+
+    // Block until all buffered steps are executed / cleaned
+    static void synchronize();
+
+    // Wait for moves to finish and disable all steppers
+    static void finish_and_disable();
+
+    // Periodic tick to handle cleaning timeouts
+    // Called from the Temperature ISR at ~1kHz
+    static void tick() {
+      if (cleaning_buffer_counter) {
+        --cleaning_buffer_counter;
+        #if ENABLED(SD_FINISHED_STEPPERRELEASE) && defined(SD_FINISHED_RELEASECOMMAND)
+          if (!cleaning_buffer_counter) enqueue_and_echo_commands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
+        #endif
+      }
+    }
+
     /**
      * Does the buffer have any blocks queued?
      */
     FORCE_INLINE static bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); }
 
-    //
-    // Block until all buffered steps are executed
-    //
-    static void synchronize();
-
-    /**
-     * "Discard" the block and "release" the memory.
-     * Called when the current block is no longer needed.
-     */
-    FORCE_INLINE static void discard_current_block() {
-      if (has_blocks_queued())
-        block_buffer_tail = BLOCK_MOD(block_buffer_tail + 1);
-    }
-
-    /**
-     * "Discard" the next block if it's continued.
-     * Called after an interrupted move to throw away the rest of the move.
-     */
-    FORCE_INLINE static bool discard_continued_block() {
-      const bool discard = has_blocks_queued() && TEST(block_buffer[block_buffer_tail].flag, BLOCK_BIT_CONTINUED);
-      if (discard) discard_current_block();
-      return discard;
-    }
-
     /**
      * The current block. NULL if the buffer is empty.
      * This also marks the block as busy.
      * WARNING: Called from Stepper ISR context!
      */
     static block_t* get_current_block() {
-      if (has_blocks_queued()) {
+
+      // Get the number of moves in the planner queue so far
+      uint8_t nr_moves = movesplanned();
+
+      // If there are any moves queued ...
+      if (nr_moves) {
+
+        // If there is still delay of delivery of blocks running, decrement it
+        if (delay_before_delivering) {
+          --delay_before_delivering;
+          // If the number of movements queued is less than 3, and there is still time
+          //  to wait, do not deliver anything
+          if (nr_moves < 3 && delay_before_delivering) return NULL;
+          delay_before_delivering = 0;
+        }
+
+        // If we are here, there is no excuse to deliver the block
         block_t * const block = &block_buffer[block_buffer_tail];
 
-        // If the block has no trapezoid calculated, it's unsafe to execute.
+        // No trapezoid calculated? Don't execute yet.
-        if (movesplanned() > 1) {
+        if (TEST(block->flag, BLOCK_BIT_RECALCULATE)) return NULL;
-          const block_t * const next = &block_buffer[next_block_index(block_buffer_tail)];
-          if (TEST(block->flag, BLOCK_BIT_RECALCULATE) || TEST(next->flag, BLOCK_BIT_RECALCULATE))
-            return NULL;
-        }
-        else if (TEST(block->flag, BLOCK_BIT_RECALCULATE))
-          return NULL;
 
         #if ENABLED(ULTRA_LCD)
           block_buffer_runtime_us -= block->segment_time_us; // We can't be sure how long an active block will take, so don't count it.
         #endif
+
+        // Mark the block as busy, so the planner does not attempt to replan it
         SBI(block->flag, BLOCK_BIT_BUSY);
         return block;
       }
-      else {
+
+      // The queue became empty
       #if ENABLED(ULTRA_LCD)
         clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero.
       #endif
+
       return NULL;
     }
+
+    /**
+     * "Discard" the block and "release" the memory.
+     * Called when the current block is no longer needed.
+     * NB: There MUST be a current block to call this function!!
+     */
+    FORCE_INLINE static void discard_current_block() {
+      if (has_blocks_queued()) { // Discard non-empty buffer.
+        uint8_t block_index = next_block_index( block_buffer_tail );
+
+        // Push block_buffer_planned pointer, if encountered.
+        if (!has_blocks_queued()) block_buffer_planned = block_index;
+
+        block_buffer_tail = block_index;
+      }
     }
 
     #if ENABLED(ULTRA_LCD)
 
       static uint16_t block_buffer_runtime() {
-        CRITICAL_SECTION_START
+        #ifdef __AVR__
+          // Protect the access to the variable. Only required for AVR, as
+          //  any 32bit CPU offers atomic access to 32bit variables
+          bool was_enabled = STEPPER_ISR_ENABLED();
+          if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
+        #endif
+
         millis_t bbru = block_buffer_runtime_us;
-        CRITICAL_SECTION_END
+
+        #ifdef __AVR__
+          // Reenable Stepper ISR
+          if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
+        #endif
+
         // To translate µs to ms a division by 1000 would be required.
         // We introduce 2.4% error here by dividing by 1024.
         // Doesn't matter because block_buffer_runtime_us is already too small an estimation.
@@ -634,9 +719,19 @@ class Planner {
       }
 
       static void clear_block_buffer_runtime() {
-        CRITICAL_SECTION_START
+        #ifdef __AVR__
+          // Protect the access to the variable. Only required for AVR, as
+          //  any 32bit CPU offers atomic access to 32bit variables
+          bool was_enabled = STEPPER_ISR_ENABLED();
+          if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
+        #endif
+
         block_buffer_runtime_us = 0;
-        CRITICAL_SECTION_END
+
+        #ifdef __AVR__
+          // Reenable Stepper ISR
+          if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
+        #endif
       }
 
     #endif
@@ -653,8 +748,8 @@ class Planner {
     /**
      * Get the index of the next / previous block in the ring buffer
      */
-    static constexpr int8_t next_block_index(const int8_t block_index) { return BLOCK_MOD(block_index + 1); }
+    static constexpr uint8_t next_block_index(const uint8_t block_index) { return BLOCK_MOD(block_index + 1); }
-    static constexpr int8_t prev_block_index(const int8_t block_index) { return BLOCK_MOD(block_index - 1); }
+    static constexpr uint8_t prev_block_index(const uint8_t block_index) { return BLOCK_MOD(block_index - 1); }
 
     /**
      * Calculate the distance (not time) it takes to accelerate
@@ -679,12 +774,12 @@ class Planner {
     }
 
     /**
-     * Calculate the maximum allowable speed at this point, in order
+     * Calculate the maximum allowable speed squared at this point, in order
-     * to reach 'target_velocity' using 'acceleration' within a given
+     * to reach 'target_velocity_sqr' using 'acceleration' within a given
      * 'distance'.
      */
-    static float max_allowable_speed(const float &accel, const float &target_velocity, const float &distance) {
+    static float max_allowable_speed_sqr(const float &accel, const float &target_velocity_sqr, const float &distance) {
-      return SQRT(sq(target_velocity) - 2 * accel * distance);
+      return target_velocity_sqr - 2 * accel * distance;
     }
 
     #if ENABLED(BEZIER_JERK_CONTROL)
@@ -699,7 +794,7 @@ class Planner {
     static void calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor);
 
     static void reverse_pass_kernel(block_t* const current, const block_t * const next);
-    static void forward_pass_kernel(const block_t * const previous, block_t* const current);
+    static void forward_pass_kernel(const block_t * const previous, block_t* const current, uint8_t block_index);
 
     static void reverse_pass();
     static void forward_pass();

Marlin/src/module/planner_bezier.cpp

@@ -194,9 +194,11 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS]
     #if HAS_UBL_AND_CURVES
       float pos[XYZ] = { bez_target[X_AXIS], bez_target[Y_AXIS], bez_target[Z_AXIS] };
       planner.apply_leveling(pos);
-      planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], bez_target[E_AXIS], fr_mm_s, active_extruder);
+      if (!planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], bez_target[E_AXIS], fr_mm_s, active_extruder))
+        break;
     #else
-      planner.buffer_line_kinematic(bez_target, fr_mm_s, extruder);
+      if (!planner.buffer_line_kinematic(bez_target, fr_mm_s, extruder))
+        break;
     #endif
   }
 }

Marlin/src/module/stepper.cpp

@@ -86,10 +86,6 @@ Stepper stepper; // Singleton
 
 block_t* Stepper::current_block = NULL;  // A pointer to the block currently being traced
 
-#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
-  bool Stepper::abort_on_endstop_hit = false;
-#endif
-
 #if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
   bool Stepper::performing_homing = false;
 #endif
@@ -100,8 +96,10 @@ block_t* Stepper::current_block = NULL;  // A pointer to the block currently bei
 
 // private:
 
-uint8_t Stepper::last_direction_bits = 0;        // The next stepping-bits to be output
+uint8_t Stepper::last_direction_bits = 0,       // The next stepping-bits to be output
-int16_t Stepper::cleaning_buffer_counter = 0;
+        Stepper::last_movement_extruder = 0xFF; // Last movement extruder, as computed when the last movement was fetched from planner
+bool Stepper::abort_current_block,              // Signals to the stepper that current block should be aborted
+     Stepper::last_movement_non_null[NUM_AXIS]; // Last Movement in the given direction is not null, as computed when the last movement was fetched from planner
 
 #if ENABLED(X_DUAL_ENDSTOPS)
   bool Stepper::locked_x_motor = false, Stepper::locked_x2_motor = false;
@@ -118,7 +116,7 @@ int32_t Stepper::counter_X = 0,
         Stepper::counter_Z = 0,
         Stepper::counter_E = 0;
 
-volatile uint32_t Stepper::step_events_completed = 0; // The number of step events executed in the current block
+uint32_t Stepper::step_events_completed = 0; // The number of step events executed in the current block
 
 #if ENABLED(BEZIER_JERK_CONTROL)
   int32_t __attribute__((used)) Stepper::bezier_A __asm__("bezier_A");    // A coefficient in Bézier speed curve with alias for assembler
@@ -132,14 +130,15 @@ volatile uint32_t Stepper::step_events_completed = 0; // The number of step even
   bool Stepper::bezier_2nd_half;    // =false If Bézier curve has been initialized or not
 #endif
 
+uint32_t Stepper::nextMainISR = 0;
+bool Stepper::all_steps_done = false;
+
 #if ENABLED(LIN_ADVANCE)
 
   uint32_t Stepper::LA_decelerate_after;
 
-  constexpr hal_timer_t ADV_NEVER = HAL_TIMER_TYPE_MAX;
+  constexpr uint32_t ADV_NEVER = 0xFFFFFFFF;
-
+  uint32_t Stepper::nextAdvanceISR = ADV_NEVER,
-  hal_timer_t Stepper::nextMainISR = 0,
-              Stepper::nextAdvanceISR = ADV_NEVER,
            Stepper::eISR_Rate = ADV_NEVER;
   uint16_t Stepper::current_adv_steps = 0,
            Stepper::final_adv_steps,
@@ -157,7 +156,7 @@ volatile uint32_t Stepper::step_events_completed = 0; // The number of step even
 
 #endif // LIN_ADVANCE
 
-int32_t Stepper::acceleration_time, Stepper::deceleration_time;
+uint32_t Stepper::acceleration_time, Stepper::deceleration_time;
 
 volatile int32_t Stepper::count_position[NUM_AXIS] = { 0 };
 volatile signed char Stepper::count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
@@ -166,11 +165,11 @@ volatile signed char Stepper::count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
   int32_t Stepper::counter_m[MIXING_STEPPERS];
 #endif
 
+uint32_t Stepper::ticks_nominal;
 uint8_t Stepper::step_loops, Stepper::step_loops_nominal;
 
-hal_timer_t Stepper::OCR1A_nominal;
 #if DISABLED(BEZIER_JERK_CONTROL)
-  hal_timer_t Stepper::acc_step_rate; // needed for deceleration start point
+  uint32_t Stepper::acc_step_rate; // needed for deceleration start point
 #endif
 
 volatile int32_t Stepper::endstops_trigsteps[XYZ];
@@ -185,12 +184,12 @@ volatile int32_t Stepper::endstops_trigsteps[XYZ];
   #define DUAL_ENDSTOP_APPLY_STEP(A,V)                                                                                                           \
     if (performing_homing) {                                                                                                                        \
       if (A##_HOME_DIR < 0) {                                                                                                                    \
-        if (!(TEST(endstops.old_endstop_bits, A##_MIN) && count_direction[_AXIS(A)] < 0) && !LOCKED_##A##_MOTOR) A##_STEP_WRITE(V);     \
+        if (!(TEST(endstops.current_endstop_bits, A##_MIN) && count_direction[_AXIS(A)] < 0) && !LOCKED_##A##_MOTOR) A##_STEP_WRITE(V);     \
-        if (!(TEST(endstops.old_endstop_bits, A##2_MIN) && count_direction[_AXIS(A)] < 0) && !LOCKED_##A##2_MOTOR) A##2_STEP_WRITE(V);  \
+        if (!(TEST(endstops.current_endstop_bits, A##2_MIN) && count_direction[_AXIS(A)] < 0) && !LOCKED_##A##2_MOTOR) A##2_STEP_WRITE(V);  \
       }                                                                                                                                             \
       else {                                                                                                                                        \
-        if (!(TEST(endstops.old_endstop_bits, A##_MAX) && count_direction[_AXIS(A)] > 0) && !LOCKED_##A##_MOTOR) A##_STEP_WRITE(V);     \
+        if (!(TEST(endstops.current_endstop_bits, A##_MAX) && count_direction[_AXIS(A)] > 0) && !LOCKED_##A##_MOTOR) A##_STEP_WRITE(V);     \
-        if (!(TEST(endstops.old_endstop_bits, A##2_MAX) && count_direction[_AXIS(A)] > 0) && !LOCKED_##A##2_MOTOR) A##2_STEP_WRITE(V);  \
+        if (!(TEST(endstops.current_endstop_bits, A##2_MAX) && count_direction[_AXIS(A)] > 0) && !LOCKED_##A##2_MOTOR) A##2_STEP_WRITE(V);  \
       }                                                                                                                                             \
     }                                                                                                                                               \
     else {                                                                                                                                          \
@@ -319,10 +318,6 @@ void Stepper::set_directions() {
   #endif // !LIN_ADVANCE
 }
 
-#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
-  extern volatile uint8_t e_hit;
-#endif
-
 #if ENABLED(BEZIER_JERK_CONTROL)
   /**
    *   We are using a quintic (fifth-degree) Bézier polynomial for the velocity curve.
@@ -379,7 +374,7 @@ void Stepper::set_directions() {
    *
    *   Floating point arithmetic execution time cost is prohibitive, so we will transform the math to
    * use fixed point values to be able to evaluate it in realtime. Assuming a maximum of 250000 steps
-   * per second (driver pulses should at least be 2uS hi/2uS lo), and allocating 2 bits to avoid
+   * per second (driver pulses should at least be 2µS hi/2µS lo), and allocating 2 bits to avoid
    * overflows on the evaluation of the Bézier curve, means we can use
    *
    *   t: unsigned Q0.32 (0 <= t < 1) |range 0 to 0xFFFFFFFF unsigned
@@ -1149,11 +1144,27 @@ void Stepper::set_directions() {
 HAL_STEP_TIMER_ISR {
   HAL_timer_isr_prologue(STEP_TIMER_NUM);
 
-  #if ENABLED(LIN_ADVANCE)
+  // Program timer compare for the maximum period, so it does NOT
-    Stepper::advance_isr_scheduler();
+  // flag an interrupt while this ISR is running - So changes from small
-  #else
+  // periods to big periods are respected and the timer does not reset to 0
-    Stepper::isr();
+  HAL_timer_set_compare(STEP_TIMER_NUM, HAL_TIMER_TYPE_MAX);
-  #endif
+
+  // Call the ISR scheduler
+  hal_timer_t ticks = Stepper::isr_scheduler();
+
+  // Now 'ticks' contains the period to the next Stepper ISR.
+  // Potential problem: Since the timer continues to run, the requested
+  // compare value may already have passed.
+  //
+  // Assuming at least 6µs between calls to this ISR...
+  // On AVR the ISR epilogue is estimated at 40 instructions - close to 2.5µS.
+  // On ARM the ISR epilogue is estimated at 10 instructions - close to 200nS.
+  // In either case leave at least 4µS for other tasks to execute.
+  const hal_timer_t minticks = HAL_timer_get_count(STEP_TIMER_NUM) + hal_timer_t((HAL_TICKS_PER_US) * 4); // ISR never takes more than 1ms, so this shouldn't cause trouble
+  NOLESS(ticks, MAX(minticks, hal_timer_t((STEP_TIMER_MIN_INTERVAL) * (HAL_TICKS_PER_US))));
+
+  // Set the next ISR to fire at the proper time
+  HAL_timer_set_compare(STEP_TIMER_NUM, ticks);
 
   HAL_timer_isr_epilogue(STEP_TIMER_NUM);
 }
@@ -1164,168 +1175,73 @@ HAL_STEP_TIMER_ISR {
   #define STEP_MULTIPLY(A,B) MultiU24X32toH16(A, B)
 #endif
 
-void Stepper::isr() {
+hal_timer_t Stepper::isr_scheduler() {
+  uint32_t interval;
 
-  #define ENDSTOP_NOMINAL_OCR_VAL 1500 * HAL_TICKS_PER_US // Check endstops every 1.5ms to guarantee two stepper ISRs within 5ms for BLTouch
+  // Run main stepping pulse phase ISR if we have to
-  #define OCR_VAL_TOLERANCE        500 * HAL_TICKS_PER_US // First max delay is 2.0ms, last min delay is 0.5ms, all others 1.5ms
+  if (!nextMainISR) Stepper::stepper_pulse_phase_isr();
-
-  hal_timer_t ocr_val;
-  static uint32_t step_remaining = 0;  // SPLIT function always runs.  This allows 16 bit timers to be
-                                       // used to generate the stepper ISR.
-  #define SPLIT(L) do { \
-    if (L > ENDSTOP_NOMINAL_OCR_VAL) { \
-      const uint32_t remainder = (uint32_t)L % (ENDSTOP_NOMINAL_OCR_VAL); \
-      ocr_val = (remainder < OCR_VAL_TOLERANCE) ? ENDSTOP_NOMINAL_OCR_VAL + remainder : ENDSTOP_NOMINAL_OCR_VAL; \
-      step_remaining = (uint32_t)L - ocr_val; \
-    } \
-    else \
-      ocr_val = L;\
-  }while(0)
-
-  // Time remaining before the next step?
-  if (step_remaining) {
-
-    // Make sure endstops are updated
-    if (ENDSTOPS_ENABLED) endstops.update();
-
-    // Next ISR either for endstops or stepping
-    ocr_val = step_remaining <= ENDSTOP_NOMINAL_OCR_VAL ? step_remaining : ENDSTOP_NOMINAL_OCR_VAL;
-    step_remaining -= ocr_val;
-    _NEXT_ISR(ocr_val);
-
-    #if DISABLED(LIN_ADVANCE)
-      HAL_timer_restrain(STEP_TIMER_NUM, STEP_TIMER_MIN_INTERVAL * HAL_TICKS_PER_US);
-    #endif
-
-    return;
-  }
-
-  //
-  // When cleaning, discard the current block and run fast
-  //
-  if (cleaning_buffer_counter) {
-    if (cleaning_buffer_counter < 0) {          // Count up for endstop hit
-      if (current_block) planner.discard_current_block(); // Discard the active block that led to the trigger
-      if (!planner.discard_continued_block())   // Discard next CONTINUED block
-        cleaning_buffer_counter = 0;            // Keep discarding until non-CONTINUED
-    }
-    else {
-      planner.discard_current_block();
-      --cleaning_buffer_counter;                // Count down for abort print
-      #if ENABLED(SD_FINISHED_STEPPERRELEASE) && defined(SD_FINISHED_RELEASECOMMAND)
-        if (!cleaning_buffer_counter) enqueue_and_echo_commands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
-      #endif
-    }
-    current_block = NULL;                       // Prep to get a new block after cleaning
-    _NEXT_ISR(HAL_STEPPER_TIMER_RATE / 10000);  // Run at max speed - 10 KHz
-    return;
-  }
-
-  // If there is no current block, attempt to pop one from the buffer
-  if (!current_block) {
-
-    // Anything in the buffer?
-    if ((current_block = planner.get_current_block())) {
-
-      // Sync block? Sync the stepper counts and return
-      while (TEST(current_block->flag, BLOCK_BIT_SYNC_POSITION)) {
-        _set_position(
-          current_block->steps[A_AXIS], current_block->steps[B_AXIS],
-          current_block->steps[C_AXIS], current_block->steps[E_AXIS]
-        );
-        planner.discard_current_block();
-        if (!(current_block = planner.get_current_block())) return;
-      }
-
-      // Initialize the trapezoid generator from the current block.
-      static int8_t last_extruder = -1;
 
   #if ENABLED(LIN_ADVANCE)
-        #if E_STEPPERS > 1
+    // Run linear advance stepper ISR if we have to
-          if (current_block->active_extruder != last_extruder) {
+    if (!nextAdvanceISR) nextAdvanceISR = Stepper::advance_isr();
-            current_adv_steps = 0; // If the now active extruder wasn't in use during the last move, its pressure is most likely gone.
-            LA_active_extruder = current_block->active_extruder;
-          }
   #endif
 
-        if ((use_advance_lead = current_block->use_advance_lead)) {
+  // ^== Time critical. NOTHING besides pulse generation should be above here!!!
-          LA_decelerate_after = current_block->decelerate_after;
+
-          final_adv_steps = current_block->final_adv_steps;
+  // Run main stepping block processing ISR if we have to
-          max_adv_steps = current_block->max_adv_steps;
+  if (!nextMainISR) nextMainISR = Stepper::stepper_block_phase_isr();
-        }
+
+  #if ENABLED(LIN_ADVANCE)
+    // Select the closest interval in time
+    interval = (nextAdvanceISR <= nextMainISR)
+      ? nextAdvanceISR
+      : nextMainISR;
+
+  #else // !ENABLED(LIN_ADVANCE)
+
+    // The interval is just the remaining time to the stepper ISR
+    interval = nextMainISR;
   #endif
 
-      if (current_block->direction_bits != last_direction_bits || current_block->active_extruder != last_extruder) {
+  // Limit the value to the maximum possible value of the timer
-        last_direction_bits = current_block->direction_bits;
+  if (interval > HAL_TIMER_TYPE_MAX)
-        last_extruder = current_block->active_extruder;
+    interval = HAL_TIMER_TYPE_MAX;
-        set_directions();
+
+  // Compute the time remaining for the main isr
+  nextMainISR -= interval;
+
+  #if ENABLED(LIN_ADVANCE)
+    // Compute the time remaining for the advance isr
+    if (nextAdvanceISR != ADV_NEVER)
+      nextAdvanceISR -= interval;
+  #endif
+
+  return (hal_timer_t)interval;
 }
 
-      // No acceleration / deceleration time elapsed so far
+// This part of the ISR should ONLY create the pulses for the steppers
-      acceleration_time = deceleration_time = 0;
+// -- Nothing more, nothing less -- We want to avoid jitter from where
+// the pulses should be generated (when the interrupt triggers) to the
+// time pulses are actually created. So, PLEASE DO NOT PLACE ANY CODE
+// above this line that can conditionally change that time (we are trying
+// to keep the delay between the interrupt triggering and pulse generation
+// as constant as possible!!!!
+void Stepper::stepper_pulse_phase_isr() {
 
-      // No step events completed so far
+  // If we must abort the current block, do so!
-      step_events_completed = 0;
+  if (abort_current_block) {
-
+    abort_current_block = false;
-      // step_rate to timer interval
+    if (current_block) {
-      OCR1A_nominal = calc_timer_interval(current_block->nominal_rate);
+      current_block = NULL;
-
+      planner.discard_current_block();
-      // make a note of the number of step loops required at nominal speed
-      step_loops_nominal = step_loops;
-
-      #if DISABLED(BEZIER_JERK_CONTROL)
-        // Set as deceleration point the initial rate of the block
-        acc_step_rate = current_block->initial_rate;
-      #endif
-
-      #if ENABLED(BEZIER_JERK_CONTROL)
-        // Initialize the Bézier speed curve
-        _calc_bezier_curve_coeffs(current_block->initial_rate, current_block->cruise_rate, current_block->acceleration_time_inverse);
-
-        // We have not started the 2nd half of the trapezoid
-        bezier_2nd_half = false;
-      #endif
-
-      // Initialize Bresenham counters to 1/2 the ceiling
-      counter_X = counter_Y = counter_Z = counter_E = -(current_block->step_event_count >> 1);
-      #if ENABLED(MIXING_EXTRUDER)
-        MIXING_STEPPERS_LOOP(i)
-          counter_m[i] = -(current_block->mix_event_count[i] >> 1);
-      #endif
-
-      #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
-        e_hit = 2; // Needed for the case an endstop is already triggered before the new move begins.
-                   // No 'change' can be detected.
-      #endif
-
-      #if ENABLED(Z_LATE_ENABLE)
-        // If delayed Z enable, postpone move for 1mS
-        if (current_block->steps[Z_AXIS] > 0) {
-          enable_Z();
-          _NEXT_ISR(HAL_STEPPER_TIMER_RATE / 1000); // Run at slow speed - 1 KHz
-          return;
-        }
-      #endif
-    }
-    else {
-      // If no more queued moves, postpone next check for 1mS
-      _NEXT_ISR(HAL_STEPPER_TIMER_RATE / 1000); // Run at slow speed - 1 KHz
-      return;
     }
   }
 
-  // Update endstops state, if enabled
+  // If there is no current block, do nothing
-  #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
+  if (!current_block) return;
-    if (e_hit && ENDSTOPS_ENABLED) {
-      endstops.update();
-      e_hit--;
-    }
-  #else
-    if (ENDSTOPS_ENABLED) endstops.update();
-  #endif
 
   // Take multiple steps per interrupt (For high speed moves)
-  bool all_steps_done = false;
+  all_steps_done = false;
   for (uint8_t i = step_loops; i--;) {
 
     #define _COUNTER(AXIS) counter_## AXIS
@@ -1520,13 +1436,26 @@ void Stepper::isr() {
     #endif
 
   } // steps_loop
+}
+
+// This is the last half of the stepper interrupt: This one processes and
+// properly schedules blocks from the planner. This is executed after creating
+// the step pulses, so it is not time critical, as pulses are already done.
+
+uint32_t Stepper::stepper_block_phase_isr() {
+
+  // If no queued movements, just wait 1ms for the next move
+  uint32_t interval = (HAL_STEPPER_TIMER_RATE / 1000);
+
+  // If there is a current block
+  if (current_block) {
 
     // Calculate new timer value
-  if (step_events_completed <= (uint32_t)current_block->accelerate_until) {
+    if (step_events_completed <= current_block->accelerate_until) {
 
       #if ENABLED(BEZIER_JERK_CONTROL)
         // Get the next speed to use (Jerk limited!)
-      hal_timer_t acc_step_rate =
+        uint32_t acc_step_rate =
           acceleration_time < current_block->acceleration_time
             ? _eval_bezier_curve(acceleration_time)
             : current_block->cruise_rate;
@@ -1536,11 +1465,7 @@ void Stepper::isr() {
       #endif
 
       // step_rate to timer interval
-    const hal_timer_t interval = calc_timer_interval(acc_step_rate);
+      interval = calc_timer_interval(acc_step_rate);
-
-    SPLIT(interval);  // split step into multiple ISRs if larger than ENDSTOP_NOMINAL_OCR_VAL
-    _NEXT_ISR(ocr_val);
-
       acceleration_time += interval;
 
       #if ENABLED(LIN_ADVANCE)
@@ -1556,13 +1481,12 @@ void Stepper::isr() {
         }
       #endif // LIN_ADVANCE
     }
-  else if (step_events_completed > (uint32_t)current_block->decelerate_after) {
+    else if (step_events_completed > current_block->decelerate_after) {
-    hal_timer_t step_rate;
+      uint32_t step_rate;
 
       #if ENABLED(BEZIER_JERK_CONTROL)
         // If this is the 1st time we process the 2nd half of the trapezoid...
         if (!bezier_2nd_half) {
-
           // Initialize the Bézier speed curve
           _calc_bezier_curve_coeffs(current_block->cruise_rate, current_block->final_rate, current_block->deceleration_time_inverse);
           bezier_2nd_half = true;
@@ -1582,20 +1506,15 @@ void Stepper::isr() {
         }
         else
           step_rate = current_block->final_rate;
-
       #endif
 
       // step_rate to timer interval
-    const hal_timer_t interval = calc_timer_interval(step_rate);
+      interval = calc_timer_interval(step_rate);
-
-    SPLIT(interval);  // split step into multiple ISRs if larger than ENDSTOP_NOMINAL_OCR_VAL
-    _NEXT_ISR(ocr_val);
-
       deceleration_time += interval;
 
       #if ENABLED(LIN_ADVANCE)
         if (current_block->use_advance_lead) {
-        if (step_events_completed <= (uint32_t)current_block->decelerate_after + step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) {
+          if (step_events_completed <= current_block->decelerate_after + step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) {
             nextAdvanceISR = 0; // Wake up eISR on first deceleration loop
             eISR_Rate = current_block->advance_speed;
           }
@@ -1609,22 +1528,17 @@ void Stepper::isr() {
     else {
 
       #if ENABLED(LIN_ADVANCE)
-      // If we have esteps to execute, fire the next advance_isr "now"
+        // If there are any esteps, fire the next advance_isr "now"
         if (e_steps && eISR_Rate != current_block->advance_speed) nextAdvanceISR = 0;
       #endif
 
-    SPLIT(OCR1A_nominal);  // split step into multiple ISRs if larger than ENDSTOP_NOMINAL_OCR_VAL
+      // The timer interval is just the nominal value for the nominal speed
-    _NEXT_ISR(ocr_val);
+      interval = ticks_nominal;
 
-    // ensure we're running at the correct step rate, even if we just came off an acceleration
+      // Ensure this runs at the correct step rate, even if it just came off an acceleration
       step_loops = step_loops_nominal;
     }
 
-  #if DISABLED(LIN_ADVANCE)
-    // Make sure stepper ISR doesn't monopolize the CPU
-    HAL_timer_restrain(STEP_TIMER_NUM, STEP_TIMER_MIN_INTERVAL * HAL_TICKS_PER_US);
-  #endif
-
     // If current block is finished, reset pointer
     if (all_steps_done) {
       current_block = NULL;
@@ -1632,14 +1546,113 @@ void Stepper::isr() {
     }
   }
 
+  // If there is no current block at this point, attempt to pop one from the buffer
+  // and prepare its movement
+  if (!current_block) {
+
+    // Anything in the buffer?
+    if ((current_block = planner.get_current_block())) {
+
+      // Sync block? Sync the stepper counts and return
+      while (TEST(current_block->flag, BLOCK_BIT_SYNC_POSITION)) {
+        _set_position(
+          current_block->position[A_AXIS], current_block->position[B_AXIS],
+          current_block->position[C_AXIS], current_block->position[E_AXIS]
+        );
+        planner.discard_current_block();
+
+        // Try to get a new block
+        if (!(current_block = planner.get_current_block()))
+          return interval; // No more queued movements!
+      }
+
+      // Compute movement direction for proper endstop handling
+      LOOP_NA(i) last_movement_non_null[i] = !!current_block->steps[i];
+
+      // Initialize the trapezoid generator from the current block.
+      #if ENABLED(LIN_ADVANCE)
+        #if E_STEPPERS > 1
+          if (current_block->active_extruder != last_movement_extruder) {
+            current_adv_steps = 0; // If the now active extruder wasn't in use during the last move, its pressure is most likely gone.
+            LA_active_extruder = current_block->active_extruder;
+          }
+        #endif
+
+        if ((use_advance_lead = current_block->use_advance_lead)) {
+          LA_decelerate_after = current_block->decelerate_after;
+          final_adv_steps = current_block->final_adv_steps;
+          max_adv_steps = current_block->max_adv_steps;
+        }
+      #endif
+
+      if (current_block->direction_bits != last_direction_bits || current_block->active_extruder != last_movement_extruder) {
+        last_direction_bits = current_block->direction_bits;
+        last_movement_extruder = current_block->active_extruder;
+        set_directions();
+      }
+
+      // At this point, we must ensure the movement about to execute isn't
+      // trying to force the head against a limit switch. If using interrupt-
+      // driven change detection, and already against a limit then no call to
+      // the endstop_triggered method will be done and the movement will be
+      // done against the endstop. So, check the limits here: If the movement
+      // is against the limits, the block will be marked as to be killed, and
+      // on the next call to this ISR, will be discarded.
+      endstops.check_possible_change();
+
+      // No acceleration / deceleration time elapsed so far
+      acceleration_time = deceleration_time = 0;
+
+      // No step events completed so far
+      step_events_completed = 0;
+
+      // step_rate to timer interval for the nominal speed
+      ticks_nominal = calc_timer_interval(current_block->nominal_rate);
+
+      // make a note of the number of step loops required at nominal speed
+      step_loops_nominal = step_loops;
+
+      #if DISABLED(BEZIER_JERK_CONTROL)
+        // Set as deceleration point the initial rate of the block
+        acc_step_rate = current_block->initial_rate;
+      #endif
+
+      #if ENABLED(BEZIER_JERK_CONTROL)
+        // Initialize the Bézier speed curve
+        _calc_bezier_curve_coeffs(current_block->initial_rate, current_block->cruise_rate, current_block->acceleration_time_inverse);
+
+        // We have not started the 2nd half of the trapezoid
+        bezier_2nd_half = false;
+      #endif
+
+      // Initialize Bresenham counters to 1/2 the ceiling
+      counter_X = counter_Y = counter_Z = counter_E = -((int32_t)(current_block->step_event_count >> 1));
+      #if ENABLED(MIXING_EXTRUDER)
+        MIXING_STEPPERS_LOOP(i)
+          counter_m[i] = -(current_block->mix_event_count[i] >> 1);
+      #endif
+
+      #if ENABLED(Z_LATE_ENABLE)
+        // If delayed Z enable, enable it now. This option will severely interfere with
+        //  timing between pulses when chaining motion between blocks, and it could lead
+        //  to lost steps in both X and Y axis, so avoid using it unless strictly necessary!!
+        if (current_block->steps[Z_AXIS]) enable_Z();
+      #endif
+    }
+  }
+
+  // Return the interval to wait
+  return interval;
+}
+
 #if ENABLED(LIN_ADVANCE)
 
   #define CYCLES_EATEN_E (E_STEPPERS * 5)
   #define EXTRA_CYCLES_E (STEP_PULSE_CYCLES - (CYCLES_EATEN_E))
 
   // Timer interrupt for E. e_steps is set in the main routine;
-
+  uint32_t Stepper::advance_isr() {
-  void Stepper::advance_isr() {
+    uint32_t interval;
 
     #if ENABLED(MK2_MULTIPLEXER) // For SNMM even-numbered steppers are reversed
       #define SET_E_STEP_DIR(INDEX) do{ if (e_steps) E0_DIR_WRITE(e_steps < 0 ? !INVERT_E## INDEX ##_DIR ^ TEST(INDEX, 0) : INVERT_E## INDEX ##_DIR ^ TEST(INDEX, 0)); }while(0)
@@ -1700,21 +1713,21 @@ void Stepper::isr() {
       if (step_events_completed > LA_decelerate_after && current_adv_steps > final_adv_steps) {
         e_steps--;
         current_adv_steps--;
-        nextAdvanceISR = eISR_Rate;
+        interval = eISR_Rate;
       }
       else if (step_events_completed < LA_decelerate_after && current_adv_steps < max_adv_steps) {
              //step_events_completed <= (uint32_t)current_block->accelerate_until) {
         e_steps++;
         current_adv_steps++;
-        nextAdvanceISR = eISR_Rate;
+        interval = eISR_Rate;
       }
       else {
-        nextAdvanceISR = ADV_NEVER;
+        interval = ADV_NEVER;
         eISR_Rate = ADV_NEVER;
       }
     }
     else
-      nextAdvanceISR = ADV_NEVER;
+      interval = ADV_NEVER;
 
     switch (LA_active_extruder) {
       case 0: SET_E_STEP_DIR(0); break;
@@ -1787,39 +1800,9 @@ void Stepper::isr() {
       #endif
 
     } // e_steps
+
+    return interval;
   }
-
-  void Stepper::advance_isr_scheduler() {
-
-    // Run main stepping ISR if flagged
-    if (!nextMainISR) isr();
-
-    // Run Advance stepping ISR if flagged
-    if (!nextAdvanceISR) advance_isr();
-
-    // Is the next advance ISR scheduled before the next main ISR?
-    if (nextAdvanceISR <= nextMainISR) {
-      // Set up the next interrupt
-      HAL_timer_set_compare(STEP_TIMER_NUM, nextAdvanceISR);
-      // New interval for the next main ISR
-      if (nextMainISR) nextMainISR -= nextAdvanceISR;
-      // Will call Stepper::advance_isr on the next interrupt
-      nextAdvanceISR = 0;
-    }
-    else {
-      // The next main ISR comes first
-      HAL_timer_set_compare(STEP_TIMER_NUM, nextMainISR);
-      // New interval for the next advance ISR, if any
-      if (nextAdvanceISR && nextAdvanceISR != ADV_NEVER)
-        nextAdvanceISR -= nextMainISR;
-      // Will call Stepper::isr on the next interrupt
-      nextMainISR = 0;
-    }
-
-    // Make sure stepper ISR doesn't monopolize the CPU
-    HAL_timer_restrain(STEP_TIMER_NUM, STEP_TIMER_MIN_INTERVAL * HAL_TICKS_PER_US);
-  }
-
 #endif // LIN_ADVANCE
 
 void Stepper::init() {
@@ -1924,9 +1907,6 @@ void Stepper::init() {
     if (!E_ENABLE_ON) E4_ENABLE_WRITE(HIGH);
   #endif
 
-  // Init endstops and pullups
-  endstops.init();
-
   #define _STEP_INIT(AXIS) AXIS ##_STEP_INIT
   #define _WRITE_STEP(AXIS, HIGHLOW) AXIS ##_STEP_WRITE(HIGHLOW)
   #define _DISABLE(AXIS) disable_## AXIS()
@@ -2048,31 +2028,33 @@ void Stepper::_set_position(const int32_t &a, const int32_t &b, const int32_t &c
  * Get a stepper's position in steps.
  */
 int32_t Stepper::position(const AxisEnum axis) {
-  CRITICAL_SECTION_START;
+  #ifdef __AVR__
-  const int32_t count_pos = count_position[axis];
+    // Protect the access to the position. Only required for AVR, as
-  CRITICAL_SECTION_END;
+    //  any 32bit CPU offers atomic access to 32bit variables
-  return count_pos;
+    const bool was_enabled = STEPPER_ISR_ENABLED();
-}
+    if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
-
-void Stepper::finish_and_disable() {
-  planner.synchronize();
-  disable_all_steppers();
-}
-
-void Stepper::quick_stop() {
-  DISABLE_STEPPER_DRIVER_INTERRUPT();
-  kill_current_block();
-  current_block = NULL;
-  cleaning_buffer_counter = 5000;
-  planner.clear_block_buffer();
-  ENABLE_STEPPER_DRIVER_INTERRUPT();
-  #if ENABLED(ULTRA_LCD)
-    planner.clear_block_buffer_runtime();
   #endif
+
+  int32_t v = count_position[axis];
+
+  #ifdef __AVR__
+    // Reenable Stepper ISR
+    if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
+  #endif
+  return v;
 }
 
+// Signal endstops were triggered - This function can be called from
+// an ISR context  (Temperature, Stepper or limits ISR), so we must
+// be very careful here. If the interrupt being preempted was the
+// Stepper ISR (this CAN happen with the endstop limits ISR) then
+// when the stepper ISR resumes, we must be very sure that the movement
+// is properly cancelled
 void Stepper::endstop_triggered(const AxisEnum axis) {
 
+  const bool was_enabled = STEPPER_ISR_ENABLED();
+  if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
+
   #if IS_CORE
 
     endstops_trigsteps[axis] = 0.5f * (
@@ -2086,16 +2068,41 @@ void Stepper::endstop_triggered(const AxisEnum axis) {
 
   #endif // !COREXY && !COREXZ && !COREYZ
 
-  kill_current_block();
+  // Discard the rest of the move if there is a current block
-  cleaning_buffer_counter = -1; // Discard the rest of the move
+  quick_stop();
+
+  if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
+}
+
+int32_t Stepper::triggered_position(const AxisEnum axis) {
+  #ifdef __AVR__
+    // Protect the access to the position. Only required for AVR, as
+    //  any 32bit CPU offers atomic access to 32bit variables
+    const bool was_enabled = STEPPER_ISR_ENABLED();
+    if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
+  #endif
+
+  const int32_t v = endstops_trigsteps[axis];
+
+  #ifdef __AVR__
+    // Reenable Stepper ISR
+    if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
+  #endif
+
+  return v;
 }
 
 void Stepper::report_positions() {
-  CRITICAL_SECTION_START;
+
+  // Protect the access to the position.
+  const bool was_enabled = STEPPER_ISR_ENABLED();
+  if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
+
   const int32_t xpos = count_position[X_AXIS],
                 ypos = count_position[Y_AXIS],
                 zpos = count_position[Z_AXIS];
-  CRITICAL_SECTION_END;
+
+  if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
 
   #if CORE_IS_XY || CORE_IS_XZ || IS_DELTA || IS_SCARA
     SERIAL_PROTOCOLPGM(MSG_COUNT_A);

Marlin/src/module/stepper.h

@@ -62,10 +62,6 @@ class Stepper {
 
     static block_t* current_block;  // A pointer to the block currently being traced
 
-    #if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
-      static bool abort_on_endstop_hit;
-    #endif
-
     #if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
       static bool performing_homing;
     #endif
@@ -77,11 +73,12 @@ class Stepper {
       static uint32_t motor_current_setting[3];
     #endif
 
-    static int16_t cleaning_buffer_counter;
-
   private:
 
-    static uint8_t last_direction_bits;        // The next stepping-bits to be output
+    static uint8_t last_direction_bits,           // The next stepping-bits to be output
+                   last_movement_extruder;        // Last movement extruder, as computed when the last movement was fetched from planner
+    static bool abort_current_block,              // Signals to the stepper that current block should be aborted
+                last_movement_non_null[NUM_AXIS]; // Last Movement in the given direction is not null, as computed when the last movement was fetched from planner
 
     #if ENABLED(X_DUAL_ENDSTOPS)
       static bool locked_x_motor, locked_x2_motor;
@@ -95,7 +92,7 @@ class Stepper {
 
     // Counter variables for the Bresenham line tracer
     static int32_t counter_X, counter_Y, counter_Z, counter_E;
-    static volatile uint32_t step_events_completed; // The number of step events executed in the current block
+    static uint32_t step_events_completed; // The number of step events executed in the current block
 
     #if ENABLED(BEZIER_JERK_CONTROL)
       static int32_t bezier_A,     // A coefficient in Bézier speed curve
@@ -109,12 +106,14 @@ class Stepper {
       static bool bezier_2nd_half; // If Bézier curve has been initialized or not
     #endif
 
+    static uint32_t nextMainISR;   // time remaining for the next Step ISR
+    static bool all_steps_done;    // all steps done
+
     #if ENABLED(LIN_ADVANCE)
 
       static uint32_t LA_decelerate_after; // Copy from current executed block. Needed because current_block is set to NULL "too early".
-      static hal_timer_t nextMainISR, nextAdvanceISR, eISR_Rate;
+      static uint32_t nextAdvanceISR, eISR_Rate;
       static uint16_t current_adv_steps, final_adv_steps, max_adv_steps; // Copy from current executed block. Needed because current_block is set to NULL "too early".
-      #define _NEXT_ISR(T) nextMainISR = T
       static int8_t e_steps;
       static bool use_advance_lead;
       #if E_STEPPERS > 1
@@ -123,18 +122,14 @@ class Stepper {
         static constexpr int8_t LA_active_extruder = 0;
       #endif
 
-    #else // !LIN_ADVANCE
+    #endif // LIN_ADVANCE
 
-      #define _NEXT_ISR(T) HAL_timer_set_compare(STEP_TIMER_NUM, T);
+    static uint32_t acceleration_time, deceleration_time;
-
-    #endif // !LIN_ADVANCE
-
-    static int32_t acceleration_time, deceleration_time;
     static uint8_t step_loops, step_loops_nominal;
 
-    static hal_timer_t OCR1A_nominal;
+    static uint32_t ticks_nominal;
     #if DISABLED(BEZIER_JERK_CONTROL)
-      static hal_timer_t acc_step_rate; // needed for deceleration start point
+      static uint32_t acc_step_rate; // needed for deceleration start point
     #endif
 
     static volatile int32_t endstops_trigsteps[XYZ];
@@ -167,88 +162,53 @@ class Stepper {
     //
     Stepper() { };
 
-    //
     // Initialize stepper hardware
-    //
     static void init();
 
-    //
     // Interrupt Service Routines
-    //
 
-    static void isr();
+    // The ISR scheduler
+    static hal_timer_t isr_scheduler();
+
+    // The stepper pulse phase ISR
+    static void stepper_pulse_phase_isr();
+
+    // The stepper block processing phase ISR
+    static uint32_t stepper_block_phase_isr();
 
     #if ENABLED(LIN_ADVANCE)
-      static void advance_isr();
+      // The Linear advance stepper ISR
-      static void advance_isr_scheduler();
+      static uint32_t advance_isr();
     #endif
 
-    //
-    // Set the current position in steps
-    //
-    static void _set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e);
-
-    FORCE_INLINE static void _set_position(const AxisEnum a, const int32_t &v) { count_position[a] = v; }
-
-    FORCE_INLINE static void set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) {
-      planner.synchronize();
-      CRITICAL_SECTION_START;
-      _set_position(a, b, c, e);
-      CRITICAL_SECTION_END;
-    }
-
-    static void set_position(const AxisEnum a, const int32_t &v) {
-      planner.synchronize();
-      CRITICAL_SECTION_START;
-      count_position[a] = v;
-      CRITICAL_SECTION_END;
-    }
-
-    FORCE_INLINE static void _set_e_position(const int32_t &e) { count_position[E_AXIS] = e; }
-
-    static void set_e_position(const int32_t &e) {
-      planner.synchronize();
-      CRITICAL_SECTION_START;
-      count_position[E_AXIS] = e;
-      CRITICAL_SECTION_END;
-    }
-
-    //
-    // Set direction bits for all steppers
-    //
-    static void set_directions();
-
-    //
     // Get the position of a stepper, in steps
-    //
     static int32_t position(const AxisEnum axis);
 
-    //
     // Report the positions of the steppers, in steps
-    //
     static void report_positions();
 
-    //
     // 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.
-    //
     static void wake_up();
 
-    //
+    // Quickly stop all steppers
-    // Wait for moves to finish and disable all steppers
+    FORCE_INLINE static void quick_stop() { abort_current_block = true; }
-    //
-    static void finish_and_disable();
 
-    //
-    // Quickly stop all steppers and clear the blocks queue
-    //
-    static void quick_stop();
-
-    //
     // The direction of a single motor
-    //
     FORCE_INLINE static bool motor_direction(const AxisEnum axis) { return TEST(last_direction_bits, axis); }
 
+    // The last movement direction was not null on the specified axis. Note that motor direction is not necessarily the same.
+    FORCE_INLINE static bool movement_non_null(const AxisEnum axis) { return last_movement_non_null[axis]; }
+
+    // The extruder associated to the last movement
+    FORCE_INLINE static uint8_t movement_extruder() { return last_movement_extruder; }
+
+    // Handle a triggered endstop
+    static void endstop_triggered(const AxisEnum axis);
+
+    // Triggered position of an axis in steps
+    static int32_t triggered_position(const AxisEnum axis);
+
     #if HAS_DIGIPOTSS || HAS_MOTOR_CURRENT_PWM
       static void digitalPotWrite(const int16_t address, const int16_t value);
       static void digipot_current(const uint8_t driver, const int16_t current);
@@ -280,34 +240,24 @@ class Stepper {
       static void babystep(const AxisEnum axis, const bool direction); // perform a short step with a single stepper motor, outside of any convention
     #endif
 
-    static inline void kill_current_block() {
-      step_events_completed = current_block->step_event_count;
-    }
-
-    //
-    // Handle a triggered endstop
-    //
-    static void endstop_triggered(const AxisEnum axis);
-
-    //
-    // Triggered position of an axis in mm (not core-savvy)
-    //
-    FORCE_INLINE static float triggered_position_mm(const AxisEnum axis) {
-      return endstops_trigsteps[axis] * planner.steps_to_mm[axis];
-    }
-
     #if HAS_MOTOR_CURRENT_PWM
       static void refresh_motor_power();
     #endif
 
   private:
 
-    FORCE_INLINE static hal_timer_t calc_timer_interval(hal_timer_t step_rate) {
+    // Set the current position in steps
-      hal_timer_t timer;
+    static void _set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e);
 
-      NOMORE(step_rate, MAX_STEP_FREQUENCY);
+    // Set direction bits for all steppers
+    static void set_directions();
 
-      // TODO: HAL: tidy this up, use condtionals_post.h
+    FORCE_INLINE static uint32_t calc_timer_interval(uint32_t step_rate) {
+      uint32_t timer;
+
+      NOMORE(step_rate, uint32_t(MAX_STEP_FREQUENCY));
+
+      // TODO: HAL: tidy this up, use Conditionals_post.h
       #ifdef CPU_32_BIT
         #if ENABLED(DISABLE_MULTI_STEPPING)
           step_loops = 1;
@@ -344,20 +294,20 @@ class Stepper {
         timer = uint32_t(HAL_STEPPER_TIMER_RATE) / step_rate;
         NOLESS(timer, min_time_per_step); // (STEP_DOUBLER_FREQUENCY * 2 kHz - this should never happen)
       #else
-        NOLESS(step_rate, F_CPU / 500000);
+        NOLESS(step_rate, uint32_t(F_CPU / 500000U));
         step_rate -= F_CPU / 500000; // Correct for minimal speed
         if (step_rate >= (8 * 256)) { // higher step rate
           uint8_t tmp_step_rate = (step_rate & 0x00FF);
-          uint16_t table_address = (uint16_t)&speed_lookuptable_fast[(uint8_t)(step_rate >> 8)][0];
+          uint16_t table_address = (uint16_t)&speed_lookuptable_fast[(uint8_t)(step_rate >> 8)][0],
-          uint16_t gain = (uint16_t)pgm_read_word_near(table_address + 2);
+                   gain = (uint16_t)pgm_read_word_near(table_address + 2);
           timer = MultiU16X8toH16(tmp_step_rate, gain);
           timer = (uint16_t)pgm_read_word_near(table_address) - timer;
         }
         else { // lower step rates
           uint16_t table_address = (uint16_t)&speed_lookuptable_slow[0][0];
           table_address += ((step_rate) >> 1) & 0xFFFC;
-          timer = (uint16_t)pgm_read_word_near(table_address);
+          timer = (uint16_t)pgm_read_word_near(table_address)
-          timer -= (((uint16_t)pgm_read_word_near(table_address + 2) * (uint8_t)(step_rate & 0x0007)) >> 3);
+                - (((uint16_t)pgm_read_word_near(table_address + 2) * (uint8_t)(step_rate & 0x0007)) >> 3);
         }
         if (timer < 100) { // (20kHz - this should never happen)
           timer = 100;

Marlin/src/module/temperature.cpp

@@ -25,6 +25,7 @@
  */
 
 #include "temperature.h"
+#include "endstops.h"
 
 #include "../Marlin.h"
 #include "../lcd/ultralcd.h"
@@ -40,10 +41,6 @@
   #include "stepper.h"
 #endif
 
-#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE) || ENABLED(PINS_DEBUGGING)
-  #include "endstops.h"
-#endif
-
 #include "printcounter.h"
 
 #if ENABLED(FILAMENT_WIDTH_SENSOR)
@@ -1085,9 +1082,7 @@ void Temperature::updateTemperaturesFromRawValues() {
     watchdog_reset();
   #endif
 
-  CRITICAL_SECTION_START;
   temp_meas_ready = false;
-  CRITICAL_SECTION_END;
 }
 
 
@@ -1727,6 +1722,7 @@ void Temperature::set_current_temp_raw() {
  *  - Step the babysteps value for each axis towards 0
  *  - For PINS_DEBUGGING, monitor and report endstop pins
  *  - For ENDSTOP_INTERRUPTS_FEATURE check endstops if flagged
+ *  - Call planner.tick to count down its "ignore" time
  */
 HAL_TEMP_TIMER_ISR {
   HAL_timer_isr_prologue(TEMP_TIMER_NUM);
@@ -2247,19 +2243,11 @@ void Temperature::isr() {
     }
   #endif // BABYSTEPPING
 
-  #if ENABLED(PINS_DEBUGGING)
+  // Poll endstops state, if required
-    endstops.run_monitor();  // report changes in endstop status
+  endstops.poll();
-  #endif
 
-  #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
+  // Periodically call the planner timer
-
+  planner.tick();
-    extern volatile uint8_t e_hit;
-
-    if (e_hit && ENDSTOPS_ENABLED) {
-      endstops.update();  // call endstop update routine
-      e_hit--;
-    }
-  #endif
 }
 
 #if HAS_TEMP_SENSOR

Marlin/src/sd/cardreader.cpp

@@ -29,7 +29,6 @@
 #include "../Marlin.h"
 #include "../lcd/ultralcd.h"
 #include "../module/planner.h"
-#include "../module/stepper.h"
 #include "../module/printcounter.h"
 #include "../core/language.h"
 #include "../gcode/queue.h"
@@ -983,7 +982,7 @@ void CardReader::printingHasFinished() {
     #endif
 
     #if ENABLED(SD_FINISHED_STEPPERRELEASE) && defined(SD_FINISHED_RELEASECOMMAND)
-      stepper.cleaning_buffer_counter = 1; // The command will fire from the Stepper ISR
+      planner.finish_and_disable();
     #endif
     print_job_timer.stop();
     if (print_job_timer.duration() > 60)