diff --git a/klippy/gcode.py b/klippy/gcode.py
index 5dee80cf..0c306147 100644
--- a/klippy/gcode.py
+++ b/klippy/gcode.py
@@ -22,7 +22,6 @@ class GCodeParser:
         self.input_commands = [""]
         self.bytes_read = 0
         self.input_log = collections.deque([], 50)
-        self.busy_state = None
         # Command handling
         self.gcode_handlers = {}
         self.is_printer_ready = False
@@ -114,12 +113,6 @@ class GCodeParser:
                 logging.exception("Exception in command handler")
                 self.toolhead.force_shutdown()
                 self.respond_error('Internal error on command:"%s"' % (cmd,))
-            # Check if machine can process next command or must stall input
-            if self.busy_state is not None:
-                self.busy_handler(eventtime)
-            if self.is_printer_ready and self.toolhead.check_busy(eventtime):
-                self.set_busy(self.toolhead)
-                self.busy_handler(eventtime)
             self.ack()
         del self.input_commands[:i+1]
     def process_data(self, eventtime):
@@ -160,26 +153,6 @@ class GCodeParser:
         for line in lines[:-1]:
             self.respond('// %s' % (line.strip(),))
         self.respond('!! %s' % (lines[-1].strip(),))
-    # Busy handling
-    def set_busy(self, busy_handler):
-        self.busy_state = busy_handler
-    def busy_handler(self, eventtime):
-        while 1:
-            try:
-                busy = self.busy_state.check_busy(eventtime)
-            except homing.EndstopError, e:
-                self.respond_error(str(e))
-                busy = False
-            except:
-                logging.exception("Exception in busy handler")
-                self.toolhead.force_shutdown()
-                self.respond_error('Internal error in busy handler')
-                busy = False
-            if not busy:
-                break
-            self.toolhead.reset_motor_off_time(eventtime)
-            eventtime = self.reactor.pause(eventtime + self.RETRY_TIME)
-        self.busy_state = None
     # Temperature wrappers
     def get_temp(self):
         if not self.is_printer_ready:
diff --git a/klippy/mcu.py b/klippy/mcu.py
index ba441474..b49c29cc 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -506,6 +506,8 @@ class MCU:
         est_mcu_time = self.serial.get_clock(eventtime) / self._mcu_freq
         self._print_start_time = est_mcu_time
     def get_print_buffer_time(self, eventtime, print_time):
+        if self.is_shutdown:
+            return 0.
         mcu_time = print_time + self._print_start_time
         est_mcu_time = self.serial.get_clock(eventtime) / self._mcu_freq
         return mcu_time - est_mcu_time
diff --git a/klippy/toolhead.py b/klippy/toolhead.py
index eedda4b5..4036b805 100644
--- a/klippy/toolhead.py
+++ b/klippy/toolhead.py
@@ -173,9 +173,10 @@ class ToolHead:
         self.move_flush_time = config.getfloat('move_flush_time', 0.050)
         self.motor_off_delay = config.getfloat('motor_off_time', 60.000)
         self.print_time = 0.
+        self.need_check_stall = -1.
         self.print_time_stall = 0
         self.motor_off_time = self.reactor.NEVER
-        self.flush_timer = self.reactor.register_timer(self.flush_handler)
+        self.flush_timer = self.reactor.register_timer(self._flush_handler)
     def build_config(self):
         self.kin.set_max_jerk(0.005 * self.max_accel, self.max_accel) # XXX
         self.kin.build_config()
@@ -200,18 +201,27 @@ class ToolHead:
         self.move_queue.flush()
         self.printer.mcu.flush_moves(self.print_time)
         self.print_time = 0.
+        self.need_check_stall = -1.
         self.reset_motor_off_time(time.time())
         self.reactor.update_timer(self.flush_timer, self.motor_off_time)
-    def check_busy(self, eventtime):
+    def _check_stall(self):
         if not self.print_time:
             # XXX - find better way to flush initial move_queue items
             if self.move_queue.queue:
-                self.reactor.update_timer(self.flush_timer, eventtime + 0.100)
-            return False
-        buffer_time = self.printer.mcu.get_print_buffer_time(
-            eventtime, self.print_time)
-        return buffer_time > self.buffer_time_high
-    def flush_handler(self, eventtime):
+                self.reactor.update_timer(self.flush_timer, time.time() + 0.100)
+            return
+        eventtime = time.time()
+        while 1:
+            buffer_time = self.printer.mcu.get_print_buffer_time(
+                eventtime, self.print_time)
+            stall_time = buffer_time - self.buffer_time_high
+            if stall_time <= 0.:
+                break
+            eventtime = self.reactor.pause(eventtime + stall_time)
+            if not self.print_time:
+                return
+        self.need_check_stall = self.print_time - stall_time + 0.100
+    def _flush_handler(self, eventtime):
         try:
             if not self.print_time:
                 self.move_queue.flush()
@@ -260,11 +270,14 @@ class ToolHead:
             self.extruder.check_move(move)
         self.commanded_pos[:] = newpos
         self.move_queue.add_move(move)
+        if self.print_time > self.need_check_stall:
+            self._check_stall()
     def home(self, homing_state):
         self.kin.home(homing_state)
     def dwell(self, delay):
         self.get_last_move_time()
         self.update_move_time(delay)
+        self._check_stall()
     def motor_off(self):
         self.dwell(STALL_TIME)
         last_move_time = self.get_last_move_time()