Fix stepper/planner block handling, race conditions (#11098)

- Allow planner to alter the deceleration phase of the currently executing block.
- Remove BUSY flag, as it is NON ATOMIC to set bits in the Stepper ISR and Planner at the same time.

Marlin/src/module/planner.cpp

@@ -105,10 +105,11 @@ Planner planner;
  */
 block_t Planner::block_buffer[BLOCK_BUFFER_SIZE];
 volatile uint8_t Planner::block_buffer_head,    // Index of the next block to be pushed
+                 Planner::block_buffer_nonbusy, // Index of the first non-busy block
+                 Planner::block_buffer_planned, // Index of the optimally planned block
                  Planner::block_buffer_tail;    // Index of the busy block, if any
 uint16_t Planner::cleaning_buffer_counter;      // A counter to disable queuing of blocks
-uint8_t Planner::delay_before_delivering,     // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
+uint8_t Planner::delay_before_delivering;       // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
-        Planner::block_buffer_planned;        // Index of the optimally planned block
 
 uint32_t Planner::max_acceleration_mm_per_s2[XYZE_N],    // (mm/s^2) M201 XYZE
          Planner::max_acceleration_steps_per_s2[XYZE_N], // (steps/s^2) Derived from mm_per_s2
@@ -240,7 +241,6 @@ void Planner::init() {
     bed_level_matrix.set_to_identity();
   #endif
   clear_block_buffer();
-  block_buffer_planned = 0;
   delay_before_delivering = 0;
 }
 
@@ -703,6 +703,12 @@ void Planner::init() {
 /**
  * Calculate trapezoid parameters, multiplying the entry- and exit-speeds
  * by the provided factors.
+ **
+ * ############ VERY IMPORTANT ############
+ * NOTE that the PRECONDITION to call this function is that the block is
+ * NOT BUSY and it is marked as RECALCULATE. That WARRANTIES the Stepper ISR
+ * is not and will not use the block while we modify it, so it is safe to
+ * alter its values.
  */
 void Planner::calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor) {
 
@@ -744,9 +750,6 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
       cruise_rate = block->nominal_rate;
   #endif
 
-  // block->accelerate_until = accelerate_steps;
-  // block->decelerate_after = accelerate_steps+plateau_steps;
-
   #if ENABLED(S_CURVE_ACCELERATION)
     // Jerk controlled speed requires to express speed versus time, NOT steps
     uint32_t acceleration_time = ((float)(cruise_rate - initial_rate) / accel) * (STEPPER_TIMER_RATE),
@@ -755,19 +758,9 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
     // And to offload calculations from the ISR, we also calculate the inverse of those times here
     uint32_t acceleration_time_inverse = get_period_inverse(acceleration_time);
     uint32_t deceleration_time_inverse = get_period_inverse(deceleration_time);
-
   #endif
 
-  // Fill variables used by the stepper in a critical section
+  // Store new block parameters
-  const bool was_enabled = STEPPER_ISR_ENABLED();
-  if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
-
-  // Don't update variables if block is busy; it is being interpreted by the planner.
-  // If this happens, there's a problem... The block speed is inconsistent. Some values
-  // have already been updated, but the Stepper ISR is already using the block. Fortunately,
-  // the values being used by the Stepper ISR weren't touched, so just stop here...
-  // TODO: There may be a way to update a running block, depending on the stepper ISR position.
-  if (!TEST(block->flag, BLOCK_BIT_BUSY)) {
   block->accelerate_until = accelerate_steps;
   block->decelerate_after = accelerate_steps + plateau_steps;
   block->initial_rate = initial_rate;
@@ -779,8 +772,6 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
     block->cruise_rate = cruise_rate;
   #endif
   block->final_rate = final_rate;
-  }
-  if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
 }
 
 /*                            PLANNER SPEED DEFINITION
@@ -831,7 +822,7 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
       streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the
       planner buffer that don't change with the addition of a new block, as describe above. In addition,
       this block can never be less than block_buffer_tail and will always be pushed forward and maintain
-      this requirement when encountered by the plan_discard_current_block() routine during a cycle.
+      this requirement when encountered by the Planner::discard_current_block() routine during a cycle.
 
   NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short
   line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't
@@ -875,11 +866,22 @@ void Planner::reverse_pass_kernel(block_t* const current, const block_t * const
         // ISR does not consume the block before being recalculated
         SBI(current->flag, BLOCK_BIT_RECALCULATE);
 
-        // Set the new entry speed
+        // But there is an inherent race condition here, as the block may have
+        // become BUSY just before being marked RECALCULATE, so check for that!
+        if (stepper.is_block_busy(current)) {
+          // Block became busy. Clear the RECALCULATE flag (no point in
+          // recalculating BUSY blocks). And don't set its speed, as it can't
+          // be updated at this time.
+          CBI(current->flag, BLOCK_BIT_RECALCULATE);
+        }
+        else {
+          // Block is not BUSY so this is ahead of the Stepper ISR:
+          // Just Set the new entry speed.
           current->entry_speed_sqr = new_entry_speed_sqr;
         }
       }
     }
+  }
 }
 
 /**
@@ -902,12 +904,11 @@ void Planner::reverse_pass() {
   // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last
   // block in buffer. Cease planning when the last optimal planned or tail pointer is reached.
   // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
-  block_t *current;
   const block_t *next = NULL;
   while (block_index != planned_block_index) {
 
     // Perform the reverse pass
-    current = &block_buffer[block_index];
+    block_t *current = &block_buffer[block_index];
 
     // Only consider non sync blocks
     if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {
@@ -917,6 +918,18 @@ void Planner::reverse_pass() {
 
     // Advance to the next
     block_index = prev_block_index(block_index);
+
+    // The ISR could advance the block_buffer_planned while we were doing the reverse pass.
+    // We must try to avoid using an already consumed block as the last one - So follow
+    // changes to the pointer and make sure to limit the loop to the currently busy block
+    while (planned_block_index != block_buffer_planned) {
+
+      // If we reached the busy block or an already processed block, break the loop now
+      if (block_index == planned_block_index) return;
+
+      // Advance the pointer, following the busy block
+      planned_block_index = next_block_index(planned_block_index);
+    }
   }
 }
 
@@ -940,6 +953,18 @@ void Planner::forward_pass_kernel(const block_t* const previous, block_t* const
         // so the stepper ISR does not consume the block before being recalculated
         SBI(current->flag, BLOCK_BIT_RECALCULATE);
 
+        // But there is an inherent race condition here, as the block maybe
+        // became BUSY, just before it was marked as RECALCULATE, so check
+        // if that is the case!
+        if (stepper.is_block_busy(current)) {
+          // Block became busy. Clear the RECALCULATE flag (no point in
+          //  recalculating BUSY blocks and don't set its speed, as it can't
+          //  be updated at this time.
+          CBI(current->flag, BLOCK_BIT_RECALCULATE);
+        }
+        else {
+          // Block is not BUSY, we won the race against the Stepper ISR:
+
           // Always <= max_entry_speed_sqr. Backward pass sets this.
           current->entry_speed_sqr = new_entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this.
 
@@ -947,6 +972,7 @@ void Planner::forward_pass_kernel(const block_t* const previous, block_t* const
           block_buffer_planned = block_index;
         }
       }
+    }
 
     // Any block set at its maximum entry speed also creates an optimal plan up to this
     // point in the buffer. When the plan is bracketed by either the beginning of the
@@ -981,6 +1007,12 @@ void Planner::forward_pass() {
 
     // Skip SYNC blocks
     if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {
+      // If there's no previous block or the previous block is not
+      // BUSY (thus, modifiable) run the forward_pass_kernel. Otherwise,
+      // the previous block became BUSY, so assume the current block's
+      // entry speed can't be altered (since that would also require
+      // updating the exit speed of the previous block).
+      if (!previous || !stepper.is_block_busy(previous))
         forward_pass_kernel(previous, current, block_index);
       previous = current;
     }
@@ -996,16 +1028,15 @@ void Planner::forward_pass() {
  */
 void Planner::recalculate_trapezoids() {
   // The tail may be changed by the ISR so get a local copy.
-  uint8_t block_index = block_buffer_tail;
+  uint8_t block_index = block_buffer_tail,
-
+          head_block_index = block_buffer_head;
-  // As there could be a sync block in the head of the queue, and the next loop must not
+  // Since there could be a sync block in the head of the queue, and the
-  // recalculate the head block (as it needs to be specially handled), scan backwards until
+  // next loop must not recalculate the head block (as it needs to be
-  // we find the first non SYNC block
+  // specially handled), scan backwards to the first non-SYNC block.
-  uint8_t head_block_index = block_buffer_head;
   while (head_block_index != block_index) {
 
     // Go back (head always point to the first free block)
-    uint8_t prev_index = prev_block_index(head_block_index);
+    const uint8_t prev_index = prev_block_index(head_block_index);
 
     // Get the pointer to the block
     block_t *prev = &block_buffer[prev_index];
@@ -1015,7 +1046,7 @@ void Planner::recalculate_trapezoids() {
 
     // Examine the previous block. This and all following are SYNC blocks
     head_block_index = prev_index;
-  };
+  }
 
   // Go from the tail (currently executed block) to the first block, without including it)
   block_t *current = NULL, *next = NULL;
@@ -1037,6 +1068,12 @@ void Planner::recalculate_trapezoids() {
           // RECALCULATE yet, but the next one is. That's the reason for the following line.
           SBI(current->flag, BLOCK_BIT_RECALCULATE);
 
+          // But there is an inherent race condition here, as the block maybe
+          // became BUSY, just before it was marked as RECALCULATE, so check
+          // if that is the case!
+          if (!stepper.is_block_busy(current)) {
+            // Block is not BUSY, we won the race against the Stepper ISR:
+
             // NOTE: Entry and exit factors always > 0 by all previous logic operations.
             const float current_nominal_speed = SQRT(current->nominal_speed_sqr),
                         nomr = 1.0 / current_nominal_speed;
@@ -1048,6 +1085,7 @@ void Planner::recalculate_trapezoids() {
                 current->final_adv_steps = next_entry_speed * comp;
               }
             #endif
+          }
 
           // Reset current only to ensure next trapezoid is computed - The
           // stepper is free to use the block from now on.
@@ -1070,6 +1108,12 @@ void Planner::recalculate_trapezoids() {
     // marked as RECALCULATE yet. That's the reason for the following line.
     SBI(next->flag, BLOCK_BIT_RECALCULATE);
 
+    // But there is an inherent race condition here, as the block maybe
+    // became BUSY, just before it was marked as RECALCULATE, so check
+    // if that is the case!
+    if (!stepper.is_block_busy(current)) {
+      // Block is not BUSY, we won the race against the Stepper ISR:
+
       const float next_nominal_speed = SQRT(next->nominal_speed_sqr),
                   nomr = 1.0 / next_nominal_speed;
       calculate_trapezoid_for_block(next, next_entry_speed * nomr, (MINIMUM_PLANNER_SPEED) * nomr);
@@ -1080,6 +1124,7 @@ void Planner::recalculate_trapezoids() {
           next->final_adv_steps = (MINIMUM_PLANNER_SPEED) * comp;
         }
       #endif
+    }
 
     // Reset next only to ensure its trapezoid is computed - The stepper is free to use
     // the block from now on.
@@ -1423,7 +1468,7 @@ void Planner::quick_stop() {
   if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
 
   // Drop all queue entries
-  block_buffer_planned = block_buffer_head = block_buffer_tail;
+  block_buffer_nonbusy = block_buffer_planned = block_buffer_head = block_buffer_tail;
 
   // Restart the block delay for the first movement - As the queue was
   // forced to empty, there's no risk the ISR will touch this.
@@ -1906,7 +1951,8 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
   // Example: At 120mm/s a 60mm move takes 0.5s. So this will give 2.0.
   float inverse_secs = fr_mm_s * inverse_millimeters;
 
-  const uint8_t moves_queued = movesplanned();
+  // Get the number of non busy movements in queue (non busy means that they can be altered)
+  const uint8_t moves_queued = nonbusy_movesplanned();
 
   // Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
   #if ENABLED(SLOWDOWN) || ENABLED(ULTRA_LCD) || defined(XY_FREQUENCY_LIMIT)

Marlin/src/module/planner.h

@@ -54,9 +54,6 @@ enum BlockFlagBit : char {
   // from a safe speed (in consideration of jerking from zero speed).
   BLOCK_BIT_NOMINAL_LENGTH,
 
-  // The block is busy, being interpreted by the stepper ISR
-  BLOCK_BIT_BUSY,
-
   // The block is segment 2+ of a longer move
   BLOCK_BIT_CONTINUED,
 
@@ -67,7 +64,6 @@ enum BlockFlagBit : char {
 enum BlockFlag : char {
   BLOCK_FLAG_RECALCULATE          = _BV(BLOCK_BIT_RECALCULATE),
   BLOCK_FLAG_NOMINAL_LENGTH       = _BV(BLOCK_BIT_NOMINAL_LENGTH),
-  BLOCK_FLAG_BUSY                 = _BV(BLOCK_BIT_BUSY),
   BLOCK_FLAG_CONTINUED            = _BV(BLOCK_BIT_CONTINUED),
   BLOCK_FLAG_SYNC_POSITION        = _BV(BLOCK_BIT_SYNC_POSITION)
 };
@@ -83,7 +79,7 @@ enum BlockFlag : char {
  */
 typedef struct {
 
-  uint8_t flag;                             // Block flags (See BlockFlag enum above)
+  volatile uint8_t flag;                    // Block flags (See BlockFlag enum above) - Modified by ISR and main thread!
 
   // Fields used by the motion planner to manage acceleration
   float nominal_speed_sqr,                  // The nominal speed for this block in (mm/sec)^2
@@ -175,10 +171,12 @@ 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_nonbusy,   // Index of the first non busy block
+                            block_buffer_planned,   // Index of the optimally planned block
                             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
+    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
@@ -443,11 +441,14 @@ class Planner {
       #define ARG_Z const float &rz
     #endif
 
-    // Number of moves currently in the planner
+    // Number of moves currently in the planner including the busy block, if any
     FORCE_INLINE static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail); }
 
+    // Number of nonbusy moves currently in the planner
+    FORCE_INLINE static uint8_t nonbusy_movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_nonbusy); }
+
     // Remove all blocks from the buffer
-    FORCE_INLINE static void clear_block_buffer() { block_buffer_head = block_buffer_tail = 0; }
+    FORCE_INLINE static void clear_block_buffer() { block_buffer_nonbusy = block_buffer_planned = 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); }
@@ -649,7 +650,7 @@ class Planner {
     static block_t* get_current_block() {
 
       // Get the number of moves in the planner queue so far
-      uint8_t nr_moves = movesplanned();
+      const uint8_t nr_moves = movesplanned();
 
       // If there are any moves queued ...
       if (nr_moves) {
@@ -673,8 +674,14 @@ class Planner {
           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
+        // As this block is busy, advance the nonbusy block pointer
-        SBI(block->flag, BLOCK_BIT_BUSY);
+        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;
       }
 
@@ -692,14 +699,8 @@ class Planner {
      * 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.
+      if (has_blocks_queued())
-        uint8_t block_index = next_block_index( block_buffer_tail );
+        block_buffer_tail = 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)

Marlin/src/module/stepper.cpp

@@ -107,8 +107,6 @@ Stepper stepper; // Singleton
 
 // public:
 
-block_t* Stepper::current_block = NULL;  // A pointer to the block currently being traced
-
 #if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
   bool Stepper::homing_dual_axis = false;
 #endif
@@ -119,6 +117,8 @@ block_t* Stepper::current_block = NULL;  // A pointer to the block currently bei
 
 // private:
 
+block_t* Stepper::current_block = NULL; // A pointer to the block currently being traced
+
 uint8_t Stepper::last_direction_bits = 0,
         Stepper::axis_did_move;
 
@@ -1665,6 +1665,7 @@ uint32_t Stepper::stepper_block_phase_isr() {
       acceleration_time = deceleration_time = 0;
 
       uint8_t oversampling = 0;                         // Assume we won't use it
+
       #if ENABLED(ADAPTIVE_STEP_SMOOTHING)
         // At this point, we must decide if we can use Stepper movement axis smoothing.
         uint32_t max_rate = current_block->nominal_rate;  // Get the maximum rate (maximum event speed)
@@ -1874,6 +1875,34 @@ uint32_t Stepper::stepper_block_phase_isr() {
   }
 #endif // LIN_ADVANCE
 
+// Check if the given block is busy or not - Must not be called from ISR contexts
+// The current_block could change in the middle of the read by an Stepper ISR, so
+// we must explicitly prevent that!
+bool Stepper::is_block_busy(const block_t* const block) {
+  #ifdef __AVR__
+    // A SW memory barrier, to ensure GCC does not overoptimize loops
+    #define sw_barrier() asm volatile("": : :"memory");
+
+    // Keep reading until 2 consecutive reads return the same value,
+    // meaning there was no update in-between caused by an interrupt.
+    // This works because stepper ISRs happen at a slower rate than
+    // successive reads of a variable, so 2 consecutive reads with
+    // the same value means no interrupt updated it.
+    block_t* vold, *vnew = current_block;
+    sw_barrier();
+    do {
+      vold = vnew;
+      vnew = current_block;
+      sw_barrier();
+    } while (vold != vnew);
+  #else
+    block_t *vnew = current_block;
+  #endif
+
+  // Return if the block is busy or not
+  return block == vnew;
+}
+
 void Stepper::init() {
 
   // Init Digipot Motor Current

Marlin/src/module/stepper.h

@@ -234,8 +234,6 @@ class Stepper {
 
   public:
 
-    static block_t* current_block;  // A pointer to the block currently being traced
-
     #if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
       static bool homing_dual_axis;
     #endif
@@ -249,6 +247,8 @@ class Stepper {
 
   private:
 
+    static block_t* current_block;          // A pointer to the block currently being traced
+
     static uint8_t last_direction_bits,     // The next stepping-bits to be output
                    axis_did_move;           // Last Movement in the given direction is not null, as computed when the last movement was fetched from planner
 
@@ -360,6 +360,9 @@ class Stepper {
       static uint32_t advance_isr();
     #endif
 
+    // Check if the given block is busy or not - Must not be called from ISR contexts
+    static bool is_block_busy(const block_t* const block);
+
     // Get the position of a stepper, in steps
     static int32_t position(const AxisEnum axis);