diff --git a/stepper_plan.c b/stepper_plan.c
index 613e745..f5e5609 100644
--- a/stepper_plan.c
+++ b/stepper_plan.c
@@ -204,7 +204,8 @@ void planner_reverse_pass() {
     block[1]= block[0];
     block[0] = &block_buffer[block_index];
     planner_reverse_pass_kernel(block[0], block[1], block[2]);
-    block_index = (block_index-1) % BLOCK_BUFFER_SIZE;
+    block_index--;
+    if(block_index < 0) {block_index = BLOCK_BUFFER_SIZE-1;}
   }
   planner_reverse_pass_kernel(NULL, block[0], block[1]);
 }