diff --git a/Marlin/src/Marlin.cpp b/Marlin/src/Marlin.cpp
index 3f98c565b3..0045b93a4b 100644
--- a/Marlin/src/Marlin.cpp
+++ b/Marlin/src/Marlin.cpp
@@ -75,7 +75,7 @@
 #endif
 
 #if HAS_SERVOS
-  #include "HAL/servo.h"
+  #include "module/servo.h"
 #endif
 
 #if HAS_DIGIPOTSS
@@ -247,35 +247,6 @@ void setup_powerhold() {
   #endif
 }
 
-#if HAS_SERVOS
-
-  HAL_SERVO_LIB servo[NUM_SERVOS];
-
-  void servo_init() {
-    #if NUM_SERVOS >= 1 && HAS_SERVO_0
-      servo[0].attach(SERVO0_PIN);
-      servo[0].detach(); // Just set up the pin. We don't have a position yet. Don't move to a random position.
-    #endif
-    #if NUM_SERVOS >= 2 && HAS_SERVO_1
-      servo[1].attach(SERVO1_PIN);
-      servo[1].detach();
-    #endif
-    #if NUM_SERVOS >= 3 && HAS_SERVO_2
-      servo[2].attach(SERVO2_PIN);
-      servo[2].detach();
-    #endif
-    #if NUM_SERVOS >= 4 && HAS_SERVO_3
-      servo[3].attach(SERVO3_PIN);
-      servo[3].detach();
-    #endif
-
-    #if HAS_Z_SERVO_ENDSTOP
-      servo_probe_init();
-    #endif
-  }
-
-#endif // HAS_SERVOS
-
 /**
  * Stepper Reset (RigidBoard, et.al.)
  */
@@ -745,6 +716,10 @@ void setup() {
     servo_init();
   #endif
 
+  #if HAS_Z_SERVO_ENDSTOP
+    servo_probe_init();
+  #endif
+
   #if HAS_PHOTOGRAPH
     OUT_WRITE(PHOTOGRAPH_PIN, LOW);
   #endif
diff --git a/Marlin/src/Marlin.h b/Marlin/src/Marlin.h
index 79dba4fcf0..9fa0fc964e 100644
--- a/Marlin/src/Marlin.h
+++ b/Marlin/src/Marlin.h
@@ -186,16 +186,6 @@ extern volatile bool wait_for_heatup;
 // Inactivity shutdown timer
 extern millis_t max_inactive_time, stepper_inactive_time;
 
-#if HAS_SERVOS
-  #include "HAL/servo.h"
-  extern HAL_SERVO_LIB servo[NUM_SERVOS];
-  #define MOVE_SERVO(I, P) servo[I].move(P)
-  #if HAS_Z_SERVO_ENDSTOP
-    #define DEPLOY_Z_SERVO() MOVE_SERVO(Z_ENDSTOP_SERVO_NR, z_servo_angle[0])
-    #define STOW_Z_SERVO() MOVE_SERVO(Z_ENDSTOP_SERVO_NR, z_servo_angle[1])
-  #endif
-#endif
-
 #if FAN_COUNT > 0
   extern int16_t fanSpeeds[FAN_COUNT];
   #if ENABLED(EXTRA_FAN_SPEED)
diff --git a/Marlin/src/gcode/config/M43.cpp b/Marlin/src/gcode/config/M43.cpp
index 9b7fc47538..b600e95459 100644
--- a/Marlin/src/gcode/config/M43.cpp
+++ b/Marlin/src/gcode/config/M43.cpp
@@ -31,6 +31,7 @@
 
 #if HAS_Z_SERVO_ENDSTOP
   #include "../../module/probe.h"
+  #include "../../module/servo.h"
 #endif
 
 inline void toggle_pins() {
diff --git a/Marlin/src/gcode/control/M280.cpp b/Marlin/src/gcode/control/M280.cpp
index 9d0f5c4e7f..1cfb6c89fd 100644
--- a/Marlin/src/gcode/control/M280.cpp
+++ b/Marlin/src/gcode/control/M280.cpp
@@ -25,7 +25,7 @@
 #if HAS_SERVOS
 
 #include "../gcode.h"
-#include "../../Marlin.h" // for servo[]
+#include "../../module/servo.h"
 
 /**
  * M280: Get or set servo position. P<index> [S<angle>]
diff --git a/Marlin/src/module/probe.cpp b/Marlin/src/module/probe.cpp
index 43ab0f9d98..127c6b0035 100644
--- a/Marlin/src/module/probe.cpp
+++ b/Marlin/src/module/probe.cpp
@@ -53,6 +53,7 @@
 float zprobe_zoffset; // Initialized by settings.load()
 
 #if HAS_Z_SERVO_ENDSTOP
+  #include "../module/servo.h"
   const int z_servo_angle[2] = Z_SERVO_ANGLES;
 #endif
 
diff --git a/Marlin/src/module/servo.cpp b/Marlin/src/module/servo.cpp
new file mode 100644
index 0000000000..a1f9633f78
--- /dev/null
+++ b/Marlin/src/module/servo.cpp
@@ -0,0 +1,54 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * module/servo.cpp
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if HAS_SERVOS
+
+#include "servo.h"
+
+HAL_SERVO_LIB servo[NUM_SERVOS];
+
+void servo_init() {
+  #if NUM_SERVOS >= 1 && HAS_SERVO_0
+    servo[0].attach(SERVO0_PIN);
+    servo[0].detach(); // Just set up the pin. We don't have a position yet. Don't move to a random position.
+  #endif
+  #if NUM_SERVOS >= 2 && HAS_SERVO_1
+    servo[1].attach(SERVO1_PIN);
+    servo[1].detach();
+  #endif
+  #if NUM_SERVOS >= 3 && HAS_SERVO_2
+    servo[2].attach(SERVO2_PIN);
+    servo[2].detach();
+  #endif
+  #if NUM_SERVOS >= 4 && HAS_SERVO_3
+    servo[3].attach(SERVO3_PIN);
+    servo[3].detach();
+  #endif
+}
+
+#endif // HAS_SERVOS
diff --git a/Marlin/src/module/servo.h b/Marlin/src/module/servo.h
new file mode 100644
index 0000000000..cb5080e2b0
--- /dev/null
+++ b/Marlin/src/module/servo.h
@@ -0,0 +1,44 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * module/servo.h
+ */
+
+#ifndef _SERVO_H_
+#define _SERVO_H_
+
+#include "../HAL/servo.h"
+
+extern HAL_SERVO_LIB servo[NUM_SERVOS];
+extern void servo_init();
+
+#define MOVE_SERVO(I, P) servo[I].move(P)
+
+#include "../inc/MarlinConfig.h"
+
+#if HAS_Z_SERVO_ENDSTOP
+  #define DEPLOY_Z_SERVO() MOVE_SERVO(Z_ENDSTOP_SERVO_NR, z_servo_angle[0])
+  #define STOW_Z_SERVO() MOVE_SERVO(Z_ENDSTOP_SERVO_NR, z_servo_angle[1])
+#endif
+
+#endif // _SERVO_H_
diff --git a/Marlin/src/module/tool_change.cpp b/Marlin/src/module/tool_change.cpp
index 10210653e7..cb74c59137 100644
--- a/Marlin/src/module/tool_change.cpp
+++ b/Marlin/src/module/tool_change.cpp
@@ -34,6 +34,10 @@
   #include "../gcode/gcode.h" // for dwell()
 #endif
 
+#if ENABLED(SWITCHING_EXTRUDER) || ENABLED(SWITCHING_NOZZLE)
+  #include "../module/servo.h"
+#endif
+
 #if ENABLED(EXT_SOLENOID) && !ENABLED(PARKING_EXTRUDER)
   #include "../feature/solenoid.h"
 #endif