diff --git a/Marlin/src/Marlin.cpp b/Marlin/src/Marlin.cpp
index 34a7f9ce12..5dc6866a1c 100644
--- a/Marlin/src/Marlin.cpp
+++ b/Marlin/src/Marlin.cpp
@@ -370,10 +370,6 @@ bool pin_is_protected(const int8_t pin) {
   return false;
 }
 
-#if ENABLED(PINS_DEBUGGING)
-  #include "gcode/config/M43.h"
-#endif
-
 #if ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST)
   #include "gcode/calibrate/M48.h"
 #endif
diff --git a/Marlin/src/gcode/config/M43.h b/Marlin/src/gcode/config/M43.cpp
similarity index 97%
rename from Marlin/src/gcode/config/M43.h
rename to Marlin/src/gcode/config/M43.cpp
index c87607c6d8..fd0d1c9c7d 100644
--- a/Marlin/src/gcode/config/M43.h
+++ b/Marlin/src/gcode/config/M43.cpp
@@ -20,9 +20,18 @@
  *
  */
 
-#include "../gcode.h"
+#include "../../inc/MarlinConfig.h"
 
+#if ENABLED(PINS_DEBUGGING)
+
+#include "../gcode.h"
+#include "../../Marlin.h" // for pin_is_protected
 #include "../../pins/pinsDebug.h"
+#include "../../module/endstops.h"
+
+#if HAS_Z_SERVO_ENDSTOP
+  #include "../../module/probe.h"
+#endif
 
 inline void toggle_pins() {
   const bool I_flag = parser.boolval('I');
@@ -227,7 +236,7 @@ inline void servo_probe_test() {
  *  M43 S       - Servo probe test
  *                  P<index> - Probe index (optional - defaults to 0
  */
-void gcode_M43() {
+void GcodeSuite::M43() {
 
   if (parser.seen('T')) {   // must be first or else its "S" and "E" parameters will execute endstop or servo test
     toggle_pins();
@@ -311,3 +320,5 @@ void gcode_M43() {
   for (uint8_t pin = first_pin; pin <= last_pin; pin++)
     if (VALID_PIN(pin)) report_pin_state_extended(pin, ignore_protection, true);
 }
+
+#endif // PINS_DEBUGGING
diff --git a/Marlin/src/gcode/gcode.cpp b/Marlin/src/gcode/gcode.cpp
index 547b03b242..ca69bcbeda 100644
--- a/Marlin/src/gcode/gcode.cpp
+++ b/Marlin/src/gcode/gcode.cpp
@@ -117,7 +117,6 @@ void GcodeSuite::dwell(millis_t time) {
 // Placeholders for non-migrated codes
 //
 extern void gcode_M18_M84();
-extern void gcode_M43();
 extern void gcode_M48();
 extern void gcode_M75();
 extern void gcode_M76();
@@ -435,11 +434,9 @@ void GcodeSuite::process_next_command() {
       case 42: M42(); break;      // M42: Change pin state
 
       #if ENABLED(PINS_DEBUGGING)
-        case 43: // M43: Read pin state
-          gcode_M43(); break;
+        case 43: M43(); break;    // M43: Read pin state
       #endif
 
-
       #if ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST)
         case 48: // M48: Z probe repeatability test
           gcode_M48();