[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_epilogue_0 do{ cli(); ENABLE_TEMPERATURE_INTERRUPT(); }while(0)
-
-#define HAL_timer_isr_prologue_1 \
-  const bool temp_isr_was_enabled = TEMPERATURE_ISR_ENABLED(); \
-  do{ \
-    DISABLE_TEMPERATURE_INTERRUPT(); \
-    DISABLE_STEPPER_DRIVER_INTERRUPT(); \
-    sei(); \
-  }while(0)
-
-#define HAL_timer_isr_epilogue_1 do{ cli(); ENABLE_STEPPER_DRIVER_INTERRUPT(); if (temp_isr_was_enabled) ENABLE_TEMPERATURE_INTERRUPT(); }while(0)
-
-#define HAL_timer_isr_prologue(TIMER_NUM) _CAT(HAL_timer_isr_prologue_, TIMER_NUM)
-#define HAL_timer_isr_epilogue(TIMER_NUM) _CAT(HAL_timer_isr_epilogue_, TIMER_NUM)
-
-#define HAL_STEP_TIMER_ISR ISR(TIMER1_COMPA_vect)
-#define HAL_TEMP_TIMER_ISR ISR(TIMER0_COMPB_vect)
+#define HAL_timer_isr_prologue(TIMER_NUM)
+#define HAL_timer_isr_epilogue(TIMER_NUM)
+
+/* 18 cycles maximum latency */
+#define HAL_STEP_TIMER_ISR \
+extern "C" void TIMER1_COMPA_vect (void) __attribute__ ((signal, naked, used, externally_visible)); \
+extern "C" void TIMER1_COMPA_vect_bottom (void) asm ("TIMER1_COMPA_vect_bottom") __attribute__ ((used, externally_visible, noinline)); \
+void TIMER1_COMPA_vect (void) { \
+  __asm__ __volatile__ ( \
+    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)
+
+/* 14 cycles maximum latency */
+#define HAL_TEMP_TIMER_ISR \
+extern "C" void TIMER0_COMPB_vect (void) __attribute__ ((signal, naked, used, externally_visible)); \
+extern "C" void TIMER0_COMPB_vect_bottom(void)  asm ("TIMER0_COMPB_vect_bottom") __attribute__ ((used, externally_visible, noinline)); \
+void TIMER0_COMPB_vect (void) { \
+  __asm__ __volatile__ ( \
+    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>
-
-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.
-}
+#include "../../module/endstops.h"
 
 // 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];
-  pConfig->pTimerRegs->TC_CHANNEL[pConfig->channel].TC_IER = TC_IER_CPCS;
+  IRQn_Type irq = TimerConfig[timer_num].IRQ_Id;
+  NVIC_EnableIRQ(irq);
 }
 
 void HAL_timer_disable_interrupt(const uint8_t timer_num) {
-  const tTimerConfig * const pConfig = &TimerConfig[timer_num];
-  pConfig->pTimerRegs->TC_CHANNEL[pConfig->channel].TC_IDR = TC_IDR_CPCS;
+  IRQn_Type irq = TimerConfig[timer_num].IRQ_Id;
+  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];
-  return (pConfig->pTimerRegs->TC_CHANNEL[pConfig->channel].TC_IMR & TC_IMR_CPCS) != 0;
+  IRQn_Type irq = TimerConfig[timer_num].IRQ_Id;
+  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.
-                            // 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.
-}
+#include "../../module/endstops.h"
 
 // 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);