diff --git a/Marlin/src/Marlin.cpp b/Marlin/src/Marlin.cpp
index f0b5d971a..8e2123002 100644
--- a/Marlin/src/Marlin.cpp
+++ b/Marlin/src/Marlin.cpp
@@ -359,10 +359,6 @@ void quickstop_stepper() {
   SYNC_PLAN_POSITION_KINEMATIC();
 }
 
-#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
-  #include "gcode/config/M540.h"
-#endif
-
 #if ENABLED(MK2_MULTIPLEXER)
   #include "gcode/feature/snmm/M702.h"
 #endif
diff --git a/Marlin/src/gcode/config/M540.h b/Marlin/src/gcode/config/M540.cpp
similarity index 75%
rename from Marlin/src/gcode/config/M540.h
rename to Marlin/src/gcode/config/M540.cpp
index c12887ade..498aa9e76 100644
--- a/Marlin/src/gcode/config/M540.h
+++ b/Marlin/src/gcode/config/M540.cpp
@@ -20,9 +20,21 @@
  *
  */
 
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
+
+#include "../gcode.h"
+#include "../../module/stepper.h"
+
 /**
  * M540: Set whether SD card print should abort on endstop hit (M540 S<0|1>)
  */
-void gcode_M540() {
-  if (parser.seen('S')) stepper.abort_on_endstop_hit = parser.value_bool();
+void GcodeSuite::M540() {
+
+  if (parser.seen('S'))
+    stepper.abort_on_endstop_hit = parser.value_bool();
+
 }
+
+#endif // ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED
diff --git a/Marlin/src/gcode/gcode.cpp b/Marlin/src/gcode/gcode.cpp
index 205f01502..6d9e77aa0 100644
--- a/Marlin/src/gcode/gcode.cpp
+++ b/Marlin/src/gcode/gcode.cpp
@@ -122,7 +122,6 @@ extern void gcode_M165();
 extern void gcode_M350();
 extern void gcode_M351();
 extern void gcode_M355();
-extern void gcode_M540();
 extern void gcode_M605();
 extern void gcode_M702();
 extern void gcode_M900();
@@ -637,9 +636,7 @@ void GcodeSuite::process_next_command() {
       #endif
 
       #if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
-        case 540: // M540: Set abort on endstop hit for SD printing
-          gcode_M540();
-          break;
+        case 540: M540(); break;  // M540: Set abort on endstop hit for SD printing
       #endif
 
       #if HAS_BED_PROBE