diff --git a/klippy/gcode.py b/klippy/gcode.py
index 9127fb31..b1e856d6 100644
--- a/klippy/gcode.py
+++ b/klippy/gcode.py
@@ -44,7 +44,8 @@ class GCodeParser:
         self.fan = self.printer.objects.get('fan')
     def build_handlers(self):
         handlers = ['G1', 'G4', 'G20', 'G21', 'G28', 'G90', 'G91', 'G92',
-                    'M18', 'M82', 'M83', 'M105', 'M110', 'M112', 'M114', 'M206',
+                    'M18', 'M82', 'M83', 'M105', 'M110', 'M112', 'M114',
+                    'M206', 'M400',
                     'HELP', 'QUERY_ENDSTOPS', 'RESTART', 'CLEAR_SHUTDOWN',
                     'STATUS']
         if self.heater_nozzle is not None:
@@ -323,6 +324,9 @@ class GCodeParser:
                 v = float(params[a])
                 self.base_position[p] += self.homing_add[p] - v
                 self.homing_add[p] = v
+    def cmd_M400(self, params):
+        # Wait for current moves to finish
+        self.toolhead.wait_moves()
     cmd_QUERY_ENDSTOPS_help = "Report on the status of each endstop"
     cmd_QUERY_ENDSTOPS_aliases = ["M119"]
     def cmd_QUERY_ENDSTOPS(self, params):
diff --git a/klippy/toolhead.py b/klippy/toolhead.py
index 504848f3..f4edd521 100644
--- a/klippy/toolhead.py
+++ b/klippy/toolhead.py
@@ -287,6 +287,11 @@ class ToolHead:
         self.extruder.motor_off(last_move_time)
         self.dwell(STALL_TIME)
         logging.debug('; Max time of %f' % (last_move_time,))
+    def wait_moves(self):
+        self.move_queue.flush()
+        eventtime = time.time()
+        while self.print_time:
+            eventtime = self.reactor.pause(eventtime + 0.100)
     def query_endstops(self):
         last_move_time = self.get_last_move_time()
         return self.kin.query_endstops(last_move_time)