diff --git a/Marlin/src/Marlin.cpp b/Marlin/src/Marlin.cpp
index 8e2123002..986cf2ae6 100644
--- a/Marlin/src/Marlin.cpp
+++ b/Marlin/src/Marlin.cpp
@@ -359,10 +359,6 @@ void quickstop_stepper() {
   SYNC_PLAN_POSITION_KINEMATIC();
 }
 
-#if ENABLED(MK2_MULTIPLEXER)
-  #include "gcode/feature/snmm/M702.h"
-#endif
-
 #if ENABLED(DUAL_X_CARRIAGE) || ENABLED(DUAL_NOZZLE_DUPLICATION_MODE)
   #include "gcode/control/M605.h"
 #endif
diff --git a/Marlin/src/feature/snmm.cpp b/Marlin/src/feature/snmm.cpp
new file mode 100644
index 000000000..e9ef540d6
--- /dev/null
+++ b/Marlin/src/feature/snmm.cpp
@@ -0,0 +1,38 @@
+/**
+ * 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/>.
+ *
+ */
+
+#include "../inc/MarlinConfig.h"
+
+#if ENABLED(MK2_MULTIPLEXER)
+
+#include "../module/stepper.h"
+
+void select_multiplexed_stepper(const uint8_t e) {
+  stepper.synchronize();
+  disable_e_steppers();
+  WRITE(E_MUX0_PIN, TEST(e, 0) ? HIGH : LOW);
+  WRITE(E_MUX1_PIN, TEST(e, 1) ? HIGH : LOW);
+  WRITE(E_MUX2_PIN, TEST(e, 2) ? HIGH : LOW);
+  safe_delay(100);
+}
+
+#endif // MK2_MULTIPLEXER
diff --git a/Marlin/src/feature/snmm.h b/Marlin/src/feature/snmm.h
new file mode 100644
index 000000000..b15f9147a
--- /dev/null
+++ b/Marlin/src/feature/snmm.h
@@ -0,0 +1,28 @@
+/**
+ * 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/>.
+ *
+ */
+
+#ifndef __SNMM_H__
+#define __SNMM_H__
+
+void select_multiplexed_stepper(const uint8_t e);
+
+#endif // __SNMM_H__
diff --git a/Marlin/src/gcode/feature/snmm/M702.h b/Marlin/src/gcode/feature/snmm/M702.cpp
similarity index 81%
rename from Marlin/src/gcode/feature/snmm/M702.h
rename to Marlin/src/gcode/feature/snmm/M702.cpp
index efc99cac4..b4f3861b2 100644
--- a/Marlin/src/gcode/feature/snmm/M702.h
+++ b/Marlin/src/gcode/feature/snmm/M702.cpp
@@ -20,21 +20,19 @@
  *
  */
 
-inline void select_multiplexed_stepper(const uint8_t e) {
-  stepper.synchronize();
-  disable_e_steppers();
-  WRITE(E_MUX0_PIN, TEST(e, 0) ? HIGH : LOW);
-  WRITE(E_MUX1_PIN, TEST(e, 1) ? HIGH : LOW);
-  WRITE(E_MUX2_PIN, TEST(e, 2) ? HIGH : LOW);
-  safe_delay(100);
-}
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(MK2_MULTIPLEXER)
+
+#include "../../gcode.h"
+#include "../../../feature/snmm.h"
 
 /**
  * M702: Unload all extruders
  */
-void gcode_M702() {
+void GcodeSuite::M702() {
   for (uint8_t s = 0; s < E_STEPPERS; s++) {
-    select_multiplexed_stepper(e);
+    select_multiplexed_stepper(s);
     // TODO: standard unload filament function
     // MK2 firmware behavior:
     //  - Make sure temperature is high enough
@@ -48,3 +46,5 @@ void gcode_M702() {
   select_multiplexed_stepper(active_extruder);
   disable_e_steppers();
 }
+
+#endif // MK2_MULTIPLEXER
diff --git a/Marlin/src/gcode/gcode.cpp b/Marlin/src/gcode/gcode.cpp
index 6d9e77aa0..bdea78350 100644
--- a/Marlin/src/gcode/gcode.cpp
+++ b/Marlin/src/gcode/gcode.cpp
@@ -123,7 +123,6 @@ extern void gcode_M350();
 extern void gcode_M351();
 extern void gcode_M355();
 extern void gcode_M605();
-extern void gcode_M702();
 extern void gcode_M900();
 extern void gcode_M906();
 extern void gcode_M911();
@@ -658,9 +657,7 @@ void GcodeSuite::process_next_command() {
       #endif // DUAL_X_CARRIAGE
 
       #if ENABLED(MK2_MULTIPLEXER)
-        case 702: // M702: Unload all extruders
-          gcode_M702();
-          break;
+        case 702: M702(); break;  // M702: Unload all extruders
       #endif
 
       #if ENABLED(LIN_ADVANCE)
diff --git a/Marlin/src/module/tool_change.cpp b/Marlin/src/module/tool_change.cpp
index a52473cc9..b8fab7cc8 100644
--- a/Marlin/src/module/tool_change.cpp
+++ b/Marlin/src/module/tool_change.cpp
@@ -38,6 +38,10 @@
   #include "../feature/solenoid.h"
 #endif
 
+#if ENABLED(MK2_MULTIPLEXER)
+  #include "../feature/snmm.h"
+#endif
+
 #if ENABLED(SWITCHING_EXTRUDER)
 
   #if EXTRUDERS > 3