diff --git a/Marlin/src/gcode/calibrate/M666.cpp b/Marlin/src/gcode/calibrate/M666.cpp
index a3a48cd3fc..8ff51f0e3f 100644
--- a/Marlin/src/gcode/calibrate/M666.cpp
+++ b/Marlin/src/gcode/calibrate/M666.cpp
@@ -71,29 +71,27 @@
     #endif
     #if ENABLED(Z_MULTI_ENDSTOPS)
       if (parser.seenval('Z')) {
-        #if NUM_Z_STEPPER_DRIVERS >= 3
-          const float z_adj = parser.value_linear_units();
-          const int ind = parser.intval('S');
-          if (!ind || ind == 2) endstops.z2_endstop_adj = z_adj;
-          if (!ind || ind == 3) endstops.z3_endstop_adj = z_adj;
-          #if NUM_Z_STEPPER_DRIVERS >= 4
-            if (!ind || ind == 4) endstops.z4_endstop_adj = z_adj;
-          #endif
+        const float z_adj = parser.value_linear_units();
+        #if NUM_Z_STEPPER_DRIVERS == 2
+          endstops.z2_endstop_adj = z_adj;
         #else
-          endstops.z2_endstop_adj = parser.value_linear_units();
+          const int ind = parser.intval('S');
+          #define _SET_ZADJ(N) if (!ind || ind == N) endstops.z##N##_endstop_adj = z_adj;
+          REPEAT_S(2, INCREMENT(NUM_Z_STEPPER_DRIVERS), _SET_ZADJ)
         #endif
       }
     #endif
     if (!parser.seen("XYZ")) {
+      auto echo_adj = [](PGM_P const label, const_float_t value) { SERIAL_ECHOPAIR_P(label, value); };
       SERIAL_ECHOPGM("Dual Endstop Adjustment (mm): ");
       #if ENABLED(X_DUAL_ENDSTOPS)
-        SERIAL_ECHOPAIR(" X2:", endstops.x2_endstop_adj);
+        echo_adj(PSTR(" X2:"), endstops.x2_endstop_adj);
       #endif
       #if ENABLED(Y_DUAL_ENDSTOPS)
-        SERIAL_ECHOPAIR(" Y2:", endstops.y2_endstop_adj);
+        echo_adj(PSTR(" Y2:"), endstops.y2_endstop_adj);
       #endif
       #if ENABLED(Z_MULTI_ENDSTOPS)
-        #define _ECHO_ZADJ(N) SERIAL_ECHOPAIR(" Z" STRINGIFY(N) ":", endstops.z##N##_endstop_adj);
+        #define _ECHO_ZADJ(N) echo_adj(PSTR(" Z" STRINGIFY(N) ":"), endstops.z##N##_endstop_adj);
         REPEAT_S(2, INCREMENT(NUM_Z_STEPPER_DRIVERS), _ECHO_ZADJ)
       #endif
       SERIAL_EOL();
diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp
index 062049ec77..0ff909d7cc 100644
--- a/Marlin/src/module/stepper.cpp
+++ b/Marlin/src/module/stepper.cpp
@@ -314,26 +314,18 @@ xyze_int8_t Stepper::count_direction{0};
     A##3_STEP_WRITE(V);                           \
   }
 
-#define QUAD_ENDSTOP_APPLY_STEP(A,V)                                                                                        \
-  if (separate_multi_axis) {                                                                                                \
-    if (ENABLED(A##_HOME_TO_MIN)) {                                                                                         \
-      if (!(TEST(endstops.state(), A##_MIN) && count_direction[_AXIS(A)] < 0) && !locked_##A##_motor) A##_STEP_WRITE(V);    \
-      if (!(TEST(endstops.state(), A##2_MIN) && count_direction[_AXIS(A)] < 0) && !locked_##A##2_motor) A##2_STEP_WRITE(V); \
-      if (!(TEST(endstops.state(), A##3_MIN) && count_direction[_AXIS(A)] < 0) && !locked_##A##3_motor) A##3_STEP_WRITE(V); \
-      if (!(TEST(endstops.state(), A##4_MIN) && count_direction[_AXIS(A)] < 0) && !locked_##A##4_motor) A##4_STEP_WRITE(V); \
-    }                                                                                                                       \
-    else {                                                                                                                  \
-      if (!(TEST(endstops.state(), A##_MAX) && count_direction[_AXIS(A)] > 0) && !locked_##A##_motor) A##_STEP_WRITE(V);    \
-      if (!(TEST(endstops.state(), A##2_MAX) && count_direction[_AXIS(A)] > 0) && !locked_##A##2_motor) A##2_STEP_WRITE(V); \
-      if (!(TEST(endstops.state(), A##3_MAX) && count_direction[_AXIS(A)] > 0) && !locked_##A##3_motor) A##3_STEP_WRITE(V); \
-      if (!(TEST(endstops.state(), A##4_MAX) && count_direction[_AXIS(A)] > 0) && !locked_##A##4_motor) A##4_STEP_WRITE(V); \
-    }                                                                                                                       \
-  }                                                                                                                         \
-  else {                                                                                                                    \
-    A##_STEP_WRITE(V);                                                                                                      \
-    A##2_STEP_WRITE(V);                                                                                                     \
-    A##3_STEP_WRITE(V);                                                                                                     \
-    A##4_STEP_WRITE(V);                                                                                                     \
+#define QUAD_ENDSTOP_APPLY_STEP(A,V) \
+  if (separate_multi_axis) {         \
+    if (!(TEST(endstops.state(), (TERN(A##_HOME_TO_MIN, A##_MIN,  A##_MAX)))  && count_direction[_AXIS(A)] < 0) && !locked_##A##_motor)  A##_STEP_WRITE(V);  \
+    if (!(TEST(endstops.state(), (TERN(A##_HOME_TO_MIN, A##2_MIN, A##2_MAX))) && count_direction[_AXIS(A)] < 0) && !locked_##A##2_motor) A##2_STEP_WRITE(V); \
+    if (!(TEST(endstops.state(), (TERN(A##_HOME_TO_MIN, A##3_MIN, A##3_MAX))) && count_direction[_AXIS(A)] < 0) && !locked_##A##3_motor) A##3_STEP_WRITE(V); \
+    if (!(TEST(endstops.state(), (TERN(A##_HOME_TO_MIN, A##4_MIN, A##4_MAX))) && count_direction[_AXIS(A)] < 0) && !locked_##A##4_motor) A##4_STEP_WRITE(V); \
+  }       \
+  else {  \
+    A##_STEP_WRITE(V);  \
+    A##2_STEP_WRITE(V); \
+    A##3_STEP_WRITE(V); \
+    A##4_STEP_WRITE(V); \
   }
 
 #define QUAD_SEPARATE_APPLY_STEP(A,V)             \
diff --git a/buildroot/tests/BIGTREE_GTR_V1_0 b/buildroot/tests/BIGTREE_GTR_V1_0
index 24293a4932..58cbe4a142 100755
--- a/buildroot/tests/BIGTREE_GTR_V1_0
+++ b/buildroot/tests/BIGTREE_GTR_V1_0
@@ -20,11 +20,12 @@ exec_test $1 $2 "BigTreeTech GTR | 8 Extruders | Auto-Fan | Mixed TMC Drivers |
 
 restore_configs
 opt_set MOTHERBOARD BOARD_BTT_GTR_V1_0 SERIAL_PORT -1 \
-        EXTRUDERS 6 TEMP_SENSOR_1 1 TEMP_SENSOR_2 1 TEMP_SENSOR_3 1 TEMP_SENSOR_4 1 TEMP_SENSOR_5 1 \
-        NUM_Z_STEPPER_DRIVERS 3 \
-        DEFAULT_Kp_LIST '{ 22.2, 20.0, 21.0, 19.0, 18.0, 17.0 }' DEFAULT_Ki_LIST '{ 1.08 }' DEFAULT_Kd_LIST '{ 114.0, 112.0, 110.0, 108.0 }'
-opt_enable TOOLCHANGE_FILAMENT_SWAP TOOLCHANGE_MIGRATION_FEATURE TOOLCHANGE_FS_INIT_BEFORE_SWAP TOOLCHANGE_FS_PRIME_FIRST_USED PID_PARAMS_PER_HOTEND
-exec_test $1 $2 "BigTreeTech GTR | 6 Extruders | Triple Z" "$3"
+        EXTRUDERS 5 TEMP_SENSOR_1 1 TEMP_SENSOR_2 1 TEMP_SENSOR_3 1 TEMP_SENSOR_4 1 \
+        NUM_Z_STEPPER_DRIVERS 4 \
+        DEFAULT_Kp_LIST '{ 22.2, 20.0, 21.0, 19.0, 18.0 }' DEFAULT_Ki_LIST '{ 1.08 }' DEFAULT_Kd_LIST '{ 114.0, 112.0, 110.0, 108.0 }'
+opt_enable TOOLCHANGE_FILAMENT_SWAP TOOLCHANGE_MIGRATION_FEATURE TOOLCHANGE_FS_INIT_BEFORE_SWAP TOOLCHANGE_FS_PRIME_FIRST_USED \
+           PID_PARAMS_PER_HOTEND Z_MULTI_ENDSTOPS
+exec_test $1 $2 "BigTreeTech GTR | 6 Extruders | Quad Z + Endstops" "$3"
 
 restore_configs
 opt_set MOTHERBOARD BOARD_BTT_GTR_V1_0 SERIAL_PORT -1 \