diff --git a/klippy/extras/motion_queuing.py b/klippy/extras/motion_queuing.py
index c13b4970..a06b556e 100644
--- a/klippy/extras/motion_queuing.py
+++ b/klippy/extras/motion_queuing.py
@@ -53,10 +53,12 @@ class PrinterMotionQueuing:
             fcbs = list(self.flush_callbacks)
             fcbs.remove(callback)
             self.flush_callbacks = fcbs
-    def flush_motion_queues(self, must_flush_time, max_step_gen_time):
+    def flush_motion_queues(self, must_flush_time, max_step_gen_time,
+                            trapq_free_time):
         # Invoke flush callbacks (if any)
         for cb in self.flush_callbacks:
             cb(must_flush_time, max_step_gen_time)
+        # Generate stepper movement and transmit
         for mcu, ss in self.steppersyncs:
             clock = max(0, mcu.print_time_to_clock(must_flush_time))
             # Generate steps
@@ -69,7 +71,7 @@ class PrinterMotionQueuing:
             if ret:
                 raise mcu.error("Internal error in MCU '%s' stepcompress"
                                 % (mcu.get_name(),))
-    def clean_motion_queues(self, trapq_free_time):
+        # Determine maximum history to keep
         clear_history_time = self.clear_history_time
         if self.is_debugoutput:
             clear_history_time = trapq_free_time - MOVE_HISTORY_EXPIRE
diff --git a/klippy/toolhead.py b/klippy/toolhead.py
index 7f53ef44..21aeca3d 100644
--- a/klippy/toolhead.py
+++ b/klippy/toolhead.py
@@ -275,11 +275,10 @@ class ToolHead:
         sg_flush_want = min(flush_time + STEPCOMPRESS_FLUSH_TIME,
                             self.print_time - self.kin_flush_delay)
         sg_flush_time = max(sg_flush_want, flush_time)
-        self.motion_queuing.flush_motion_queues(flush_time, sg_flush_time)
+        trapq_free_time = sg_flush_time - self.kin_flush_delay
+        self.motion_queuing.flush_motion_queues(flush_time, sg_flush_time,
+                                                trapq_free_time)
         self.min_restart_time = max(self.min_restart_time, sg_flush_time)
-        # Free trapq entries that are no longer needed
-        free_time = sg_flush_time - self.kin_flush_delay
-        self.motion_queuing.clean_motion_queues(free_time)
         self.last_flush_time = flush_time
     def _advance_move_time(self, next_print_time):
         pt_delay = self.kin_flush_delay + STEPCOMPRESS_FLUSH_TIME