diff --git a/klippy/cartesian.py b/klippy/cartesian.py
index 0f6f8420..5171297e 100644
--- a/klippy/cartesian.py
+++ b/klippy/cartesian.py
@@ -76,8 +76,9 @@ class CartKinematics:
                 self.steppers[i].motor_enable(move_time, 1)
             need_motor_enable |= self.steppers[i].need_motor_enable
         self.need_motor_enable = need_motor_enable
-    def query_endstops(self, query_state):
-        query_state.set_steppers(self.steppers)
+    def query_endstops(self, print_time):
+        endstops = [(s, s.query_endstop(print_time)) for s in self.steppers]
+        return [(s.name, es.query_endstop_wait()) for s, es in endstops]
     def _check_endstops(self, move):
         end_pos = move.end_pos
         for i in StepList:
diff --git a/klippy/delta.py b/klippy/delta.py
index e6ed326c..bbb5f6f0 100644
--- a/klippy/delta.py
+++ b/klippy/delta.py
@@ -111,8 +111,9 @@ class DeltaKinematics:
         for i in StepList:
             self.steppers[i].motor_enable(move_time, 1)
         self.need_motor_enable = False
-    def query_endstops(self, query_state):
-        query_state.set_steppers(self.steppers)
+    def query_endstops(self, print_time):
+        endstops = [(s, s.query_endstop(print_time)) for s in self.steppers]
+        return [(s.name, es.query_endstop_wait()) for s, es in endstops]
     def check_move(self, move):
         end_pos = move.end_pos
         xy2 = end_pos[0]**2 + end_pos[1]**2
diff --git a/klippy/gcode.py b/klippy/gcode.py
index 0d5e832a..f280b43e 100644
--- a/klippy/gcode.py
+++ b/klippy/gcode.py
@@ -345,10 +345,13 @@ class GCodeParser:
         # Get Endstop Status
         if self.is_fileinput:
             return
-        print_time = self.toolhead.get_last_move_time()
-        query_state = homing.QueryEndstops(print_time, self.respond)
-        self.toolhead.query_endstops(query_state)
-        self.set_busy(query_state)
+        try:
+            res = self.toolhead.query_endstops()
+        except self.printer.mcu.error, e:
+            self.respond_error(str(e))
+            return
+        self.respond(" ".join(["%s:%s" % (name, ["open", "TRIGGERED"][not not t])
+                               for name, t in res]))
     cmd_PID_TUNE_help = "Run PID Tuning"
     cmd_PID_TUNE_aliases = ["M303"]
     def cmd_PID_TUNE(self, params):
diff --git a/klippy/homing.py b/klippy/homing.py
index 384da3dd..b1126b57 100644
--- a/klippy/homing.py
+++ b/klippy/homing.py
@@ -90,40 +90,6 @@ class Homing:
     def do_calc_position(self, callback):
         self.toolhead.set_position(self.fill_coord(callback(self)))
 
-# Helper code for querying and reporting the status of the endstops
-class QueryEndstops:
-    def __init__(self, print_time, respond_cb):
-        self.print_time = print_time
-        self.respond_cb = respond_cb
-        self.endstops = []
-        self.busy = []
-    def set_steppers(self, steppers):
-        for stepper in steppers:
-            es = stepper.query_endstop(self.print_time)
-            if es is None:
-                continue
-            self.endstops.append((stepper.name, es))
-            self.busy.append((stepper.name, es))
-    def check_busy(self, eventtime):
-        # Check if all endstop queries have been received
-        while self.busy:
-            try:
-                if self.busy[0][1].check_busy(eventtime):
-                    return True
-            except mcu.error, e:
-                raise EndstopError("Failed to query endstop %s: %s" % (
-                    self.busy[0][0], str(e)))
-            self.busy.pop(0)
-        # All responses received - report status
-        msgs = []
-        for name, es in self.endstops:
-            msg = "open"
-            if es.get_last_triggered():
-                msg = "TRIGGERED"
-            msgs.append("%s:%s" % (name, msg))
-        self.respond_cb(" ".join(msgs))
-        return False
-
 class EndstopError(Exception):
     pass
 
diff --git a/klippy/mcu.py b/klippy/mcu.py
index 4c2bbc68..091222b2 100644
--- a/klippy/mcu.py
+++ b/klippy/mcu.py
@@ -188,7 +188,10 @@ class MCU_endstop:
         self._homing = False
         self._min_query_time = time.time()
         self._next_query_clock = clock
-    def get_last_triggered(self):
+    def query_endstop_wait(self):
+        eventtime = time.time()
+        while self.check_busy(eventtime):
+            eventtime = self._mcu.pause(eventtime + 0.1)
         return self._last_state.get('pin', self._invert) ^ self._invert
 
 class MCU_digital_out:
@@ -299,6 +302,7 @@ class MCU_adc:
         self._callback = callback
 
 class MCU:
+    error = error
     COMM_TIMEOUT = 3.5
     def __init__(self, printer, config):
         self._printer = printer
@@ -515,5 +519,7 @@ class MCU:
         mcu_time = print_time + self._print_start_time
         clock = int(mcu_time * self._mcu_freq)
         self.ffi_lib.steppersync_flush(self._steppersync, clock)
+    def pause(self, waketime):
+        return self._printer.reactor.pause(waketime)
     def __del__(self):
         self.disconnect()
diff --git a/klippy/stepper.py b/klippy/stepper.py
index 71a5c8c6..038412ff 100644
--- a/klippy/stepper.py
+++ b/klippy/stepper.py
@@ -78,8 +78,6 @@ class PrinterStepper:
         self.mcu_endstop.home(mcu_time, step_time)
         return self.mcu_endstop
     def query_endstop(self, print_time):
-        if self.mcu_endstop is None:
-            return None
         mcu_time = self.mcu_endstop.print_to_mcu_time(print_time)
         self.mcu_endstop.query_endstop(mcu_time)
         return self.mcu_endstop
diff --git a/klippy/toolhead.py b/klippy/toolhead.py
index ec215d6b..eedda4b5 100644
--- a/klippy/toolhead.py
+++ b/klippy/toolhead.py
@@ -272,8 +272,9 @@ class ToolHead:
         self.extruder.motor_off(last_move_time)
         self.dwell(STALL_TIME)
         logging.debug('; Max time of %f' % (last_move_time,))
-    def query_endstops(self, query_state):
-        return self.kin.query_endstops(query_state)
+    def query_endstops(self):
+        last_move_time = self.get_last_move_time()
+        return self.kin.query_endstops(last_move_time)
     def force_shutdown(self):
         self.printer.mcu.force_shutdown()
         self.move_queue.reset()