Clean up stepper and babystep (#16857)

Marlin/src/HAL/HAL_ESP32/i2s.cpp

         remaining--;
       }
       else {
-        Stepper::stepper_pulse_phase_isr();
-        remaining = Stepper::stepper_block_phase_isr();
+        Stepper::pulse_phase_isr();
+        remaining = Stepper::block_phase_isr();
       }
     }
   }

Marlin/src/feature/babystep.cpp

   }
 }
 
-void Babystep::task() {
-  #if EITHER(BABYSTEP_XY, I2C_POSITION_ENCODERS)
-    LOOP_XYZ(axis) step_axis((AxisEnum)axis);
-  #else
-    step_axis(Z_AXIS);
-  #endif
-}
-
 void Babystep::add_mm(const AxisEnum axis, const float &mm) {
   add_steps(axis, mm * planner.settings.axis_steps_per_mm[axis]);
 }

Marlin/src/feature/babystep.h

 
   static void add_steps(const AxisEnum axis, const int16_t distance);
   static void add_mm(const AxisEnum axis, const float &mm);
-  static void task();
+
+  //
+  // Called by the Temperature ISR to
+  // apply accumulated babysteps to the axes.
+  //
+  static inline void task() {
+    LOOP_L_N(axis, BS_TODO_AXIS(Z_AXIS)) step_axis((AxisEnum)axis);
+  }
+
 private:
   static void step_axis(const AxisEnum axis);
 };

Marlin/src/module/planner.cpp

 #define MINIMAL_STEP_RATE 120
 
 /**
+ * Get the current block for processing
+ * and mark the block as busy.
+ * Return nullptr if the buffer is empty
+ * or if there is a first-block delay.
+ *
+ * WARNING: Called from Stepper ISR context!
+ */
+block_t* Planner::get_current_block() {
+  // Get the number of moves in the planner queue so far
+  const 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 nullptr;
+      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];
+
+    // No trapezoid calculated? Don't execute yet.
+    if (TEST(block->flag, BLOCK_BIT_RECALCULATE)) return nullptr;
+
+    #if HAS_SPI_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
+
+    // As this block is busy, advance the nonbusy block pointer
+    block_buffer_nonbusy = next_block_index(block_buffer_tail);
+
+    // Push block_buffer_planned pointer, if encountered.
+    if (block_buffer_tail == block_buffer_planned)
+      block_buffer_planned = block_buffer_nonbusy;
+
+    // Return the block
+    return block;
+  }
+
+  // The queue became empty
+  #if HAS_SPI_LCD
+    clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero.
+  #endif
+
+  return nullptr;
+}
+
+/**
  * Calculate trapezoid parameters, multiplying the entry- and exit-speeds
  * by the provided factors.
  **
   // must be handled: The tail could change between the read and the assignment
   // so this must be enclosed in a critical section
 
-  const bool was_enabled = STEPPER_ISR_ENABLED();
-  if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
+  const bool was_enabled = stepper.suspend();
 
   // Drop all queue entries
   block_buffer_nonbusy = block_buffer_planned = block_buffer_head = block_buffer_tail;
   cleaning_buffer_counter = 1000;
 
   // Reenable Stepper ISR
-  if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
+  if (was_enabled) stepper.wake_up();
 
   // And stop the stepper ISR
   stepper.quick_stop();
     if (axis == CORE_AXIS_1 || axis == CORE_AXIS_2) {
 
       // Protect the access to the position.
-      const bool was_enabled = STEPPER_ISR_ENABLED();
-      if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
+      const bool was_enabled = stepper.suspend();
 
       const int32_t p1 = stepper.position(CORE_AXIS_1),
                     p2 = stepper.position(CORE_AXIS_2);
 
-      if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
+      if (was_enabled) stepper.wake_up();
 
       // ((a1+a2)+(a1-a2))/2 -> (a1+a2+a1-a2)/2 -> (a1+a1)/2 -> a1
       // ((a1+a2)-(a1-a2))/2 -> (a1+a2-a1+a2)/2 -> (a2+a2)/2 -> a2
 
   #if HAS_SPI_LCD
     // Protect the access to the position.
-    const bool was_enabled = STEPPER_ISR_ENABLED();
-    if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
+    const bool was_enabled = stepper.suspend();
 
     block_buffer_runtime_us += segment_time_us;
     block->segment_time_us = segment_time_us;
 
-    if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
+    if (was_enabled) stepper.wake_up();
   #endif
 
   block->nominal_speed_sqr = sq(block->millimeters * inverse_secs);   // (mm/sec)^2 Always > 0
   #endif
 }
 
+#if HAS_SPI_LCD
+
+  uint16_t Planner::block_buffer_runtime() {
+    #ifdef __AVR__
+      // Protect the access to the variable. Only required for AVR, as
+      //  any 32bit CPU offers atomic access to 32bit variables
+      const bool was_enabled = stepper.suspend();
+    #endif
+
+    millis_t bbru = block_buffer_runtime_us;
+
+    #ifdef __AVR__
+      // Reenable Stepper ISR
+      if (was_enabled) stepper.wake_up();
+    #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.
+    bbru >>= 10;
+    // limit to about a minute.
+    NOMORE(bbru, 0xFFFFul);
+    return bbru;
+  }
+
+  void Planner::clear_block_buffer_runtime() {
+    #ifdef __AVR__
+      // Protect the access to the variable. Only required for AVR, as
+      //  any 32bit CPU offers atomic access to 32bit variables
+      const bool was_enabled = stepper.suspend();
+    #endif
+
+    block_buffer_runtime_us = 0;
+
+    #ifdef __AVR__
+      // Reenable Stepper ISR
+      if (was_enabled) stepper.wake_up();
+    #endif
+  }
+
+#endif
+
 #if ENABLED(AUTOTEMP)
 
   void Planner::autotemp_M104_M109() {

Marlin/src/module/planner.h

     FORCE_INLINE static bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); }
 
     /**
-     * The current block. nullptr if the buffer is empty.
-     * This also marks the block as busy.
+     * Get the current block for processing
+     * and mark the block as busy.
+     * Return nullptr if the buffer is empty
+     * or if there is a first-block delay.
+     *
      * WARNING: Called from Stepper ISR context!
      */
-    static block_t* get_current_block() {
-
-      // Get the number of moves in the planner queue so far
-      const 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 nullptr;
-          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];
-
-        // No trapezoid calculated? Don't execute yet.
-        if (TEST(block->flag, BLOCK_BIT_RECALCULATE)) return nullptr;
-
-        #if HAS_SPI_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
-
-        // As this block is busy, advance the nonbusy block pointer
-        block_buffer_nonbusy = next_block_index(block_buffer_tail);
-
-        // Push block_buffer_planned pointer, if encountered.
-        if (block_buffer_tail == block_buffer_planned)
-          block_buffer_planned = block_buffer_nonbusy;
-
-        // Return the block
-        return block;
-      }
-
-      // The queue became empty
-      #if HAS_SPI_LCD
-        clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero.
-      #endif
-
-      return nullptr;
-    }
+    static block_t* get_current_block();
 
     /**
      * "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())
     }
 
     #if HAS_SPI_LCD
-
-      static uint16_t block_buffer_runtime() {
-        #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;
-
-        #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.
-        bbru >>= 10;
-        // limit to about a minute.
-        NOMORE(bbru, 0xFFFFul);
-        return bbru;
-      }
-
-      static void clear_block_buffer_runtime() {
-        #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;
-
-        #ifdef __AVR__
-          // Reenable Stepper ISR
-          if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
-        #endif
-      }
-
+      static uint16_t block_buffer_runtime();
+      static void clear_block_buffer_runtime();
     #endif
 
     #if ENABLED(AUTOTEMP)

Marlin/src/module/stepper.cpp

   bool Stepper::bezier_2nd_half;    // =false If Bézier curve has been initialized or not
 #endif
 
-uint32_t Stepper::nextMainISR = 0;
-
 #if ENABLED(LIN_ADVANCE)
 
-  constexpr uint32_t LA_ADV_NEVER = 0xFFFFFFFF;
   uint32_t Stepper::nextAdvanceISR = LA_ADV_NEVER,
            Stepper::LA_isr_rate = LA_ADV_NEVER;
   uint16_t Stepper::LA_current_adv_steps = 0,
 #define PULSE_HIGH_TICK_COUNT hal_timer_t(NS_TO_PULSE_TIMER_TICKS(_MIN_PULSE_HIGH_NS - _MIN(_MIN_PULSE_HIGH_NS, TIMER_SETUP_NS)))
 #define PULSE_LOW_TICK_COUNT hal_timer_t(NS_TO_PULSE_TIMER_TICKS(_MIN_PULSE_LOW_NS - _MIN(_MIN_PULSE_LOW_NS, TIMER_SETUP_NS)))
 
-#define USING_TIMED_PULSE() hal_timer_t end_tick_count = 0
-#define START_TIMED_PULSE(DIR) (end_tick_count = HAL_timer_get_count(PULSE_TIMER_NUM) + PULSE_##DIR##_TICK_COUNT)
-#define AWAIT_TIMED_PULSE() while (HAL_timer_get_count(PULSE_TIMER_NUM) < end_tick_count) { }
+#define USING_TIMED_PULSE() hal_timer_t start_pulse_count = 0
+#define START_TIMED_PULSE(DIR) (start_pulse_count = HAL_timer_get_count(PULSE_TIMER_NUM))
+#define AWAIT_TIMED_PULSE(DIR) while (PULSE_##DIR##_TICK_COUNT > HAL_timer_get_count(PULSE_TIMER_NUM) - start_pulse_count) { }
 #define START_HIGH_PULSE()  START_TIMED_PULSE(HIGH)
+#define AWAIT_HIGH_PULSE()  AWAIT_TIMED_PULSE(HIGH)
 #define START_LOW_PULSE()   START_TIMED_PULSE(LOW)
-#define AWAIT_HIGH_PULSE()  AWAIT_TIMED_PULSE()
-#define AWAIT_LOW_PULSE()   AWAIT_TIMED_PULSE()
+#define AWAIT_LOW_PULSE()   AWAIT_TIMED_PULSE(LOW)
 
 #if MINIMUM_STEPPER_PRE_DIR_DELAY > 0
   #define DIR_WAIT_BEFORE() DELAY_NS(MINIMUM_STEPPER_PRE_DIR_DELAY)
   #define DIR_WAIT_AFTER()
 #endif
 
-void Stepper::wake_up() {
-  // TCNT1 = 0;
-  ENABLE_STEPPER_DRIVER_INTERRUPT();
-}
-
 /**
  * Set the stepper direction of each axis
  *
 #endif
 
 void Stepper::isr() {
+
+  static uint32_t nextMainISR = 0;  // Interval until the next main Stepper Pulse phase (0 = Now)
+
   #ifndef __AVR__
     // Disable interrupts, to avoid ISR preemption while we reprogram the period
     // (AVR enters the ISR with global interrupts disabled, so no need to do it here)
     // Enable ISRs to reduce USART processing latency
     ENABLE_ISRS();
 
-    // Run main stepping pulse phase ISR if we have to
-    if (!nextMainISR) Stepper::stepper_pulse_phase_isr();
+    if (!nextMainISR) pulse_phase_isr();                            // 0 = Do coordinated axes Stepper pulses
 
     #if ENABLED(LIN_ADVANCE)
-      // Run linear advance stepper ISR if we have to
-      if (!nextAdvanceISR) nextAdvanceISR = Stepper::advance_isr();
+      if (!nextAdvanceISR) nextAdvanceISR = advance_isr();          // 0 = Do Linear Advance E Stepper pulses
     #endif
 
     // ^== Time critical. NOTHING besides pulse generation should be above here!!!
 
-    // Run main stepping block processing ISR if we have to
-    if (!nextMainISR) nextMainISR = Stepper::stepper_block_phase_isr();
+    if (!nextMainISR) nextMainISR = block_phase_isr();  // Manage acc/deceleration, get next block
 
-    uint32_t interval =
+    // Get the interval to the next ISR call
+    const uint32_t interval = _MIN(
+      nextMainISR                                       // Time until the next Stepper ISR
       #if ENABLED(LIN_ADVANCE)
-        _MIN(nextAdvanceISR, nextMainISR)  // Nearest time interval
-      #else
-        nextMainISR                       // Remaining stepper ISR time
+        , nextAdvanceISR                                // Come back early for Linear Advance?
       #endif
-    ;
+      , uint32_t(HAL_TIMER_TYPE_MAX)                    // Come back in a very long time
+    );
 
-    // Limit the value to the maximum possible value of the timer
-    NOMORE(interval, uint32_t(HAL_TIMER_TYPE_MAX));
+    //
+    // Compute remaining time for each ISR phase
+    //     NEVER : The phase is idle
+    //      Zero : The phase will occur on the next ISR call
+    //  Non-zero : The phase will occur on a future ISR call
+    //
 
-    // Compute the time remaining for the main isr
     nextMainISR -= interval;
 
     #if ENABLED(LIN_ADVANCE)
-      // Compute the time remaining for the advance isr
       if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval;
     #endif
 
  * call to this method that might cause variation in the timing. The aim
  * is to keep pulse timing as regular as possible.
  */
-void Stepper::stepper_pulse_phase_isr() {
+void Stepper::pulse_phase_isr() {
 
   // If we must abort the current block, do so!
   if (abort_current_block) {
           // Don't step E here - But remember the number of steps to perform
           motor_direction(E_AXIS) ? --LA_steps : ++LA_steps;
         #else
-          step_needed.e = delta_error.e >= 0;
+          step_needed.e = true;
         #endif
       }
     #elif HAS_E0_STEP
 
     #if DISABLED(LIN_ADVANCE)
       #if ENABLED(MIXING_EXTRUDER)
-
         if (delta_error.e >= 0) {
           delta_error.e -= advance_divisor;
           E_STEP_WRITE(mixer.get_stepper(), INVERT_E_STEP_PIN);
         }
-
-      #else // !MIXING_EXTRUDER
-
-        #if HAS_E0_STEP
-          PULSE_STOP(E);
-        #endif
-
-      #endif  // !MIXING_EXTRUDER
-    #endif // !LIN_ADVANCE
+      #elif HAS_E0_STEP
+        PULSE_STOP(E);
+      #endif
+    #endif
 
     #if ISR_MULTI_STEPS
       if (events_to_do) START_LOW_PULSE();
 // 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() {
+uint32_t Stepper::block_phase_isr() {
 
-  // If no queued movements, just wait 1ms for the next move
-  uint32_t interval = (STEPPER_TIMER_RATE) / 1000;
+  // If no queued movements, just wait 1ms for the next block
+  uint32_t interval = (STEPPER_TIMER_RATE) / 1000UL;
 
   // If there is a current block
   if (current_block) {
         // acc_step_rate is in steps/second
 
         // step_rate to timer interval and steps per stepper isr
-        interval = calc_timer_interval(acc_step_rate, oversampling_factor, &steps_per_isr);
+        interval = calc_timer_interval(acc_step_rate, &steps_per_isr);
         acceleration_time += interval;
 
         #if ENABLED(LIN_ADVANCE)
-          if (LA_use_advance_lead) {
-            // Fire ISR if final adv_rate is reached
-            if (LA_steps && LA_isr_rate != current_block->advance_speed) nextAdvanceISR = 0;
-          }
-          else if (LA_steps) nextAdvanceISR = 0;
-        #endif // LIN_ADVANCE
+          // Fire ISR if final adv_rate is reached
+          if (LA_steps && (!LA_use_advance_lead || LA_isr_rate != current_block->advance_speed))
+            initiateLA();
+        #endif
       }
       // Are we in Deceleration phase ?
       else if (step_events_completed > decelerate_after) {
         // step_rate is in steps/second
 
         // step_rate to timer interval and steps per stepper isr
-        interval = calc_timer_interval(step_rate, oversampling_factor, &steps_per_isr);
+        interval = calc_timer_interval(step_rate, &steps_per_isr);
         deceleration_time += interval;
 
         #if ENABLED(LIN_ADVANCE)
           if (LA_use_advance_lead) {
             // Wake up eISR on first deceleration loop and fire ISR if final adv_rate is reached
             if (step_events_completed <= decelerate_after + steps_per_isr || (LA_steps && LA_isr_rate != current_block->advance_speed)) {
-              nextAdvanceISR = 0;
+              initiateLA();
               LA_isr_rate = current_block->advance_speed;
             }
           }
-          else if (LA_steps) nextAdvanceISR = 0;
-        #endif // LIN_ADVANCE
+          else if (LA_steps) initiateLA();
+        #endif
       }
       // We must be in cruise phase otherwise
       else {
 
         #if ENABLED(LIN_ADVANCE)
           // If there are any esteps, fire the next advance_isr "now"
-          if (LA_steps && LA_isr_rate != current_block->advance_speed) nextAdvanceISR = 0;
+          if (LA_steps && LA_isr_rate != current_block->advance_speed) initiateLA();
         #endif
 
         // Calculate the ticks_nominal for this nominal speed, if not done yet
         if (ticks_nominal < 0) {
           // step_rate to timer interval and loops for the nominal speed
-          ticks_nominal = calc_timer_interval(current_block->nominal_rate, oversampling_factor, &steps_per_isr);
+          ticks_nominal = calc_timer_interval(current_block->nominal_rate, &steps_per_isr);
         }
 
         // The timer interval is just the nominal value for the nominal speed
       // No acceleration / deceleration time elapsed so far
       acceleration_time = deceleration_time = 0;
 
-      uint8_t oversampling = 0;                         // Assume we won't use it
+      uint8_t oversampling = 0;                           // Assume no axis smoothing (via oversampling)
 
       #if ENABLED(ADAPTIVE_STEP_SMOOTHING)
-        // At this point, we must decide if we can use Stepper movement axis smoothing.
+        // Decide if axis smoothing is possible
         uint32_t max_rate = current_block->nominal_rate;  // Get the maximum rate (maximum event speed)
-        while (max_rate < MIN_STEP_ISR_FREQUENCY) {
-          max_rate <<= 1;
-          if (max_rate >= MAX_STEP_ISR_FREQUENCY_1X) break;
-          ++oversampling;
+        while (max_rate < MIN_STEP_ISR_FREQUENCY) {         // As long as more ISRs are possible...
+          max_rate <<= 1;                                   // Try to double the rate
+          if (max_rate >= MAX_STEP_ISR_FREQUENCY_1X) break; // Don't exceed the estimated ISR limit
+          ++oversampling;                                   // Increase the oversampling (used for left-shift)
         }
-        oversampling_factor = oversampling;
+        oversampling_factor = oversampling;                 // For all timer interval calculations
       #endif
 
       // Based on the oversampling factor, do the calculations
         if ((LA_use_advance_lead = current_block->use_advance_lead)) {
           LA_final_adv_steps = current_block->final_adv_steps;
           LA_max_adv_steps = current_block->max_adv_steps;
-          //Start the ISR
-          nextAdvanceISR = 0;
+          initiateLA(); // Start the ISR
           LA_isr_rate = current_block->advance_speed;
         }
         else LA_isr_rate = LA_ADV_NEVER;
       #endif
 
       // Calculate the initial timer interval
-      interval = calc_timer_interval(current_block->initial_rate, oversampling_factor, &steps_per_isr);
+      interval = calc_timer_interval(current_block->initial_rate, &steps_per_isr);
     }
   }
 
 
     return interval;
   }
+
 #endif // LIN_ADVANCE
 
 // Check if the given block is busy or not - Must not be called from ISR contexts
       digipot_motor = 255 * (motor_current[i] / 2.5);
       dac084s085::setValue(i, digipot_motor);
     }
-  #endif//MB(ALLIGATOR)
+  #endif
 
   // Init Microstepping Pins
   #if HAS_MICROSTEPS
 
   #if DISABLED(I2S_STEPPER_STREAM)
     HAL_timer_start(STEP_TIMER_NUM, 122); // Init Stepper ISR to 122 Hz for quick starting
-    ENABLE_STEPPER_DRIVER_INTERRUPT();
+    wake_up();
     sei();
   #endif
 
   #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();
+    const bool was_enabled = suspend();
   #endif
 
   const int32_t v = count_position[axis];
 
   #ifdef __AVR__
     // Reenable Stepper ISR
-    if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
+    if (was_enabled) wake_up();
   #endif
   return v;
 }
 
+// Set the current position in steps
+void Stepper::set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) {
+  planner.synchronize();
+  const bool was_enabled = suspend();
+  _set_position(a, b, c, e);
+  if (was_enabled) wake_up();
+}
+
+void Stepper::set_axis_position(const AxisEnum a, const int32_t &v) {
+  planner.synchronize();
+
+  #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 = suspend();
+  #endif
+
+  count_position[a] = v;
+
+  #ifdef __AVR__
+    // Reenable Stepper ISR
+    if (was_enabled) wake_up();
+  #endif
+}
+
 // 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
 // is properly canceled
 void Stepper::endstop_triggered(const AxisEnum axis) {
 
-  const bool was_enabled = STEPPER_ISR_ENABLED();
-  if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
+  const bool was_enabled = suspend();
   endstops_trigsteps[axis] = (
     #if IS_CORE
       (axis == CORE_AXIS_2
   // Discard the rest of the move if there is a current block
   quick_stop();
 
-  if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
+  if (was_enabled) wake_up();
 }
 
 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();
+    const bool was_enabled = suspend();
   #endif
 
   const int32_t v = endstops_trigsteps[axis];
 
   #ifdef __AVR__
     // Reenable Stepper ISR
-    if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
+    if (was_enabled) wake_up();
   #endif
 
   return v;
 
   #ifdef __AVR__
     // Protect the access to the position.
-    const bool was_enabled = STEPPER_ISR_ENABLED();
-    if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
+    const bool was_enabled = suspend();
   #endif
 
   const xyz_long_t pos = count_position;
 
   #ifdef __AVR__
-    if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
+    if (was_enabled) wake_up();
   #endif
 
   #if CORE_IS_XY || CORE_IS_XZ || ENABLED(DELTA) || IS_SCARA
           Z_STEP_WRITE(INVERT_Z_STEP_PIN);
 
           // Restore direction bits
+          DIR_WAIT_BEFORE();
+
           X_DIR_WRITE(old_dir.x);
           Y_DIR_WRITE(old_dir.y);
           Z_DIR_WRITE(old_dir.z);
 
+          DIR_WAIT_AFTER();
+
         #endif
 
       } break;
 
       default: break;
     }
+
     sei();
   }
 

Marlin/src/module/stepper.h

       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
     #if ENABLED(LIN_ADVANCE)
+      static constexpr uint32_t LA_ADV_NEVER = 0xFFFFFFFF;
       static uint32_t nextAdvanceISR, LA_isr_rate;
       static uint16_t LA_current_adv_steps, LA_final_adv_steps, LA_max_adv_steps; // Copy from current executed block. Needed because current_block is set to NULL "too early".
       static int8_t LA_steps;
       static bool LA_use_advance_lead;
-    #endif // LIN_ADVANCE
+    #endif
 
     static int32_t ticks_nominal;
     #if DISABLED(S_CURVE_ACCELERATION)
 
   public:
 
-    //
-    // Constructor / initializer
-    //
-    Stepper() {};
-
     // Initialize stepper hardware
     static void init();
 
-    // Interrupt Service Routines
+    // Interrupt Service Routine and phases
+
+    // 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 inline void wake_up() { ENABLE_STEPPER_DRIVER_INTERRUPT(); }
+
+    static inline bool is_awake() { return STEPPER_ISR_ENABLED(); }
+
+    static inline bool suspend() {
+      const bool awake = is_awake();
+      if (awake) DISABLE_STEPPER_DRIVER_INTERRUPT();
+      return awake;
+    }
 
     // The ISR scheduler
     static void isr();
 
-    // The stepper pulse phase ISR
-    static void stepper_pulse_phase_isr();
+    // The stepper pulse ISR phase
+    static void pulse_phase_isr();
 
-    // The stepper block processing phase ISR
-    static uint32_t stepper_block_phase_isr();
+    // The stepper block processing ISR phase
+    static uint32_t block_phase_isr();
 
     #if ENABLED(LIN_ADVANCE)
-      // The Linear advance stepper ISR
+      // The Linear advance ISR phase
       static uint32_t advance_isr();
+      FORCE_INLINE static void initiateLA() { nextAdvanceISR = 0; }
     #endif
 
     // Check if the given block is busy or not - Must not be called from ISR contexts
     // Get the position of a stepper, in steps
     static int32_t position(const AxisEnum axis);
 
+    // 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);
+    static inline void set_position(const xyze_long_t &abce) { set_position(abce.a, abce.b, abce.c, abce.e); }
+    static void set_axis_position(const AxisEnum a, const int32_t &v);
+
     // 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
     FORCE_INLINE static void quick_stop() { abort_current_block = true; }
 
       static void refresh_motor_power();
     #endif
 
-    // Set the current position in steps
-    static inline void set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) {
-      planner.synchronize();
-      const bool was_enabled = STEPPER_ISR_ENABLED();
-      if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
-      _set_position(a, b, c, e);
-      if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
-    }
-    static inline void set_position(const xyze_long_t &abce) { set_position(abce.a, abce.b, abce.c, abce.e); }
-
-    static inline void set_axis_position(const AxisEnum a, const int32_t &v) {
-      planner.synchronize();
-
-      #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
-
-      count_position[a] = v;
-
-      #ifdef __AVR__
-        // Reenable Stepper ISR
-        if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
-      #endif
-    }
-
     // Set direction bits for all steppers
     static void set_directions();
 
     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 abce_long_t &spos) { _set_position(spos.a, spos.b, spos.c, spos.e); }
 
-    FORCE_INLINE static uint32_t calc_timer_interval(uint32_t step_rate, uint8_t scale, uint8_t* loops) {
+    FORCE_INLINE static uint32_t calc_timer_interval(uint32_t step_rate, uint8_t* loops) {
       uint32_t timer;
 
       // Scale the frequency, as requested by the caller
-      step_rate <<= scale;
+      step_rate <<= oversampling_factor;
 
       uint8_t multistep = 1;
       #if DISABLED(DISABLE_MULTI_STEPPING)

Marlin/src/module/temperature.cpp

   #include "../libs/private_spi.h"
 #endif
 
-#if EITHER(BABYSTEPPING, PID_EXTRUSION_SCALING)
+#if ENABLED(PID_EXTRUSION_SCALING)
   #include "stepper.h"
 #endif
 
 #if ENABLED(BABYSTEPPING)
   #include "../feature/babystep.h"
-  #if ENABLED(BABYSTEP_ALWAYS_AVAILABLE)
-    #include "../gcode/gcode.h"
-  #endif
 #endif
 
 #include "printcounter.h"