diff --git a/klippy/gcode.py b/klippy/gcode.py
index 8db551bd..c86549ff 100644
--- a/klippy/gcode.py
+++ b/klippy/gcode.py
@@ -344,7 +344,7 @@ class GCodeParser:
                              % (key_param, key))
         values[key_param](gcmd)
     all_handlers = [
-        'G1', 'G4', 'G28', 'M400',
+        'G1', 'G28',
         'G20', 'M82', 'M83', 'G90', 'G91', 'G92', 'M114', 'M220', 'M221',
         'SET_GCODE_OFFSET', 'SAVE_GCODE_STATE', 'RESTORE_GCODE_STATE',
         'M112', 'M115', 'IGNORE', 'GET_POSITION',
@@ -382,10 +382,6 @@ class GCodeParser:
             raise gcmd.error("Unable to parse move '%s'"
                              % (gcmd.get_commandline(),))
         self.move_with_transform(self.last_position, self.speed)
-    def cmd_G4(self, gcmd):
-        # Dwell
-        delay = gcmd.get_float('P', 0., minval=0.) / 1000.
-        self.toolhead.dwell(delay)
     def cmd_G28(self, gcmd):
         # Move to origin
         axes = []
@@ -400,9 +396,6 @@ class GCodeParser:
         homing_state.home_axes(axes)
         for axis in homing_state.get_axes():
             self.base_position[axis] = self.homing_position[axis]
-    def cmd_M400(self, gcmd):
-        # Wait for current moves to finish
-        self.toolhead.wait_moves()
     # G-Code coordinate manipulation
     def cmd_G20(self, gcmd):
         # Set units to inches
diff --git a/klippy/toolhead.py b/klippy/toolhead.py
index fedcf481..c0b72d84 100644
--- a/klippy/toolhead.py
+++ b/klippy/toolhead.py
@@ -259,8 +259,10 @@ class ToolHead:
             msg = "Error loading kinematics '%s'" % (kin_name,)
             logging.exception(msg)
             raise config.error(msg)
-        # SET_VELOCITY_LIMIT command
+        # Register commands
         gcode = self.printer.lookup_object('gcode')
+        gcode.register_command('G4', self.cmd_G4)
+        gcode.register_command('M400', self.cmd_M400)
         gcode.register_command('SET_VELOCITY_LIMIT',
                                self.cmd_SET_VELOCITY_LIMIT,
                                desc=self.cmd_SET_VELOCITY_LIMIT_help)
@@ -540,6 +542,13 @@ class ToolHead:
         self.junction_deviation = scv2 * (math.sqrt(2.) - 1.) / self.max_accel
         self.max_accel_to_decel = min(self.requested_accel_to_decel,
                                       self.max_accel)
+    def cmd_G4(self, gcmd):
+        # Dwell
+        delay = gcmd.get_float('P', 0., minval=0.) / 1000.
+        self.dwell(delay)
+    def cmd_M400(self, gcmd):
+        # Wait for current moves to finish
+        self.wait_moves()
     cmd_SET_VELOCITY_LIMIT_help = "Set printer velocity limits"
     def cmd_SET_VELOCITY_LIMIT(self, gcmd):
         print_time = self.get_last_move_time()