diff --git a/Configuration.h b/Configuration.h
index 6476a1c799..e85e397868 100644
--- a/Configuration.h
+++ b/Configuration.h
@@ -1172,7 +1172,7 @@
 #define X_ENABLE_ON               1
 #define Y_ENABLE_ON               1
 #define Z_ENABLE_ON               1
-#define A_ENABLE_ON               1
+#define A_ENABLE_ON          1
 
 // Disable axis steppers immediately when they're not being stepped.
 // WARNING: When motors turn off there is a chance of losing position accuracy!
diff --git a/Configuration_adv.h b/Configuration_adv.h
index e36735f316..5ebf47f2e9 100644
--- a/Configuration_adv.h
+++ b/Configuration_adv.h
@@ -663,9 +663,9 @@
 // #define SENSORLESS_BACKOFF_MM  { 40, 40, 40 }     // (mm) Backoff from endstops before sensorless homing
 
 #define HOMING_BUMP_MM \
-	{ 15, 15, 15 } // (mm) Backoff from endstops after first bump
+	{ 15, 15, 15, 15, 15, 15 } // (mm) Backoff from endstops after first bump
 #define HOMING_BUMP_DIVISOR \
-	{ 10, 10, 5 } // Re-Bump Speed Divisor (Divides the Homing Feedrate)
+	{ 10, 10, 5, 10, 10, 10 } // Re-Bump Speed Divisor (Divides the Homing Feedrate)
 
 // #define HOMING_BACKOFF_POST_MM { 5, 5, 2 }  // (mm) Backoff from endstops after homing
 
@@ -857,7 +857,9 @@
 #define INVERT_X_STEP_PIN             true
 #define INVERT_Y_STEP_PIN             true
 #define INVERT_Z_STEP_PIN             true
-#define INVERT_E_STEP_PIN             true
+#define INVERT_A_STEP_PIN             true
+#define INVERT_B_STEP_PIN             true
+#define INVERT_C_STEP_PIN             true
 
 /**
  * Idle Stepper Shutdown
@@ -868,7 +870,9 @@
 #define DISABLE_INACTIVE_X            false
 #define DISABLE_INACTIVE_Y            false
 #define DISABLE_INACTIVE_Z            false // Set 'false' if the nozzle could fall onto your printed part!
-#define DISABLE_INACTIVE_E            false
+#define DISABLE_INACTIVE_A            false
+#define DISABLE_INACTIVE_B            false
+#define DISABLE_INACTIVE_C            false
 
 // If the Nozzle or Bed falls when the Z stepper is disabled, set its resting position here.
 // #define Z_AFTER_DEACTIVATE Z_HOME_POS
diff --git a/src/marlin/HAL/SAMD51/endstop_interrupts.h b/src/marlin/HAL/SAMD51/endstop_interrupts.h
index 170e7ef403..9582da01b8 100644
--- a/src/marlin/HAL/SAMD51/endstop_interrupts.h
+++ b/src/marlin/HAL/SAMD51/endstop_interrupts.h
@@ -108,6 +108,16 @@
 #else
   #define MATCH_Z4_MIN_EILINE(P) false
 #endif
+#if HAS_A_MAX
+  #define MATCH_A_MAX_EILINE(P) MATCH_EILINE(P, A_MAX_PIN)
+#else
+  #define MATCH_A_MAX_EILINE(P) false
+#endif
+#if HAS_A_MIN
+  #define MATCH_A_MIN_EILINE(P) MATCH_EILINE(P, A_MIN_PIN)
+#else
+  #define MATCH_A_MIN_EILINE(P) false
+#endif
 #if HAS_Z_MIN_PROBE_PIN
   #define MATCH_Z_MIN_PROBE_EILINE(P)   MATCH_EILINE(P, Z_MIN_PROBE_PIN)
 #else
@@ -120,6 +130,7 @@
                                  && !MATCH_Z2_MAX_EILINE(P) && !MATCH_Z2_MIN_EILINE(P)  \
                                  && !MATCH_Z3_MAX_EILINE(P) && !MATCH_Z3_MIN_EILINE(P)  \
                                  && !MATCH_Z4_MAX_EILINE(P) && !MATCH_Z4_MIN_EILINE(P) \
+																 && !MATCH_A_MAX_EILINE(P) && !MATCH_A_MIN_EILINE(P)  \
                                  && !MATCH_Z_MIN_PROBE_EILINE(P))
 
 // One ISR for all EXT-Interrupts
@@ -210,6 +221,18 @@ void setup_endstop_interrupts() {
       #error "Z4_MIN_PIN has no EXTINT line available."
     #endif
     _ATTACH(Z4_MIN_PIN);
+  #endif
+	#if HAS_A_MAX
+    #if !AVAILABLE_EILINE(A_MAX_PIN)
+      #error "A_MAX_PIN has no EXTINT line available."
+    #endif
+    _ATTACH(A_MAX_PIN);
+  #endif
+  #if HAS_A_MIN
+    #if !AVAILABLE_EILINE(A_MIN_PIN)
+      #error "A_MIN_PIN has no EXTINT line available."
+    #endif
+    _ATTACH(A_MIN_PIN);
   #endif
   #if HAS_Z_MIN_PROBE_PIN
     #if !AVAILABLE_EILINE(Z_MIN_PROBE_PIN)
diff --git a/src/marlin/MarlinCore.cpp b/src/marlin/MarlinCore.cpp
index 957291cd04..112adcb1ee 100644
--- a/src/marlin/MarlinCore.cpp
+++ b/src/marlin/MarlinCore.cpp
@@ -229,7 +229,9 @@ PGMSTR(SP_E_STR, " E");
 PGMSTR(SP_X_LBL, " X:");
 PGMSTR(SP_Y_LBL, " Y:");
 PGMSTR(SP_Z_LBL, " Z:");
-PGMSTR(SP_E_LBL, " E:");
+PGMSTR(SP_A_LBL, " A:");
+PGMSTR(SP_B_LBL, " B:");
+PGMSTR(SP_C_LBL, " C:");
 
 MarlinState marlin_state = MF_INITIALIZING;
 
@@ -344,47 +346,28 @@ void protected_pin_err() {
 void quickstop_stepper() {
 	planner.quick_stop();
 	planner.synchronize();
-	set_current_from_steppers_for_axis(ALL_AXES);
+	set_current_from_steppers_for_axis(AxisValue::All);
 	sync_plan_position();
 }
 
-void enable_e_steppers() {
-#define _ENA_E(N) ENABLE_AXIS_E##N();
-	REPEAT(E_STEPPERS, _ENA_E)
-}
-
 void enable_all_steppers() {
-	TERN_(AUTO_POWER_CONTROL, powerManager.power_on());
 	ENABLE_AXIS_X();
 	ENABLE_AXIS_Y();
 	ENABLE_AXIS_Z();
-	enable_e_steppers();
-
-	TERN_(EXTENSIBLE_UI, ExtUI::onSteppersEnabled());
-}
-
-void disable_e_steppers() {
-#define _DIS_E(N) DISABLE_AXIS_E##N();
-	REPEAT(E_STEPPERS, _DIS_E)
-}
+	ENABLE_AXIS_A();
+	ENABLE_AXIS_B();
+	ENABLE_AXIS_C();
 
-void disable_e_stepper(const uint8_t e) {
-#define _CASE_DIS_E(N) \
-	case N: \
-		DISABLE_AXIS_E##N(); \
-		break;
-	switch (e) {
-		REPEAT(EXTRUDERS, _CASE_DIS_E)
-	}
+	safe_delay(200);
 }
 
 void disable_all_steppers() {
 	DISABLE_AXIS_X();
 	DISABLE_AXIS_Y();
 	DISABLE_AXIS_Z();
-	disable_e_steppers();
-
-	TERN_(EXTENSIBLE_UI, ExtUI::onSteppersDisabled());
+	DISABLE_AXIS_A();
+	DISABLE_AXIS_B();
+	DISABLE_AXIS_C();
 }
 
 #if ENABLED(G29_RETRY_AND_RECOVER)
@@ -544,8 +527,6 @@ inline void manage_inactivity(const bool ignore_stepper_queue = false) {
 					DISABLE_AXIS_Y();
 				if (ENABLED(DISABLE_INACTIVE_Z))
 					DISABLE_AXIS_Z();
-				if (ENABLED(DISABLE_INACTIVE_E))
-					disable_e_steppers();
 
 				TERN_(AUTO_BED_LEVELING_UBL, ubl.steppers_were_disabled());
 			}
@@ -891,8 +872,10 @@ void minkill(const bool steppers_off /*=false*/) {
 
 	TERN_(HAS_CUTTER, cutter.enabled(false)); // Reiterate cutter shutdown
 
-	// Power off all steppers (for M112) or just the E steppers
-	steppers_off ? disable_all_steppers() : disable_e_steppers();
+	// Power off all steppers (for M112)
+	if (steppers_off) {
+		disable_all_steppers();
+	}
 
 	TERN_(PSU_CONTROL, PSU_OFF());
 
diff --git a/src/marlin/MarlinCore.h b/src/marlin/MarlinCore.h
index 52c7e13150..f28bf59c5a 100644
--- a/src/marlin/MarlinCore.h
+++ b/src/marlin/MarlinCore.h
@@ -52,10 +52,7 @@ extern bool G38_did_trigger; // Flag from the ISR to indicate the endstop change
 /**
  * The axis order in all axis related arrays is X, Y, Z, E
  */
-void enable_e_steppers();
 void enable_all_steppers();
-void disable_e_stepper(const uint8_t e);
-void disable_e_steppers();
 void disable_all_steppers();
 
 void kill(PGM_P const lcd_error = nullptr, PGM_P const lcd_component = nullptr, const bool steppers_off = false);
@@ -142,4 +139,4 @@ void event_probe_failure();
 extern const char NUL_STR[], M112_KILL_STR[], G28_STR[], M21_STR[], M23_STR[], M24_STR[],
 		SP_A_STR[], SP_B_STR[], SP_C_STR[],
 		SP_P_STR[], SP_T_STR[], SP_X_STR[], SP_Y_STR[], SP_Z_STR[], SP_E_STR[],
-		X_LBL[], Y_LBL[], Z_LBL[], E_LBL[], SP_X_LBL[], SP_Y_LBL[], SP_Z_LBL[], SP_E_LBL[];
+		X_LBL[], Y_LBL[], Z_LBL[], E_LBL[], SP_X_LBL[], SP_Y_LBL[], SP_Z_LBL[], SP_A_LBL[], SP_B_LBL[], SP_C_LBL[];
diff --git a/src/marlin/core/drivers.h b/src/marlin/core/drivers.h
index 17413db431..1c299f239e 100644
--- a/src/marlin/core/drivers.h
+++ b/src/marlin/core/drivers.h
@@ -68,24 +68,17 @@
 #define AXIS_DRIVER_TYPE_Z3(T) (NUM_Z_STEPPER_DRIVERS >= 3 && _AXIS_DRIVER_TYPE(Z3,T))
 #define AXIS_DRIVER_TYPE_Z4(T) (NUM_Z_STEPPER_DRIVERS >= 4 && _AXIS_DRIVER_TYPE(Z4,T))
 
-#define AXIS_DRIVER_TYPE_E(N,T) (E_STEPPERS > N && _AXIS_DRIVER_TYPE(E##N,T))
-#define AXIS_DRIVER_TYPE_E0(T) AXIS_DRIVER_TYPE_E(0,T)
-#define AXIS_DRIVER_TYPE_E1(T) AXIS_DRIVER_TYPE_E(1,T)
-#define AXIS_DRIVER_TYPE_E2(T) AXIS_DRIVER_TYPE_E(2,T)
-#define AXIS_DRIVER_TYPE_E3(T) AXIS_DRIVER_TYPE_E(3,T)
-#define AXIS_DRIVER_TYPE_E4(T) AXIS_DRIVER_TYPE_E(4,T)
-#define AXIS_DRIVER_TYPE_E5(T) AXIS_DRIVER_TYPE_E(5,T)
-#define AXIS_DRIVER_TYPE_E6(T) AXIS_DRIVER_TYPE_E(6,T)
-#define AXIS_DRIVER_TYPE_E7(T) AXIS_DRIVER_TYPE_E(7,T)
+#define AXIS_DRIVER_TYPE_A(T) _AXIS_DRIVER_TYPE(A,T)
+#define AXIS_DRIVER_TYPE_B(T) _AXIS_DRIVER_TYPE(B,T)
+#define AXIS_DRIVER_TYPE_C(T) _AXIS_DRIVER_TYPE(C,T)
 
 #define AXIS_DRIVER_TYPE(A,T) AXIS_DRIVER_TYPE_##A(T)
 
-#define _OR_ADTE(N,T)   || AXIS_DRIVER_TYPE_E(N,T)
-#define HAS_E_DRIVER(T) (0 RREPEAT2(E_STEPPERS, _OR_ADTE, T))
 
 #define HAS_DRIVER(T) (  AXIS_DRIVER_TYPE_X(T)  || AXIS_DRIVER_TYPE_Y(T)  || AXIS_DRIVER_TYPE_Z(T)  \
                       || AXIS_DRIVER_TYPE_X2(T) || AXIS_DRIVER_TYPE_Y2(T) || AXIS_DRIVER_TYPE_Z2(T) \
-                      || AXIS_DRIVER_TYPE_Z3(T) || AXIS_DRIVER_TYPE_Z4(T) || HAS_E_DRIVER(T) )
+                      || AXIS_DRIVER_TYPE_Z3(T) || AXIS_DRIVER_TYPE_Z4(T) || AXIS_DRIVER_TYPE_A(T) \
+											|| AXIS_DRIVER_TYPE_B(T) || AXIS_DRIVER_TYPE_C(T) )
 
 //
 // Trinamic Stepper Drivers
@@ -117,6 +110,7 @@
   #define HAS_TMC220x 1
 #endif
 
+/*
 #define AXIS_IS_TMC(A)   (    AXIS_DRIVER_TYPE(A,TMC2130) || AXIS_DRIVER_TYPE(A,TMC2160) \
                            || AXIS_DRIVER_TYPE(A,TMC2208) || AXIS_DRIVER_TYPE(A,TMC2209) \
                            || AXIS_DRIVER_TYPE(A,TMC2660) \
@@ -142,7 +136,7 @@
 #define AXIS_HAS_STEALTHCHOP(A)  (    AXIS_DRIVER_TYPE(A,TMC2130) || AXIS_DRIVER_TYPE(A,TMC2160) \
                                    || AXIS_DRIVER_TYPE(A,TMC2208) || AXIS_DRIVER_TYPE(A,TMC2209) \
                                    || AXIS_DRIVER_TYPE(A,TMC5130) || AXIS_DRIVER_TYPE(A,TMC5160) )
-																	 
+
 #define AXIS_HAS_SG_RESULT(A)    (    AXIS_DRIVER_TYPE(A,TMC2130) || AXIS_DRIVER_TYPE(A,TMC2160) \
                                    || AXIS_DRIVER_TYPE(A,TMC2208) || AXIS_DRIVER_TYPE(A,TMC2209) \
 																	 || AXIS_DRIVER_TYPE(A,TMC2660) )
@@ -158,7 +152,8 @@
 
 #define ANY_AXIS_HAS(T) (    AXIS_HAS_##T(X)  || AXIS_HAS_##T(Y)  || AXIS_HAS_##T(Z)  \
                           || AXIS_HAS_##T(X2) || AXIS_HAS_##T(Y2) || AXIS_HAS_##T(Z2) \
-                          || AXIS_HAS_##T(Z3) || AXIS_HAS_##T(Z4) || E_AXIS_HAS(T) )
+                          || AXIS_HAS_##T(Z3) || AXIS_HAS_##T(Z4) || AXIS_HAS_##T(A) \
+													|| AXIS_HAS_##T(B) || AXIS_HAS_##T(C) )
 
 #if ANY_AXIS_HAS(STEALTHCHOP)
   #define HAS_STEALTHCHOP 1
@@ -178,6 +173,7 @@
 #if ANY_AXIS_HAS(SPI)
   #define HAS_TMC_SPI 1
 #endif
+*/
 
 //
 // TMC26XX Stepper Drivers
diff --git a/src/marlin/core/language.h b/src/marlin/core/language.h
index ab664ffb3c..cc0361d813 100644
--- a/src/marlin/core/language.h
+++ b/src/marlin/core/language.h
@@ -154,6 +154,8 @@
 #define STR_Z3_MAX                      "z3_max"
 #define STR_Z4_MIN                      "z4_min"
 #define STR_Z4_MAX                      "z4_max"
+#define STR_A_MAX                       "a_max"
+#define STR_A_MIN                       "a_min"
 #define STR_Z_PROBE                     "z_probe"
 #define STR_ESTOP                       "estop"
 #define STR_WORK_PROBE                  "work_probe"
diff --git a/src/marlin/core/macros.h b/src/marlin/core/macros.h
index 1b3a02eab7..9afb6a5b36 100644
--- a/src/marlin/core/macros.h
+++ b/src/marlin/core/macros.h
@@ -31,7 +31,7 @@
 #define XYZ            3
 #define XY             2
 
-#define _AXIS(A)       (A##_AXIS)
+#define _AXIS(A)       (Axis::A())
 
 #define _XMIN_         100
 #define _YMIN_         200
diff --git a/src/marlin/core/serial.h b/src/marlin/core/serial.h
index b082c6da4e..d6c793269f 100644
--- a/src/marlin/core/serial.h
+++ b/src/marlin/core/serial.h
@@ -21,7 +21,7 @@
  */
 #pragma once
 
-#include <Eigen/Core>
+#include <swordfish/math.h>
 
 #include "../inc/MarlinConfig.h"
 
@@ -908,8 +908,12 @@ inline void print_xyz(const xyz_pos_t& xyz, PGM_P const prefix = nullptr, PGM_P
 	print_xyz(xyz.x, xyz.y, xyz.z, prefix, suffix);
 }
 
-inline void print_xyz(const Eigen::Vector3f& xyz, PGM_P const prefix = nullptr, PGM_P const suffix = nullptr) {
-	print_xyz(xyz(0), xyz(1), xyz(2), prefix, suffix);
+inline void print_xyz(const swordfish::math::Vector3f32& xyz, PGM_P const prefix = nullptr, PGM_P const suffix = nullptr) {
+	print_xyz(xyz.x(), xyz.y(), xyz.z(), prefix, suffix);
+}
+
+inline void print_xyz(const swordfish::math::Vector6f32& xyz, PGM_P const prefix = nullptr, PGM_P const suffix = nullptr) {
+	print_xyz(xyz.x(), xyz.y(), xyz.z(), prefix, suffix);
 }
 
 #define SERIAL_POS(SUFFIX, VAR) \
diff --git a/src/marlin/core/types.h b/src/marlin/core/types.h
index 8237b065c8..8b34fcdb4a 100644
--- a/src/marlin/core/types.h
+++ b/src/marlin/core/types.h
@@ -38,7 +38,7 @@ typedef const __FlashStringHelper* progmem_str;
 //  - A_AXIS, B_AXIS, and C_AXIS should be used for Steppers, corresponding to XYZ on Cartesians
 //  - X_HEAD, Y_HEAD, and Z_HEAD should be used for Steppers on Core kinematics
 //
-enum AxisEnum : uint8_t {
+/*enum AxisEnum : uint8_t {
 	X_AXIS = 0,
 	A_AXIS = 0,
 	Y_AXIS = 1,
@@ -59,17 +59,7 @@ enum AxisEnum : uint8_t {
 	E7_AXIS,
 	ALL_AXES = 0xFE,
 	NO_AXIS = 0xFF
-};
-
-//
-// Loop over XYZE axes
-//
-#define LOOP_XYZ(VAR)    LOOP_S_LE_N(VAR, X_AXIS, Z_AXIS)
-#define LOOP_XYZE(VAR)   LOOP_S_LE_N(VAR, X_AXIS, E_AXIS)
-#define LOOP_XYZE_N(VAR) LOOP_S_L_N(VAR, X_AXIS, XYZE_N)
-#define LOOP_ABC(VAR)    LOOP_S_LE_N(VAR, A_AXIS, C_AXIS)
-#define LOOP_ABCE(VAR)   LOOP_S_LE_N(VAR, A_AXIS, E_AXIS)
-#define LOOP_ABCE_N(VAR) LOOP_S_L_N(VAR, A_AXIS, XYZE_N)
+};*/
 
 //
 // Conditional type assignment magic. For example...
diff --git a/src/marlin/gcode/calibrate/G28.cpp b/src/marlin/gcode/calibrate/G28.cpp
index 8161bc8625..8552c6b026 100644
--- a/src/marlin/gcode/calibrate/G28.cpp
+++ b/src/marlin/gcode/calibrate/G28.cpp
@@ -27,8 +27,6 @@
 #include "../../module/stepper.h"
 #include "../../module/endstops.h"
 
-#include "../../module/probe.h"
-
 #define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
 #include "../../core/debug_out.h"
 
@@ -39,128 +37,6 @@ using namespace swordfish::tools;
 using namespace swordfish::motion;
 using namespace swordfish::status;
 
-#if ENABLED(QUICK_HOME)
-
-static void quick_home_xy() {
-
-	// Pretend the current position is 0,0
-	current_position.set(0.0, 0.0);
-	sync_plan_position();
-
-	const int x_axis_home_dir = x_home_dir(active_extruder);
-
-	const float mlx = max_length(X_AXIS),
-							mly = max_length(Y_AXIS),
-							mlratio = mlx > mly ? mly / mlx : mlx / mly,
-							fr_mm_s = _MIN(homing_feedrate(X_AXIS), homing_feedrate(Y_AXIS)) * SQRT(sq(mlratio) + 1.0);
-
-#	if ENABLED(SENSORLESS_HOMING)
-	sensorless_t stealth_states {
-		tmc_enable_stallguard(stepperX), tmc_enable_stallguard(stepperY), false, false
-#		if AXIS_HAS_STALLGUARD(X2)
-																																								 || tmc_enable_stallguard(stepperX2)
-#		endif
-																																								 ,
-				false
-#		if AXIS_HAS_STALLGUARD(Y2)
-						|| tmc_enable_stallguard(stepperY2)
-#		endif
-	};
-#	endif
-
-	do_blocking_move_to_xy(1.5 * mlx * x_axis_home_dir, 1.5 * mly * home_dir(Y_AXIS), fr_mm_s);
-
-	endstops.validate_homing_move();
-
-	current_position.set(0.0, 0.0);
-
-#	if ENABLED(SENSORLESS_HOMING)
-	tmc_disable_stallguard(stepperX, stealth_states.x);
-	tmc_disable_stallguard(stepperY, stealth_states.y);
-#		if AXIS_HAS_STALLGUARD(X2)
-	tmc_disable_stallguard(stepperX2, stealth_states.x2);
-#		endif
-#		if AXIS_HAS_STALLGUARD(Y2)
-	tmc_disable_stallguard(stepperY2, stealth_states.y2);
-#		endif
-#	endif
-}
-
-#endif // QUICK_HOME
-
-#if ENABLED(Z_SAFE_HOMING)
-
-inline void home_z_safely() {
-	DEBUG_SECTION(log_G28, "home_z_safely", DEBUGGING(LEVELING));
-
-	// Disallow Z homing if X or Y homing is needed
-	if (homing_needed_error(_BV(X_AXIS) | _BV(Y_AXIS))) {
-		return;
-	}
-
-	sync_plan_position();
-
-	/**
-	 * Move the Z probe (or just the nozzle) to the safe homing point
-	 * (Z is already at the right height)
-	 */
-	constexpr xy_float_t safe_homing_xy = { Z_SAFE_HOMING_X_POINT, Z_SAFE_HOMING_Y_POINT };
-#	if HAS_HOME_OFFSET
-	xy_float_t okay_homing_xy = safe_homing_xy;
-	okay_homing_xy -= home_offset;
-#	else
-	constexpr xy_float_t okay_homing_xy = safe_homing_xy;
-#	endif
-
-	destination.set(okay_homing_xy, current_position.z);
-
-	TERN_(HOMING_Z_WITH_PROBE, destination -= probe.offset_xy);
-
-	if (position_is_reachable(destination)) {
-
-		if (DEBUGGING(LEVELING))
-			DEBUG_POS("home_z_safely", destination);
-
-		// Free the active extruder for movement
-		TERN_(DUAL_X_CARRIAGE, idex_set_parked(false));
-
-		TERN_(SENSORLESS_HOMING, safe_delay(500)); // Short delay needed to settle
-
-		do_blocking_move_to_xy(destination);
-		homeaxis(Z_AXIS);
-	} else {
-		LCD_MESSAGEPGM(MSG_ZPROBE_OUT);
-		SERIAL_ECHO_MSG(STR_ZPROBE_OUT_SER);
-	}
-}
-
-#endif // Z_SAFE_HOMING
-
-#if ENABLED(IMPROVE_HOMING_RELIABILITY)
-
-slow_homing_t begin_slow_homing() {
-	slow_homing_t slow_homing { 0 };
-	slow_homing.acceleration.set(planner.settings.max_acceleration_mm_per_s2[X_AXIS],
-	                             planner.settings.max_acceleration_mm_per_s2[Y_AXIS]);
-	planner.settings.max_acceleration_mm_per_s2[X_AXIS] = 100;
-	planner.settings.max_acceleration_mm_per_s2[Y_AXIS] = 100;
-#	if HAS_CLASSIC_JERK
-	slow_homing.jerk_xy = planner.max_jerk;
-	planner.max_jerk.set(0, 0);
-#	endif
-	planner.reset_acceleration_rates();
-	return slow_homing;
-}
-
-void end_slow_homing(const slow_homing_t& slow_homing) {
-	planner.settings.max_acceleration_mm_per_s2[X_AXIS] = slow_homing.acceleration.x;
-	planner.settings.max_acceleration_mm_per_s2[Y_AXIS] = slow_homing.acceleration.y;
-	TERN_(HAS_CLASSIC_JERK, planner.max_jerk = slow_homing.jerk_xy);
-	planner.reset_acceleration_rates();
-}
-
-#endif // IMPROVE_HOMING_RELIABILITY
-
 /**
  * G28: Home all axes according to settings
  *
@@ -179,38 +55,23 @@ void end_slow_homing(const slow_homing_t& slow_homing) {
  *  Y   Home to the Y endstop
  *  Z   Home to the Z endstop
  */
-void GcodeSuite::G28() {
+void GcodeSuite::G28(const int8_t subcode) {
 	auto& toolsModule = ToolsModule::getInstance();
 	auto& motionModule = MotionModule::getInstance();
 
-	DEBUG_SECTION(log_G28, "G28", DEBUGGING(LEVELING));
-	if (DEBUGGING(LEVELING))
-		log_machine_info();
+	auto referencing = subcode == 2;
+	auto homing = subcode == 0;
 
-	TERN_(LASER_MOVE_G28_OFF, cutter.set_inline_enabled(false)); // turn off laser
-
-	bool homing = axes_should_home() || parser.seen('A');
-
-	// Home (O)nly if position is unknown
-	if (!homing && parser.boolval('O')) {
-		if (DEBUGGING(LEVELING))
-			DEBUG_ECHOLNPGM("> homing not needed, skip");
-		return;
+	if (homing && axes_should_home()) {
+		throw CommandException("One or more axes need to be recovered.");
 	}
 
 	planner.synchronize(); // Wait for planner moves to finish!
 
 	TemporaryState temp(MachineState::Homing);
 
-// Disable the leveling matrix before homing
-#if HAS_LEVELING
-	const bool leveling_restore_state = parser.boolval('L', TERN(RESTORE_LEVELING_AFTER_G28, planner.leveling_active, ENABLED(ENABLE_LEVELING_AFTER_G28)));
-	IF_ENABLED(PROBE_MANUALLY, g29_in_progress = false); // Cancel the active G29 session
-	set_bed_leveling_enabled(false);
-#endif
-
 	// Reset to the XY plane
-	TERN_(CNC_WORKSPACE_PLANES, workspace_plane = PLANE_XY);
+	workspace_plane = PLANE_XY;
 
 	// Count this command as movement / activity
 	reset_stepper_timeout();
@@ -221,123 +82,52 @@ void GcodeSuite::G28() {
 
 	remember_feedrate_scaling_off();
 
-	const bool homeZ = parser.seen('Z'),
-						 needX = homeZ && TERN0(Z_SAFE_HOMING, axes_should_home(_BV(X_AXIS))),
-						 needY = homeZ && TERN0(Z_SAFE_HOMING, axes_should_home(_BV(Y_AXIS))),
-						 homeX = needX || parser.seen('X'), homeY = needY || parser.seen('Y'),
-						 home_all = homeX == homeY && homeX == homeZ, // All or None
-			doX = home_all || homeX, doY = home_all || homeY, doZ = home_all || homeZ;
 
-#if ENABLED(HOME_Z_FIRST)
+	const bool doX = parser.seen('X');
+	const bool doY = parser.seen('Y');
+	const bool forceZ = parser.seen('Z');
+	const bool doA = parser.seen('A');
+
+	const bool doZ = doX || doY || doA || forceZ;
 
 	if (doZ) {
-		if (homing) {
-			homeaxis(Z_AXIS);
+		if (referencing && (!axis_is_trusted(Axis::Z()) || forceZ)) {
+			reference_linear_axis(Axis::Z());
 		} else {
-			do_blocking_move_to_z(MachineState::Homing, home_dir(Z_AXIS) > 0 ? Z_MAX_POS : Z_MIN_POS);
+			do_blocking_move_to_z(MachineState::Homing, home_dir(Axis::Z()) > 0 ? Z_MAX_POS : Z_MIN_POS);
 		}
 	}
 
-#endif
-
-	const float z_homing_height = TERN1(UNKNOWN_Z_NO_RAISE, axis_is_trusted(Z_AXIS))
-	                                  ? (parser.seenval('R') ? parser.value_linear_units() : Z_HOMING_HEIGHT)
-	                                  : 0;
-
-	if (z_homing_height && (doX || doY || TERN0(Z_SAFE_HOMING, doZ))) {
-		// Raise Z before homing any other axes and z is not already high enough (never lower z)
-		if (DEBUGGING(LEVELING))
-			DEBUG_ECHOLNPAIR("Raise Z (before homing) by ", z_homing_height);
-		do_z_clearance(MachineState::Homing, z_homing_height, axis_is_trusted(Z_AXIS), DISABLED(UNKNOWN_Z_NO_RAISE));
-	}
-
-#if ENABLED(QUICK_HOME)
-
-	if (doX && doY) {
-		if (homing) {
-			quick_home_xy();
+	if (doA && motionModule.shouldHomeFourth()) {
+		if (referencing) {
+			reference_rotary_axis(Axis::A());
 		} else {
-			do_blocking_move_to_xy(
-					home_dir(X_AXIS) > 0 ? X_MAX_POS : X_MIN_POS,
-					home_dir(Y_AXIS) > 0 ? Y_MAX_POS : Y_MIN_POS);
+			debug()("home A");
+
+			do_blocking_move_to_a(MachineState::Homing, 0);
 		}
 	}
 
-#endif
-
-	// Home Y (before X)
-	if (ENABLED(HOME_Y_BEFORE_X) && (doY || TERN0(CODEPENDENT_XY_HOMING, doX)))
-		homeaxis(Y_AXIS);
-
-	// Home X
-	if (doX || (doY && ENABLED(CODEPENDENT_XY_HOMING) && DISABLED(HOME_Y_BEFORE_X))) {
-
-#if ENABLED(DUAL_X_CARRIAGE)
-
-		// Always home the 2nd (right) extruder first
-		active_extruder = 1;
-		homeaxis(X_AXIS);
-
-		// Remember this extruder's position for later tool change
-		inactive_extruder_x = current_position.x;
-
-		// Home the 1st (left) extruder
-		active_extruder = 0;
-		homeaxis(X_AXIS);
-
-		// Consider the active extruder to be in its "parked" position
-		idex_set_parked();
-
-#else
-
-		if (homing) {
-			homeaxis(X_AXIS);
+	if (doY) {
+		if (referencing) {
+			reference_linear_axis(Axis::Y());
 		} else {
-			do_blocking_move_to_x(MachineState::Homing, home_dir(X_AXIS) > 0 ? X_MAX_POS : X_MIN_POS);
+			do_blocking_move_to_y(MachineState::Homing, home_dir(Axis::Y()) > 0 ? Y_MAX_POS : Y_MIN_POS);
 		}
-
-#endif
 	}
 
-	// Home Y (after X)
-	if (DISABLED(HOME_Y_BEFORE_X) && doY) {
-		if (homing) {
-			homeaxis(Y_AXIS);
+	if (doX) {
+		if (referencing) {
+			reference_linear_axis(Axis::X());
 		} else {
-			do_blocking_move_to_y(MachineState::Homing, home_dir(Y_AXIS) > 0 ? Y_MAX_POS : Y_MIN_POS);
+			do_blocking_move_to_x(MachineState::Homing, home_dir(Axis::X()) > 0 ? X_MAX_POS : X_MIN_POS);
 		}
 	}
 
-	TERN_(IMPROVE_HOMING_RELIABILITY, end_slow_homing(slow_homing));
-
-// Home Z last if homing towards the bed
-#if DISABLED(HOME_Z_FIRST)
-	if (doZ) {
-#	if EITHER(Z_MULTI_ENDSTOPS, Z_STEPPER_AUTO_ALIGN)
-		stepper.set_all_z_lock(false);
-		stepper.set_separate_multi_axis(false);
-#	endif
-
-		TERN_(BLTOUCH, bltouch.init());
-		TERN(Z_SAFE_HOMING, home_z_safely(), homeaxis(Z_AXIS));
-		probe.move_z_after_homing();
-	}
-#endif
-
 	sync_plan_position();
 
 	endstops.not_homing();
 
-	// Clear endstop state for polled stallGuard endstops
-	TERN_(SPI_ENDSTOPS, endstops.clear_endstop_state());
-
-#if BOTH(DELTA, DELTA_HOME_TO_SAFE_ZONE)
-	// move to a height where we can use the full xy-area
-	do_blocking_move_to_z(delta_clip_start_height);
-#endif
-
-	TERN_(HAS_LEVELING, set_bed_leveling_enabled(leveling_restore_state));
-
 	restore_feedrate_and_scaling();
 
 	if (toolsModule.isAutomatic()) {
diff --git a/src/marlin/gcode/config/M201-M205.cpp b/src/marlin/gcode/config/M201-M205.cpp
index 4b044cd8b4..a2124458bf 100644
--- a/src/marlin/gcode/config/M201-M205.cpp
+++ b/src/marlin/gcode/config/M201-M205.cpp
@@ -42,10 +42,9 @@ void GcodeSuite::M201() {
 		planner.xy_freq_min_speed_factor = constrain(parser.value_float(), 1, 100) / 100;
 #endif
 
-	LOOP_XYZE(i) {
-		if (parser.seen(axis_codes[i])) {
-			const uint8_t a = (i == E_AXIS ? uint8_t(E_AXIS_N(target_extruder)) : i);
-			planner.set_max_acceleration(a, parser.value_axis_units((AxisEnum) a));
+	for (auto axis : all_axes) {
+		if (parser.seen(axis.to_char())) {
+			planner.set_max_acceleration(axis, parser.value_axis_units(axis));
 		}
 	}
 }
@@ -56,15 +55,10 @@ void GcodeSuite::M201() {
  *       With multiple extruders use T to specify which one.
  */
 void GcodeSuite::M203() {
-
-	const int8_t target_extruder = get_target_extruder_from_command();
-	if (target_extruder < 0)
-		return;
-
-	LOOP_XYZE(i)
-	if (parser.seen(axis_codes[i])) {
-		const uint8_t a = (i == E_AXIS ? uint8_t(E_AXIS_N(target_extruder)) : i);
-		planner.set_max_feedrate(a, MMM_TO_MMS(parser.value_axis_units((AxisEnum) a)));
+	for (auto axis : all_axes) {
+		if (parser.seen(axis.to_char())) {
+			planner.set_max_feedrate(axis, MMM_TO_MMS(parser.value_axis_units(axis)));
+		}
 	}
 }
 
@@ -118,9 +112,9 @@ void GcodeSuite::M205() {
 	if (parser.seen('B'))
 		planner.settings.min_segment_time_us = parser.value_ulong();
 	if (parser.seen('S'))
-		planner.settings.min_feedrate_mm_s = parser.value_linear_units();
+		planner.settings.min_feedrate_unit_per_s = parser.value_linear_units();
 	if (parser.seen('T'))
-		planner.settings.min_travel_feedrate_mm_s = parser.value_linear_units();
+		planner.settings.min_travel_feedrate_unit_per_s = parser.value_linear_units();
 #if HAS_JUNCTION_DEVIATION
 	if (parser.seen('J')) {
 		const float junc_dev = parser.value_linear_units();
diff --git a/src/marlin/gcode/config/M92.cpp b/src/marlin/gcode/config/M92.cpp
index 0a7d52b633..3f71ef03d5 100644
--- a/src/marlin/gcode/config/M92.cpp
+++ b/src/marlin/gcode/config/M92.cpp
@@ -23,26 +23,14 @@
 #include "../gcode.h"
 #include "../../module/planner.h"
 
-void report_M92(const bool echo=true, const int8_t e=-1) {
+void report_M92(const bool echo=true) {
   if (echo) SERIAL_ECHO_START(); else SERIAL_CHAR(' ');
-  SERIAL_ECHOPAIR_P(PSTR(" M92 X"), LINEAR_UNIT(planner.settings.axis_steps_per_mm[X_AXIS]),
-                          SP_Y_STR, LINEAR_UNIT(planner.settings.axis_steps_per_mm[Y_AXIS]),
-                          SP_Z_STR, LINEAR_UNIT(planner.settings.axis_steps_per_mm[Z_AXIS]));
-  #if DISABLED(DISTINCT_E_FACTORS)
-    SERIAL_ECHOPAIR_P(SP_E_STR, VOLUMETRIC_UNIT(planner.settings.axis_steps_per_mm[E_AXIS]));
-  #endif
-  SERIAL_EOL();
+  SERIAL_ECHOPAIR_P(PSTR(" M92 X"), parser.linear_value_to_mm(planner.settings.axis_steps_per_unit[Axis::X()]));
+	SERIAL_ECHOPAIR_P(SP_Y_STR, parser.linear_value_to_mm(planner.settings.axis_steps_per_unit[Axis::Y()]));
+	SERIAL_ECHOPAIR_P(SP_Z_STR, parser.linear_value_to_mm(planner.settings.axis_steps_per_unit[Axis::Z()]));
+	SERIAL_ECHOPAIR_P(SP_A_STR, parser.radial_value_to_degrees(planner.settings.axis_steps_per_unit[Axis::A()]));
 
-  #if ENABLED(DISTINCT_E_FACTORS)
-    LOOP_L_N(i, E_STEPPERS) {
-      if (e >= 0 && i != e) continue;
-      if (echo) SERIAL_ECHO_START(); else SERIAL_CHAR(' ');
-      SERIAL_ECHOLNPAIR_P(PSTR(" M92 T"), (int)i,
-                        SP_E_STR, VOLUMETRIC_UNIT(planner.settings.axis_steps_per_mm[E_AXIS_N(i)]));
-    }
-  #endif
-
-  UNUSED_E(e);
+	SERIAL_EOL();
 }
 
 /**
@@ -59,56 +47,16 @@ void report_M92(const bool echo=true, const int8_t e=-1) {
  *      'L' specifies a desired layer height. Nearest good heights are shown.
  */
 void GcodeSuite::M92() {
-
-  const int8_t target_extruder = get_target_extruder_from_command();
-  if (target_extruder < 0) return;
-
   // No arguments? Show M92 report.
-  if (!parser.seen("XYZE"
-    #if ENABLED(MAGIC_NUMBERS_GCODE)
-      "HL"
-    #endif
-  )) return report_M92(true, target_extruder);
+  if (!parser.seen("XYZABC")) {
+		return report_M92(true);
+	}
 
-  LOOP_XYZE(i) {
-    if (parser.seenval(axis_codes[i])) {
-      if (i == E_AXIS) {
-        const float value = parser.value_per_axis_units((AxisEnum)(E_AXIS_N(target_extruder)));
-        if (value < 20) {
-          float factor = planner.settings.axis_steps_per_mm[E_AXIS_N(target_extruder)] / value; // increase e constants if M92 E14 is given for netfab.
-          #if HAS_CLASSIC_JERK && HAS_CLASSIC_E_JERK
-            planner.max_jerk.e *= factor;
-          #endif
-          planner.settings.max_feedrate_mm_s[E_AXIS_N(target_extruder)] *= factor;
-          planner.max_acceleration_steps_per_s2[E_AXIS_N(target_extruder)] *= factor;
-        }
-        planner.settings.axis_steps_per_mm[E_AXIS_N(target_extruder)] = value;
-      }
-      else {
-        planner.settings.axis_steps_per_mm[i] = parser.value_per_axis_units((AxisEnum)i);
-      }
+  for (auto axis : all_axes) {
+    if (parser.seenval(axis.to_char())) {
+      planner.settings.axis_steps_per_unit[axis] = parser.value_per_axis_units(axis);
     }
   }
-  planner.refresh_positioning();
 
-  #if ENABLED(MAGIC_NUMBERS_GCODE)
-    #ifndef Z_MICROSTEPS
-      #define Z_MICROSTEPS 16
-    #endif
-    const float wanted = parser.floatval('L');
-    if (parser.seen('H') || wanted) {
-      const uint16_t argH = parser.ushortval('H'),
-                     micro_steps = argH ?: Z_MICROSTEPS;
-      const float z_full_step_mm = micro_steps * planner.steps_to_mm[Z_AXIS];
-      SERIAL_ECHO_START();
-      SERIAL_ECHOPAIR("{ micro_steps:", micro_steps, ", z_full_step_mm:", z_full_step_mm);
-      if (wanted) {
-        const float best = uint16_t(wanted / z_full_step_mm) * z_full_step_mm;
-        SERIAL_ECHOPAIR(", best:[", best);
-        if (best != wanted) { SERIAL_CHAR(','); SERIAL_DECIMAL(best + z_full_step_mm); }
-        SERIAL_CHAR(']');
-      }
-      SERIAL_ECHOLNPGM(" }");
-    }
-  #endif
+  planner.refresh_positioning();
 }
diff --git a/src/marlin/gcode/control/G10_G11.cpp b/src/marlin/gcode/control/G10_G11.cpp
index 0fb3c4916e..3871fbad1d 100644
--- a/src/marlin/gcode/control/G10_G11.cpp
+++ b/src/marlin/gcode/control/G10_G11.cpp
@@ -144,13 +144,13 @@ void GcodeSuite::G10() {
 
 			xyz_pos_t offset { 0, 0, 0 };
 
-			LOOP_XYZ(i) {
-				if (parser.seenval(XYZ_CHAR(i))) {
+			for (auto axis : linear_axes) {
+				if (parser.seenval(axis.to_char())) {
 					const float v = parser.value_linear_units();
 
-					offset[i] = axis_is_relative((AxisEnum) i) ? toLogical(current_position[i], (AxisEnum) i) + v : v;
+					offset[axis] = axis_is_relative(axis) ? toLogical(current_position[axis], axis) + v : v;
 				} else {
-					offset[i] = toLogical(current_position[i], (AxisEnum) i);
+					offset[axis] = toLogical(current_position[axis], axis);
 				}
 			}
 
@@ -666,17 +666,17 @@ void GcodeSuite::set_work_offsets(const uint8_t p) {
 
 	if (parser.seen('X')) {
 		// offset.x(toNative(parser.value_axis_units(X_AXIS), X_AXIS) - current_position[0]);
-		offset.x(parser.value_axis_units(X_AXIS) - homeOffset[X_AXIS] - toolOffset[X_AXIS] - current_position[X_AXIS]);
+		offset.x(parser.value_axis_units(Axis::X()) - homeOffset[Axis::X()] - toolOffset[Axis::X()] - current_position[Axis::X()]);
 	}
 
 	if (parser.seen('Y')) {
 		// offset.y(toNative(parser.value_axis_units(Y_AXIS), Y_AXIS) - current_position[1]);
-		offset.y(parser.value_axis_units(Y_AXIS) - homeOffset[Y_AXIS] - toolOffset[Y_AXIS] - current_position[Y_AXIS]);
+		offset.y(parser.value_axis_units(Axis::Y()) - homeOffset[Axis::Y()] - toolOffset[Axis::Y()] - current_position[Axis::Y()]);
 	}
 
 	if (parser.seen('Z')) {
 		// offset.z(toNative(parser.value_axis_units(Z_AXIS), Z_AXIS) - current_position[2]);
-		offset.z(parser.value_axis_units(Z_AXIS) - homeOffset[Z_AXIS] - toolOffset[Z_AXIS] - current_position[Z_AXIS]);
+		offset.z(parser.value_axis_units(Axis::Z()) - homeOffset[Axis::Z()] - toolOffset[Axis::Z()] - current_position[Axis::Z()]);
 		// offset.z(current_position[Z_AXIS] - parser.value_axis_units(Z_AXIS) - homeOffset[Z_AXIS] - toolOffset[Z_AXIS]);
 	}
 
diff --git a/src/marlin/gcode/control/M17_M18_M84.cpp b/src/marlin/gcode/control/M17_M18_M84.cpp
index 78df55ab2c..9de8d40570 100644
--- a/src/marlin/gcode/control/M17_M18_M84.cpp
+++ b/src/marlin/gcode/control/M17_M18_M84.cpp
@@ -36,7 +36,9 @@ void GcodeSuite::M17() {
     if (parser.seen('X')) ENABLE_AXIS_X();
     if (parser.seen('Y')) ENABLE_AXIS_Y();
     if (parser.seen('Z')) ENABLE_AXIS_Z();
-    if (TERN0(HAS_E_STEPPER_ENABLE, parser.seen('E'))) enable_e_steppers();
+		if (parser.seen('A')) ENABLE_AXIS_A();
+		if (parser.seen('B')) ENABLE_AXIS_B();
+		if (parser.seen('C')) ENABLE_AXIS_C();
   }
   else {
     //LCD_MESSAGEPGM(MSG_NO_MOVE);
@@ -58,7 +60,9 @@ void GcodeSuite::M18_M84() {
       if (parser.seen('X')) DISABLE_AXIS_X();
       if (parser.seen('Y')) DISABLE_AXIS_Y();
       if (parser.seen('Z')) DISABLE_AXIS_Z();
-      if (TERN0(HAS_E_STEPPER_ENABLE, parser.seen('E'))) disable_e_steppers();
+			if (parser.seen('A')) DISABLE_AXIS_A();
+			if (parser.seen('B')) DISABLE_AXIS_B();
+			if (parser.seen('C')) DISABLE_AXIS_C();
     }
     else
       planner.finish_and_disable();
diff --git a/src/marlin/gcode/control/M6.cpp b/src/marlin/gcode/control/M6.cpp
index cffcd38f80..64f9a5961c 100644
--- a/src/marlin/gcode/control/M6.cpp
+++ b/src/marlin/gcode/control/M6.cpp
@@ -36,9 +36,9 @@
 using namespace swordfish::tools;
 
 void GcodeSuite::M6() {
-	if(homing_needed_error(_BV(X_AXIS)|_BV(Y_AXIS)|_BV(Z_AXIS))) {
+	if(homing_needed_error(_BV(Axis::X())|_BV(Axis::Y())|_BV(Axis::Z()))) {
 		return;
 	}
-	
+
 	ToolsModule::getInstance().change();
 }
\ No newline at end of file
diff --git a/src/marlin/gcode/gcode.cpp b/src/marlin/gcode/gcode.cpp
index a6b0bba2cf..caf10c5789 100644
--- a/src/marlin/gcode/gcode.cpp
+++ b/src/marlin/gcode/gcode.cpp
@@ -25,9 +25,9 @@
  *             Most will migrate to classes, by feature.
  */
 
-#include <Eigen/Core>
+#include <swordfish/math.h>
 
-using namespace Eigen;
+using namespace swordfish::math;
 
 #include "gcode.h"
 GcodeSuite gcode;
@@ -160,10 +160,10 @@ int8_t GcodeSuite::get_target_e_stepper_from_command() {
  *  - Set to current for missing axis codes
  *  - Set the feedrate, if included
  */
-void GcodeSuite::get_destination_from_command() {
+void GcodeSuite::get_destination_from_command(bool rapid_move) {
 	auto& motionModule = MotionModule::getInstance();
 
-	xyze_bool_t seen = { false, false, false, false };
+	Vector6u8 seen = { false, false, false, false, false, false };
 
 #if ENABLED(CANCEL_OBJECTS)
 	const bool& skip_move = cancelable.skipping;
@@ -171,24 +171,48 @@ void GcodeSuite::get_destination_from_command() {
 	constexpr bool skip_move = false;
 #endif
 
+	u8 linear_moves = 0;
+	u8 radial_moves = 0;
+
+	debug()("rapid_move: ", rapid_move);
+
+	// validate move
+	for (auto axis : all_axes) {
+		if (parser.seen(axis.to_char())) {
+			if (axis.is_linear()) {
+				linear_moves ++;
+			} else {
+				radial_moves ++;
+			}
+		}
+	}
+
+	debug()("linear_moves: ", linear_moves, ", radial_moves: ", radial_moves);
+
+	if (!rapid_move && linear_moves > 0 && radial_moves > 0 && parser.feedrate_type != FeedRateType::InverseTime) {
+		throw CommandException("Linear and radial moves can only be combined in G93 mode");
+	}
+
+	if (parser.feedrate_type == FeedRateType::InverseTime && !parser.seen('F')) {
+		throw CommandException("Expected F parameter while in G93 mode.");
+	}
+
 	// Get new XYZ position, whether absolute or relative
-	LOOP_XYZ(i) {
-		if ((seen[i] = parser.seenval(XYZ_CHAR(i)))) {
-			const float v = parser.value_axis_units((AxisEnum) i);
+	for (auto axis : all_axes) {
+
+		if ((seen[axis] = parser.seenval(axis.to_char()))) {
+			const float v = parser.value_axis_units(axis);
+
+			debug()("axis: ", (usize)axis, " (", axis.to_char(), "), ", v);
+
 			if (skip_move)
-				destination[i] = current_position[i];
+				destination[axis] = current_position[axis];
 			else
-				destination[i] = axis_is_relative(AxisEnum(i)) ? current_position[i] + v : motionModule.toNative((AxisEnum) i, v);
+				destination[axis] = axis_is_relative(axis) ? current_position[axis] + v : motionModule.toNative(axis, v);
 		} else
-			destination[i] = current_position[i];
+			destination[axis] = current_position[axis];
 	}
 
-#if ENABLED(POWER_LOSS_RECOVERY) && !PIN_EXISTS(POWER_LOSS)
-	// Only update power loss recovery on moves with E
-	if (recovery.enabled && IS_SD_PRINTING() && seen.e && (seen.x || seen.y))
-		recovery.save();
-#endif
-
 	if (parser.linearval('F') > 0)
 		feedrate_mm_s = parser.value_feedrate();
 
@@ -414,7 +438,7 @@ void GcodeSuite::process_parsed_command(const bool no_ok /*=false*/) {
 #endif
 
 					case 28:
-						G28();
+						G28(parser.subcode);
 						break; // G28: Home one or more axes
 
 #if HAS_LEVELING
@@ -557,9 +581,22 @@ void GcodeSuite::process_parsed_command(const bool no_ok /*=false*/) {
 						break;
 					}
 
-					case 92:
+					case 92: {
 						G92();
 						break; // G92: Set current axis position(s)
+					}
+
+					case 93: {
+						G93(); // set feed rate mode to inverse time
+
+						break;
+					}
+
+					case 94: {
+						G94(); // set feed rate mode to mm/s
+
+						break;
+					}
 
 #if HAS_MESH
 					case 42:
@@ -1113,8 +1150,26 @@ const char* GcodeSuite::get_state() {
 	}
 }
 
-inline void report_position_json(const xyz_pos_t& pos) {
-	SERIAL_PRINTF("{\"x\":%lf,\"y\":%lf,\"z\":%lf}", pos.x, pos.y, pos.z);
+inline void report_axis(Axis axis, const Vector6f32& pos) {
+	SERIAL_PRINTF("\"%c\":%lf", tolower(axis.to_char()), isnan(pos[axis]) ? 0.0 : pos[axis]);
+}
+
+inline void report_position_json(const Vector6f32& pos) {
+	bool first = true;
+
+	SERIAL_ECHO("{");
+
+	for (auto axis : all_axes) {
+		if (!first) {
+			SERIAL_ECHO(",");
+		}
+
+		first = false;
+
+		report_axis(axis, pos);
+	}
+
+	SERIAL_ECHO("}");
 }
 
 extern int16_t rapidrate_percentage;
@@ -1122,7 +1177,7 @@ extern int16_t rapidrate_percentage;
 void GcodeSuite::report_state() {
 	// get_cartesian_from_steppers();
 
-	xyz_pos_t pos = current_position;
+	auto pos = current_position;
 	// xyz_pos_t pos = cartes;
 
 	const char* state = get_state();
@@ -1135,7 +1190,7 @@ void GcodeSuite::report_state() {
 	SERIAL_ECHO("\",\"mpos\":");
 	report_position_json(pos);
 	SERIAL_ECHO(",\"wpos\":");
-	report_position_json(pos.asLogical());
+	report_position_json(toLogical(pos));
 	// SERIAL_ECHO(",\"spos\":");
 	// SERIAL_PRINTF("{\"x\":%lf,\"y\":%lf,\"z\":%lf}", Stepper::count_position.x, Stepper::count_position.y, Stepper::count_position.z);
 	SERIAL_PRINTF(",\"wcs\":%ld", motionManager.getActiveCoordinateSystem().getIndex() + 1);
diff --git a/src/marlin/gcode/gcode.h b/src/marlin/gcode/gcode.h
index d34b1f5504..812ccfe86c 100644
--- a/src/marlin/gcode/gcode.h
+++ b/src/marlin/gcode/gcode.h
@@ -334,14 +334,8 @@ class GcodeSuite {
 	static bool aborted_;
 	static uint8_t axis_relative;
 
-	static inline bool axis_is_relative(const AxisEnum a) {
-		if (a == E_AXIS) {
-			if (TEST(axis_relative, E_MODE_REL))
-				return true;
-			if (TEST(axis_relative, E_MODE_ABS))
-				return false;
-		}
-		return TEST(axis_relative, a);
+	static inline bool axis_is_relative(const Axis axis) {
+		return TEST(axis_relative, axis);
 	}
 
 	static inline uint8_t relative_mode() {
@@ -400,7 +394,7 @@ class GcodeSuite {
 
 	static int8_t get_target_extruder_from_command();
 	static int8_t get_target_e_stepper_from_command();
-	static void get_destination_from_command();
+	static void get_destination_from_command(bool rapid_move);
 
 	static void process_parsed_command(const bool no_ok = false);
 	static void process_next_command();
@@ -541,7 +535,7 @@ class GcodeSuite {
 
 	TERN_(NOZZLE_PARK_FEATURE, static void G27());
 
-	static void G28();
+	static void G28(const int8_t subcode);
 
 #if HAS_LEVELING
 #	if ENABLED(G29_RETRY_AND_RECOVER)
@@ -607,6 +601,8 @@ class GcodeSuite {
 	TERN_(GCODE_MOTION_MODES, static void G80());
 
 	static void G92(bool report = true);
+	static void G93();
+	static void G94();
 
 	TERN_(CALIBRATION_GCODE, static void G425());
 
diff --git a/src/marlin/gcode/geometry/CMakeLists.txt b/src/marlin/gcode/geometry/CMakeLists.txt
index f14f5dd9aa..a111b339f1 100644
--- a/src/marlin/gcode/geometry/CMakeLists.txt
+++ b/src/marlin/gcode/geometry/CMakeLists.txt
@@ -4,5 +4,7 @@ target_sources(${PROJECT_NAME}.elf
 		G43_G49.cpp
 		G53-G59.cpp
 		G92.cpp
+		G43_G49.cpp
+		G93-G94.cpp
 		M206_M428.cpp
 )
diff --git a/src/marlin/gcode/geometry/G43_G49.cpp b/src/marlin/gcode/geometry/G43_G49.cpp
index 0ecede16bd..edc178c516 100644
--- a/src/marlin/gcode/geometry/G43_G49.cpp
+++ b/src/marlin/gcode/geometry/G43_G49.cpp
@@ -5,8 +5,6 @@
  *  Author: smohekey
  */
 
-#include <Eigen/Core>
-
 #include "../gcode.h"
 #include "../../module/motion.h"
 
@@ -15,16 +13,16 @@
 #if HAS_TOOL_OFFSET
 
 using namespace swordfish;
+using namespace swordfish::math;
 using namespace swordfish::motion;
 using namespace swordfish::tools;
-using namespace Eigen;
 
 void GcodeSuite::G43() {
 	auto& toolManager = ToolsModule::getInstance();
 	auto& motionManager = MotionModule::getInstance();
 	auto& tools = toolManager.getTools();
 
-	Vector3f offset { 0, 0, 0 };
+	Vector3f32 offset { 0, 0, 0 };
 
 	if (parser.seenval('H')) {
 		int16_t h = parser.intval('H');
@@ -43,12 +41,11 @@ void GcodeSuite::G43() {
 			return;
 		}
 
-		offset(Z) = -tool->getGeometry().length();
+		offset.z() = -tool->getGeometry().length();
 	}
-
-	LOOP_XYZ(i) {
-		if (parser.seenval(XYZ_CHAR(i))) {
-			offset(i) += parser.value_axis_units((AxisEnum) i);
+	for (auto axis : linear_axes) {
+		if (parser.seenval(axis.to_char())) {
+			offset[axis] += parser.value_axis_units(axis);
 		}
 	}
 
diff --git a/src/marlin/gcode/geometry/G92.cpp b/src/marlin/gcode/geometry/G92.cpp
index cce7cc8290..9c68bece1a 100644
--- a/src/marlin/gcode/geometry/G92.cpp
+++ b/src/marlin/gcode/geometry/G92.cpp
@@ -36,6 +36,7 @@
 using namespace Eigen;
 
 using namespace swordfish;
+using namespace swordfish::math;
 using namespace swordfish::core;
 using namespace swordfish::motion;
 
@@ -74,26 +75,26 @@ void GcodeSuite::G92(bool report) {
       } break;
     #endif
     case 0: {
-			Vector3f workOffset { 0, 0, 0 };
-				
-      LOOP_XYZE(i) {
-        if (parser.seenval(axis_codes[i])) {
-          const float l = parser.value_axis_units((AxisEnum)i),
-                      v = i == E_AXIS ? l : toNative(l, (AxisEnum)i),
+			Vector3f32 workOffset { 0, 0, 0 };
+
+      for (auto i : linear_axes) {
+        if (parser.seenval(i.to_char())) {
+          const float l = parser.value_axis_units(i),
+                      v = toNative(l, i),
                       d = v - current_position[i];
           if (!NEAR_ZERO(d)) {
             #if IS_SCARA || !HAS_POSITION_SHIFT
               if (i == E_AXIS) sync_E = true; else sync_XYZ = true;
               current_position[i] = v;        // Without workspaces revert to Marlin 1.0 behavior
             #elif HAS_POSITION_SHIFT
-               workOffset(i) += d;       // Other axes simply offset the coordinate space
+               workOffset[i] += d;       // Other axes simply offset the coordinate space
             #endif
           }
         }
       }
-			
+
 			motionManager.setWorkOffset(workOffset);
-			
+
     } break;
   }
 
diff --git a/src/marlin/gcode/geometry/G93-G94.cpp b/src/marlin/gcode/geometry/G93-G94.cpp
new file mode 100644
index 0000000000..41b278b23b
--- /dev/null
+++ b/src/marlin/gcode/geometry/G93-G94.cpp
@@ -0,0 +1,29 @@
+#include <swordfish/modules/motion/FeedRate.h>
+
+using namespace swordfish::motion;
+
+#include "../gcode.h"
+
+void GcodeSuite::G93() {
+	auto old_feedrate_type = parser.feedrate_type;
+
+	parser.set_feed_rate_type(FeedRateType::InverseTime);
+
+	if (parser.chain()) {
+		process_parsed_command(true);
+
+		parser.set_feed_rate_type(old_feedrate_type);
+	}
+}
+
+void GcodeSuite::G94() {
+	auto old_feedrate_type = parser.feedrate_type;
+
+	parser.set_feed_rate_type(FeedRateType::UnitsPerSecond);
+
+	if (parser.chain()) {
+		process_parsed_command(true);
+
+		parser.set_feed_rate_type(old_feedrate_type);
+	}
+}
\ No newline at end of file
diff --git a/src/marlin/gcode/geometry/M206_M428.cpp b/src/marlin/gcode/geometry/M206_M428.cpp
index 3ca49dac16..03503f2f90 100644
--- a/src/marlin/gcode/geometry/M206_M428.cpp
+++ b/src/marlin/gcode/geometry/M206_M428.cpp
@@ -20,8 +20,6 @@
  *
  */
 
-#include <Eigen/Core>
-
 #include "../../inc/MarlinConfig.h"
 
 #if HAS_M206_COMMAND
@@ -33,9 +31,8 @@
 
 #	include <swordfish/Controller.h>
 
-using namespace Eigen;
-
 using namespace swordfish;
+using namespace swordfish::math;
 using namespace swordfish::motion;
 
 extern const char SP_Y_STR[], SP_Z_STR[];
@@ -43,7 +40,7 @@ extern const char SP_Y_STR[], SP_Z_STR[];
 void m206_report() {
 	auto homeOffset = MotionModule::getInstance().getHomeOffset();
 
-	SERIAL_ECHOLNPAIR_P(PSTR("M206 X"), homeOffset(X), SP_Y_STR, homeOffset(Y), SP_Z_STR, homeOffset(Z));
+	SERIAL_ECHOLNPAIR_P(PSTR("M206 X"), homeOffset.x(), SP_Y_STR, homeOffset.y(), SP_Z_STR, homeOffset.z());
 }
 
 /**
@@ -54,11 +51,11 @@ void m206_report() {
  * ***              In the 2.0 release, it will simply be disabled by default.
  */
 void GcodeSuite::M206() {
-	Vector3f homeOffset { 0, 0, 0 };
+	Vector3f32 homeOffset { 0, 0, 0 };
 
-	LOOP_XYZ(i) {
-		if (parser.seen(XYZ_CHAR(i)))
-			homeOffset(i) = parser.value_linear_units();
+	for (auto i : linear_axes) {
+		if (parser.seen(i.to_char()))
+			homeOffset[i] = parser.value_linear_units();
 	}
 
 	MotionModule::getInstance().setHomeOffset(homeOffset);
@@ -84,13 +81,14 @@ void GcodeSuite::M206() {
  *       Use M206 to set these values directly.
  */
 void GcodeSuite::M428() {
-	if (homing_needed_error())
+if (homing_needed_error())
 		return;
 
-	Vector3f diff;
-	LOOP_XYZ(i) {
-		diff[i] = base_home_pos((AxisEnum) i) - current_position[i];
-		if (!WITHIN(diff[i], -20, 20) && home_dir((AxisEnum) i) > 0)
+	Vector3f32 diff;
+
+	for (auto i : linear_axes) {
+		diff[i] = base_home_pos(i) - current_position[i];
+		if (!WITHIN(diff[i], -20, 20) && home_dir(i) > 0)
 			diff[i] = -current_position[i];
 		if (!WITHIN(diff[i], -20, 20)) {
 			SERIAL_ERROR_MSG(STR_ERR_M428_TOO_FAR);
diff --git a/src/marlin/gcode/host/M115.cpp b/src/marlin/gcode/host/M115.cpp
index 2337847aee..41fecfaff8 100644
--- a/src/marlin/gcode/host/M115.cpp
+++ b/src/marlin/gcode/host/M115.cpp
@@ -89,15 +89,15 @@ void GcodeSuite::M115() {
 			",\"area\":{"
 			"\"full\":{"
 			"\"min\":{\"x\":",
-			lmin(X), ",\"y\":", lmin(Y), ",\"z\":", lmin(Z), "},"
+			lmin.x(), ",\"y\":", lmin.y(), ",\"z\":", lmin.z(), "},"
 																											 "\"max\":{\"x\":",
-			lmax(X), ",\"y\":", lmax(Y), ",\"z\":", lmax(Z), "}"
+			lmax.x(), ",\"y\":", lmax.y(), ",\"z\":", lmax.z(), "}"
 																											 "},"
 																											 "\"work\":{"
 																											 "\"min\":{\"x\":",
-			wmin(X), ",\"y\":", wmin(Y), ",\"z\":", wmin(Z), "},"
+			wmin.x(), ",\"y\":", wmin.y(), ",\"z\":", wmin.z(), "},"
 																											 "\"max\":{\"x\":",
-			wmax(X), ",\"y\":", wmax(Y), ",\"z\":", wmax(Z), "}",
+			wmax.x(), ",\"y\":", wmax.y(), ",\"z\":", wmax.z(), "}",
 			"}"
 			"}"
 			"}");
diff --git a/src/marlin/gcode/motion/G0_G1.cpp b/src/marlin/gcode/motion/G0_G1.cpp
index 8a7a9cb685..1f6090f369 100644
--- a/src/marlin/gcode/motion/G0_G1.cpp
+++ b/src/marlin/gcode/motion/G0_G1.cpp
@@ -23,6 +23,7 @@
 #include <swordfish/Controller.h>
 
 using namespace swordfish;
+using namespace swordfish::motion;
 using namespace swordfish::status;
 
 #include "../gcode.h"
@@ -33,7 +34,7 @@ using namespace swordfish::status;
 #include <swordfish/Controller.h>
 
 #if ENABLED(VARIABLE_G0_FEEDRATE)
-  feedRate_t rapidrate_mm_s = MMM_TO_MMS(G0_FEEDRATE);
+  FeedRate rapidrate_mm_s = FeedRate::UnitsPerSecond(MMM_TO_MMS(G0_FEEDRATE));
 #endif
 
 #if HAS_FAST_MOVES
@@ -43,7 +44,7 @@ using namespace swordfish::status;
 /**
  * G0, G1: Coordinated movement of X Y Z E axes
  */
-void GcodeSuite::G0_G1(const bool fast_move/* = false*/) {
+void GcodeSuite::G0_G1(const bool rapid_move/* = false*/) {
 	if (IsRunning()
     #if ENABLED(NO_MOTION_BEFORE_HOMING)
       && !homing_needed_error(
@@ -53,34 +54,26 @@ void GcodeSuite::G0_G1(const bool fast_move/* = false*/) {
     #endif
   ) {
 
-    feedRate_t old_feedrate;
+    FeedRate old_feedrate = feedrate_mm_s;
 		MachineState machine_state = MachineState::FeedMove;
 
-		if (fast_move) {
-			machine_state = MachineState::RapidMove;
+    get_destination_from_command(rapid_move);                 // Get X Y Z E F (and set cutter power)
 
-			old_feedrate = feedrate_mm_s;             // Back up the (old) motion mode feedrate
-			feedrate_mm_s = rapidrate_mm_s * 0.01f * rapidrate_percentage;       // Get G0 feedrate from last usage
-		}
+    if (rapid_move) {
+			machine_state = MachineState::RapidMove;
 
-    get_destination_from_command();                 // Get X Y Z E F (and set cutter power)
+      rapidrate_mm_s = feedrate_mm_s;       // Save feedrate for the next G0
 
-    if (fast_move) {
-      #if ENABLED(VARIABLE_G0_FEEDRATE)
-        rapidrate_mm_s = feedrate_mm_s;       // Save feedrate for the next G0
-      #else
-        old_feedrate = feedrate_mm_s;             // Back up the (new) motion mode feedrate
-        feedrate_mm_s = MMM_TO_MMS(G0_FEEDRATE);  // Get the fixed G0 feedrate
-      #endif
+			feedrate_mm_s = rapidrate_mm_s * 0.01f * rapidrate_percentage;
     }
 
-		debug()("accel_mm_s2: ", fast_move ? planner.settings.travel_acceleration : planner.settings.acceleration);
-		debug()("feedrate_mm_s: ", feedrate_mm_s);
+		debug()("accel_mm_s2: ", rapid_move ? planner.settings.travel_acceleration : planner.settings.acceleration);
+		debug()("feed_rate: type=", (i32) feedrate_mm_s.type(), ", value=", feedrate_mm_s.value());
 
-    prepare_line_to_destination(machine_state, fast_move ? planner.settings.travel_acceleration : planner.settings.acceleration);
+    prepare_line_to_destination(machine_state, rapid_move ? planner.settings.travel_acceleration : planner.settings.acceleration);
 
 		// Restore the motion mode feedrate
-    if (fast_move) {
+    if (rapid_move) {
 			feedrate_mm_s = old_feedrate;
 		}
   }
diff --git a/src/marlin/gcode/motion/G2_G3.cpp b/src/marlin/gcode/motion/G2_G3.cpp
index 64753f0b2a..f23aa6e90f 100644
--- a/src/marlin/gcode/motion/G2_G3.cpp
+++ b/src/marlin/gcode/motion/G2_G3.cpp
@@ -27,6 +27,7 @@
 #	include <swordfish/Controller.h>
 
 using namespace swordfish;
+using namespace swordfish::math;
 using namespace swordfish::motion;
 using namespace swordfish::status;
 
@@ -55,43 +56,42 @@ using namespace swordfish::status;
  * boards to produce much smoother curved surfaces.
  */
 void plan_arc(
-		const xyze_pos_t& cart, // Destination position
-		const ab_float_t& offset, // Center of rotation relative to current_position
+		const Vector6f32& cart, // Destination position
+		const Vector2f32& offset, // Center of rotation relative to current_position
 		const bool clockwise, // Clockwise?
 		const uint8_t circles // Take the scenic route
 ) {
 	auto& limits = MotionModule::getInstance().getLimits();
 
-#	if ENABLED(CNC_WORKSPACE_PLANES)
-	AxisEnum p_axis, q_axis, l_axis;
+	Axis p_axis = Axis::X();
+	Axis q_axis = Axis::Y();
+	Axis l_axis = Axis::Z();
+
 	switch (gcode.workspace_plane) {
 		default:
 		case GcodeSuite::PLANE_XY:
-			p_axis = X_AXIS;
-			q_axis = Y_AXIS;
-			l_axis = Z_AXIS;
+			p_axis = Axis::X();
+			q_axis = Axis::Y();
+			l_axis = Axis::Z();
 			break;
 		case GcodeSuite::PLANE_YZ:
-			p_axis = Y_AXIS;
-			q_axis = Z_AXIS;
-			l_axis = X_AXIS;
+			p_axis = Axis::Y();
+			q_axis = Axis::Z();
+			l_axis = Axis::X();
 			break;
 		case GcodeSuite::PLANE_ZX:
-			p_axis = Z_AXIS;
-			q_axis = X_AXIS;
-			l_axis = Y_AXIS;
+			p_axis = Axis::Z();
+			q_axis = Axis::X();
+			l_axis = Axis::Y();
 			break;
 	}
-#	else
-	constexpr AxisEnum p_axis = X_AXIS, q_axis = Y_AXIS, l_axis = Z_AXIS;
-#	endif
 
 	// Radius vector from center to current location
-	ab_float_t rvec = -offset;
+	Vector2f32 rvec = -offset;
 
-	const float radius = HYPOT(rvec.a, rvec.b),
-							center_P = current_position[p_axis] - rvec.a,
-							center_Q = current_position[q_axis] - rvec.b,
+	const float radius = HYPOT(rvec.x(), rvec.y()),
+							center_P = current_position[p_axis] - rvec.x(),
+							center_Q = current_position[q_axis] - rvec.y(),
 							rt_X = cart[p_axis] - center_P,
 							rt_Y = cart[q_axis] - center_Q,
 							start_L = current_position[l_axis];
@@ -110,7 +110,7 @@ void plan_arc(
 		min_segments = MIN_CIRCLE_SEGMENTS;
 	} else {
 		// Calculate the angle
-		angular_travel = ATAN2(rvec.a * rt_Y - rvec.b * rt_X, rvec.a * rt_X + rvec.b * rt_Y);
+		angular_travel = ATAN2(rvec.x() * rt_Y - rvec.y() * rt_X, rvec.x() * rt_X + rvec.y() * rt_Y);
 
 		// Angular travel too small to detect? Just return.
 		if (!angular_travel)
@@ -132,23 +132,20 @@ void plan_arc(
 		min_segments = CEIL((MIN_CIRCLE_SEGMENTS) *portion_of_circle);
 	}
 
-	float linear_travel = cart[l_axis] - start_L,
-				extruder_travel = 0;
+	float linear_travel = cart[l_axis] - start_L;
 
 	// If circling around...
 	if (ENABLED(ARC_P_CIRCLES) && circles) {
 		const float total_angular = angular_travel + circles * RADIANS(360), // Total rotation with all circles and remainder
 				part_per_circle = RADIANS(360) / total_angular, // Each circle's part of the total
-				l_per_circle = linear_travel * part_per_circle, // L movement per circle
-				e_per_circle = extruder_travel * part_per_circle; // E movement per circle
-		xyze_pos_t temp_position = current_position; // for plan_arc to compare to current_position
+				l_per_circle = linear_travel * part_per_circle; // L movement per circle
+
+		Vector6f32 temp_position = current_position; // for plan_arc to compare to current_position
 		for (uint16_t n = circles; n--;) {
-			temp_position.e += e_per_circle; // Destination E axis
 			temp_position[l_axis] += l_per_circle; // Destination L axis
 			plan_arc(temp_position, offset, clockwise, 0); // Plan a single whole circle
 		}
 		linear_travel = cart[l_axis] - current_position[l_axis];
-		extruder_travel = 0;
 	}
 
 	const float flat_mm = radius * angular_travel,
@@ -156,14 +153,15 @@ void plan_arc(
 	if (mm_of_travel < 0.001f)
 		return;
 
-	const feedRate_t scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s);
+	//const FeedRate scaled_feed_rate = MMS_SCALED(feedrate_mm_s);
+	const FeedRate scaled_feed_rate = feedrate_mm_s;
 
 	// Start with a nominal segment length
 	float seg_length = (
 #	ifdef ARC_SEGMENTS_PER_R
 			constrain(MM_PER_ARC_SEGMENT * radius, MM_PER_ARC_SEGMENT, ARC_SEGMENTS_PER_R)
 #	elif ARC_SEGMENTS_PER_SEC
-			_MAX(scaled_fr_mm_s * RECIPROCAL(ARC_SEGMENTS_PER_SEC), MM_PER_ARC_SEGMENT)
+			_MAX(scaled_feed_rate * RECIPROCAL(ARC_SEGMENTS_PER_SEC), MM_PER_ARC_SEGMENT)
 #	else
 			MM_PER_ARC_SEGMENT
 #	endif
@@ -200,10 +198,10 @@ void plan_arc(
 	 * This is important when there are successive arc motions.
 	 */
 	// Vector rotation matrix values
-	xyze_pos_t raw;
+	Vector6f32 raw = { 0, 0, 0, 0, 0, 0 };
+
 	const float theta_per_segment = angular_travel / segments,
 							linear_per_segment = linear_travel / segments,
-							extruder_per_segment = extruder_travel / segments,
 							sq_theta_per_segment = sq(theta_per_segment),
 							sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6,
 							cos_T = 1 - 0.5f * sq_theta_per_segment; // Small angle approximation
@@ -232,9 +230,9 @@ void plan_arc(
 #	if N_ARC_CORRECTION > 1
 		if (--arc_recalc_count) {
 			// Apply vector rotation matrix to previous rvec.a / 1
-			const float r_new_Y = rvec.a * sin_T + rvec.b * cos_T;
-			rvec.a = rvec.a * cos_T - rvec.b * sin_T;
-			rvec.b = r_new_Y;
+			const float r_new_Y = rvec.x() * sin_T + rvec.y() * cos_T;
+			rvec.x() = rvec.x() * cos_T - rvec.y() * sin_T;
+			rvec.y() = r_new_Y;
 		} else
 #	endif
 		{
@@ -247,22 +245,16 @@ void plan_arc(
 			// To reduce stuttering, the sin and cos could be computed at different times.
 			// For now, compute both at the same time.
 			const float cos_Ti = cos(i * theta_per_segment), sin_Ti = sin(i * theta_per_segment);
-			rvec.a = -offset[0] * cos_Ti + offset[1] * sin_Ti;
-			rvec.b = -offset[0] * sin_Ti - offset[1] * cos_Ti;
+			rvec.x() = -offset.x() * cos_Ti + offset.y() * sin_Ti;
+			rvec.y() = -offset.x() * sin_Ti - offset.y() * cos_Ti;
 		}
 
 		// Update raw location
-		raw[p_axis] = center_P + rvec.a;
-		raw[q_axis] = center_Q + rvec.b;
-#	if ENABLED(AUTO_BED_LEVELING_UBL)
-		raw[l_axis] = start_L;
-		UNUSED(linear_per_segment);
-#	else
+		raw[p_axis] = center_P + rvec.x();
+		raw[q_axis] = center_Q + rvec.y();
 		raw[l_axis] += linear_per_segment;
-#	endif
-		raw.e += extruder_per_segment;
 
-		Eigen::Vector3f raw3f { raw.x, raw.y, raw.z };
+		Vector3f32 raw3f { raw.x(), raw.y(), raw.z() };
 
 		limits.throwIfOutside(raw3f);
 		gcode.throwIfAborted();
@@ -271,7 +263,7 @@ void plan_arc(
 		planner.apply_leveling(raw);
 #	endif
 
-		if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, MachineState::FeedMove, 0
+		if (!planner.buffer_line(raw, scaled_feed_rate, active_extruder, MachineState::FeedMove, 0
 #	if ENABLED(SCARA_FEEDRATE_SCALING)
 		                         ,
 		                         inv_duration
@@ -284,15 +276,11 @@ void plan_arc(
 	raw = cart;
 	TERN_(AUTO_BED_LEVELING_UBL, raw[l_axis] = start_L);
 
-	Eigen::Vector3f raw3f { raw.x, raw.y, raw.z };
+	Vector3f32 raw3f { raw.x(), raw.y(), raw.z() };
 
 	limits.throwIfOutside(raw3f);
 
-#	if HAS_LEVELING && !PLANNER_LEVELING
-	planner.apply_leveling(raw);
-#	endif
-
-	planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, MachineState::FeedMove, 0
+	planner.buffer_line(raw, scaled_feed_rate, active_extruder, MachineState::FeedMove, 0
 #	if ENABLED(SCARA_FEEDRATE_SCALING)
 	                    ,
 	                    inv_duration
@@ -339,22 +327,25 @@ void GcodeSuite::G2_G3(const bool clockwise) {
 		relative_mode = true;
 #	endif
 
-		get_destination_from_command(); // Get X Y Z E F (and set cutter power)
+		get_destination_from_command(false); // Get X Y Z E F (and set cutter power)
 
 		TERN_(SF_ARC_FIX, relative_mode = relative_mode_backup);
 
-		ab_float_t arc_offset = { 0, 0 };
+		Vector2f32 arc_offset = { 0, 0 };
+
 		if (parser.seenval('R')) {
 			const float r = parser.value_linear_units();
 			if (r) {
-				const xy_pos_t p1 = current_position, p2 = destination;
+				const Vector2f32 p1 = { current_position.x(), current_position.y() };
+				const Vector2f32 p2 = { destination.x(), destination.y() };
+
 				if (p1 != p2) {
-					const xy_pos_t d2 = (p2 - p1) * 0.5f; // XY vector to midpoint of move from current
+					const Vector2f32 d2 = (p2 - p1) * 0.5f; // XY vector to midpoint of move from current
 					const float e = clockwise ^ (r < 0) ? -1 : 1, // clockwise -1/1, counterclockwise 1/-1
-							len = d2.magnitude(), // Distance to mid-point of move from current
+							len = d2.norm(), // Distance to mid-point of move from current
 							h2 = (r - len) * (r + len), // factored to reduce rounding error
 							h = (h2 >= 0) ? SQRT(h2) : 0.0f; // Distance to the arc pivot-point from midpoint
-					const xy_pos_t s = { -d2.y, d2.x }; // Perpendicular bisector. (Divide by len for unit vector.)
+					const Vector2f32 s = { -d2.y(), d2.x() }; // Perpendicular bisector. (Divide by len for unit vector.)
 					arc_offset = d2 + s / len * e * h; // The calculated offset (mid-point if |r| <= len)
 				}
 			}
@@ -380,12 +371,12 @@ void GcodeSuite::G2_G3(const bool clockwise) {
 			constexpr char achar = 'I', bchar = 'J';
 #	endif
 			if (parser.seenval(achar))
-				arc_offset.a = parser.value_linear_units();
+				arc_offset.x() = parser.value_linear_units();
 			if (parser.seenval(bchar))
-				arc_offset.b = parser.value_linear_units();
+				arc_offset.y() = parser.value_linear_units();
 		}
 
-		if (arc_offset) {
+		if (arc_offset.norm()) {
 #	if ENABLED(ARC_P_CIRCLES)
 			// P indicates number of circles to do
 			const int8_t circles_to_do = parser.byteval('P');
diff --git a/src/marlin/gcode/parser.cpp b/src/marlin/gcode/parser.cpp
index 8b0e6f6787..78f1399011 100644
--- a/src/marlin/gcode/parser.cpp
+++ b/src/marlin/gcode/parser.cpp
@@ -28,14 +28,19 @@
 
 #include "../MarlinCore.h"
 
+#include <swordfish/Controller.h>
+
+using namespace swordfish::motion;
+
 // Must be declared for allocation and to satisfy the linker
 // Zero values need no initialization.
 
 bool GCodeParser::volumetric_enabled;
 
-#if ENABLED(INCH_MODE_SUPPORT)
-float GCodeParser::linear_unit_factor, GCodeParser::volumetric_unit_factor;
-#endif
+
+f32 GCodeParser::linear_unit_factor;
+f32 GCodeParser::radial_unit_factor;
+FeedRateType GCodeParser::feedrate_type;
 
 #if ENABLED(TEMPERATURE_UNITS_SUPPORT)
 TempUnit GCodeParser::input_temp_units = TEMPUNIT_C;
diff --git a/src/marlin/gcode/parser.h b/src/marlin/gcode/parser.h
index 29802be4d4..a0026f5a20 100644
--- a/src/marlin/gcode/parser.h
+++ b/src/marlin/gcode/parser.h
@@ -33,6 +33,9 @@
 #include <string_view>
 #include <stdlib.h>
 
+#include <swordfish/types.h>
+#include <swordfish/modules/motion/FeedRate.h>
+
 #if ENABLED(DEBUG_GCODE_PARSER)
 #	include "../libs/hex_print.h"
 #endif
@@ -77,9 +80,10 @@ class GCodeParser {
 
 	static bool volumetric_enabled;
 
-#if ENABLED(INCH_MODE_SUPPORT)
-	static float linear_unit_factor, volumetric_unit_factor;
-#endif
+	static f32 linear_unit_factor;
+	static f32 radial_unit_factor;
+	static swordfish::motion::FeedRateType feedrate_type;
+
 
 #if ENABLED(TEMPERATURE_UNITS_SUPPORT)
 	static TempUnit input_temp_units;
@@ -392,17 +396,16 @@ class GCodeParser {
 
 	// Units modes: Inches, Fahrenheit, Kelvin
 
-#if ENABLED(INCH_MODE_SUPPORT)
 	static inline float mm_to_linear_unit(const float mm) {
 		return mm / linear_unit_factor;
 	}
-	static inline float mm_to_volumetric_unit(const float mm) {
-		return mm / (volumetric_enabled ? volumetric_unit_factor : linear_unit_factor);
-	}
 
 	// Init linear units by constructor
 	GCodeParser() {
 		set_input_linear_units(LINEARUNIT_MM);
+		set_feed_rate_type(swordfish::motion::FeedRateType::UnitsPerSecond);
+
+		radial_unit_factor = 1.0f;
 	}
 
 	static inline void set_input_linear_units(const LinearUnit units) {
@@ -415,43 +418,31 @@ class GCodeParser {
 				linear_unit_factor = 25.4f;
 				break;
 		}
-		volumetric_unit_factor = POW(linear_unit_factor, 3);
 	}
 
-	static inline float axis_unit_factor(const AxisEnum axis) {
-		return (axis >= E_AXIS && volumetric_enabled ? volumetric_unit_factor : linear_unit_factor);
+	static inline void set_feed_rate_type(const swordfish::motion::FeedRateType type) {
+		feedrate_type = type;
 	}
 
-	static inline float linear_value_to_mm(const float v) {
-		return v * linear_unit_factor;
-	}
-	static inline float axis_value_to_mm(const AxisEnum axis, const float v) {
-		return v * axis_unit_factor(axis);
+	static inline float axis_unit_factor(const Axis axis) {
+		return (axis.is_radial() ? radial_unit_factor : linear_unit_factor);
 	}
-	static inline float per_axis_value(const AxisEnum axis, const float v) {
-		return v / axis_unit_factor(axis);
-	}
-
-#else
 
-	static inline float mm_to_linear_unit(const float mm) {
-		return mm;
-	}
-	static inline float mm_to_volumetric_unit(const float mm) {
-		return mm;
+	static inline float linear_value_to_mm(const f32 v) {
+		return v * linear_unit_factor;
 	}
 
-	static inline float linear_value_to_mm(const float v) {
-		return v;
-	}
-	static inline float axis_value_to_mm(const AxisEnum, const float v) {
-		return v;
+	static inline f32 radial_value_to_degrees(const f32 v) {
+		return v * radial_unit_factor;
 	}
-	static inline float per_axis_value(const AxisEnum, const float v) {
-		return v;
+
+	static inline float axis_value_to_mm(const Axis axis, const f32 v) {
+		return v * axis_unit_factor(axis);
 	}
 
-#endif
+	static inline float per_axis_value(const Axis axis, const f32 v) {
+		return v / axis_unit_factor(axis);
+	}
 
 	static inline bool using_inch_units() {
 		return mm_to_linear_unit(1.0f) != 1.0f;
@@ -460,15 +451,14 @@ class GCodeParser {
 #define IN_TO_MM(I)        ((I) *25.4f)
 #define MM_TO_IN(M)        ((M) / 25.4f)
 #define LINEAR_UNIT(V)     parser.mm_to_linear_unit(V)
-#define VOLUMETRIC_UNIT(V) parser.mm_to_volumetric_unit(V)
 
 	static inline float value_linear_units() {
 		return linear_value_to_mm(value_float());
 	}
-	static inline float value_axis_units(const AxisEnum axis) {
+	static inline float value_axis_units(const Axis axis) {
 		return axis_value_to_mm(axis, value_float());
 	}
-	static inline float value_per_axis_units(const AxisEnum axis) {
+	static inline float value_per_axis_units(const Axis axis) {
 		return per_axis_value(axis, value_float());
 	}
 
@@ -541,8 +531,17 @@ class GCodeParser {
 
 #endif // !TEMPERATURE_UNITS_SUPPORT
 
-	static inline feedRate_t value_feedrate() {
-		return MMM_TO_MMS(value_linear_units());
+	static inline swordfish::motion::FeedRate value_feedrate() {
+		switch (feedrate_type) {
+			case swordfish::motion::FeedRateType::InverseTime: {
+				return swordfish::motion::FeedRate::InverseTime(value_float());
+			}
+
+			default:
+			case swordfish::motion::FeedRateType::UnitsPerSecond: {
+				return swordfish::motion::FeedRate::UnitsPerSecond(MMM_TO_MMS(value_linear_units()));
+			}
+		}
 	}
 
 	void unknown_command_warning();
diff --git a/src/marlin/gcode/probe/G37.cpp b/src/marlin/gcode/probe/G37.cpp
index 0c03a89d1b..5a6eb4cca4 100644
--- a/src/marlin/gcode/probe/G37.cpp
+++ b/src/marlin/gcode/probe/G37.cpp
@@ -39,13 +39,14 @@
 #	include "../../module/planner.h"
 
 using namespace swordfish;
+using namespace swordfish::math;
 using namespace swordfish::motion;
 using namespace swordfish::status;
 
 #	define TOOL_PROBE_X (X_MIN_POS + 35)
 #	define TOOL_PROBE_Y (Y_MAX_POS - 10)
 
-inline bool do_single_probe(EndstopEnum endstop /*, const uint8_t move_value*/) {
+inline bool do_single_probe(EndstopValue endstop /*, const uint8_t move_value*/) {
 	auto& motionModule = MotionModule::getInstance();
 
 	auto limitsEnabled = motionModule.areLimitsEnabled();
@@ -60,7 +61,7 @@ inline bool do_single_probe(EndstopEnum endstop /*, const uint8_t move_value*/)
 
 	endstops.hit_on_purpose();
 
-	set_current_from_steppers_for_axis(ALL_AXES);
+	set_current_from_steppers_for_axis(AxisValue::All);
 	sync_plan_position();
 
 	endstops.enable(false);
@@ -70,29 +71,34 @@ inline bool do_single_probe(EndstopEnum endstop /*, const uint8_t move_value*/)
 	return triggered;
 }
 
-void backoff(xyz_float_t& retract_mm) {
-	REMEMBER(fr, feedrate_mm_s, MMM_TO_MMS(G0_FEEDRATE));
+void backoff(Vector6f32& retract_mm) {
+	auto old_feed_rate = feedrate_mm_s;
+
+	feedrate_mm_s = FeedRate::UnitsPerSecond(MMM_TO_MMS(G0_FEEDRATE));
 
 	// Move away by the retract distance
 	destination = current_position + retract_mm;
 
 	prepare_line_to_destination(MachineState::Probing);
+
+	feedrate_mm_s = old_feed_rate;
+
 	planner.synchronize();
 }
 
-bool run_probe(AxisEnum axis, EndstopEnum endstop, float distance, float retract) {
+bool run_probe(Axis axis, EndstopValue endstop, float distance, float retract) {
 	bool triggered = false;
 
 	debug()("probe distance: ", distance);
 
-	xyz_pos_t delta = { 0, 0, 0 };
+	Vector6f32 delta = { 0, 0, 0, 0, 0, 0 };
 
 	delta[axis] = distance;
 
 	destination = current_position + delta;
 
 #	if MULTIPLE_PROBING > 1
-	xyz_float_t retract_mm = { 0, 0, 0 };
+	Vector6f32 retract_mm = { 0, 0, 0, 0, 0, 0 };
 	retract_mm[axis] = /* home_bump_mm(axis) */ retract * (distance > 0 ? -1 : 1);
 #	endif
 
@@ -135,20 +141,20 @@ bool run_probe(AxisEnum axis, EndstopEnum endstop, float distance, float retract
  * G37 Probe Tool Offset
  */
 void GcodeSuite::G37() {
-	if (homing_needed_error(_BV(X_AXIS) | _BV(Y_AXIS) | _BV(Z_AXIS))) {
+	if (homing_needed_error(_BV(Axis::X()) | _BV(Axis::Y()) | _BV(Axis::Z()))) {
 		return;
 	}
 
 	remember_feedrate_scaling_off();
 
-	feedrate_mm_s = parser.seenval('F') ? parser.value_feedrate() : (homing_feedrate(Z_AXIS) * 0.3);
+	feedrate_mm_s = parser.seenval('F') ? parser.value_feedrate() : (homing_feedrate(Axis::Z()) * 0.3);
 	float retract = parser.seenval('R') ? parser.value_float() : 5;
 
-	const auto delta = -abs(home_dir(Z_AXIS) > 0 ? Z_MAX_POS - Z_MIN_POS : Z_MIN_POS - Z_MAX_POS);
+	const auto delta = -abs(home_dir(Axis::Z()) > 0 ? Z_MAX_POS - Z_MIN_POS : Z_MIN_POS - Z_MAX_POS);
 
 	endstops.enable_tool_probe(true);
 
-	run_probe(Z_AXIS, TOOL_PROBE, delta, retract);
+	run_probe(Axis::Z(), TOOL_PROBE, delta, retract);
 
 	endstops.enable_tool_probe(false);
 
diff --git a/src/marlin/gcode/probe/G38.cpp b/src/marlin/gcode/probe/G38.cpp
index 8be7c45dba..f7ec408768 100644
--- a/src/marlin/gcode/probe/G38.cpp
+++ b/src/marlin/gcode/probe/G38.cpp
@@ -33,7 +33,7 @@
 
 using namespace swordfish::status;
 
-extern bool run_probe(AxisEnum axis, EndstopEnum endstop, float distance, float retract);
+extern bool run_probe(Axis axis, EndstopValue endstop, float distance, float retract);
 
 /**
  * G38 Probe Target
@@ -58,23 +58,23 @@ void GcodeSuite::G38(const int8_t subcode) {
 			;
 
 	// If any axis has enough movement, do the move
-	LOOP_XYZ(i) {
-		if (parser.seenval(XYZ_CHAR(i))) {
-			const float v = parser.value_axis_units((AxisEnum) i);
-			const float dest = axis_is_relative((AxisEnum) i) ? current_position[i] + v : toNative(v, (AxisEnum) i);
+	for (auto i : linear_axes) {
+		if (parser.seenval(i.to_char())) {
+			const float v = parser.value_axis_units(i);
+			const float dest = axis_is_relative(i) ? current_position[i] + v : toNative(v, i);
 
 			if (ABS(dest - current_position[i]) >= G38_MINIMUM_MOVE) {
 				// destination[i] = dest;
 
-				endstops.enable_work_probe((AxisEnum) i, true);
+				endstops.enable_work_probe( i, true);
 
-				feedrate_mm_s = parser.seenval('F') ? parser.value_feedrate() : (homing_feedrate((AxisEnum) i) * 0.5);
+				feedrate_mm_s = parser.seenval('F') ? parser.value_feedrate() : (homing_feedrate(i) * 0.5);
 				float retract = parser.seenval('R') ? parser.value_float() : 5;
 
-				if (!run_probe((AxisEnum) i, WORK_PROBE, v, retract) && error_on_fail)
+				if (!run_probe(i, WORK_PROBE, v, retract) && error_on_fail)
 					SERIAL_ERROR_MSG("Failed to reach target");
 
-				endstops.enable_work_probe((AxisEnum) i, false);
+				endstops.enable_work_probe(i, false);
 
 				break;
 			}
diff --git a/src/marlin/inc/Conditionals_LCD.h b/src/marlin/inc/Conditionals_LCD.h
index a5bb24f27c..ea1dde2d8c 100644
--- a/src/marlin/inc/Conditionals_LCD.h
+++ b/src/marlin/inc/Conditionals_LCD.h
@@ -631,24 +631,6 @@
   #endif
 #endif
 
-/**
- * DISTINCT_E_FACTORS affects how some E factors are accessed
- */
-#if ENABLED(DISTINCT_E_FACTORS) && E_STEPPERS > 1
-  #define DISTINCT_E E_STEPPERS
-  #define XYZE_N (XYZ + E_STEPPERS)
-  #define E_INDEX_N(E) (E)
-  #define E_AXIS_N(E) AxisEnum(E_AXIS + E)
-  #define UNUSED_E(E) NOOP
-#else
-  #undef DISTINCT_E_FACTORS
-  #define DISTINCT_E 1
-  #define XYZE_N XYZE
-  #define E_INDEX_N(E) 0
-  #define E_AXIS_N(E) E_AXIS
-  #define UNUSED_E(E) UNUSED(E)
-#endif
-
 #if ENABLED(DWIN_CREALITY_LCD)
   #define SERIAL_CATCHALL 0
   #ifndef LCD_SERIAL_PORT
@@ -1044,10 +1026,15 @@
 #ifndef INVERT_Z_DIR
   #define INVERT_Z_DIR false
 #endif
-#ifndef INVERT_E_DIR
-  #define INVERT_E_DIR false
+#ifndef INVERT_A_DIR
+  #define INVERT_A_DIR false
+#endif
+#ifndef INVERT_B_DIR
+  #define INVERT_B_DIR false
+#endif
+#ifndef INVERT_C_DIR
+  #define INVERT_C_DIR false
 #endif
-
 /**
  * This setting is also used by M109 when trying to calculate
  * a ballpark safe margin to prevent wait-forever situation.
diff --git a/src/marlin/inc/Conditionals_post.h b/src/marlin/inc/Conditionals_post.h
index d9e02c3778..6ade44972b 100644
--- a/src/marlin/inc/Conditionals_post.h
+++ b/src/marlin/inc/Conditionals_post.h
@@ -1342,7 +1342,7 @@
  */
 
 // Steppers
-#if PIN_EXISTS(X_ENABLE) || (ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(X))
+#if PIN_EXISTS(X_ENABLE)
   #define HAS_X_ENABLE 1
 #endif
 #if PIN_EXISTS(X_DIR)
@@ -1355,7 +1355,7 @@
   #define HAS_X_MS_PINS 1
 #endif
 
-#if PIN_EXISTS(X2_ENABLE) || (ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(X2))
+#if PIN_EXISTS(X2_ENABLE)
   #define HAS_X2_ENABLE 1
 #endif
 #if PIN_EXISTS(X2_DIR)
@@ -1368,7 +1368,7 @@
   #define HAS_X2_MS_PINS 1
 #endif
 
-#if PIN_EXISTS(Y_ENABLE) || (ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(Y))
+#if PIN_EXISTS(Y_ENABLE)
   #define HAS_Y_ENABLE 1
 #endif
 #if PIN_EXISTS(Y_DIR)
@@ -1381,7 +1381,7 @@
   #define HAS_Y_MS_PINS 1
 #endif
 
-#if PIN_EXISTS(Y2_ENABLE) || (ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(Y2))
+#if PIN_EXISTS(Y2_ENABLE)
   #define HAS_Y2_ENABLE 1
 #endif
 #if PIN_EXISTS(Y2_DIR)
@@ -1394,7 +1394,7 @@
   #define HAS_Y2_MS_PINS 1
 #endif
 
-#if PIN_EXISTS(Z_ENABLE) || (ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(Z))
+#if PIN_EXISTS(Z_ENABLE)
   #define HAS_Z_ENABLE 1
 #endif
 #if PIN_EXISTS(Z_DIR)
@@ -1410,7 +1410,7 @@
 	#define HAS_Z_BRAKE 1
 #endif
 
-#if PIN_EXISTS(Z2_ENABLE) || (ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(Z2))
+#if PIN_EXISTS(Z2_ENABLE)
   #define HAS_Z2_ENABLE 1
 #endif
 #if PIN_EXISTS(Z2_DIR)
@@ -1423,7 +1423,7 @@
   #define HAS_Z2_MS_PINS 1
 #endif
 
-#if PIN_EXISTS(Z3_ENABLE) || (ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(Z3))
+#if PIN_EXISTS(Z3_ENABLE)
   #define HAS_Z3_ENABLE 1
 #endif
 #if PIN_EXISTS(Z3_DIR)
@@ -1436,7 +1436,7 @@
   #define HAS_Z3_MS_PINS 1
 #endif
 
-#if PIN_EXISTS(Z4_ENABLE) || (ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(Z4))
+#if PIN_EXISTS(Z4_ENABLE)
   #define HAS_Z4_ENABLE 1
 #endif
 #if PIN_EXISTS(Z4_DIR)
@@ -1450,132 +1450,43 @@
 #endif
 
 // Extruder steppers and solenoids
-#if PIN_EXISTS(E0_ENABLE) || (ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(E0))
-  #define HAS_E0_ENABLE 1
+#if PIN_EXISTS(A_ENABLE)
+  #define HAS_A_ENABLE 1
 #endif
-#if PIN_EXISTS(E0_DIR)
-  #define HAS_E0_DIR 1
+#if PIN_EXISTS(A_DIR)
+  #define HAS_A_DIR 1
 #endif
-#if PIN_EXISTS(E0_STEP)
-  #define HAS_E0_STEP 1
+#if PIN_EXISTS(A_STEP)
+  #define HAS_A_STEP 1
 #endif
-#if PIN_EXISTS(E0_MS1)
-  #define HAS_E0_MS_PINS 1
-#endif
-#if PIN_EXISTS(SOL0)
-  #define HAS_SOLENOID_0 1
-#endif
-
-#if PIN_EXISTS(E1_ENABLE) || (ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(E1))
-  #define HAS_E1_ENABLE 1
-#endif
-#if PIN_EXISTS(E1_DIR)
-  #define HAS_E1_DIR 1
-#endif
-#if PIN_EXISTS(E1_STEP)
-  #define HAS_E1_STEP 1
-#endif
-#if PIN_EXISTS(E1_MS1)
-  #define HAS_E1_MS_PINS 1
-#endif
-#if PIN_EXISTS(SOL1)
-  #define HAS_SOLENOID_1 1
-#endif
-
-#if PIN_EXISTS(E2_ENABLE) || (ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(E2))
-  #define HAS_E2_ENABLE 1
-#endif
-#if PIN_EXISTS(E2_DIR)
-  #define HAS_E2_DIR 1
-#endif
-#if PIN_EXISTS(E2_STEP)
-  #define HAS_E2_STEP 1
-#endif
-#if PIN_EXISTS(E2_MS1)
-  #define HAS_E2_MS_PINS 1
-#endif
-#if PIN_EXISTS(SOL2)
-  #define HAS_SOLENOID_2 1
-#endif
-
-#if PIN_EXISTS(E3_ENABLE) || (ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(E3))
-  #define HAS_E3_ENABLE 1
-#endif
-#if PIN_EXISTS(E3_DIR)
-  #define HAS_E3_DIR 1
-#endif
-#if PIN_EXISTS(E3_STEP)
-  #define HAS_E3_STEP 1
-#endif
-#if PIN_EXISTS(E3_MS1)
-  #define HAS_E3_MS_PINS 1
-#endif
-#if PIN_EXISTS(SOL3)
-  #define HAS_SOLENOID_3 1
-#endif
-
-#if PIN_EXISTS(E4_ENABLE) || (ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(E4))
-  #define HAS_E4_ENABLE 1
-#endif
-#if PIN_EXISTS(E4_DIR)
-  #define HAS_E4_DIR 1
-#endif
-#if PIN_EXISTS(E4_STEP)
-  #define HAS_E4_STEP 1
-#endif
-#if PIN_EXISTS(E4_MS1)
-  #define HAS_E4_MS_PINS 1
-#endif
-#if PIN_EXISTS(SOL4)
-  #define HAS_SOLENOID_4 1
-#endif
-
-#if PIN_EXISTS(E5_ENABLE) || (ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(E5))
-  #define HAS_E5_ENABLE 1
-#endif
-#if PIN_EXISTS(E5_DIR)
-  #define HAS_E5_DIR 1
-#endif
-#if PIN_EXISTS(E5_STEP)
-  #define HAS_E5_STEP 1
-#endif
-#if PIN_EXISTS(E5_MS1)
-  #define HAS_E5_MS_PINS 1
-#endif
-#if PIN_EXISTS(SOL5)
-  #define HAS_SOLENOID_5 1
+#if PIN_EXISTS(A_MS1)
+  #define HAS_A_MS_PINS 1
 #endif
 
-#if PIN_EXISTS(E6_ENABLE) || (ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(E6))
-  #define HAS_E6_ENABLE 1
-#endif
-#if PIN_EXISTS(E6_DIR)
-  #define HAS_E6_DIR 1
+#if PIN_EXISTS(B_ENABLE)
+  #define HAS_B_ENABLE 1
 #endif
-#if PIN_EXISTS(E6_STEP)
-  #define HAS_E6_STEP 1
+#if PIN_EXISTS(B_DIR)
+  #define HAS_B_DIR 1
 #endif
-#if PIN_EXISTS(E6_MS1)
-  #define HAS_E6_MS_PINS 1
+#if PIN_EXISTS(B_STEP)
+  #define HAS_B_STEP 1
 #endif
-#if PIN_EXISTS(SOL6)
-  #define HAS_SOLENOID_6 1
+#if PIN_EXISTS(B_MS1)
+  #define HAS_B_MS_PINS 1
 #endif
 
-#if PIN_EXISTS(E7_ENABLE) || (ENABLED(SOFTWARE_DRIVER_ENABLE) && AXIS_IS_TMC(E7))
-  #define HAS_E7_ENABLE 1
-#endif
-#if PIN_EXISTS(E7_DIR)
-  #define HAS_E7_DIR 1
+#if PIN_EXISTS(C_ENABLE)
+  #define HAS_C_ENABLE 1
 #endif
-#if PIN_EXISTS(E7_STEP)
-  #define HAS_E7_STEP 1
+#if PIN_EXISTS(C_DIR)
+  #define HAS_C_DIR 1
 #endif
-#if PIN_EXISTS(E7_MS1)
-  #define HAS_E7_MS_PINS 1
+#if PIN_EXISTS(C_STEP)
+  #define HAS_C_STEP 1
 #endif
-#if PIN_EXISTS(SOL7)
-  #define HAS_SOLENOID_7 1
+#if PIN_EXISTS(C_MS1)
+  #define HAS_C_MS_PINS 1
 #endif
 
 //
@@ -1723,19 +1634,6 @@
   #endif
 #endif
 
-#if (HAS_E_DRIVER(TMC2660) \
-  || ( E0_ENABLE_PIN != X_ENABLE_PIN && E1_ENABLE_PIN != X_ENABLE_PIN   \
-    && E0_ENABLE_PIN != Y_ENABLE_PIN && E1_ENABLE_PIN != Y_ENABLE_PIN ) )
-  #define HAS_E_STEPPER_ENABLE 1
-#endif
-
-#if ANY_AXIS_HAS(HW_SERIAL)
-  #define HAS_TMC_HW_SERIAL 1
-#endif
-#if ANY_AXIS_HAS(SW_SERIAL)
-  #define HAS_TMC_SW_SERIAL 1
-#endif
-
 //
 // Endstops and bed probe
 //
@@ -1807,6 +1705,12 @@
 #if PIN_EXISTS(Z4_MAX)
   #define HAS_Z4_MAX 1
 #endif
+#if PIN_EXISTS(A_MIN)
+  #define HAS_A_MIN 1
+#endif
+#if PIN_EXISTS(A_MAX)
+  #define HAS_A_MAX 1
+#endif
 #if HAS_CUSTOM_PROBE_PIN && PIN_EXISTS(Z_MIN_PROBE)
   #define HAS_Z_MIN_PROBE_PIN 1
 #endif
diff --git a/src/marlin/inc/SanityCheck.h b/src/marlin/inc/SanityCheck.h
index ebdf6af206..ff015c701f 100644
--- a/src/marlin/inc/SanityCheck.h
+++ b/src/marlin/inc/SanityCheck.h
@@ -34,6 +34,8 @@
   #error "Marlin requires C++11 support (gcc >= 4.7, Arduino IDE >= 1.6.8). Please upgrade your toolchain."
 #endif
 
+#include <swordfish/types.h>
+
 // Make sure macros aren't borked
 #define TEST1
 #define TEST2 1
@@ -1513,11 +1515,6 @@ static_assert(Y_MAX_LENGTH >= Y_BED_SIZE, "Movement bounds (Y_MIN_POS, Y_MAX_POS
 /**
  * Homing
  */
-constexpr float hbm[] = HOMING_BUMP_MM;
-static_assert(COUNT(hbm) == XYZ, "HOMING_BUMP_MM requires X, Y, and Z elements.");
-static_assert(hbm[X_AXIS] >= 0, "HOMING_BUMP_MM.X must be greater than or equal to 0.");
-static_assert(hbm[Y_AXIS] >= 0, "HOMING_BUMP_MM.Y must be greater than or equal to 0.");
-static_assert(hbm[Z_AXIS] >= 0, "HOMING_BUMP_MM.Z must be greater than or equal to 0.");
 
 #if ENABLED(CODEPENDENT_XY_HOMING)
   #if ENABLED(QUICK_HOME)
@@ -2425,168 +2422,6 @@ static_assert(hbm[Z_AXIS] >= 0, "HOMING_BUMP_MM.Z must be greater than or equal
   #error "LED_USER_PRESET_STARTUP is required for FYSETC_MINI_12864 2.x displays."
 #endif
 
-/**
- * Check existing CS pins against enabled TMC SPI drivers.
- */
-#define INVALID_TMC_SPI(ST) (AXIS_HAS_SPI(ST) && !PIN_EXISTS(ST##_CS))
-#if INVALID_TMC_SPI(X)
-  #error "An SPI driven TMC driver on X requires X_CS_PIN."
-#elif INVALID_TMC_SPI(X2)
-  #error "An SPI driven TMC driver on X2 requires X2_CS_PIN."
-#elif INVALID_TMC_SPI(Y)
-  #error "An SPI driven TMC driver on Y requires Y_CS_PIN."
-#elif INVALID_TMC_SPI(Y2)
-  #error "An SPI driven TMC driver on Y2 requires Y2_CS_PIN."
-#elif INVALID_TMC_SPI(Z)
-  #error "An SPI driven TMC driver on Z requires Z_CS_PIN."
-#elif INVALID_TMC_SPI(Z2)
-  #error "An SPI driven TMC driver on Z2 requires Z2_CS_PIN."
-#elif INVALID_TMC_SPI(Z3)
-  #error "An SPI driven TMC driver on Z3 requires Z3_CS_PIN."
-#elif INVALID_TMC_SPI(Z4)
-  #error "An SPI driven TMC driver on Z4 requires Z4_CS_PIN."
-#elif INVALID_TMC_SPI(E0)
-  #error "An SPI driven TMC driver on E0 requires E0_CS_PIN."
-#elif INVALID_TMC_SPI(E1)
-  #error "An SPI driven TMC driver on E1 requires E1_CS_PIN."
-#elif INVALID_TMC_SPI(E2)
-  #error "An SPI driven TMC driver on E2 requires E2_CS_PIN."
-#elif INVALID_TMC_SPI(E3)
-  #error "An SPI driven TMC driver on E3 requires E3_CS_PIN."
-#elif INVALID_TMC_SPI(E4)
-  #error "An SPI driven TMC driver on E4 requires E4_CS_PIN."
-#elif INVALID_TMC_SPI(E5)
-  #error "An SPI driven TMC driver on E5 requires E5_CS_PIN."
-#elif INVALID_TMC_SPI(E6)
-  #error "An SPI driven TMC driver on E6 requires E6_CS_PIN."
-#elif INVALID_TMC_SPI(E7)
-  #error "An SPI driven TMC driver on E7 requires E7_CS_PIN."
-#endif
-#undef INVALID_TMC_SPI
-
-/**
- * Check existing RX/TX pins against enable TMC UART drivers.
- */
-#define INVALID_TMC_UART(ST) (AXIS_HAS_UART(ST) && !(defined(ST##_HARDWARE_SERIAL) || (PINS_EXIST(ST##_SERIAL_RX, ST##_SERIAL_TX))))
-#if INVALID_TMC_UART(X)
-  #error "TMC2208 or TMC2209 on X requires X_HARDWARE_SERIAL or X_SERIAL_(RX|TX)_PIN."
-#elif INVALID_TMC_UART(X2)
-  #error "TMC2208 or TMC2209 on X2 requires X2_HARDWARE_SERIAL or X2_SERIAL_(RX|TX)_PIN."
-#elif INVALID_TMC_UART(Y)
-  #error "TMC2208 or TMC2209 on Y requires Y_HARDWARE_SERIAL or Y_SERIAL_(RX|TX)_PIN."
-#elif INVALID_TMC_UART(Y2)
-  #error "TMC2208 or TMC2209 on Y2 requires Y2_HARDWARE_SERIAL or Y2_SERIAL_(RX|TX)_PIN."
-#elif INVALID_TMC_UART(Z)
-  #error "TMC2208 or TMC2209 on Z requires Z_HARDWARE_SERIAL or Z_SERIAL_(RX|TX)_PIN."
-#elif INVALID_TMC_UART(Z2)
-  #error "TMC2208 or TMC2209 on Z2 requires Z2_HARDWARE_SERIAL or Z2_SERIAL_(RX|TX)_PIN."
-#elif INVALID_TMC_UART(Z3)
-  #error "TMC2208 or TMC2209 on Z3 requires Z3_HARDWARE_SERIAL or Z3_SERIAL_(RX|TX)_PIN."
-#elif INVALID_TMC_UART(Z4)
-  #error "TMC2208 or TMC2209 on Z4 requires Z4_HARDWARE_SERIAL or Z4_SERIAL_(RX|TX)_PIN."
-#elif INVALID_TMC_UART(E0)
-  #error "TMC2208 or TMC2209 on E0 requires E0_HARDWARE_SERIAL or E0_SERIAL_(RX|TX)_PIN."
-#elif INVALID_TMC_UART(E1)
-  #error "TMC2208 or TMC2209 on E1 requires E1_HARDWARE_SERIAL or E1_SERIAL_(RX|TX)_PIN."
-#elif INVALID_TMC_UART(E2)
-  #error "TMC2208 or TMC2209 on E2 requires E2_HARDWARE_SERIAL or E2_SERIAL_(RX|TX)_PIN."
-#elif INVALID_TMC_UART(E3)
-  #error "TMC2208 or TMC2209 on E3 requires E3_HARDWARE_SERIAL or E3_SERIAL_(RX|TX)_PIN."
-#elif INVALID_TMC_UART(E4)
-  #error "TMC2208 or TMC2209 on E4 requires E4_HARDWARE_SERIAL or E4_SERIAL_(RX|TX)_PIN."
-#elif INVALID_TMC_UART(E5)
-  #error "TMC2208 or TMC2209 on E5 requires E5_HARDWARE_SERIAL or E5_SERIAL_(RX|TX)_PIN."
-#elif INVALID_TMC_UART(E6)
-  #error "TMC2208 or TMC2209 on E6 requires E6_HARDWARE_SERIAL or E6_SERIAL_(RX|TX)_PIN."
-#elif INVALID_TMC_UART(E7)
-  #error "TMC2208 or TMC2209 on E7 requires E7_HARDWARE_SERIAL or E7_SERIAL_(RX|TX)_PIN."
-#endif
-#undef INVALID_TMC_UART
-
-/**
- * TMC2209 slave address values
- */
-#define INVALID_TMC_ADDRESS(ST) static_assert(0 <= ST##_SLAVE_ADDRESS && ST##_SLAVE_ADDRESS <= 3, "TMC2209 slave address must be 0, 1, 2 or 3")
-#if AXIS_DRIVER_TYPE_X(TMC2209)
-  INVALID_TMC_ADDRESS(X);
-#elif AXIS_DRIVER_TYPE_X2(TMC2209)
-  INVALID_TMC_ADDRESS(X2);
-#elif AXIS_DRIVER_TYPE_Y(TMC2209)
-  INVALID_TMC_ADDRESS(Y);
-#elif AXIS_DRIVER_TYPE_Y2(TMC2209)
-  INVALID_TMC_ADDRESS(Y2);
-#elif AXIS_DRIVER_TYPE_Z(TMC2209)
-  INVALID_TMC_ADDRESS(Z);
-#elif AXIS_DRIVER_TYPE_Z2(TMC2209)
-  INVALID_TMC_ADDRESS(Z2);
-#elif AXIS_DRIVER_TYPE_Z3(TMC2209)
-  INVALID_TMC_ADDRESS(Z3);
-#elif AXIS_DRIVER_TYPE_Z4(TMC2209)
-  INVALID_TMC_ADDRESS(Z4);
-#elif AXIS_DRIVER_TYPE_E0(TMC2209)
-  INVALID_TMC_ADDRESS(E0);
-#elif AXIS_DRIVER_TYPE_E1(TMC2209)
-  INVALID_TMC_ADDRESS(E1);
-#elif AXIS_DRIVER_TYPE_E2(TMC2209)
-  INVALID_TMC_ADDRESS(E2);
-#elif AXIS_DRIVER_TYPE_E3(TMC2209)
-  INVALID_TMC_ADDRESS(E3);
-#elif AXIS_DRIVER_TYPE_E4(TMC2209)
-  INVALID_TMC_ADDRESS(E4);
-#elif AXIS_DRIVER_TYPE_E5(TMC2209)
-  INVALID_TMC_ADDRESS(E5);
-#elif AXIS_DRIVER_TYPE_E6(TMC2209)
-  INVALID_TMC_ADDRESS(E6);
-#elif AXIS_DRIVER_TYPE_E7(TMC2209)
-  INVALID_TMC_ADDRESS(E7);
-#endif
-#undef INVALID_TMC_ADDRESS
-
-#define _TMC_MICROSTEP_IS_VALID(MS) (MS == 0 || MS == 2 || MS == 4 || MS == 8 || MS == 16 || MS == 32 || MS == 64 || MS == 128 || MS == 256)
-#define TMC_MICROSTEP_IS_VALID(M) (!AXIS_IS_TMC(M) || _TMC_MICROSTEP_IS_VALID(M##_MICROSTEPS))
-#define INVALID_TMC_MS(ST) static_assert(0, "Invalid " STRINGIFY(ST) "_MICROSTEPS. Valid values are 0, 2, 4, 8, 16, 32, 64, 128, and 256.")
-
-#if !TMC_MICROSTEP_IS_VALID(X)
-  INVALID_TMC_MS(X);
-#elif !TMC_MICROSTEP_IS_VALID(Y)
-  INVALID_TMC_MS(Y)
-#elif !TMC_MICROSTEP_IS_VALID(Z)
-  INVALID_TMC_MS(Z)
-#elif !TMC_MICROSTEP_IS_VALID(X2)
-  INVALID_TMC_MS(X2);
-#elif !TMC_MICROSTEP_IS_VALID(Y2)
-  INVALID_TMC_MS(Y2)
-#elif !TMC_MICROSTEP_IS_VALID(Z2)
-  INVALID_TMC_MS(Z2)
-#elif !TMC_MICROSTEP_IS_VALID(Z3)
-  INVALID_TMC_MS(Z3)
-#elif !TMC_MICROSTEP_IS_VALID(Z4)
-  INVALID_TMC_MS(Z4)
-#elif !TMC_MICROSTEP_IS_VALID(E0)
-  INVALID_TMC_MS(E0)
-#elif !TMC_MICROSTEP_IS_VALID(E1)
-  INVALID_TMC_MS(E1)
-#elif !TMC_MICROSTEP_IS_VALID(E2)
-  INVALID_TMC_MS(E2)
-#elif !TMC_MICROSTEP_IS_VALID(E3)
-  INVALID_TMC_MS(E3)
-#elif !TMC_MICROSTEP_IS_VALID(E4)
-  INVALID_TMC_MS(E4)
-#elif !TMC_MICROSTEP_IS_VALID(E5)
-  INVALID_TMC_MS(E5)
-#elif !TMC_MICROSTEP_IS_VALID(E6)
-  INVALID_TMC_MS(E6)
-#elif !TMC_MICROSTEP_IS_VALID(E7)
-  INVALID_TMC_MS(E7)
-#endif
-#undef INVALID_TMC_MS
-#undef TMC_MICROSTEP_IS_VALID
-#undef _TMC_MICROSTEP_IS_VALID
-
-#if ENABLED(DELTA) && (ENABLED(STEALTHCHOP_XY) != ENABLED(STEALTHCHOP_Z))
-  #error "STEALTHCHOP_XY and STEALTHCHOP_Z must be the same on DELTA."
-#endif
-
 #if ENABLED(SENSORLESS_HOMING)
   // Require STEALTHCHOP for SENSORLESS_HOMING on DELTA as the transition from spreadCycle to stealthChop
   // is necessary in order to reset the stallGuard indication between the initial movement of all three
@@ -2782,22 +2617,22 @@ constexpr float sanity_arr_1[] = DEFAULT_AXIS_STEPS_PER_UNIT,
 
 #define _ARR_TEST(N,I) (sanity_arr_##N[_MIN(I,int(COUNT(sanity_arr_##N))-1)] > 0)
 
-static_assert(COUNT(sanity_arr_1) >= XYZE,   "DEFAULT_AXIS_STEPS_PER_UNIT requires X, Y, Z and E elements.");
-static_assert(COUNT(sanity_arr_1) <= XYZE_N, "DEFAULT_AXIS_STEPS_PER_UNIT has too many elements. (Did you forget to enable DISTINCT_E_FACTORS?)");
+static_assert(COUNT(sanity_arr_1) >= 4,   "DEFAULT_AXIS_STEPS_PER_UNIT requires X, Y, Z and E elements.");
+static_assert(COUNT(sanity_arr_1) <= 5, "DEFAULT_AXIS_STEPS_PER_UNIT has too many elements. (Did you forget to enable DISTINCT_E_FACTORS?)");
 static_assert(   _ARR_TEST(1,0) && _ARR_TEST(1,1) && _ARR_TEST(1,2)
               && _ARR_TEST(1,3) && _ARR_TEST(1,4) && _ARR_TEST(1,5)
               && _ARR_TEST(1,6) && _ARR_TEST(1,7) && _ARR_TEST(1,8),
               "DEFAULT_AXIS_STEPS_PER_UNIT values must be positive.");
 
-static_assert(COUNT(sanity_arr_2) >= XYZE,   "DEFAULT_MAX_FEEDRATE requires X, Y, Z and E elements.");
-static_assert(COUNT(sanity_arr_2) <= XYZE_N, "DEFAULT_MAX_FEEDRATE has too many elements. (Did you forget to enable DISTINCT_E_FACTORS?)");
+static_assert(COUNT(sanity_arr_2) >= 4,   "DEFAULT_MAX_FEEDRATE requires X, Y, Z and E elements.");
+static_assert(COUNT(sanity_arr_2) <= 5, "DEFAULT_MAX_FEEDRATE has too many elements. (Did you forget to enable DISTINCT_E_FACTORS?)");
 static_assert(   _ARR_TEST(2,0) && _ARR_TEST(2,1) && _ARR_TEST(2,2)
               && _ARR_TEST(2,3) && _ARR_TEST(2,4) && _ARR_TEST(2,5)
               && _ARR_TEST(2,6) && _ARR_TEST(2,7) && _ARR_TEST(2,8),
               "DEFAULT_MAX_FEEDRATE values must be positive.");
 
-static_assert(COUNT(sanity_arr_3) >= XYZE,   "DEFAULT_MAX_ACCELERATION requires X, Y, Z and E elements.");
-static_assert(COUNT(sanity_arr_3) <= XYZE_N, "DEFAULT_MAX_ACCELERATION has too many elements. (Did you forget to enable DISTINCT_E_FACTORS?)");
+static_assert(COUNT(sanity_arr_3) >= 4,   "DEFAULT_MAX_ACCELERATION requires X, Y, Z and E elements.");
+static_assert(COUNT(sanity_arr_3) <= 5, "DEFAULT_MAX_ACCELERATION has too many elements. (Did you forget to enable DISTINCT_E_FACTORS?)");
 static_assert(   _ARR_TEST(3,0) && _ARR_TEST(3,1) && _ARR_TEST(3,2)
               && _ARR_TEST(3,3) && _ARR_TEST(3,4) && _ARR_TEST(3,5)
               && _ARR_TEST(3,6) && _ARR_TEST(3,7) && _ARR_TEST(3,8),
diff --git a/src/marlin/module/endstops.cpp b/src/marlin/module/endstops.cpp
index 95a5350898..107237c6e2 100644
--- a/src/marlin/module/endstops.cpp
+++ b/src/marlin/module/endstops.cpp
@@ -31,6 +31,8 @@
 #include "../sd/cardreader.h"
 #include "temperature.h"
 
+#include <swordfish/types.h>
+
 #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
 // clang-format off
 #	include HAL_PATH(../HAL, endstop_interrupts.h)
@@ -75,6 +77,8 @@ uint8_t Endstops::endstop_poll_count;
 volatile bool Endstops::z_probe_enabled = false;
 #endif
 
+volatile bool Endstops::a_home_enabled = false;
+
 #if HAS_TOOL_PROBE
 bool Endstops::tool_probe_enabled = false;
 #endif
@@ -193,6 +197,16 @@ void Endstops::init() {
 #	endif
 #endif
 
+#if HAS_A_MIN
+#	if ENABLED(ENDSTOPPULLUP_AMIN)
+	SET_INPUT_PULLUP(A_MIN_PIN);
+#	elif ENABLED(ENDSTOPPULLDOWN_AMIN)
+	SET_INPUT_PULLDOWN(A_MIN_PIN);
+#	else
+	SET_INPUT(A_MIN_PIN);
+#	endif
+#endif
+
 #if HAS_X_MAX
 #	if ENABLED(ENDSTOPPULLUP_XMAX)
 	SET_INPUT_PULLUP(X_MAX_PIN);
@@ -273,6 +287,16 @@ void Endstops::init() {
 #	endif
 #endif
 
+#if HAS_A_MAX
+#	if ENABLED(ENDSTOPPULLUP_AMAX)
+	SET_INPUT_PULLUP(A_MAX_PIN);
+#	elif ENABLED(ENDSTOPPULLDOWN_AMAX)
+	SET_INPUT_PULLDOWN(A_MAX_PIN);
+#	else
+	SET_INPUT(A_MAX_PIN);
+#	endif
+#endif
+
 #if PIN_EXISTS(CALIBRATION)
 #	if ENABLED(CALIBRATION_PIN_PULLUP)
 	SET_INPUT_PULLUP(CALIBRATION_PIN);
@@ -497,6 +521,9 @@ void OPT_O2 Endstops::report_states() {
 #if HAS_Z4_MIN
 	ES_REPORT(Z4_MIN);
 #endif
+#if HAS_A_MIN
+	ES_REPORT(A_MIN);
+#endif
 #if HAS_Z_MAX
 	ES_REPORT(Z_MAX);
 #endif
@@ -509,6 +536,9 @@ void OPT_O2 Endstops::report_states() {
 #if HAS_Z4_MAX
 	ES_REPORT(Z4_MAX);
 #endif
+#if HAS_A_MAX
+	ES_REPORT(A_MAX);
+#endif
 #if BOTH(MARLIN_DEV_MODE, PROBE_ACTIVATION_SWITCH)
 	print_es_state(probe_switch_activated(), PSTR(STR_PROBE_EN));
 #endif
@@ -675,6 +705,14 @@ void Endstops::update() {
 #	endif
 #endif
 
+#if HAS_A_MIN && !A_SPI_SENSORLESS
+	UPDATE_ENDSTOP_BIT(A, MIN);
+#endif
+
+#if HAS_A_MAX && !A_SPI_SENSORLESS
+	UPDATE_ENDSTOP_BIT(A, MAX);
+#endif
+
 #if HAS_BED_PROBE
 	// When closing the gap check the enabled probe
 	if (probe_switch_activated())
@@ -849,8 +887,8 @@ void Endstops::update() {
 
 	// Signal, after validation, if an endstop limit is pressed or not
 
-	if (stepper.axis_is_moving(X_AXIS)) {
-		if (stepper.motor_direction(X_AXIS_HEAD)) { // -direction
+	if (stepper.axis_is_moving(Axis::X())) {
+		if (stepper.motor_direction(Axis::X())) { // -direction
 #if HAS_X_MIN || (X_SPI_SENSORLESS && X_HOME_DIR < 0)
 			PROCESS_ENDSTOP_X(MIN);
 #	if CORE_DIAG(XY, Y, MIN)
@@ -865,10 +903,10 @@ void Endstops::update() {
 #endif
 
 #if HAS_WORK_PROBE
-			if (work_probe_enabled[X_AXIS] && TEST_ENDSTOP(WORK_PROBE)) {
+			if (work_probe_enabled[Axis::X()] && TEST_ENDSTOP(WORK_PROBE)) {
 				SBI(hit_state, WORK_PROBE);
 
-				planner.endstop_triggered(X_AXIS);
+				planner.endstop_triggered(Axis::X());
 			}
 #endif
 		} else { // +direction
@@ -886,17 +924,17 @@ void Endstops::update() {
 #endif
 
 #if HAS_WORK_PROBE
-			if (work_probe_enabled[X_AXIS] && TEST_ENDSTOP(WORK_PROBE)) {
+			if (work_probe_enabled[Axis::X()] && TEST_ENDSTOP(WORK_PROBE)) {
 				SBI(hit_state, WORK_PROBE);
 
-				planner.endstop_triggered(X_AXIS);
+				planner.endstop_triggered(Axis::X());
 			}
 #endif
 		}
 	}
 
-	if (stepper.axis_is_moving(Y_AXIS)) {
-		if (stepper.motor_direction(Y_AXIS_HEAD)) { // -direction
+	if (stepper.axis_is_moving(Axis::Y())) {
+		if (stepper.motor_direction(Axis::Y())) { // -direction
 #if HAS_Y_MIN || (Y_SPI_SENSORLESS && Y_HOME_DIR < 0)
 			PROCESS_ENDSTOP_Y(MIN);
 #	if CORE_DIAG(XY, X, MIN)
@@ -911,10 +949,10 @@ void Endstops::update() {
 #endif
 
 #if HAS_WORK_PROBE
-			if (work_probe_enabled[Y_AXIS] && TEST_ENDSTOP(WORK_PROBE)) {
+			if (work_probe_enabled[Axis::Y()] && TEST_ENDSTOP(WORK_PROBE)) {
 				SBI(hit_state, WORK_PROBE);
 
-				planner.endstop_triggered(Y_AXIS);
+				planner.endstop_triggered(Axis::Y());
 			}
 #endif
 		} else { // +direction
@@ -932,17 +970,17 @@ void Endstops::update() {
 #endif
 
 #if HAS_WORK_PROBE
-			if (work_probe_enabled[Y_AXIS] && TEST_ENDSTOP(WORK_PROBE)) {
+			if (work_probe_enabled[Axis::Y()] && TEST_ENDSTOP(WORK_PROBE)) {
 				SBI(hit_state, WORK_PROBE);
 
-				planner.endstop_triggered(Y_AXIS);
+				planner.endstop_triggered(Axis::Y());
 			}
 #endif
 		}
 	}
 
-	if (stepper.axis_is_moving(Z_AXIS)) {
-		if (stepper.motor_direction(Z_AXIS_HEAD)) { // Z -direction. Gantry down, bed up.
+	if (stepper.axis_is_moving(Axis::Z())) {
+		if (stepper.motor_direction(Axis::Z())) { // Z -direction. Gantry down, bed up.
 
 #if HAS_Z_MIN || (Z_SPI_SENSORLESS && Z_HOME_DIR < 0)
 			if (TERN1(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN, z_probe_enabled) && TERN1(HAS_CUSTOM_PROBE_PIN, !z_probe_enabled))
@@ -968,15 +1006,15 @@ void Endstops::update() {
 			if (tool_probe_enabled && TEST_ENDSTOP(TOOL_PROBE)) {
 				SBI(hit_state, TOOL_PROBE);
 
-				planner.endstop_triggered(Z_AXIS);
+				planner.endstop_triggered(Axis::Z());
 			}
 #endif
 
 #if HAS_WORK_PROBE
-			if (work_probe_enabled[Z_AXIS] && TEST_ENDSTOP(WORK_PROBE)) {
+			if (work_probe_enabled[Axis::Z()] && TEST_ENDSTOP(WORK_PROBE)) {
 				SBI(hit_state, WORK_PROBE);
 
-				planner.endstop_triggered(Z_AXIS);
+				planner.endstop_triggered(Axis::Z());
 			}
 #endif
 		} else { // Z +direction. Gantry up, bed down.
@@ -998,6 +1036,10 @@ void Endstops::update() {
 #endif
 		}
 	}
+
+	if (a_home_enabled && stepper.axis_is_moving(Axis::A())) {
+		PROCESS_ENDSTOP(A, MAX);
+	}
 } // Endstops::update()
 
 #if ENABLED(SPI_ENDSTOPS)
@@ -1129,6 +1171,12 @@ void Endstops::monitor() {
 #	if HAS_Z4_MAX
 	ES_GET_STATE(Z4_MAX);
 #	endif
+#	if HAS_A_MIN
+	ES_GET_STATE(A_MIN);
+#	endif
+#	if HAS_A_MAX
+	ES_GET_STATE(A_MAX);
+#	endif
 
 	esbits_t endstop_change = live_state_local ^ old_live_state_local;
 #	define ES_REPORT_CHANGE(S) \
@@ -1192,6 +1240,13 @@ void Endstops::monitor() {
 		local_LED_status ^= 255;
 		old_live_state_local = live_state_local;
 	}
+
+	#	if HAS_A_MIN
+		ES_REPORT_CHANGE(A_MIN);
+#	endif
+#	if HAS_A_MAX
+		ES_REPORT_CHANGE(A_MAX);
+#	endif
 }
 
 #endif // PINS_DEBUGGING
diff --git a/src/marlin/module/endstops.h b/src/marlin/module/endstops.h
index dc94189317..152139fa9d 100644
--- a/src/marlin/module/endstops.h
+++ b/src/marlin/module/endstops.h
@@ -28,34 +28,15 @@
 #include "../inc/MarlinConfig.h"
 #include <stdint.h>
 
-enum EndstopEnum : char {
-  X_MIN       = 0,
-	Y_MIN       = 1,
-	Z_MIN       = 2,
-	Z_MIN_PROBE = 3,
-	WORK_PROBE  = 4,
-	TOOL_PROBE  = 5,
-	UNUSED			= 6,
-  X_MAX       = 7,
-	Y_MAX       = 8,
-	Z_MAX       = 9,
-  X2_MIN      = 10,
-	X2_MAX      = 11,
-  Y2_MIN      = 12,
-	Y2_MAX      = 13,
-  Z2_MIN      = 14,
-	Z2_MAX      = 15,
-  Z3_MIN      = 16,
-	Z3_MAX      = 17,
-  Z4_MIN      = 18,
-	Z4_MAX      = 19
-};
+#include <swordfish/types.h>
+
+#define X_ENDSTOP			(X_HOME_DIR < 0 ? EndstopValue::X_MIN : EndstopValue::X_MAX)
+#define X2_ENDSTOP		(X_HOME_DIR < 0 ? EndstopValue::X2_MIN : EndstopValue::X2_MAX)
+#define Y_ENDSTOP			(Y_HOME_DIR < 0 ? EndstopValue::Y_MIN : EndstopValue::Y_MAX)
+#define Y2_ENDSTOP		(Y_HOME_DIR < 0 ? EndstopValue::Y2_MIN : EndstopValue::Y2_MAX)
+#define Z_ENDSTOP			(Z_HOME_DIR < 0 ? EndstopValue::Z_MIN, EndstopValue::Z_MAX)
+#define A_ENDSTOP EndstopValue::A_MAX
 
-#define X_ENDSTOP			(X_HOME_DIR < 0 ? X_MIN : X_MAX)
-#define X2_ENDSTOP		(X_HOME_DIR < 0 ? X2_MIN : X2_MAX)
-#define Y_ENDSTOP			(Y_HOME_DIR < 0 ? Y_MIN : Y_MAX)
-#define Y2_ENDSTOP		(Y_HOME_DIR < 0 ? Y2_MIN : Y2_MAX)
-#define Z_ENDSTOP			(Z_HOME_DIR < 0 ? TERN(HOMING_Z_WITH_PROBE, Z_MIN, Z_MIN_PROBE) : Z_MAX)
 #define WORK_ENDSTOP	WORK_PROBE
 
 class Endstops {
@@ -174,23 +155,28 @@ class Endstops {
       static volatile bool z_probe_enabled;
       static void enable_z_probe(const bool onoff=true);
     #endif
-		
+
 		#if HAS_TOOL_PROBE
 			static bool tool_probe_enabled;
-			
+
 			static void enable_tool_probe(const bool enabled) {
 				tool_probe_enabled = enabled;
 			}
 		#endif
-		
+
 		#if HAS_WORK_PROBE
 			static xyz_bool_t work_probe_enabled;
-			
-			static void enable_work_probe(const AxisEnum axis, const bool enabled = true) {
+
+			static void enable_work_probe(const Axis axis, const bool enabled = true) {
 				work_probe_enabled[axis] = enabled;
 			}
 		#endif
 
+		static volatile bool a_home_enabled;
+		static void enable_a_home(const bool enabled) {
+			a_home_enabled = enabled;
+		}
+
     static void resync();
 
     // Debugging of endstops
diff --git a/src/marlin/module/motion.cpp b/src/marlin/module/motion.cpp
index 0e38916ff6..f1fb605f5a 100644
--- a/src/marlin/module/motion.cpp
+++ b/src/marlin/module/motion.cpp
@@ -30,7 +30,8 @@
 
 using namespace Eigen;
 
-//using namespace swordfish;
+using namespace swordfish;
+using namespace swordfish::math;
 using namespace swordfish::motion;
 using namespace swordfish::status;
 
@@ -104,7 +105,7 @@ bool relative_mode; // = false;
  *   Used by 'line_to_current_position' to do a move after changing it.
  *   Used by 'sync_plan_position' to update 'planner.position'.
  */
-xyz_pos_t current_position { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
+ Vector6f32 current_position { X_HOME_POS, Y_HOME_POS, Z_HOME_POS, 0, 0, 0 };
 
 /**
  * Cartesian Destination
@@ -112,7 +113,7 @@ xyz_pos_t current_position { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
  *   and expected by functions like 'prepare_line_to_destination'.
  *   G-codes can set destination using 'get_destination_from_command'
  */
-xyz_pos_t destination { 0, 0, 0 };
+Vector6f32 destination { 0, 0, 0, 0, 0, 0 };
 
 // G60/G61 Position Save and Return
 #if SAVED_POSITIONS
@@ -151,11 +152,11 @@ void reset_hotend_offsets() {
 // no other feedrate is specified. Overridden for special moves.
 // Set by the last G0 through G5 command's "F" parameter.
 // Functions that override this for custom moves *must always* restore it!
-feedRate_t feedrate_mm_s = MMM_TO_MMS(1500);
+FeedRate feedrate_mm_s = FeedRate::UnitsPerSecond(MMM_TO_MMS(homing_feedrate_mm_m.x));
 int16_t feedrate_percentage = 100;
 
 // Cartesian conversion result goes here:
-xyz_pos_t cartes;
+Vector6f32 cartes;
 
 #if IS_KINEMATIC
 
@@ -210,16 +211,16 @@ inline void report_more_positions() {
 }
 
 // Report the logical position for a given machine position
-inline void report_logical_position(const xyz_pos_t& rpos) {
-	const xyz_pos_t lpos = rpos.asLogical();
-	SERIAL_ECHOPAIR_P(X_LBL, lpos.x, SP_Y_LBL, lpos.y, SP_Z_LBL, lpos.z);
+inline void report_logical_position(const Vector6f32& rpos) {
+	const Vector6f32 lpos = toLogical(rpos);
+	SERIAL_ECHOPAIR_P(X_LBL, lpos.x(), SP_Y_LBL, lpos.y(), SP_Z_LBL, lpos.z());
 }
 
 // Report the real current position according to the steppers.
 // Forward kinematics and un-leveling are applied.
 void report_real_position() {
 	get_cartesian_from_steppers();
-	xyz_pos_t npos = cartes;
+	auto npos = cartes;
 
 #if HAS_POSITION_MODIFIERS
 	planner.unapply_modifiers(npos, true);
@@ -274,18 +275,9 @@ void sync_plan_position_e() {
  * suitable for current_position, etc.
  */
 void get_cartesian_from_steppers() {
-#if ENABLED(DELTA)
-	forward_kinematics_DELTA(planner.get_axis_positions_mm());
-#else
-#	if IS_SCARA
-	forward_kinematics_SCARA(
-			planner.get_axis_position_degrees(A_AXIS),
-			planner.get_axis_position_degrees(B_AXIS));
-#	else
-	cartes.set(planner.get_axis_position_mm(X_AXIS), planner.get_axis_position_mm(Y_AXIS));
-#	endif
-	cartes.z = planner.get_axis_position_mm(Z_AXIS);
-#endif
+	cartes.x() = planner.get_axis_position_mm(Axis::X());
+	cartes.y() = planner.get_axis_position_mm(Axis::Y());
+	cartes.z() = planner.get_axis_position_mm(Axis::Z());
 }
 
 /**
@@ -299,27 +291,23 @@ void get_cartesian_from_steppers() {
  * To keep hosts in sync, always call report_current_position
  * after updating the current_position.
  */
-void set_current_from_steppers_for_axis(const AxisEnum axis) {
+void set_current_from_steppers_for_axis(const AxisValue axis) {
 	get_cartesian_from_steppers();
-	xyze_pos_t pos = cartes;
-	pos.e = planner.get_axis_position_mm(E_AXIS);
-
-#if HAS_POSITION_MODIFIERS
-	planner.unapply_modifiers(pos, true);
-#endif
+	auto pos = cartes;
 
-	if (axis == ALL_AXES)
+	if (axis == AxisValue::All) {
 		current_position = pos;
-	else
-		current_position[axis] = pos[axis];
+	} else {
+		current_position[(u8)axis] = pos[(u8)axis];
+	}
 }
 
 /**
  * Move the planner to the current position from wherever it last moved
  * (or from wherever it has been told it is located).
  */
-void line_to_current_position(const MachineState machine_state, const feedRate_t& fr_mm_s /*=feedrate_mm_s*/) {
-	planner.buffer_line(current_position, fr_mm_s, active_extruder, machine_state);
+void line_to_current_position(const MachineState machine_state, const FeedRate& feed_rate /*=feedrate_mm_s*/) {
+	planner.buffer_line(current_position, feed_rate, active_extruder, machine_state);
 }
 
 #if EXTRUDERS
@@ -360,15 +348,15 @@ void prepare_fast_move_to_destination(const feedRate_t& scaled_fr_mm_s /*=MMS_SC
  *  - Move at normal speed regardless of feedrate percentage.
  *  - Extrude the specified length regardless of flow percentage.
  */
-void _internal_move_to_destination(const MachineState machine_state, const feedRate_t& fr_mm_s /*=0.0f*/
+void _internal_move_to_destination(const MachineState machine_state, const std::optional<FeedRate>& feed_rate
 #if IS_KINEMATIC
                                    ,
                                    const bool is_fast /*=false*/
 #endif
 ) {
-	const feedRate_t old_feedrate = feedrate_mm_s;
-	if (fr_mm_s)
-		feedrate_mm_s = fr_mm_s;
+	const auto old_feedrate = feedrate_mm_s;
+	if (feed_rate.has_value())
+		feedrate_mm_s = feed_rate.value();
 
 	const uint16_t old_pct = feedrate_percentage;
 	feedrate_percentage = 100;
@@ -395,148 +383,50 @@ void _internal_move_to_destination(const MachineState machine_state, const feedR
 /**
  * Plan a move to (X, Y, Z) and set the current_position
  */
-void do_blocking_move_to(const MachineState machine_state, const float rx, const float ry, const float rz, const feedRate_t& fr_mm_s /*=0.0*/) {
+void do_blocking_move_to(const MachineState machine_state, const Vector6f32& target, const std::optional<FeedRate>& feed_rate) {
 	DEBUG_SECTION(log_move, "do_blocking_move_to", DEBUGGING(LEVELING));
 	if (DEBUGGING(LEVELING))
-		DEBUG_XYZ("> ", rx, ry, rz);
-
-	const feedRate_t z_feedrate = fr_mm_s ?: homing_feedrate(Z_AXIS),
-									 xy_feedrate = fr_mm_s ?: feedRate_t(XY_PROBE_FEEDRATE_MM_S);
-
-#if ENABLED(DELTA)
-
-	if (!position_is_reachable(rx, ry))
-		return;
-
-	REMEMBER(fr, feedrate_mm_s, xy_feedrate);
-
-	destination = current_position; // sync destination at the start
-
-	if (DEBUGGING(LEVELING))
-		DEBUG_POS("destination = current_position", destination);
-
-	// when in the danger zone
-	if (current_position.z > delta_clip_start_height) {
-		if (rz > delta_clip_start_height) { // staying in the danger zone
-			destination.set(rx, ry, rz); // move directly (uninterpolated)
-			prepare_internal_fast_move_to_destination(); // set current_position from destination
-			if (DEBUGGING(LEVELING))
-				DEBUG_POS("danger zone move", current_position);
-			return;
-		}
-		destination.z = delta_clip_start_height;
-		prepare_internal_fast_move_to_destination(); // set current_position from destination
-		if (DEBUGGING(LEVELING))
-			DEBUG_POS("zone border move", current_position);
-	}
-
-	if (rz > current_position.z) { // raising?
-		destination.z = rz;
-		prepare_internal_fast_move_to_destination(z_feedrate); // set current_position from destination
-		if (DEBUGGING(LEVELING))
-			DEBUG_POS("z raise move", current_position);
-	}
-
-	destination.set(rx, ry);
-	prepare_internal_move_to_destination(); // set current_position from destination
-	if (DEBUGGING(LEVELING))
-		DEBUG_POS("xy move", current_position);
-
-	if (rz < current_position.z) { // lowering?
-		destination.z = rz;
-		prepare_internal_fast_move_to_destination(z_feedrate); // set current_position from destination
-		if (DEBUGGING(LEVELING))
-			DEBUG_POS("z lower move", current_position);
-	}
-
-#elif IS_SCARA
+		DEBUG_XYZ("> ", target.x(), target.y(), target.z());
 
-	if (!position_is_reachable(rx, ry))
-		return;
-
-	destination = current_position;
+	const FeedRate z_feedrate = feed_rate ? *feed_rate : feedrate_mm_s;
+	const FeedRate xy_feedrate = feed_rate ? *feed_rate : feedrate_mm_s;
 
 	// If Z needs to raise, do it before moving XY
-	if (destination.z < rz) {
-		destination.z = rz;
-		prepare_internal_fast_move_to_destination(z_feedrate);
-	}
-
-	destination.set(rx, ry);
-	prepare_internal_fast_move_to_destination(xy_feedrate);
-
-	// If Z needs to lower, do it after moving XY
-	if (destination.z > rz) {
-		destination.z = rz;
-		prepare_internal_fast_move_to_destination(z_feedrate);
-	}
-
-#else
+	if (current_position.z() < target.z()) {
+		current_position.z() = target.z();
 
-	// If Z needs to raise, do it before moving XY
-	if (current_position.z < rz) {
-		current_position.z = rz;
 		line_to_current_position(machine_state, z_feedrate);
 	}
 
-	current_position.set(rx, ry);
+	current_position.x() = target.x();
+	current_position.y() = target.y();
+	current_position.a() = target.a();
+
 	line_to_current_position(machine_state, xy_feedrate);
 
 	// If Z needs to lower, do it after moving XY
-	if (current_position.z > rz) {
-		current_position.z = rz;
+	if (current_position.z() > target.z()) {
+		current_position.z() = target.z();
+
 		line_to_current_position(machine_state, z_feedrate);
 	}
 
-#endif
-
 	planner.synchronize();
 }
 
-void do_blocking_move_to(const MachineState machine_state, const xy_pos_t& raw, const feedRate_t& fr_mm_s /*=0.0f*/) {
-	do_blocking_move_to(machine_state, raw.x, raw.y, current_position.z, fr_mm_s);
-}
-void do_blocking_move_to(const MachineState machine_state, const xyz_pos_t& raw, const feedRate_t& fr_mm_s /*=0.0f*/) {
-	do_blocking_move_to(machine_state, raw.x, raw.y, raw.z, fr_mm_s);
-}
-void do_blocking_move_to(const MachineState machine_state, const xyze_pos_t& raw, const feedRate_t& fr_mm_s /*=0.0f*/) {
-	do_blocking_move_to(machine_state, raw.x, raw.y, raw.z, fr_mm_s);
-}
-
-void do_blocking_move_to_x(const MachineState machine_state, const float& rx, const feedRate_t& fr_mm_s /*=0.0*/) {
-	do_blocking_move_to(machine_state, rx, current_position.y, current_position.z, fr_mm_s);
-}
-void do_blocking_move_to_y(const MachineState machine_state, const float& ry, const feedRate_t& fr_mm_s /*=0.0*/) {
-	do_blocking_move_to(machine_state, current_position.x, ry, current_position.z, fr_mm_s);
-}
-void do_blocking_move_to_z(const MachineState machine_state, const float& rz, const feedRate_t& fr_mm_s /*=0.0*/) {
-	do_blocking_move_to_xy_z(machine_state, current_position, rz, fr_mm_s);
-}
-
-void do_blocking_move_to_xy(const MachineState machine_state, const float& rx, const float& ry, const feedRate_t& fr_mm_s /*=0.0*/) {
-	do_blocking_move_to(machine_state, rx, ry, current_position.z, fr_mm_s);
-}
-void do_blocking_move_to_xy(const MachineState machine_state, const xy_pos_t& raw, const feedRate_t& fr_mm_s /*=0.0f*/) {
-	do_blocking_move_to_xy(machine_state, raw.x, raw.y, fr_mm_s);
-}
-
-void do_blocking_move_to_xy_z(const MachineState machine_state, const xy_pos_t& raw, const float& z, const feedRate_t& fr_mm_s /*=0.0f*/) {
-	do_blocking_move_to(machine_state, raw.x, raw.y, z, fr_mm_s);
-}
-
 void do_z_clearance(const MachineState machine_state, const float& zclear, const bool z_trusted /*=true*/, const bool raise_on_untrusted /*=true*/, const bool lower_allowed /*=false*/) {
 	const bool rel = raise_on_untrusted && !z_trusted;
-	float zdest = zclear + (rel ? current_position.z : 0.0f);
+	float zdest = zclear + (rel ? current_position.z() : 0.0f);
 	if (!lower_allowed)
-		NOLESS(zdest, current_position.z);
-	do_blocking_move_to_z(machine_state, _MIN(zdest, Z_MAX_POS), TERN(HAS_BED_PROBE, z_probe_fast_mm_s, homing_feedrate(Z_AXIS)));
+		NOLESS(zdest, current_position.z());
+	do_blocking_move_to_z(machine_state, _MIN(zdest, Z_MAX_POS), TERN(HAS_BED_PROBE, z_probe_fast_mm_s, homing_feedrate(Axis::Z())));
 }
 
 //
 // Prepare to do endstop or probe moves with custom feedrates.
 //  - Save / restore current feedrate and multiplier
 //
-static float saved_feedrate_mm_s;
+static FeedRate saved_feedrate_mm_s = FeedRate::UnitsPerSecond(0);
 static int16_t saved_feedrate_percentage;
 void remember_feedrate_and_scaling() {
 	saved_feedrate_mm_s = feedrate_mm_s;
@@ -767,11 +657,18 @@ inline void segmented_line_to_destination(const feedRate_t& fr_mm_s, const float
  * Return true if 'current_position' was set to 'destination'
  */
 inline bool line_to_destination_cartesian(const swordfish::status::MachineState machine_state, const float32_t accel_mm_s2) {
-	const float scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s);
+FeedRate scaled_feed_rate = feedrate_mm_s;
+
+	if (parser.feedrate_type == FeedRateType::UnitsPerSecond) {
+	  //scaled_feed_rate = MMS_SCALED(feedrate_mm_s);
+
+		debug()("unscaled feed rate: ", feedrate_mm_s.value());
+		debug()("scaled feed rate: ", scaled_feed_rate.value());
+	}
 
 	planner.buffer_line(
 			destination,
-			scaled_fr_mm_s,
+			scaled_feed_rate,
 			active_extruder,
 			machine_state,
 			0.0,
@@ -925,10 +822,14 @@ void prepare_line_to_destination(const swordfish::status::MachineState machine_s
 	auto& motionModule = MotionModule::getInstance();
 	auto& limits = motionModule.getLimits();
 
-	Eigen::Vector3f dest = { destination.x, destination.y, destination.z };
+	Vector3f32 dest = { destination.x(), destination.y(), destination.z() };
 
 	limits.throwIfOutside(dest);
 
+	debug()("current_position x: ", current_position.x(), ", y: ", current_position.y(), ", z: ", current_position.z(), ", a: ", current_position.a(), ", b: ", current_position.b(), ", c: ", current_position.c());
+	debug()("destination x: ", destination.x(), ", y: ", destination.y(), ", z: ", destination.z(), ", a: ", destination.a(), ", b: ", destination.b(), ", c: ", destination.c());
+
+
 	if (line_to_destination_cartesian(machine_state, accel_mm_s2)) {
 		return;
 	}
@@ -939,12 +840,14 @@ void prepare_line_to_destination(const swordfish::status::MachineState machine_s
 uint8_t axes_should_home(uint8_t axis_bits /*=0x07*/) {
 #define SHOULD_HOME(A) TERN(HOME_AFTER_DEACTIVATE, axis_is_trusted, axis_was_homed)(A)
 	// Clear test bits that are trusted
-	if (TEST(axis_bits, X_AXIS) && SHOULD_HOME(X_AXIS))
-		CBI(axis_bits, X_AXIS);
-	if (TEST(axis_bits, Y_AXIS) && SHOULD_HOME(Y_AXIS))
-		CBI(axis_bits, Y_AXIS);
-	if (TEST(axis_bits, Z_AXIS) && SHOULD_HOME(Z_AXIS))
-		CBI(axis_bits, Z_AXIS);
+	if (TEST(axis_bits, Axis::X()) && SHOULD_HOME(Axis::X()))
+		CBI(axis_bits, Axis::X());
+	if (TEST(axis_bits, Axis::Y()) && SHOULD_HOME(Axis::Y()))
+		CBI(axis_bits, Axis::Y());
+	if (TEST(axis_bits, Axis::Z()) && SHOULD_HOME(Axis::Z()))
+		CBI(axis_bits, Axis::Z());
+	if (TEST(axis_bits, Axis::A()) && SHOULD_HOME(Axis::A()))
+		CBI(axis_bits, Axis::A());
 	return axis_bits;
 }
 
@@ -958,11 +861,7 @@ bool homing_needed_error(uint8_t axis_bits /*=0x07*/) {
 /**
  * Homing bump feedrate (mm/s)
  */
-feedRate_t get_homing_bump_feedrate(const AxisEnum axis) {
-#if HOMING_Z_WITH_PROBE
-	if (axis == Z_AXIS)
-		return MMM_TO_MMS(Z_PROBE_SPEED_SLOW);
-#endif
+FeedRate get_homing_bump_feedrate(const Axis axis) {
 	static const uint8_t homing_bump_divisor[] PROGMEM = HOMING_BUMP_DIVISOR;
 	uint8_t hbd = pgm_read_byte(&homing_bump_divisor[axis]);
 	if (hbd < 1) {
@@ -972,6 +871,7 @@ feedRate_t get_homing_bump_feedrate(const AxisEnum axis) {
 	return homing_feedrate(axis) / float(hbd);
 }
 
+
 #if ENABLED(SENSORLESS_HOMING)
 /**
  * Set sensorless homing if the axis has it, accounting for Core Kinematics.
@@ -1129,24 +1029,14 @@ void end_sensorless_homing_per_axis(const AxisEnum axis, sensorless_t enable_ste
 /**
  * Home an individual linear axis
  */
-void do_homing_move(const AxisEnum axis, const float distance, const feedRate_t fr_mm_s = 0.0, [[maybe_unused]] const bool final_approach = true) {
+void do_homing_move(const Axis axis, const float distance, const std::optional<FeedRate> feed_rate = std::nullopt, [[maybe_unused]] const bool final_approach = true) {
 	DEBUG_SECTION(log_move, "do_homing_move", DEBUGGING(LEVELING));
 
-	const feedRate_t home_fr_mm_s = fr_mm_s ?: homing_feedrate(axis);
+	const FeedRate homing_feed_rate = feed_rate ? *feed_rate : homing_feedrate(axis);
 
-	if (DEBUGGING(LEVELING)) {
-		DEBUG_ECHOPAIR("...(", axis_codes[axis], ", ", distance, ", ");
-		if (fr_mm_s)
-			DEBUG_ECHO(fr_mm_s);
-		else
-			DEBUG_ECHOPAIR("[", home_fr_mm_s, "]");
-		DEBUG_ECHOLNPGM(")");
-	}
 
 	// Only do some things when moving towards an endstop
-	const int8_t axis_home_dir = TERN0(DUAL_X_CARRIAGE, axis == X_AXIS)
-	                                 ? x_home_dir(active_extruder)
-	                                 : home_dir(axis);
+	const int8_t axis_home_dir = home_dir(axis);
 	const bool is_home_dir = (axis_home_dir > 0) == (distance > 0);
 
 #if ENABLED(SENSORLESS_HOMING)
@@ -1176,7 +1066,7 @@ void do_homing_move(const AxisEnum axis, const float distance, const feedRate_t
 	line_to_current_position(home_fr_mm_s);
 #else
 	// Get the ABC or XYZ positions in mm
-	abce_pos_t target = planner.get_axis_positions_mm();
+	Vector6f32 target = planner.get_axis_positions_mm();
 
 	target[axis] = 0; // Set the single homing axis to 0
 	planner.set_machine_position_mm(target); // Update the machine position
@@ -1193,7 +1083,7 @@ void do_homing_move(const AxisEnum axis, const float distance, const feedRate_t
 	                       cart_dist_mm
 #	endif
 	                       ,
-	                       home_fr_mm_s, active_extruder, MachineState::Homing);
+	                       homing_feed_rate, active_extruder, MachineState::Homing);
 #endif
 
 	planner.synchronize();
@@ -1230,7 +1120,7 @@ void do_homing_move(const AxisEnum axis, const float distance, const feedRate_t
  *
  * Callers must sync the planner position after calling this!
  */
-void set_axis_is_at_home(const AxisEnum axis) {
+void set_axis_is_at_home(const Axis axis) {
 	auto& motionManager = MotionModule::getInstance();
 
 	if (DEBUGGING(LEVELING))
@@ -1299,7 +1189,7 @@ void set_axis_is_at_home(const AxisEnum axis) {
 /**
  * Set an axis to be unhomed.
  */
-void set_axis_never_homed(const AxisEnum axis) {
+void set_axis_never_homed(const Axis axis) {
 	if (DEBUGGING(LEVELING))
 		DEBUG_ECHOLNPAIR(">>> set_axis_never_homed(", axis_codes[axis], ")");
 
@@ -1404,20 +1294,48 @@ void backout_to_tmc_homing_phase(const AxisEnum axis) {
  * before updating the current position.
  */
 
-void homeaxis(const AxisEnum axis) {
+void reference_rotary_axis(const Axis axis) {
+	if (axis.is_linear()) {
+		return;
+	}
+
+	const float move_length = 360;
+	const float bump = 3;
+	const float rebump = bump * 2;
+
+	debug()("approach");
+	endstops.enable_a_home(true);
+	do_homing_move(axis, move_length, homing_feedrate(axis), true);
+	endstops.enable_a_home(false);
+
+	debug()("bump");
+	do_homing_move(axis, -bump, homing_feedrate(axis), false);
+
+	debug()("final");
+	endstops.enable_a_home(true);
+	do_homing_move(axis, rebump, get_homing_bump_feedrate(axis), true);
+	endstops.enable_a_home(false);
+
+	set_axis_is_at_home(axis);
+	sync_plan_position();
+
+	destination[axis] = current_position[axis];
+}
+
+void reference_linear_axis(const Axis axis) {
 	/*#if ENABLED(TMC_DEBUG)
 	  REMEMBER(tmc_serial_port,report_tmc_status_serial_port,1);
 	  REMEMBER(tmc_interval,report_tmc_status_interval,10);
 	#endif
 	*/
 
-#define _CAN_HOME(A) (axis == _AXIS(A) && (ENABLED(A##_SPI_SENSORLESS) || (_AXIS(A) == Z_AXIS && ENABLED(HOMING_Z_WITH_PROBE)) || (A##_MIN_PIN > -1 && A##_HOME_DIR < 0) || (A##_MAX_PIN > -1 && A##_HOME_DIR > 0)))
+	#define _CAN_HOME(A) (axis == _AXIS(A) && (ENABLED(A##_SPI_SENSORLESS) || (_AXIS(A) == Axis::Z() && ENABLED(HOMING_Z_WITH_PROBE)) || (A##_MIN_PIN > -1 && A##_HOME_DIR < 0) || (A##_MAX_PIN > -1 && A##_HOME_DIR > 0)))
 
 	if (!_CAN_HOME(X) && !_CAN_HOME(Y) && !_CAN_HOME(Z))
 		return;
 
 	if (DEBUGGING(LEVELING))
-		DEBUG_ECHOLNPAIR(">>> homeaxis(", axis_codes[axis], ")");
+		DEBUG_ECHOLNPAIR(">>> homeaxis(", axis.to_char(), ")");
 
 	const int axis_home_dir = home_dir(axis);
 
@@ -1438,7 +1356,7 @@ void homeaxis(const AxisEnum axis) {
 		// Move away from the endstop by the axis HOMING_BUMP_MM
 		if (DEBUGGING(LEVELING))
 			DEBUG_ECHOLNPAIR("Move Away: ", -bump, "mm");
-		do_homing_move(axis, -bump, TERN0(HOMING_Z_WITH_PROBE, axis == Z_AXIS) ? MMM_TO_MMS(Z_PROBE_SPEED_FAST) : 0, false);
+		do_homing_move(axis, -bump, homing_feedrate(axis), false);
 
 #if ENABLED(DETECT_BROKEN_ENDSTOP)
 		// Check for a broken endstop
@@ -1456,15 +1374,15 @@ void homeaxis(const AxisEnum axis) {
 				break;
 		}
 		if (TEST(endstops.state(), es)) {
-			SERIAL_ECHO_MSG("Bad ", axis_codes[axis], " Endstop?");
+			SERIAL_ECHO_MSG("Bad ", axis.to_char(), " Endstop?");
 			kill(GET_TEXT(MSG_KILL_HOMING_FAILED));
 		}
 #endif
 
-		switch (axis) {
-			TERN_(X_DUAL_ENDSTOPS, case X_AXIS:)
-			TERN_(Y_DUAL_ENDSTOPS, case Y_AXIS:)
-			TERN_(Z_MULTI_ENDSTOPS, case Z_AXIS:)
+		switch (axis.value()) {
+			TERN_(X_DUAL_ENDSTOPS, case AxisValue::X:)
+			TERN_(Y_DUAL_ENDSTOPS, case AxisValue::Y:)
+			TERN_(Z_MULTI_ENDSTOPS, case AxisValue::Z:)
 			stepper.set_separate_multi_axis(true);
 			default:
 				break;
@@ -1476,10 +1394,10 @@ void homeaxis(const AxisEnum axis) {
 			DEBUG_ECHOLNPAIR("Re-bump: ", rebump, "mm");
 		do_homing_move(axis, rebump, get_homing_bump_feedrate(axis), true);
 
-		switch (axis) {
-			TERN_(X_DUAL_ENDSTOPS, case X_AXIS:)
-			TERN_(Y_DUAL_ENDSTOPS, case Y_AXIS:)
-			TERN_(Z_MULTI_ENDSTOPS, case Z_AXIS:)
+		switch (axis.value()) {
+			TERN_(X_DUAL_ENDSTOPS, case AxisValue::X:)
+			TERN_(Y_DUAL_ENDSTOPS, case AxisValue::Y:)
+			TERN_(Z_MULTI_ENDSTOPS, case AxisValue::Z:)
 			stepper.set_separate_multi_axis(false);
 			default:
 				break;
@@ -1491,10 +1409,10 @@ void homeaxis(const AxisEnum axis) {
 
 #if HAS_EXTRA_ENDSTOPS
 	// Set flags for X, Y, Z motor locking
-	switch (axis) {
-		TERN_(X_DUAL_ENDSTOPS, case X_AXIS:)
-		TERN_(Y_DUAL_ENDSTOPS, case Y_AXIS:)
-		TERN_(Z_MULTI_ENDSTOPS, case Z_AXIS:)
+	switch (axis.value()) {
+		TERN_(X_DUAL_ENDSTOPS, case AxisValue::X:)
+		TERN_(Y_DUAL_ENDSTOPS, case AxisValue::Y:)
+		TERN_(Z_MULTI_ENDSTOPS, case AxisValue::Z:)
 		stepper.set_separate_multi_axis(true);
 		default:
 			break;
@@ -1516,7 +1434,7 @@ void homeaxis(const AxisEnum axis) {
 	}
 #	endif
 #	if ENABLED(Y_DUAL_ENDSTOPS)
-	if (axis == Y_AXIS) {
+	if (axis == Axis::Y()) {
 		const float adj = ABS(endstops.y2_endstop_adj);
 		if (adj) {
 			if (pos_dir ? (endstops.y2_endstop_adj > 0) : (endstops.y2_endstop_adj < 0))
@@ -1638,12 +1556,12 @@ void homeaxis(const AxisEnum axis) {
 #	endif
 
 	// Reset flags for X, Y, Z motor locking
-	switch (axis) {
+	switch (axis.value()) {
 		default:
 			break;
-			TERN_(X_DUAL_ENDSTOPS, case X_AXIS:)
-			TERN_(Y_DUAL_ENDSTOPS, case Y_AXIS:)
-			TERN_(Z_MULTI_ENDSTOPS, case Z_AXIS:)
+			TERN_(X_DUAL_ENDSTOPS, case AxisValue::X:)
+			TERN_(Y_DUAL_ENDSTOPS, case AxisValue::Y:)
+			TERN_(Z_MULTI_ENDSTOPS, case AxisValue::Z:)
 			stepper.set_separate_multi_axis(false);
 	}
 #endif
@@ -1662,29 +1580,53 @@ void homeaxis(const AxisEnum axis) {
 		DEBUG_POS("> AFTER set_axis_is_at_home", current_position);
 
 	if (DEBUGGING(LEVELING))
-		DEBUG_ECHOLNPAIR("<<< homeaxis(", axis_codes[axis], ")");
-} // homeaxis()
+		DEBUG_ECHOLNPAIR("<<< homeaxis(", axis.to_char(), ")");
+}
 
-float32_t toNative(float32_t value, AxisEnum axis) {
+float32_t toNative(const float32_t value, Axis axis) {
 	return MotionModule::getInstance().toNative(axis, value);
 }
 
-float32_t toLogical(float32_t value, AxisEnum axis) {
+float32_t toLogical(const float32_t value, Axis axis) {
 	return MotionModule::getInstance().toLogical(axis, value);
 }
 
-void toLogical(xy_pos_t& raw) {
-	MotionModule::getInstance().toLogical(raw);
+Vector2f32 toLogical(const Vector2f32& raw) {
+	return MotionModule::getInstance().toLogical(raw);
+}
+
+Vector3f32 toLogical(const Vector3f32& raw) {
+	return MotionModule::getInstance().toLogical(raw);
+}
+
+Vector4f32 toLogical(const Vector4f32& raw) {
+	return  MotionModule::getInstance().toLogical(raw);
+}
+
+Vector5f32 toLogical(const Vector5f32& raw) {
+	return MotionModule::getInstance().toLogical(raw);
+}
+
+Vector6f32 toLogical(const Vector6f32& raw) {
+	return MotionModule::getInstance().toLogical(raw);
 }
 
-void toLogical(xyz_pos_t& raw) {
-	MotionModule::getInstance().toLogical(raw);
+Vector2f32 toNative(const Vector2f32& raw) {
+	return MotionModule::getInstance().toNative(raw);
 }
 
-void toNative(xy_pos_t& raw) {
-	MotionModule::getInstance().toNative(raw);
+Vector3f32 toNative(const Vector3f32& raw) {
+	return MotionModule::getInstance().toNative(raw);
 }
 
-void toNative(xyz_pos_t& raw) {
-	MotionModule::getInstance().toNative(raw);
+Vector4f32 toNative(const Vector4f32& raw) {
+	return MotionModule::getInstance().toNative(raw);
 }
+
+Vector5f32 toNative(const Vector5f32& raw) {
+	return MotionModule::getInstance().toNative(raw);
+}
+
+Vector6f32 toNative(const Vector6f32& raw) {
+	return MotionModule::getInstance().toNative(raw);
+}
\ No newline at end of file
diff --git a/src/marlin/module/motion.h b/src/marlin/module/motion.h
index 4720eefbfb..ed772583c1 100644
--- a/src/marlin/module/motion.h
+++ b/src/marlin/module/motion.h
@@ -41,8 +41,8 @@ constexpr float fslop = 0.0001;
 
 extern bool relative_mode;
 
-extern xyz_pos_t current_position,  // High-level current tool position
-                  destination;       // Destination for a move
+extern swordfish::math::Vector6f32 current_position;  // High-level current tool position
+extern swordfish::math::Vector6f32 destination;       // Destination for a move
 
 // G60/G61 Position Save and Return
 #if SAVED_POSITIONS
@@ -51,7 +51,7 @@ extern xyz_pos_t current_position,  // High-level current tool position
 #endif
 
 // Scratch space for a cartesian result
-extern xyz_pos_t cartes;
+extern swordfish::math::Vector6f32 cartes;
 
 // Until kinematics.cpp is created, declare this here
 #if IS_KINEMATIC
@@ -70,7 +70,7 @@ extern xyz_pos_t cartes;
 constexpr feedRate_t z_probe_fast_mm_s = MMM_TO_MMS(Z_PROBE_SPEED_FAST);
 
 // forward decl
-FORCE_INLINE bool axis_is_trusted(const AxisEnum axis);
+FORCE_INLINE bool axis_is_trusted(const Axis axis);
 
 /**
  * Feed rates are often configured with mm/m
@@ -78,30 +78,26 @@ FORCE_INLINE bool axis_is_trusted(const AxisEnum axis);
  */
 
 constexpr xyz_feedrate_t homing_feedrate_mm_m = HOMING_FEEDRATE_MM_M;
-FORCE_INLINE feedRate_t homing_feedrate(const AxisEnum a) {
+FORCE_INLINE swordfish::motion::FeedRate homing_feedrate(const Axis a) {
 	float v;
-	#if ENABLED(DELTA)
-	v = homing_feedrate_mm_m.z;
-	#else
 
-		switch (a) {
-			case X_AXIS: v = homing_feedrate_mm_m.x; break;
-			case Y_AXIS: v = homing_feedrate_mm_m.y; break;
-			case Z_AXIS:
-			default: v = homing_feedrate_mm_m.z; break;
-		}
+	switch (a.value()) {
+		case AxisValue::X: v = homing_feedrate_mm_m.x; break;
+		case AxisValue::Y: v = homing_feedrate_mm_m.y; break;
+		case AxisValue::Z:
+		default: v = homing_feedrate_mm_m.z; break;
+	}
 
-	#endif
-
-	return MMM_TO_MMS(v);
+	return swordfish::motion::FeedRate::UnitsPerSecond(MMM_TO_MMS(v));
 }
 
-feedRate_t get_homing_bump_feedrate(const AxisEnum axis);
+swordfish::motion::FeedRate get_homing_bump_feedrate(const Axis axis);
+
 
 /**
  * The default feedrate for many moves, set by the most recent move
  */
-extern feedRate_t feedrate_mm_s;
+extern swordfish::motion::FeedRate feedrate_mm_s;
 
 /**
  * Feedrate scaling is applied to all G0/G1, G2/G3, and G5 moves
@@ -130,7 +126,7 @@ inline float pgm_read_any(const float *p)   { return TERN(__IMXRT1062__, *p, pgm
 inline int8_t pgm_read_any(const int8_t *p) { return TERN(__IMXRT1062__, *p, pgm_read_byte(p)); }
 
 #define XYZ_DEFS(T, NAME, OPT) \
-  inline T NAME(const AxisEnum axis) { \
+  inline T NAME(const Axis axis) { \
     static const XYZval<T> NAME##_P DEFS_PROGMEM = { X_##OPT, Y_##OPT, Z_##OPT }; \
     return pgm_read_any(&NAME##_P[axis]); \
   }
@@ -140,13 +136,13 @@ XYZ_DEFS(float, base_home_pos,  HOME_POS);
 XYZ_DEFS(float, max_length,     MAX_LENGTH);
 XYZ_DEFS(int8_t, home_dir, HOME_DIR);
 
-inline float home_bump_mm(const AxisEnum axis) {
-  static const xyz_pos_t home_bump_mm_P DEFS_PROGMEM = HOMING_BUMP_MM;
+inline float home_bump_mm(const Axis axis) {
+  static const swordfish::math::Vector6f32 home_bump_mm_P DEFS_PROGMEM = HOMING_BUMP_MM;
   return pgm_read_any(&home_bump_mm_P[axis]);
 }
 
 #if HAS_WORKSPACE_OFFSET
-  void update_workspace_offset(const AxisEnum axis);
+  void update_workspace_offset(const Axis axis);
 #else
   inline void update_workspace_offset(const AxisEnum) {}
 #endif
@@ -165,7 +161,7 @@ void report_current_position();
 void report_current_position_projected();
 
 void get_cartesian_from_steppers();
-void set_current_from_steppers_for_axis(const AxisEnum axis);
+void set_current_from_steppers_for_axis(const AxisValue axis);
 
 /**
  * sync_plan_position
@@ -180,7 +176,7 @@ void sync_plan_position_e();
  * Move the planner to the current position from wherever it last moved
  * (or from wherever it has been told it is located).
  */
-void line_to_current_position(const swordfish::status::MachineState machine_state, const feedRate_t &fr_mm_s=feedrate_mm_s);
+void line_to_current_position(const swordfish::status::MachineState machine_state, const std::optional<swordfish::motion::FeedRate> &feed_rate = std::nullopt);
 
 #if EXTRUDERS
   void unscaled_e_move(const float &length, const feedRate_t &fr_mm_s);
@@ -188,14 +184,14 @@ void line_to_current_position(const swordfish::status::MachineState machine_stat
 
 void prepare_line_to_destination(const swordfish::status::MachineState machine_state, const float32_t accel_mm_s2 = 0.0);
 
-void _internal_move_to_destination(const swordfish::status::MachineState machine_state, const feedRate_t &fr_mm_s=0.0f
+void _internal_move_to_destination(const swordfish::status::MachineState machine_state, const std::optional<swordfish::motion::FeedRate> &feed_rate = std::nullopt
   #if IS_KINEMATIC
     , const bool is_fast=false
   #endif
 );
 
-inline void prepare_internal_move_to_destination(const swordfish::status::MachineState machine_state, const feedRate_t &fr_mm_s=0.0f) {
-  _internal_move_to_destination(machine_state, fr_mm_s);
+inline void prepare_internal_move_to_destination(const swordfish::status::MachineState machine_state, const std::optional<swordfish::motion::FeedRate> &feed_rate = std::nullopt) {
+  _internal_move_to_destination(machine_state, feed_rate);
 }
 
 #if IS_KINEMATIC
@@ -209,23 +205,40 @@ inline void prepare_internal_move_to_destination(const swordfish::status::Machin
 /**
  * Blocking movement and shorthand functions
  */
-void do_blocking_move_to(const swordfish::status::MachineState machine_state, const float rx, const float ry, const float rz, const feedRate_t &fr_mm_s=0.0f);
-void do_blocking_move_to(const swordfish::status::MachineState machine_state, const xy_pos_t &raw, const feedRate_t &fr_mm_s=0.0f);
-void do_blocking_move_to(const swordfish::status::MachineState machine_state, const xyz_pos_t &raw, const feedRate_t &fr_mm_s=0.0f);
-void do_blocking_move_to(const swordfish::status::MachineState machine_state, const xyze_pos_t &raw, const feedRate_t &fr_mm_s=0.0f);
+void do_blocking_move_to(const swordfish::status::MachineState machine_state, const swordfish::math::Vector6f32 &raw, const std::optional<swordfish::motion::FeedRate> &feed_rate = std::nullopt);
+
+FORCE_INLINE void do_blocking_move_to_x(const swordfish::status::MachineState machine_state, const float &rx, const std::optional<swordfish::motion::FeedRate> &feed_rate = std::nullopt) {
+	auto target = current_position;
+
+	target.x() = rx;
+
+	do_blocking_move_to(machine_state, target, feed_rate);
+}
+
+FORCE_INLINE void do_blocking_move_to_y(const swordfish::status::MachineState machine_state, const float &ry, const std::optional<swordfish::motion::FeedRate> &feed_rate = std::nullopt) {
+	auto target = current_position;
 
-void do_blocking_move_to_x(const swordfish::status::MachineState machine_state, const float &rx, const feedRate_t &fr_mm_s=0.0f);
-void do_blocking_move_to_y(const swordfish::status::MachineState machine_state, const float &ry, const feedRate_t &fr_mm_s=0.0f);
-void do_blocking_move_to_z(const swordfish::status::MachineState machine_state, const float &rz, const feedRate_t &fr_mm_s=0.0f);
+	target.y() = ry;
 
-void do_blocking_move_to_xy(const swordfish::status::MachineState machine_state, const float &rx, const float &ry, const feedRate_t &fr_mm_s=0.0f);
-void do_blocking_move_to_xy(const swordfish::status::MachineState machine_state, const xy_pos_t &raw, const feedRate_t &fr_mm_s=0.0f);
-FORCE_INLINE void do_blocking_move_to_xy(const swordfish::status::MachineState machine_state, const xyz_pos_t &raw, const feedRate_t &fr_mm_s=0.0f)  { do_blocking_move_to_xy(machine_state, xy_pos_t(raw), fr_mm_s); }
-FORCE_INLINE void do_blocking_move_to_xy(const swordfish::status::MachineState machine_state, const xyze_pos_t &raw, const feedRate_t &fr_mm_s=0.0f) { do_blocking_move_to_xy(machine_state, xy_pos_t(raw), fr_mm_s); }
+	do_blocking_move_to(machine_state, target, feed_rate);
+}
+
+FORCE_INLINE void do_blocking_move_to_z(const swordfish::status::MachineState machine_state, const float &rz, const std::optional<swordfish::motion::FeedRate> &feed_rate = std::nullopt) {
+	auto target = current_position;
+
+	target.z() = rz;
+
+	do_blocking_move_to(machine_state, target, feed_rate);
+}
+
+FORCE_INLINE void do_blocking_move_to_a(const swordfish::status::MachineState machine_state, const float &ra, const std::optional<swordfish::motion::FeedRate> &feed_rate = std::nullopt) {
+	auto target = current_position;
+
+	target.a() = ra;
+
+	do_blocking_move_to(machine_state, target, feed_rate);
+}
 
-void do_blocking_move_to_xy_z(const swordfish::status::MachineState machine_state, const xy_pos_t &raw, const float &z, const feedRate_t &fr_mm_s=0.0f);
-FORCE_INLINE void do_blocking_move_to_xy_z(const swordfish::status::MachineState machine_state, const xyz_pos_t &raw, const float &z, const feedRate_t &fr_mm_s=0.0f)  { do_blocking_move_to_xy_z(machine_state, xy_pos_t(raw), z, fr_mm_s); }
-FORCE_INLINE void do_blocking_move_to_xy_z(const swordfish::status::MachineState machine_state, const xyze_pos_t &raw, const float &z, const feedRate_t &fr_mm_s=0.0f) { do_blocking_move_to_xy_z(machine_state, xy_pos_t(raw), z, fr_mm_s); }
 
 void remember_feedrate_and_scaling();
 void remember_feedrate_scaling_off();
@@ -236,26 +249,27 @@ void do_z_clearance(const swordfish::status::MachineState machine_state, const f
 /**
  * Homing and Trusted Axes
  */
-constexpr uint8_t xyz_bits = _BV(X_AXIS) | _BV(Y_AXIS) | _BV(Z_AXIS);
+constexpr uint8_t xyz_bits = _BV(Axis::X()) | _BV(Axis::Y()) | _BV(Axis::Z());
 extern uint8_t axis_homed, axis_trusted;
 
-void homeaxis(const AxisEnum axis);
-void set_axis_is_at_home(const AxisEnum axis);
-void set_axis_never_homed(const AxisEnum axis);
+void reference_linear_axis(const Axis axis);
+void reference_rotary_axis(const Axis axis);
+void set_axis_is_at_home(const Axis axis);
+void set_axis_never_homed(const Axis axis);
 uint8_t axes_should_home(uint8_t axis_bits=0x07);
 bool homing_needed_error(uint8_t axis_bits=0x07);
 
-FORCE_INLINE bool axis_was_homed(const AxisEnum axis)     { return TEST(axis_homed, axis); }
-FORCE_INLINE bool axis_is_trusted(const AxisEnum axis)    { return TEST(axis_trusted, axis); }
-FORCE_INLINE bool axis_should_home(const AxisEnum axis)   { return (axes_should_home() & _BV(axis)) != 0; }
+FORCE_INLINE bool axis_was_homed(const Axis axis)     { return TEST(axis_homed, axis); }
+FORCE_INLINE bool axis_is_trusted(const Axis axis)    { return TEST(axis_trusted, axis); }
+FORCE_INLINE bool axis_should_home(const Axis axis)   { return (axes_should_home() & _BV(axis)) != 0; }
 FORCE_INLINE bool no_axes_homed()                         { return !axis_homed; }
 FORCE_INLINE bool all_axes_homed()                        { return xyz_bits == (axis_homed & xyz_bits); }
 FORCE_INLINE bool homing_needed()                         { return !all_axes_homed(); }
 FORCE_INLINE bool all_axes_trusted()                      { return xyz_bits == (axis_trusted & xyz_bits); }
-FORCE_INLINE void set_axis_homed(const AxisEnum axis)     { SBI(axis_homed, axis); }
-FORCE_INLINE void set_axis_unhomed(const AxisEnum axis)   { CBI(axis_homed, axis); }
-FORCE_INLINE void set_axis_trusted(const AxisEnum axis)   { SBI(axis_trusted, axis); }
-FORCE_INLINE void set_axis_untrusted(const AxisEnum axis) { CBI(axis_trusted, axis); }
+FORCE_INLINE void set_axis_homed(const Axis axis)     { SBI(axis_homed, axis); }
+FORCE_INLINE void set_axis_unhomed(const Axis axis)   { CBI(axis_homed, axis); }
+FORCE_INLINE void set_axis_trusted(const Axis axis)   { SBI(axis_trusted, axis); }
+FORCE_INLINE void set_axis_untrusted(const Axis axis) { CBI(axis_trusted, axis); }
 FORCE_INLINE void set_all_homed()                         { axis_homed = axis_trusted = xyz_bits; }
 FORCE_INLINE void set_all_unhomed()                       { axis_homed = axis_trusted = 0; }
 
@@ -270,7 +284,6 @@ FORCE_INLINE void set_all_unhomed()                       { axis_homed = axis_tr
 /**
  * Workspace offsets
  */
-#if HAS_HOME_OFFSET || HAS_POSITION_SHIFT || HAS_TOOL_OFFSET
   #if HAS_HOME_OFFSET
     //extern xyz_pos_t home_offset;
   #endif
@@ -287,24 +300,19 @@ FORCE_INLINE void set_all_unhomed()                       { axis_homed = axis_tr
   #else
     #define _WS position_shift
   #endif
-	float32_t toLogical(float32_t value, AxisEnum axis);
-  float32_t toNative(float32_t value, AxisEnum axis);
-  void toLogical(xy_pos_t &raw);
-  void toLogical(xyz_pos_t &raw);
-  //FORCE_INLINE void toLogical(xyze_pos_t &raw) { swordfish::controller.motionManager().toLogical(raw); }
-  void toNative(xy_pos_t &raw);
-  void toNative(xyz_pos_t &raw);
-  //FORCE_INLINE void toNative(xyze_pos_t &raw)  { swordfish::controller.motionManager().toNative(raw); }
-#else
-  #define toLogical(POS, AXIS) (POS)
-  #define toNative(POS, AXIS) (POS)
-  FORCE_INLINE void toLogical(xy_pos_t&)   {}
-  FORCE_INLINE void toLogical(xyz_pos_t&)  {}
-  FORCE_INLINE void toLogical(xyze_pos_t&) {}
-  FORCE_INLINE void toNative(xy_pos_t&)    {}
-  FORCE_INLINE void toNative(xyz_pos_t&)   {}
-  FORCE_INLINE void toNative(xyze_pos_t&)  {}
-#endif
+	f32 toLogical(const f32 value, Axis axis);
+  f32 toNative(const f32 value, Axis axis);
+  swordfish::math::Vector2f32 toLogical(const swordfish::math::Vector2f32 &raw);
+  swordfish::math::Vector3f32 toLogical(const swordfish::math::Vector3f32 &raw);
+	swordfish::math::Vector4f32 toLogical(const swordfish::math::Vector4f32 &raw);
+	swordfish::math::Vector5f32 toLogical(const swordfish::math::Vector5f32 &raw);
+	swordfish::math::Vector6f32 toLogical(const swordfish::math::Vector6f32 &raw);
+  swordfish::math::Vector2f32 toNative(const swordfish::math::Vector2f32 &raw);
+  swordfish::math::Vector3f32 toNative(const swordfish::math::Vector3f32 &raw);
+	swordfish::math::Vector4f32 toNative(const swordfish::math::Vector4f32 &raw);
+	swordfish::math::Vector5f32 toNative(const swordfish::math::Vector5f32 &raw);
+	swordfish::math::Vector6f32 toNative(const swordfish::math::Vector6f32 &raw);
+
 #define LOGICAL_X_POSITION(POS) NATIVE_TO_LOGICAL(POS, X_AXIS)
 #define LOGICAL_Y_POSITION(POS) NATIVE_TO_LOGICAL(POS, Y_AXIS)
 #define LOGICAL_Z_POSITION(POS) NATIVE_TO_LOGICAL(POS, Z_AXIS)
@@ -404,16 +412,16 @@ FORCE_INLINE void set_all_unhomed()                       { axis_homed = axis_tr
     FORCE_INLINE void set_duplication_enabled(const bool dupe) { extruder_duplication_enabled = dupe; }
   #endif
 
-  FORCE_INLINE int x_home_dir(const uint8_t) { return home_dir(X_AXIS); }
+  FORCE_INLINE int x_home_dir(const uint8_t) { return home_dir(Axis::X()); }
 
 #endif
 
 #if HAS_M206_COMMAND
-  void set_home_offset(const AxisEnum axis, const float v);
+  void set_home_offset(const Axis axis, const float v);
 #endif
 
 #if USE_SENSORLESS
   struct sensorless_t;
-  sensorless_t start_sensorless_homing_per_axis(const AxisEnum axis);
-  void end_sensorless_homing_per_axis(const AxisEnum axis, sensorless_t enable_stealth);
+  sensorless_t start_sensorless_homing_per_axis(const Axis axis);
+  void end_sensorless_homing_per_axis(const Axis axis, sensorless_t enable_stealth);
 #endif
diff --git a/src/marlin/module/planner.cpp b/src/marlin/module/planner.cpp
index e46e47114a..545b710815 100644
--- a/src/marlin/module/planner.cpp
+++ b/src/marlin/module/planner.cpp
@@ -67,6 +67,8 @@
 
 using namespace swordfish;
 using namespace swordfish::estop;
+using namespace swordfish::math;
+using namespace swordfish::motion;
 using namespace swordfish::status;
 
 #include "planner.h"
@@ -142,9 +144,9 @@ planner_settings_t Planner::settings; // Initialized by settings.load()
 laser_state_t Planner::laser_inline; // Current state for blocks
 #endif
 
-uint32_t Planner::max_acceleration_steps_per_s2[XYZE_N]; // (steps/s^2) Derived from mm_per_s2
+Vector6u32 Planner::max_acceleration_steps_per_s2; // (steps/s^2) Derived from mm_per_s2
 
-float Planner::steps_to_mm[XYZE_N]; // (mm) Millimeters per step
+Vector6f32 Planner::steps_to_unit; // (mm) Millimeters per step
 
 #if HAS_JUNCTION_DEVIATION
 float Planner::junction_deviation_mm; // (mm) M205 J
@@ -212,11 +214,11 @@ bool Planner::autotemp_enabled = false;
 
 // private:
 
-xyze_long_t Planner::position { 0, 0, 0 };
+Vector6i32 Planner::position { 0, 0, 0, 0, 0, 0 };
 
 uint32_t Planner::cutoff_long;
 
-xyze_float_t Planner::previous_speed;
+Vector6f32 Planner::previous_speed;
 float Planner::previous_nominal_speed_sqr;
 
 #if ENABLED(DISABLE_INACTIVE_EXTRUDER)
@@ -254,10 +256,10 @@ Planner::Planner() {
 }
 
 void Planner::init() {
-	position.reset();
+	position.fill(0.0f);
 	TERN_(HAS_POSITION_FLOAT, position_float.reset());
 	TERN_(IS_KINEMATIC, position_cart.reset());
-	previous_speed.reset();
+	previous_speed.fill(0.0);
 	previous_nominal_speed_sqr = 0;
 	TERN_(ABL_PLANAR, bed_level_matrix.set_to_identity());
 	clear_block_buffer();
@@ -1362,8 +1364,7 @@ void Planner::check_axes_activity() {
 		DISABLE_AXIS_Y();
 	if (TERN0(DISABLE_Z, !axis_active.z))
 		DISABLE_AXIS_Z();
-	if (TERN0(DISABLE_E, !axis_active.e))
-		disable_e_steppers();
+
 
 //
 // Update Fan speeds
@@ -1624,13 +1625,13 @@ void Planner::quick_stop() {
 	stepper.quick_stop();
 }
 
-void Planner::endstop_triggered(const AxisEnum axis) {
+void Planner::endstop_triggered(const Axis axis) {
 	// Record stepper position and discard the current block
 	stepper.endstop_triggered(axis);
 }
 
-float Planner::triggered_position_mm(const AxisEnum axis) {
-	return stepper.triggered_position(axis) * steps_to_mm[axis];
+float Planner::triggered_position_mm(const Axis axis) {
+	return stepper.triggered_position(axis) * steps_to_unit[axis];
 }
 
 void Planner::finish_and_disable() {
@@ -1643,7 +1644,7 @@ void Planner::finish_and_disable() {
  * Get an axis position according to stepper position(s)
  * For CORE machines apply translation from ABC to XYZ.
  */
-float Planner::get_axis_position_mm(const AxisEnum axis) {
+float Planner::get_axis_position_mm(const Axis axis) {
 	float axis_steps;
 #if IS_CORE
 
@@ -1688,7 +1689,7 @@ float Planner::get_axis_position_mm(const AxisEnum axis) {
 
 #endif
 
-	return axis_steps * steps_to_mm[axis];
+	return axis_steps * steps_to_unit[axis];
 }
 
 /**
@@ -1713,11 +1714,11 @@ void Planner::synchronize() {
  * Returns true if movement was properly queued, false otherwise (if cleaning)
  */
 bool Planner::_buffer_steps(
-		const xyze_long_t& target,
+		const Vector6i32& target,
 #if HAS_POSITION_FLOAT
 		const xyze_pos_t& target_float,
 #endif
-		const feedRate_t fr_mm_s,
+		const FeedRate feed_rate,
 		const uint8_t extruder,
 		const MachineState machine_state,
 		const float& millimeters /* = 0.0*/,
@@ -1739,7 +1740,7 @@ bool Planner::_buffer_steps(
 #if HAS_POSITION_FLOAT
 					target_float,
 #endif
-					fr_mm_s,
+					feed_rate,
 					extruder,
 					machine_state,
 					millimeters,
@@ -1783,11 +1784,11 @@ bool Planner::_buffer_steps(
 bool Planner::_populate_block(
 		block_t* const block,
 		bool split_move,
-		const abce_long_t& target,
+		const Vector6i32& target,
 #if HAS_POSITION_FLOAT
 		const xyze_pos_t& target_float,
 #endif
-		feedRate_t fr_mm_s,
+		FeedRate feed_rate,
 		const uint8_t extruder,
 		const MachineState machine_state,
 		const float& millimeters /* = 0.0*/,
@@ -1795,93 +1796,18 @@ bool Planner::_populate_block(
 ) {
 	EStopModule::getInstance().throwIfTriggered();
 
-	const int32_t da = target.a - position.a,
-								db = target.b - position.b,
-								dc = target.c - position.c;
-
-#if EXTRUDERS
-	int32_t de = target.e - position.e;
-#else
-	constexpr int32_t de = 0;
-#endif
+	const Vector6i32 delta = target - position;
 
 	accel_mm_s2 = accel_mm_s2 ?: settings.acceleration;
 
-	/* <-- add a slash to enable
-	  SERIAL_ECHOLNPAIR(
-	    "  _populate_block FR:", fr_mm_s,
-	    " A:", target.a, " (", da, " steps)"
-	    " B:", target.b, " (", db, " steps)"
-	    " C:", target.c, " (", dc, " steps)"
-	    #if EXTRUDERS
-	      " E:", target.e, " (", de, " steps)"
-	    #endif
-	  );
-	//*/
-
 	// Compute direction bit-mask for this block
-	uint8_t dm = 0;
-#if CORE_IS_XY
-	if (da < 0)
-		SBI(dm, X_HEAD); // Save the real Extruder (head) direction in X Axis
-	if (db < 0)
-		SBI(dm, Y_HEAD); // ...and Y
-	if (dc < 0)
-		SBI(dm, Z_AXIS);
-	if (da + db < 0)
-		SBI(dm, A_AXIS); // Motor A direction
-	if (CORESIGN(da - db) < 0)
-		SBI(dm, B_AXIS); // Motor B direction
-#elif CORE_IS_XZ
-	if (da < 0)
-		SBI(dm, X_HEAD); // Save the real Extruder (head) direction in X Axis
-	if (db < 0)
-		SBI(dm, Y_AXIS);
-	if (dc < 0)
-		SBI(dm, Z_HEAD); // ...and Z
-	if (da + dc < 0)
-		SBI(dm, A_AXIS); // Motor A direction
-	if (CORESIGN(da - dc) < 0)
-		SBI(dm, C_AXIS); // Motor C direction
-#elif CORE_IS_YZ
-	if (da < 0)
-		SBI(dm, X_AXIS);
-	if (db < 0)
-		SBI(dm, Y_HEAD); // Save the real Extruder (head) direction in Y Axis
-	if (dc < 0)
-		SBI(dm, Z_HEAD); // ...and Z
-	if (db + dc < 0)
-		SBI(dm, B_AXIS); // Motor B direction
-	if (CORESIGN(db - dc) < 0)
-		SBI(dm, C_AXIS); // Motor C direction
-#elif ENABLED(MARKFORGED_XY)
-	if (da < 0)
-		SBI(dm, X_HEAD); // Save the real Extruder (head) direction in X Axis
-	if (db < 0)
-		SBI(dm, Y_HEAD); // ...and Y
-	if (dc < 0)
-		SBI(dm, Z_AXIS);
-	if (da + db < 0)
-		SBI(dm, A_AXIS); // Motor A direction
-	if (db < 0)
-		SBI(dm, B_AXIS); // Motor B direction
-#else
-	if (da < 0)
-		SBI(dm, X_AXIS);
-	if (db < 0)
-		SBI(dm, Y_AXIS);
-	if (dc < 0)
-		SBI(dm, Z_AXIS);
-#endif
-	if (de < 0)
-		SBI(dm, E_AXIS);
+	u8 dm = 0;
 
-#if EXTRUDERS
-	const float esteps_float = de * e_factor[extruder];
-	const uint32_t esteps = ABS(esteps_float) + 0.5f;
-#else
-	constexpr uint32_t esteps = 0;
-#endif
+	for (auto i : all_axes) {
+		if (delta[i] < 0) {
+			dm |= i.bit_value();
+		}
+	}
 
 	block->machine_state = machine_state;
 
@@ -1898,288 +1824,170 @@ bool Planner::_populate_block(
 	block->laser.power = laser_inline.power;
 #endif
 
-	block->steps.set(ABS(da), ABS(db), ABS(dc));
-
-	/**
-	 * This part of the code calculates the total length of the movement.
-	 * For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS.
-	 * But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS
-	 * and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y.
-	 * So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head.
-	 * Having the real displacement of the head, we can calculate the total movement length and apply the desired speed.
-	 */
-	struct DistanceMM : abce_float_t {
+	for (auto axis : all_axes) {
+		block->steps[axis] = abs(delta[axis]);
+	}
 
-	} steps_dist_mm;
 
-	steps_dist_mm.a = da * steps_to_mm[A_AXIS];
-	steps_dist_mm.b = db * steps_to_mm[B_AXIS];
-	steps_dist_mm.c = dc * steps_to_mm[C_AXIS];
-	steps_dist_mm.e = de * steps_to_mm[E_AXIS];
+	debug()("block->steps.x: ", block->steps.x());
+	debug()("block->steps.y: ", block->steps.y());
+	debug()("block->steps.z: ", block->steps.z());
+	debug()("block->steps.a: ", block->steps.a());
+	debug()("block->steps.b: ", block->steps.b());
+	debug()("block->steps.c: ", block->steps.c());
+
+	Vector6f32 steps_dist_unit = {
+		block->steps.x() * steps_to_unit.x(),
+		block->steps.y() * steps_to_unit.y(),
+		block->steps.z() * steps_to_unit.z(),
+		block->steps.a() * steps_to_unit.a(),
+		block->steps.b() * steps_to_unit.b(),
+		block->steps.c() * steps_to_unit.c()
+	};
 
-	TERN_(LCD_SHOW_E_TOTAL, e_move_accumulator += steps_dist_mm.e);
+	debug()("steps_dist_unit.x: ", steps_dist_unit.x());
+	debug()("steps_dist_unit.y: ", steps_dist_unit.y());
+	debug()("steps_dist_unit.z: ", steps_dist_unit.z());
+	debug()("steps_dist_unit.a: ", steps_dist_unit.a());
+	debug()("steps_dist_unit.b: ", steps_dist_unit.b());
+	debug()("steps_dist_unit.c: ", steps_dist_unit.c());
 
-	if (block->steps.a < MIN_STEPS_PER_SEGMENT && block->steps.b < MIN_STEPS_PER_SEGMENT && block->steps.c < MIN_STEPS_PER_SEGMENT) {
-		block->millimeters = (0
-#if EXTRUDERS
-		                      + ABS(steps_dist_mm.e)
-#endif
-		);
+	if (block->steps.x() < MIN_STEPS_PER_SEGMENT && block->steps.y() < MIN_STEPS_PER_SEGMENT && block->steps.z() < MIN_STEPS_PER_SEGMENT) {
+		block->millimeters = 0;
 	} else {
-		if (millimeters)
+		if (millimeters) {
 			block->millimeters = millimeters;
-		else
-			block->millimeters = SQRT(
-#if EITHER(CORE_IS_XY, MARKFORGED_XY)
-					sq(steps_dist_mm.head.x) + sq(steps_dist_mm.head.y) + sq(steps_dist_mm.z)
-#elif CORE_IS_XZ
-					sq(steps_dist_mm.head.x) + sq(steps_dist_mm.y) + sq(steps_dist_mm.head.z)
-#elif CORE_IS_YZ
-					sq(steps_dist_mm.x) + sq(steps_dist_mm.head.y) + sq(steps_dist_mm.head.z)
-#else
-					sq(steps_dist_mm.x) + sq(steps_dist_mm.y) + sq(steps_dist_mm.z)
-#endif
-			);
-
-		/**
-		 * At this point at least one of the axes has more steps than
-		 * MIN_STEPS_PER_SEGMENT, ensuring the segment won't get dropped as
-		 * zero-length. It's important to not apply corrections
-		 * to blocks that would get dropped!
-		 *
-		 * A correction function is permitted to add steps to an axis, it
-		 * should *never* remove steps!
-		 */
-		TERN_(BACKLASH_COMPENSATION, backlash.add_correction_steps(da, db, dc, dm, block));
+		} else {
+			block->millimeters = sqrtf(
+					sq(steps_dist_unit.x()) + sq(steps_dist_unit.y()) + sq(steps_dist_unit.z()));
+		}
 	}
 
-#if EXTRUDERS
-	block->steps.e = esteps;
-#endif
+	if (block->millimeters == 0.0) {
+		debug()("faking radial feed distance");
+		for (auto axis : radial_axes) {
+			if (!isnan(block->steps[axis]) && block->steps[axis] >= MIN_STEPS_PER_SEGMENT) {
+				block->millimeters += steps_dist_unit[axis];
+			}
+		}
+	}
 
-	block->step_event_count = _MAX(block->steps.a, block->steps.b, block->steps.c, esteps);
+	block->step_event_count = block->steps.maxCoeff();
+
+	debug()("block->step_event_count: ", block->step_event_count);
+	debug()("block->millimeters: ", block->millimeters);
 
 	// Bail if this is a zero-length block
-	if (block->step_event_count < MIN_STEPS_PER_SEGMENT)
+	if (block->step_event_count < MIN_STEPS_PER_SEGMENT) {
 		return false;
+	}
 
-		// TERN_(HAS_CUTTER, block->cutter_power = cutter.power);
-
-#if HAS_FAN
-	FANS_LOOP(i)
-	block->fan_speed[i] = thermalManager.fan_speed[i];
-#endif
-
-#if ENABLED(BARICUDA)
-	block->valve_pressure = baricuda_valve_pressure;
-	block->e_to_p_pressure = baricuda_e_to_p_pressure;
-#endif
 
 	// Enable active axes
-	if (block->steps.x)
+	if (block->steps.x()) {
 		ENABLE_AXIS_X();
-	if (block->steps.y)
-		ENABLE_AXIS_Y();
-#if DISABLED(Z_LATE_ENABLE)
-	if (block->steps.z)
-		ENABLE_AXIS_Z();
-#endif
-
-// Enable extruder(s)
-#if EXTRUDERS
-	if (esteps) {
-		TERN_(AUTO_POWER_CONTROL, powerManager.power_on());
-
-#	if ENABLED(DISABLE_INACTIVE_EXTRUDER) // Enable only the selected extruder
-
-		LOOP_L_N(i, EXTRUDERS)
-		if (g_uc_extruder_last_move[i])
-			g_uc_extruder_last_move[i]--;
-
-#		define ENABLE_ONE_E(N) \
-			do { \
-				if (extruder == N) { \
-					ENABLE_AXIS_E##N(); \
-					g_uc_extruder_last_move[N] = (BLOCK_BUFFER_SIZE) *2; \
-					if ((N) == 0 && TERN0(HAS_DUPLICATION_MODE, extruder_duplication_enabled)) \
-						ENABLE_AXIS_E1(); \
-				} else if (!g_uc_extruder_last_move[N]) { \
-					DISABLE_AXIS_E##N(); \
-					if ((N) == 0 && TERN0(HAS_DUPLICATION_MODE, extruder_duplication_enabled)) \
-						DISABLE_AXIS_E1(); \
-				} \
-			} while (0);
-
-#	else
-
-#		define ENABLE_ONE_E(N) ENABLE_AXIS_E##N();
-
-#	endif
-
-		REPEAT(EXTRUDERS, ENABLE_ONE_E); // (ENABLE_ONE_E must end with semicolon)
 	}
-#endif // EXTRUDERS
 
-	if (block->steps.x) {
-		NOMORE(fr_mm_s, settings.max_feedrate_mm_s[X_AXIS]);
+	if (block->steps.y()) {
+		ENABLE_AXIS_Y();
 	}
 
-	if (block->steps.y) {
-		NOMORE(fr_mm_s, settings.max_feedrate_mm_s[Y_AXIS]);
+	if (block->steps.z()) {
+		ENABLE_AXIS_Z();
 	}
 
-	if (block->steps.z) {
-		NOMORE(fr_mm_s, settings.max_feedrate_mm_s[Z_AXIS]);
+	if (block->steps[Axis::A()]) {
+		ENABLE_AXIS_A();
 	}
 
-	debug()("max_feedrate_mm_s[X_AXIS]: ", settings.max_feedrate_mm_s[X_AXIS]);
-	debug()("max_feedrate_mm_s[Y_AXIS]: ", settings.max_feedrate_mm_s[Y_AXIS]);
-	debug()("max_feedrate_mm_s[Z_AXIS]: ", settings.max_feedrate_mm_s[Z_AXIS]);
+	debug()("max_feedrate[X]: ", settings.max_feedrate_unit_per_s[Axis::X()]);
+	debug()("max_feedrate[Y]: ", settings.max_feedrate_unit_per_s[Axis::Y()]);
+	debug()("max_feedrate[Z]: ", settings.max_feedrate_unit_per_s[Axis::Z()]);
+	debug()("max_feedrate[A]: ", settings.max_feedrate_unit_per_s[Axis::A()]);
+	debug()("max_feedrate[B]: ", settings.max_feedrate_unit_per_s[Axis::B()]);
+	debug()("max_feedrate[C]: ", settings.max_feedrate_unit_per_s[Axis::C()]);
 
-	debug()("fr_mm_s: ", fr_mm_s);
+	debug()("feed_rate: type=", (i32) feed_rate.type(), ", value=", feed_rate.value());
 
-	if (esteps)
-		NOLESS(fr_mm_s, settings.min_feedrate_mm_s);
-	else
-		NOLESS(fr_mm_s, settings.min_travel_feedrate_mm_s);
+	const f32 inverse_millimeters = 1.0f / block->millimeters; // Inverse millimeters to remove multiple divides
 
-	const float inverse_millimeters = 1.0f / block->millimeters; // Inverse millimeters to remove multiple divides
-
-	// Calculate inverse time for this move. No divide by zero due to previous checks.
-	// Example: At 120mm/s a 60mm move takes 0.5s. So this will give 2.0.
-	float inverse_secs = fr_mm_s * inverse_millimeters;
-
-	// Get the number of non busy movements in queue (non busy means that they can be altered)
-	const uint8_t moves_queued = nonbusy_movesplanned();
+	f32 inverse_secs = 0;
 
-// Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
-#if EITHER(SLOWDOWN, HAS_WIRED_LCD) || defined(XY_FREQUENCY_LIMIT)
-	// Segment time im micro seconds
-	int32_t segment_time_us = LROUND(1000000.0f / inverse_secs);
-#endif
+	switch (feed_rate.type()) {
+		case FeedRateType::InverseTime: {
+			inverse_secs = feed_rate.value();
 
-#if ENABLED(SLOWDOWN)
-#	ifndef SLOWDOWN_DIVISOR
-#		define SLOWDOWN_DIVISOR 2
-#	endif
-	if (WITHIN(moves_queued, 2, (BLOCK_BUFFER_SIZE) / (SLOWDOWN_DIVISOR) -1)) {
-		const int32_t time_diff = settings.min_segment_time_us - segment_time_us;
-		if (time_diff > 0) {
-			// Buffer is draining so add extra time. The amount of time added increases if the buffer is still emptied more.
-			const int32_t nst = segment_time_us + LROUND(2 * time_diff / moves_queued);
-			inverse_secs = 1000000.0f / nst;
-#	if defined(XY_FREQUENCY_LIMIT) || HAS_WIRED_LCD
-			segment_time_us = nst;
-#	endif
+			break;
 		}
-	}
-#endif
 
-#if HAS_WIRED_LCD
-	// Protect the access to the position.
-	const bool was_enabled = stepper.suspend();
+		case FeedRateType::UnitsPerSecond: {
+			// Calculate inverse time for this move. No divide by zero due to previous checks.
+			// Example: At 120mm/s a 60mm move takes 0.5s. So this will give 2.0.
 
-	block_buffer_runtime_us += segment_time_us;
-	block->segment_time_us = segment_time_us;
+			/*
+			u8 linear_moves = 0;
+			u8 radial_moves = 0;
 
-	if (was_enabled)
-		stepper.wake_up();
-#endif
+			for (auto axis : all_axes) {
+				if (block->steps[axis]) {
+					if (axis.is_linear()) {
+						linear_moves++;
+					} else {
+						radial_moves++;
+					}
+ 				}
+			}
 
-	block->nominal_speed_sqr = sq(block->millimeters * inverse_secs); // (mm/sec)^2 Always > 0
-	block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0
+			if (linear_moves && radial_moves) {
+				throw CommandException("Linear and radial moves can only be combined in G93 mode");
+			}
 
-#if ENABLED(FILAMENT_WIDTH_SENSOR)
-	if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM) // Only for extruder with filament sensor
-		filwidth.advance_e(steps_dist_mm.e);
-#endif
+			if (radial_moves > 1) {
+				throw CommandException("Simultaneous radial moves are only supported in G93 mode");
+			}
+			*/
 
-	// Calculate and limit speed in mm/sec
+			for (auto axis : all_axes) {
+				if (block->steps[axis]) {
+					NOMORE(feed_rate.value(), settings.max_feedrate_unit_per_s[axis]);
+				}
+			}
 
-	xyze_float_t current_speed;
-	float speed_factor = 1.0f; // factor <1 decreases speed
+			NOLESS(feed_rate.value(), (f32) settings.min_travel_feedrate_unit_per_s);
 
-	// Linear axes first with less logic
-	LOOP_XYZ(i) {
-		current_speed[i] = steps_dist_mm[i] * inverse_secs;
-		const feedRate_t cs = ABS(current_speed[i]),
-										 max_fr = settings.max_feedrate_mm_s[i];
-		if (cs > max_fr)
-			NOMORE(speed_factor, max_fr / cs);
-	}
+			debug()("feed_rate.value(): ", feed_rate.value());
 
-// Limit speed on extruders, if any
-#if EXTRUDERS
-	{
-		current_speed.e = steps_dist_mm.e * inverse_secs;
-#	if HAS_MIXER_SYNC_CHANNEL
-		// Move all mixing extruders at the specified rate
-		if (mixer.get_current_vtool() == MIXER_AUTORETRACT_TOOL)
-			current_speed.e *= MIXING_STEPPERS;
-#	endif
+			inverse_secs = feed_rate.value() * inverse_millimeters;
 
-		const feedRate_t cs = ABS(current_speed.e),
-										 max_fr = settings.max_feedrate_mm_s[E_AXIS_N(extruder)] * TERN(HAS_MIXER_SYNC_CHANNEL, MIXING_STEPPERS, 1);
+			break;
+		}
+	}
 
-		if (cs > max_fr)
-			NOMORE(speed_factor, max_fr / cs); // respect max feedrate on any movement (doesn't matter if E axes only or not)
+	debug()("inverse_secs: ", inverse_secs);
+	debug()("secs: ", 1 / inverse_secs);
 
-#	if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT)
-		const feedRate_t max_vfr = volumetric_extruder_feedrate_limit[extruder] * TERN(HAS_MIXER_SYNC_CHANNEL, MIXING_STEPPERS, 1);
+	// Get the number of non busy movements in queue (non busy means that they can be altered)
+	const uint8_t moves_queued = nonbusy_movesplanned();
 
-		// TODO: Doesn't work properly for joined segments. Set MIN_STEPS_PER_SEGMENT 1 as workaround.
+	block->nominal_speed_sqr = sq(block->millimeters * inverse_secs); // (mm/sec)^2 Always > 0
+	block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0
 
-		if (block->steps.a || block->steps.b || block->steps.c) {
+	// Calculate and limit speed in mm/sec
 
-			if (max_vfr > 0 && cs > max_vfr) {
-				NOMORE(speed_factor, max_vfr / cs); // respect volumetric extruder limit (if any)
-				/* <-- add a slash to enable
-				SERIAL_ECHOPAIR("volumetric extruder limit enforced: ", (cs * CIRCLE_AREA(filament_size[extruder] * 0.5f)));
-				SERIAL_ECHOPAIR(" mm^3/s (", cs);
-				SERIAL_ECHOPAIR(" mm/s) limited to ", (max_vfr * CIRCLE_AREA(filament_size[extruder] * 0.5f)));
-				SERIAL_ECHOPAIR(" mm^3/s (", max_vfr);
-				SERIAL_ECHOLNPGM(" mm/s)");
-				//*/
-			}
-		}
-#	endif
-	}
-#endif
+	Vector6f32 current_speed;
+	f32 speed_factor = 1.0f; // factor <1 decreases speed
 
-#ifdef XY_FREQUENCY_LIMIT
+	// Linear axes
+	for (auto i : linear_axes) {
+		current_speed[i] = steps_dist_unit[i] * inverse_secs;
+		const f32 cs = ABS(current_speed[i]);
+		const f32 max_fr = settings.max_feedrate_unit_per_s[i];
 
-	static uint8_t old_direction_bits; // = 0
-
-	if (xy_freq_limit_hz) {
-		// Check and limit the xy direction change frequency
-		const uint8_t direction_change = block->direction_bits ^ old_direction_bits;
-		old_direction_bits = block->direction_bits;
-		segment_time_us = LROUND(float(segment_time_us) / speed_factor);
-
-		static int32_t xs0, xs1, xs2, ys0, ys1, ys2;
-		if (segment_time_us > xy_freq_min_interval_us)
-			xs2 = xs1 = ys2 = ys1 = xy_freq_min_interval_us;
-		else {
-			xs2 = xs1;
-			xs1 = xs0;
-			ys2 = ys1;
-			ys1 = ys0;
-		}
-		xs0 = TEST(direction_change, X_AXIS) ? segment_time_us : xy_freq_min_interval_us;
-		ys0 = TEST(direction_change, Y_AXIS) ? segment_time_us : xy_freq_min_interval_us;
-
-		if (segment_time_us < xy_freq_min_interval_us) {
-			const int32_t least_xy_segment_time = _MIN(_MAX(xs0, xs1, xs2), _MAX(ys0, ys1, ys2));
-			if (least_xy_segment_time < xy_freq_min_interval_us) {
-				float freq_xy_feedrate = (speed_factor * least_xy_segment_time) / xy_freq_min_interval_us;
-				NOLESS(freq_xy_feedrate, xy_freq_min_speed_factor);
-				NOMORE(speed_factor, freq_xy_feedrate);
-			}
+		if (cs > max_fr) {
+			NOMORE(speed_factor, max_fr / cs);
 		}
 	}
 
-#endif // XY_FREQUENCY_LIMIT
-
 	// Correct the speed
 	if (speed_factor < 1.0f) {
 		current_speed *= speed_factor;
@@ -2188,85 +1996,25 @@ bool Planner::_populate_block(
 	}
 
 	// Compute and limit the acceleration rate for the trapezoid generator.
-	const float steps_per_mm = block->step_event_count * inverse_millimeters;
-	uint32_t accel;
-	if (!block->steps.a && !block->steps.b && !block->steps.c) {
+	const f32 steps_per_mm = block->step_event_count * inverse_millimeters;
+	u32 accel;
+
+	if (!block->steps.x() && !block->steps.y() && !block->steps.z()) {
 		// convert to: acceleration steps/sec^2
 		accel = CEIL(settings.retract_acceleration * steps_per_mm);
-		TERN_(LIN_ADVANCE, block->use_advance_lead = false);
 	} else {
-#define LIMIT_ACCEL_LONG(AXIS, INDX) \
-	do { \
-		if (block->steps[AXIS] && max_acceleration_steps_per_s2[AXIS + INDX] < accel) { \
-			const uint32_t comp = max_acceleration_steps_per_s2[AXIS + INDX] * block->step_event_count; \
-			if (accel * block->steps[AXIS] > comp) \
-				accel = comp / block->steps[AXIS]; \
-		} \
-	} while (0)
-
-#define LIMIT_ACCEL_FLOAT(AXIS, INDX) \
-	do { \
-		if (block->steps[AXIS] && max_acceleration_steps_per_s2[AXIS + INDX] < accel) { \
-			debug()("max_acceleration_steps_per_s2[", axis_codes[AXIS], "]: ", max_acceleration_steps_per_s2[AXIS + INDX]); \
-			const float comp = (float) max_acceleration_steps_per_s2[AXIS + INDX] * (float) block->step_event_count; \
-			if ((float) accel * (float) block->steps[AXIS] > comp) \
-				accel = comp / (float) block->steps[AXIS]; \
-		} \
-	} while (0)
-
-		// Start with print or travel acceleration
 		accel = CEIL(accel_mm_s2 * steps_per_mm);
 
-#if ENABLED(LIN_ADVANCE)
+		for (auto axis : all_axes) {
+			if (block->steps[axis] && max_acceleration_steps_per_s2[axis] < accel) {
+				debug()("max_acceleration_steps_per_s2[", axis_codes[axis], "]: ", max_acceleration_steps_per_s2[axis]);
+				const float comp = (float) max_acceleration_steps_per_s2[axis] * (float) block->step_event_count;
 
-#	define MAX_E_JERK(N) TERN(HAS_LINEAR_E_JERK, max_e_jerk[E_INDEX_N(N)], max_jerk.e)
-
-		/**
-		 * Use LIN_ADVANCE for blocks if all these are true:
-		 *
-		 * esteps             : This is a print move, because we checked for A, B, C steps before.
-		 *
-		 * extruder_advance_K[active_extruder] : There is an advance factor set for this extruder.
-		 *
-		 * de > 0             : Extruder is running forward (e.g., for "Wipe while retracting" (Slic3r) or "Combing" (Cura) moves)
-		 */
-		block->use_advance_lead = esteps && extruder_advance_K[active_extruder] && de > 0;
-
-		if (block->use_advance_lead) {
-			block->e_D_ratio = (target_float.e - position_float.e) /
-#	if IS_KINEMATIC
-			                   block->millimeters
-#	else
-			                   SQRT(sq(target_float.x - position_float.x) + sq(target_float.y - position_float.y) + sq(target_float.z - position_float.z))
-#	endif
-					;
-
-			// Check for unusual high e_D ratio to detect if a retract move was combined with the last print move due to min. steps per segment. Never execute this with advance!
-			// This assumes no one will use a retract length of 0mm < retr_length < ~0.2mm and no one will print 100mm wide lines using 3mm filament or 35mm wide lines using 1.75mm filament.
-			if (block->e_D_ratio > 3.0f)
-				block->use_advance_lead = false;
-			else {
-				const uint32_t max_accel_steps_per_s2 = MAX_E_JERK(extruder) / (extruder_advance_K[active_extruder] * block->e_D_ratio) * steps_per_mm;
-				if (TERN0(LA_DEBUG, accel > max_accel_steps_per_s2))
-					SERIAL_ECHOLNPGM("Acceleration limited.");
-				NOMORE(accel, max_accel_steps_per_s2);
+				if ((float) accel * (float) block->steps[axis] > comp) {
+					accel = comp / (float) block->steps[axis];
+				}
 			}
 		}
-#endif
-
-		// Limit acceleration per axis
-		// if (block->step_event_count <= cutoff_long) {
-		//  LIMIT_ACCEL_LONG(A_AXIS, 0);
-		//  LIMIT_ACCEL_LONG(B_AXIS, 0);
-		//  LIMIT_ACCEL_LONG(C_AXIS, 0);
-		//  LIMIT_ACCEL_LONG(E_AXIS, E_INDEX_N(extruder));
-		//}
-		// else {
-		LIMIT_ACCEL_FLOAT(A_AXIS, 0);
-		LIMIT_ACCEL_FLOAT(B_AXIS, 0);
-		LIMIT_ACCEL_FLOAT(C_AXIS, 0);
-		LIMIT_ACCEL_FLOAT(E_AXIS, E_INDEX_N(extruder));
-		//}
 	}
 
 	block->acceleration_steps_per_s2 = accel;
@@ -2328,9 +2076,9 @@ bool Planner::_populate_block(
 	      already calculated in a different place. */
 
 	// Unit vector of previous path line segment
-	static xyze_float_t prev_unit_vec;
+	static Vector6f32 prev_unit_vec;
 
-	xyze_float_t unit_vec { steps_dist_mm.x, steps_dist_mm.y, steps_dist_mm.z, steps_dist_mm.e };
+	Vector6f32 unit_vec = steps_dist_unit;
 
 	/**
 	 * On CoreXY the length of the vector [A,B] is SQRT(2) times the length of the head movement vector [X,Y].
@@ -2338,16 +2086,24 @@ bool Planner::_populate_block(
 	 * => normalize the complete junction vector.
 	 * Elsewise, when needed JD will factor-in the E component
 	 */
-	if (EITHER(IS_CORE, MARKFORGED_XY) || esteps > 0)
+	if (EITHER(IS_CORE, MARKFORGED_XY)) {
 		normalize_junction_vector(unit_vec); // Normalize with XYZE components
-	else
+	} else {
 		unit_vec *= inverse_millimeters; // Use pre-calculated (1 / SQRT(x^2 + y^2 + z^2))
+	}
 
 	// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
 	if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) {
 		// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
 		// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
-		float junction_cos_theta = (-prev_unit_vec.x * unit_vec.x) + (-prev_unit_vec.y * unit_vec.y) + (-prev_unit_vec.z * unit_vec.z) + (-prev_unit_vec.e * unit_vec.e);
+				float junction_cos_theta =
+				(-prev_unit_vec.x() * unit_vec.x())
+			+ (-prev_unit_vec.y() * unit_vec.y())
+			+ (-prev_unit_vec.z() * unit_vec.z())
+			//+ (-prev_unit_vec.a() * unit_vec.a())
+			//+ (-prev_unit_vec.b() * unit_vec.b())
+			//+ (-prev_unit_vec.c() * unit_vec.c())
+			;
 
 		// NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
 		if (junction_cos_theta > 0.999999f) {
@@ -2357,7 +2113,7 @@ bool Planner::_populate_block(
 			NOLESS(junction_cos_theta, -0.999999f); // Check for numerical round-off to avoid divide by zero.
 
 			// Convert delta vector to unit vector
-			xyze_float_t junction_unit_vec = unit_vec - prev_unit_vec;
+			Vector6f32 junction_unit_vec = unit_vec - prev_unit_vec;
 			normalize_junction_vector(junction_unit_vec);
 
 			const float junction_acceleration = limit_value_by_axis_maximum(block->acceleration, junction_unit_vec),
@@ -2620,7 +2376,7 @@ void Planner::buffer_sync_block() {
 	block_t* const block = get_next_free_block(next_buffer_head);
 
 	// Clear block
-	memset(block, 0, sizeof(block_t));
+	block->reset();
 
 	block->flag = BLOCK_FLAG_SYNC_POSITION;
 
@@ -2656,11 +2412,8 @@ void Planner::buffer_sync_block() {
  * Return 'false' if no segment was queued due to cleaning, cold extrusion, full queue, etc.
  */
 bool Planner::buffer_segment(
-		const float& a,
-		const float& b,
-		const float& c,
-		const float& e,
-		const feedRate_t& fr_mm_s,
+		const Vector6f32& raw,
+		const FeedRate& fr_mm_s,
 		const uint8_t extruder,
 		const MachineState machine_state,
 		const float& millimeters /* = 0.0*/,
@@ -2668,67 +2421,25 @@ bool Planner::buffer_segment(
 ) {
 
 	// If we are cleaning, do not accept queuing of movements
-	if (cleaning_buffer_counter)
+	if (cleaning_buffer_counter) {
 		return false;
-
-// When changing extruders recalculate steps corresponding to the E position
-#if ENABLED(DISTINCT_E_FACTORS)
-	if (last_extruder != extruder && settings.axis_steps_per_mm[E_AXIS_N(extruder)] != settings.axis_steps_per_mm[E_AXIS_N(last_extruder)]) {
-		position.e = LROUND(position.e * settings.axis_steps_per_mm[E_AXIS_N(extruder)] * steps_to_mm[E_AXIS_N(last_extruder)]);
-		last_extruder = extruder;
 	}
-#endif
 
 	// The target position of the tool in absolute steps
 	// Calculate target position in absolute steps
-	const abce_long_t target = {
-		int32_t(LROUND(a * settings.axis_steps_per_mm[A_AXIS])),
-		int32_t(LROUND(b * settings.axis_steps_per_mm[B_AXIS])),
-		int32_t(LROUND(c * settings.axis_steps_per_mm[C_AXIS])),
-		int32_t(LROUND(e * settings.axis_steps_per_mm[E_AXIS_N(extruder)]))
+	const Vector6i32 target = {
+		i32(LROUND(raw[Axis::X()] * settings.axis_steps_per_unit[Axis::X()])),
+		i32(LROUND(raw[Axis::Y()] * settings.axis_steps_per_unit[Axis::Y()])),
+		i32(LROUND(raw[Axis::Z()] * settings.axis_steps_per_unit[Axis::Z()])),
+		i32(LROUND(raw[Axis::A()] * settings.axis_steps_per_unit[Axis::A()])),
+		i32(LROUND(raw[Axis::B()] * settings.axis_steps_per_unit[Axis::B()])),
+		i32(LROUND(raw[Axis::C()] * settings.axis_steps_per_unit[Axis::C()]))
 	};
 
 #if HAS_POSITION_FLOAT
 	const xyze_pos_t target_float = { a, b, c, e };
 #endif
 
-	// DRYRUN prevents E moves from taking place
-	if (DEBUGGING(DRYRUN) || TERN0(CANCEL_OBJECTS, cancelable.skipping)) {
-		position.e = target.e;
-		TERN_(HAS_POSITION_FLOAT, position_float.e = e);
-	}
-
-	/* <-- add a slash to enable
-	  SERIAL_ECHOPAIR("  buffer_segment FR:", fr_mm_s);
-	  #if IS_KINEMATIC
-	    SERIAL_ECHOPAIR(" A:", a);
-	    SERIAL_ECHOPAIR(" (", position.a);
-	    SERIAL_ECHOPAIR("->", target.a);
-	    SERIAL_ECHOPAIR(") B:", b);
-	  #else
-	    SERIAL_ECHOPAIR_P(SP_X_LBL, a);
-	    SERIAL_ECHOPAIR(" (", position.x);
-	    SERIAL_ECHOPAIR("->", target.x);
-	    SERIAL_CHAR(')');
-	    SERIAL_ECHOPAIR_P(SP_Y_LBL, b);
-	  #endif
-	  SERIAL_ECHOPAIR(" (", position.y);
-	  SERIAL_ECHOPAIR("->", target.y);
-	  #if ENABLED(DELTA)
-	    SERIAL_ECHOPAIR(") C:", c);
-	  #else
-	    SERIAL_CHAR(')');
-	    SERIAL_ECHOPAIR_P(SP_Z_LBL, c);
-	  #endif
-	  SERIAL_ECHOPAIR(" (", position.z);
-	  SERIAL_ECHOPAIR("->", target.z);
-	  SERIAL_CHAR(')');
-	  SERIAL_ECHOPAIR_P(SP_E_LBL, e);
-	  SERIAL_ECHOPAIR(" (", position.e);
-	  SERIAL_ECHOPAIR("->", target.e);
-	  SERIAL_ECHOLNPGM(")");
-	//*/
-
 	debug()("accel_mm_s2: ", accel_mm_s2);
 
 	// Queue the movement. Return 'false' if the move was not queued.
@@ -2761,22 +2472,19 @@ bool Planner::buffer_segment(
  *  inv_duration - the reciprocal if the duration of the movement, if known (kinematic only if feeedrate scaling is enabled)
  */
 bool Planner::buffer_line(
-		const float& rx,
-		const float& ry,
-		const float& rz,
-		const float& e,
-		const feedRate_t& fr_mm_s,
+		const Vector6f32& machine,
+		const FeedRate& feed_rate,
 		const uint8_t extruder,
 		const MachineState machine_state,
 		const float millimeters,
 		const float32_t accel_mm_s2 /* = 0.0*/
 ) {
-	xyze_pos_t machine = { rx, ry, rz, e };
+
 	TERN_(HAS_POSITION_MODIFIERS, apply_modifiers(machine));
 
 	return buffer_segment(
 			machine,
-			fr_mm_s,
+			feed_rate,
 			extruder,
 			machine_state,
 			millimeters,
@@ -2855,13 +2563,16 @@ void Planner::buffer_page(const page_idx_t page_idx, const uint8_t extruder, con
  * The provided ABC position is in machine units.
  */
 
-void Planner::set_machine_position_mm(const float& a, const float& b, const float& c, const float& e) {
-	TERN_(DISTINCT_E_FACTORS, last_extruder = active_extruder);
-	TERN_(HAS_POSITION_FLOAT, position_float.set(a, b, c, e));
-	position.set(LROUND(a * settings.axis_steps_per_mm[A_AXIS]),
-	             LROUND(b * settings.axis_steps_per_mm[B_AXIS]),
-	             LROUND(c * settings.axis_steps_per_mm[C_AXIS]),
-	             LROUND(e * settings.axis_steps_per_mm[E_AXIS_N(active_extruder)]));
+void Planner::set_machine_position_mm(const float& x, const float& y, const float& z, const float& a) {
+	position = {
+		LROUND(x * settings.axis_steps_per_unit[Axis::X()]),
+		LROUND(y * settings.axis_steps_per_unit[Axis::Y()]),
+		LROUND(z * settings.axis_steps_per_unit[Axis::Z()]),
+		LROUND(a * settings.axis_steps_per_unit[Axis::A()]),
+		0,
+		0
+	};
+
 	if (has_blocks_queued()) {
 		// previous_nominal_speed_sqr = 0.0; // Reset planner junction speeds. Assume start from rest.
 		// previous_speed.reset();
@@ -2871,7 +2582,7 @@ void Planner::set_machine_position_mm(const float& a, const float& b, const floa
 }
 
 void Planner::set_position_mm(const float& rx, const float& ry, const float& rz, const float& e) {
-	xyze_pos_t machine = { rx, ry, rz, e };
+	Vector6f32 machine = { rx, ry, rz, e, 0, 0 };
 #if HAS_POSITION_MODIFIERS
 	apply_modifiers(machine, true);
 #endif
@@ -2884,23 +2595,6 @@ void Planner::set_position_mm(const float& rx, const float& ry, const float& rz,
 #endif
 }
 
-/**
- * Setters for planner position (also setting stepper position).
- */
-void Planner::set_e_position_mm(const float& e) {
-	const uint8_t axis_index = E_AXIS_N(active_extruder);
-	TERN_(DISTINCT_E_FACTORS, last_extruder = active_extruder);
-
-	const float e_new = e - TERN0(FWRETRACT, fwretract.current_retract[active_extruder]);
-	position.e = LROUND(settings.axis_steps_per_mm[axis_index] * e_new);
-	TERN_(HAS_POSITION_FLOAT, position_float.e = e_new);
-	TERN_(IS_KINEMATIC, position_cart.e = e);
-
-	if (has_blocks_queued())
-		buffer_sync_block();
-	else
-		stepper.set_axis_position(E_AXIS, position.e);
-}
 
 // Recalculate the steps/s^2 acceleration rates, based on the mm/s^2
 void Planner::reset_acceleration_rates() {
@@ -2910,8 +2604,8 @@ void Planner::reset_acceleration_rates() {
 #	define AXIS_CONDITION true
 #endif
 	uint32_t highest_rate = 1;
-	LOOP_XYZE_N(i) {
-		max_acceleration_steps_per_s2[i] = settings.max_acceleration_mm_per_s2[i] * settings.axis_steps_per_mm[i];
+	for (auto i : all_axes) {
+		max_acceleration_steps_per_s2[i] = settings.max_acceleration_unit_per_s2[i] * settings.axis_steps_per_unit[i];
 		if (AXIS_CONDITION)
 			NOLESS(highest_rate, max_acceleration_steps_per_s2[i]);
 	}
@@ -2921,25 +2615,27 @@ void Planner::reset_acceleration_rates() {
 
 // Recalculate position, steps_to_mm if settings.axis_steps_per_mm changes!
 void Planner::refresh_positioning() {
-	LOOP_XYZE_N(i)
-	steps_to_mm[i] = 1.0f / settings.axis_steps_per_mm[i];
+	for (auto i : all_axes) {
+		steps_to_unit[i] = 1.0f / settings.axis_steps_per_unit[i];
+	}
+
 	set_position_mm(current_position);
 	reset_acceleration_rates();
 }
 
-inline void limit_and_warn(float& val, const uint8_t axis, PGM_P const setting_name, const xyze_float_t& max_limit) {
-	const uint8_t lim_axis = axis > E_AXIS ? E_AXIS : axis;
+inline void limit_and_warn(float& val, const Axis axis, PGM_P const setting_name, const xyze_float_t& max_limit) {
+
 	const float before = val;
-	LIMIT(val, 0.1, max_limit[lim_axis]);
+	LIMIT(val, 0.1, max_limit[axis]);
 	if (before != val) {
-		SERIAL_CHAR(axis_codes[lim_axis]);
+		SERIAL_CHAR(axis.to_char());
 		SERIAL_ECHOPGM(" Max ");
 		serialprintPGM(setting_name);
 		SERIAL_ECHOLNPAIR(" limited to ", val);
 	}
 }
 
-void Planner::set_max_acceleration(const uint8_t axis, float targetValue) {
+void Planner::set_max_acceleration(const Axis axis, float targetValue) {
 #if ENABLED(LIMITED_MAX_ACCEL_EDITING)
 #	ifdef MAX_ACCEL_EDIT_VALUES
 	constexpr xyze_float_t max_accel_edit = MAX_ACCEL_EDIT_VALUES;
@@ -2950,13 +2646,13 @@ void Planner::set_max_acceleration(const uint8_t axis, float targetValue) {
 #	endif
 	limit_and_warn(targetValue, axis, PSTR("Acceleration"), max_acc_edit_scaled);
 #endif
-	settings.max_acceleration_mm_per_s2[axis] = targetValue;
+	settings.max_acceleration_unit_per_s2[axis] = targetValue;
 
 	// Update steps per s2 to agree with the units per s2 (since they are used in the planner)
 	reset_acceleration_rates();
 }
 
-void Planner::set_max_feedrate(const uint8_t axis, float targetValue) {
+void Planner::set_max_feedrate(const Axis axis, float targetValue) {
 #if ENABLED(LIMITED_MAX_FR_EDITING)
 #	ifdef MAX_FEEDRATE_EDIT_VALUES
 	constexpr xyze_float_t max_fr_edit = MAX_FEEDRATE_EDIT_VALUES;
@@ -2967,10 +2663,10 @@ void Planner::set_max_feedrate(const uint8_t axis, float targetValue) {
 #	endif
 	limit_and_warn(targetValue, axis, PSTR("Feedrate"), max_fr_edit_scaled);
 #endif
-	settings.max_feedrate_mm_s[axis] = targetValue;
+	settings.max_feedrate_unit_per_s[axis] = targetValue;
 }
 
-void Planner::set_max_jerk(const AxisEnum axis, float targetValue) {
+void Planner::set_max_jerk(const Axis axis, float targetValue) {
 #if HAS_CLASSIC_JERK
 #	if ENABLED(LIMITED_JERK_EDITING)
 	constexpr xyze_float_t max_jerk_edit =
diff --git a/src/marlin/module/planner.h b/src/marlin/module/planner.h
index b5955fcde4..96e7f4ff50 100644
--- a/src/marlin/module/planner.h
+++ b/src/marlin/module/planner.h
@@ -30,6 +30,8 @@
  * Copyright (c) 2009-2011 Simen Svale Skogsrud
  */
 
+#include <swordfish/math.h>
+
 #include "../MarlinCore.h"
 
 #if ENABLED(JD_HANDLE_SMALL_SEGMENTS)
@@ -148,6 +150,30 @@ enum BlockFlag : char {
  * may never actually be reached due to acceleration limits.
  */
 typedef struct block_t {
+	block_t() {
+		reset();
+	}
+
+	void reset() {
+		flag = 0;
+		nominal_speed_sqr = 0.0;
+		max_entry_speed_sqr = 0.0;
+		millimeters = 0.0;
+		acceleration = 0.0;
+		steps = swordfish::math::Vector6u32 { 0, 0, 0, 0, 0, 0 };
+		step_event_count = 0;
+		accelerate_until = 0;
+		decelerate_after = 0;
+		cruise_rate = 0;
+		acceleration_time = 0;
+		deceleration_time = 0;
+		acceleration_time_inverse = 0;
+		deceleration_time_inverse = 0;
+		direction_bits = 0;
+		initial_rate = 0;
+		final_rate = 0;
+		acceleration_steps_per_s2 = 0;
+	}
 
   volatile uint8_t flag;                    // Block flags (See BlockFlag enum above) - Modified by ISR and main thread!
 
@@ -159,8 +185,8 @@ typedef struct block_t {
         acceleration;                       // acceleration mm/sec^2
 
   union {
-    abce_ulong_t steps;                     // Step count along each axis
-    abce_long_t position;                   // New position to force when this sync block is executed
+    swordfish::math::Vector6u32 steps;                     // Step count along each axis
+    swordfish::math::Vector6i32 position;                   // New position to force when this sync block is executed
   };
   uint32_t step_event_count;                // The number of step events required to complete this block
 
@@ -258,15 +284,15 @@ typedef struct block_t {
 #endif
 
 typedef struct {
-   uint32_t max_acceleration_mm_per_s2[XYZE_N], // (mm/s^2) M201 XYZE
-            min_segment_time_us;                // (µs) M205 B
-      float axis_steps_per_mm[XYZE_N];          // (steps) M92 XYZE - Steps per millimeter
- feedRate_t max_feedrate_mm_s[XYZE_N];          // (mm/s) M203 XYZE - Max speeds
-      float acceleration,                       // (mm/s^2) M204 S - Normal acceleration. DEFAULT ACCELERATION for all printing moves.
-            retract_acceleration,               // (mm/s^2) M204 R - Retract acceleration. Filament pull-back and push-forward while standing still in the other axes
-            travel_acceleration;                // (mm/s^2) M204 T - Travel acceleration. DEFAULT ACCELERATION for all NON printing moves.
- feedRate_t min_feedrate_mm_s,                  // (mm/s) M205 S - Minimum linear feedrate
-            min_travel_feedrate_mm_s;           // (mm/s) M205 T - Minimum travel feedrate
+	swordfish::math::Vector6u32 max_acceleration_unit_per_s2; // (mm/s^2) M201 XYZE
+	u32 min_segment_time_us;                // (µs) M205 B
+	swordfish::math::Vector6f32 axis_steps_per_unit;          // (steps) M92 XYZE - Steps per millimeter
+	swordfish::math::Vector6f32 max_feedrate_unit_per_s;          // (mm/s) M203 XYZE - Max speeds
+	f32 acceleration;                       // (mm/s^2) M204 S - Normal acceleration. DEFAULT ACCELERATION for all printing moves.
+	f32 retract_acceleration;               // (mm/s^2) M204 R - Retract acceleration. Filament pull-back and push-forward while standing still in the other axes
+	f32 travel_acceleration;               // (mm/s^2) M204 T - Travel acceleration. DEFAULT ACCELERATION for all NON printing moves.
+	f32 min_feedrate_unit_per_s;                  // (mm/s) M205 S - Minimum linear feedrate
+	f32 min_travel_feedrate_unit_per_s;           // (mm/s) M205 T - Minimum travel feedrate
 } planner_settings_t;
 
 #if DISABLED(SKEW_CORRECTION)
@@ -350,8 +376,8 @@ class Planner {
       static laser_state_t laser_inline;
     #endif
 
-    static uint32_t max_acceleration_steps_per_s2[XYZE_N]; // (steps/s^2) Derived from mm_per_s2
-    static float steps_to_mm[XYZE_N];           // Millimeters per step
+    static swordfish::math::Vector6u32 max_acceleration_steps_per_s2; // (steps/s^2) Derived from mm_per_s2
+    static swordfish::math::Vector6f32 steps_to_unit;           // Millimeters per step
 
     #if HAS_JUNCTION_DEVIATION
       static float junction_deviation_mm;       // (mm) M205 J
@@ -385,7 +411,7 @@ class Planner {
      * The current position of the tool in absolute steps
      * Recalculated if any axis_steps_per_mm are changed by gcode
      */
-    static xyze_long_t position;
+    static swordfish::math::Vector6i32 position;
 
     #if HAS_POSITION_FLOAT
       static xyze_pos_t position_float;
@@ -423,7 +449,7 @@ class Planner {
     /**
      * Speed of previous path line segment
      */
-    static xyze_float_t previous_speed;
+    static swordfish::math::Vector6f32 previous_speed;
 
     /**
      * Nominal speed of previous path line segment (mm/s)^2
@@ -464,9 +490,9 @@ class Planner {
 
     static void reset_acceleration_rates();
     static void refresh_positioning();
-    static void set_max_acceleration(const uint8_t axis, float targetValue);
-    static void set_max_feedrate(const uint8_t axis, float targetValue);
-    static void set_max_jerk(const AxisEnum axis, float targetValue);
+    static void set_max_acceleration(const Axis axis, float targetValue);
+    static void set_max_feedrate(const Axis axis, float targetValue);
+    static void set_max_jerk(const Axis axis, float targetValue);
 
 
     #if EXTRUDERS
@@ -670,18 +696,18 @@ class Planner {
      * Add a new linear movement to the buffer (in terms of steps).
      *
      *  target      - target position in steps units
-     *  fr_mm_s     - (target) speed of the move
+     *  feed_rate     - (target) speed of the move
      *  extruder    - target extruder
      *  millimeters - the length of the movement, if known
      *
      * Returns true if movement was buffered, false otherwise
      */
     static bool _buffer_steps(
-			const xyze_long_t &target,
+			const swordfish::math::Vector6i32 &target,
       #if HAS_POSITION_FLOAT
         const xyze_pos_t &target_float,
       #endif
-      const feedRate_t fr_mm_s,
+      const swordfish::motion::FeedRate feed_rate,
 			const uint8_t extruder,
 			const swordfish::status::MachineState machine_state,
 			const float &millimeters = 0.0,
@@ -694,7 +720,7 @@ class Planner {
      * Fills a new linear movement in the block (in terms of steps).
      *
      *  target      - target position in steps units
-     *  fr_mm_s     - (target) speed of the move
+     *  feed_rate     - (target) speed of the move
      *  extruder    - target extruder
      *  millimeters - the length of the movement, if known
      *
@@ -703,11 +729,11 @@ class Planner {
     static bool _populate_block(
 			block_t * const block,
 			bool split_move,
-      const xyze_long_t &target,
+      const swordfish::math::Vector6i32 &target,
       #if HAS_POSITION_FLOAT
         const xyze_pos_t &target_float,
       #endif
-      feedRate_t fr_mm_s,
+      swordfish::motion::FeedRate feed_rate,
 			const uint8_t extruder,
 			const swordfish::status::MachineState machine_state,
 			const float &millimeters = 0.0,
@@ -735,43 +761,19 @@ class Planner {
      * Leveling and kinematics should be applied ahead of calling this.
      *
      *  a,b,c,e     - target positions in mm and/or degrees
-     *  fr_mm_s     - (target) speed of the move
+     *  feed_rate     - (target) speed of the move
      *  extruder    - target extruder
      *  millimeters - the length of the movement, if known
      */
     static bool buffer_segment(
-			const float &a,
-			const float &b,
-			const float &c,
-			const float &e,
-      const feedRate_t &fr_mm_s,
+			const swordfish::math::Vector6f32 &raw,
+      const swordfish::motion::FeedRate &feed_rate,
 			const uint8_t extruder,
 			const swordfish::status::MachineState machine_state,
 			const float &millimeters = 0.0,
 			const float32_t accel_mm_s2 = 0.0
     );
 
-    FORCE_INLINE static bool buffer_segment(
-			abce_pos_t &abce,
-      const feedRate_t &fr_mm_s,
-			const uint8_t extruder,
-			const swordfish::status::MachineState machine_state,
-			const float &millimeters = 0.0,
-			const float32_t accel_mm_s2 = 0.0
-    ) {
-      return buffer_segment(
-				abce.a,
-				abce.b,
-				abce.c,
-				abce.e,
-        fr_mm_s,
-				extruder,
-				machine_state,
-				millimeters,
-				accel_mm_s2
-			);
-    }
-
   public:
 
     /**
@@ -780,44 +782,20 @@ class Planner {
      * delta/scara if needed.
      *
      *  rx,ry,rz,e   - target position in mm or degrees
-     *  fr_mm_s      - (target) speed of the move (mm/s)
+     *  feed_rate      - (target) speed of the move (mm/s)
      *  extruder     - target extruder
      *  millimeters  - the length of the movement, if known
      *  inv_duration - the reciprocal if the duration of the movement, if known (kinematic only if feeedrate scaling is enabled)
      */
     static bool buffer_line(
-			const float &rx,
-			const float &ry,
-			const float &rz,
-			const float &e,
-			const feedRate_t &fr_mm_s,
+			const swordfish::math::Vector6f32 &cart,
+			const swordfish::motion::FeedRate &feed_rate,
 			const uint8_t extruder,
 			const swordfish::status::MachineState machine_state,
 			const float millimeters = 0.0,
 			const float32_t accel_mm_s2 = 0.0
     );
 
-    FORCE_INLINE static bool buffer_line(
-			const xyze_pos_t &cart,
-			const feedRate_t &fr_mm_s,
-			const uint8_t extruder,
-			const swordfish::status::MachineState machine_state,
-			const float millimeters = 0.0,
-			const float32_t accel_mm_s2 = 0.0
-    ) {
-      return buffer_line(
-				cart.x,
-				cart.y,
-				cart.z,
-				cart.e,
-				fr_mm_s,
-				extruder,
-				machine_state,
-				millimeters,
-				accel_mm_s2
-      );
-    }
-
     #if ENABLED(DIRECT_STEPPING)
       static void buffer_page(const page_idx_t page_idx, const uint8_t extruder, const uint16_t num_steps);
     #endif
@@ -836,7 +814,7 @@ class Planner {
      * Clears previous speed values.
      */
     static void set_position_mm(const float &rx, const float &ry, const float &rz, const float &e);
-    FORCE_INLINE static void set_position_mm(const xyze_pos_t &cart) { set_position_mm(cart.x, cart.y, cart.z, cart.e); }
+    FORCE_INLINE static void set_position_mm(const swordfish::math::Vector6f32 &cart) { set_position_mm(cart.x(), cart.y(), cart.z(), cart.a()); }
     static void set_e_position_mm(const float &e);
 
     /**
@@ -846,22 +824,23 @@ class Planner {
      * conversions are applied.
      */
     static void set_machine_position_mm(const float &a, const float &b, const float &c, const float &e);
-    FORCE_INLINE static void set_machine_position_mm(const abce_pos_t &abce) { set_machine_position_mm(abce.a, abce.b, abce.c, abce.e); }
+    FORCE_INLINE static void set_machine_position_mm(const swordfish::math::Vector6f32 &abce) { set_machine_position_mm(abce.x(), abce.y(), abce.z(), abce.a()); }
 
     /**
      * Get an axis position according to stepper position(s)
      * For CORE machines apply translation from ABC to XYZ.
      */
-    static float get_axis_position_mm(const AxisEnum axis);
-
-    static inline abce_pos_t get_axis_positions_mm() {
-      const abce_pos_t out = {
-        get_axis_position_mm(A_AXIS),
-        get_axis_position_mm(B_AXIS),
-        get_axis_position_mm(C_AXIS),
-        get_axis_position_mm(E_AXIS)
+    static float get_axis_position_mm(const Axis axis);
+
+    static inline swordfish::math::Vector6f32 get_axis_positions_mm() {
+      return {
+        get_axis_position_mm(Axis::X()),
+        get_axis_position_mm(Axis::Y()),
+        get_axis_position_mm(Axis::Z()),
+        get_axis_position_mm(Axis::A()),
+				get_axis_position_mm(Axis::A()),
+				get_axis_position_mm(Axis::A()),
       };
-      return out;
     }
 
     // SCARA AB axes are in degrees, not mm
@@ -874,10 +853,10 @@ class Planner {
     static void quick_stop();
 
     // Called when an endstop is triggered. Causes the machine to stop inmediately
-    static void endstop_triggered(const AxisEnum axis);
+    static void endstop_triggered(const Axis axis);
 
     // Triggered position of an axis in mm (not core-savvy)
-    static float triggered_position_mm(const AxisEnum axis);
+    static float triggered_position_mm(const Axis axis);
 
     // Block until all buffered steps are executed / cleaned
     static void synchronize();
@@ -998,18 +977,22 @@ class Planner {
 
     #if HAS_JUNCTION_DEVIATION
 
-      FORCE_INLINE static void normalize_junction_vector(xyze_float_t &vector) {
+      FORCE_INLINE static void normalize_junction_vector(swordfish::math::Vector6f32 &vector) {
         float magnitude_sq = 0;
-        LOOP_XYZE(idx) if (vector[idx]) magnitude_sq += sq(vector[idx]);
+				for (auto axis : all_axes) {
+        	if (vector[axis]) {
+						magnitude_sq += sq(vector[axis]);
+					}
+				}
         vector *= RSQRT(magnitude_sq);
       }
 
-      FORCE_INLINE static float limit_value_by_axis_maximum(const float &max_value, xyze_float_t &unit_vec) {
+      FORCE_INLINE static float limit_value_by_axis_maximum(const float &max_value, swordfish::math::Vector6f32 &unit_vec) {
         float limit_value = max_value;
-        LOOP_XYZE(idx) {
-          if (unit_vec[idx]) {
-            if (limit_value * ABS(unit_vec[idx]) > settings.max_acceleration_mm_per_s2[idx])
-              limit_value = ABS(settings.max_acceleration_mm_per_s2[idx] / unit_vec[idx]);
+        for (auto axis : all_axes ) {
+          if (unit_vec[axis]) {
+            if (limit_value * ABS(unit_vec[axis]) > settings.max_acceleration_unit_per_s2[axis])
+              limit_value = ABS(settings.max_acceleration_unit_per_s2[axis] / unit_vec[axis]);
           }
         }
         return limit_value;
diff --git a/src/marlin/module/settings.cpp b/src/marlin/module/settings.cpp
index a5a3361037..8605a7fd87 100644
--- a/src/marlin/module/settings.cpp
+++ b/src/marlin/module/settings.cpp
@@ -48,6 +48,7 @@
 #include <swordfish/Controller.h>
 
 using namespace swordfish;
+using namespace swordfish::math;
 using namespace swordfish::motion;
 
 #include "endstops.h"
@@ -206,7 +207,15 @@ typedef struct SettingsDataStruct {
 	//
 	uint8_t esteppers; // XYZE_N - XYZ
 
-	planner_settings_t planner_settings;
+	u32 max_acceleration_unit_per_s2[4];
+	u32 min_segment_time_us;
+	f32 axis_steps_per_unit[4];
+	f32 max_feed_rate_unit_per_s[4];
+	f32 acceleration;
+	f32 retract_acceleration;
+	f32 travel_acceleration;
+	f32 min_feedrate_unit_per_s;
+	f32 min_travel_feedrate_unit_per_s;
 
 	xyze_float_t planner_max_jerk; // M205 XYZE  planner.max_jerk
 	float planner_junction_deviation_mm; // M205 J     planner.junction_deviation_mm
@@ -503,7 +512,7 @@ float new_z_fade_height;
 #endif
 
 void MarlinSettings::postprocess() {
-	xyz_pos_t oldpos = current_position;
+	Vector6f32 oldpos = current_position;
 
 	// steps per s2 needs to be updated to agree with units per s2
 	planner.reset_acceleration_rates();
@@ -658,14 +667,33 @@ bool MarlinSettings::save() {
 
 	_FIELD_TEST(esteppers);
 
-	const uint8_t esteppers = COUNT(planner.settings.axis_steps_per_mm) - XYZ;
+	const uint8_t esteppers = 1;
 	EEPROM_WRITE(esteppers);
 
 	//
 	// Planner Motion
 	//
 	{
-		EEPROM_WRITE(planner.settings);
+		u32 tmp1[4];
+		f32 tmp2[4];
+		f32 tmp3[4];
+
+		for (auto i = 0; i < 4; i++) {
+			tmp1[i] = planner.settings.max_acceleration_unit_per_s2[i];
+			tmp2[i] = planner.settings.axis_steps_per_unit[i];
+			tmp3[i] = planner.settings.max_feedrate_unit_per_s[i];
+		}
+
+		EEPROM_WRITE(tmp1); // max_acceleration_mm_per_s2
+		EEPROM_WRITE(planner.settings.min_segment_time_us);
+		EEPROM_WRITE(tmp2); // axis_steps_per_mm
+		EEPROM_WRITE(tmp3); // max_feedrate_mm_s
+
+		EEPROM_WRITE(planner.settings.acceleration);
+		EEPROM_WRITE(planner.settings.retract_acceleration);
+		EEPROM_WRITE(planner.settings.travel_acceleration);
+		EEPROM_WRITE(planner.settings.min_feedrate_unit_per_s);
+		EEPROM_WRITE(planner.settings.min_travel_feedrate_unit_per_s);
 
 #	if HAS_CLASSIC_JERK
 		EEPROM_WRITE(planner.max_jerk);
@@ -1244,7 +1272,7 @@ bool MarlinSettings::save() {
 		_FIELD_TEST(tmc_stealth_enabled);
 
 		tmc_stealth_enabled_t tmc_stealth_enabled = { false, false, false, false, false, false, false, false, false, false, false, false, false, false, false };
-#	if AXIS_HAS_STEALTHCHOP(X)
+/*#	if AXIS_HAS_STEALTHCHOP(X)
 		tmc_stealth_enabled.X = stepperX.get_stored_stealthChop();
 #	endif
 #	if AXIS_HAS_STEALTHCHOP(Y)
@@ -1292,6 +1320,7 @@ bool MarlinSettings::save() {
 #	if AXIS_HAS_STEALTHCHOP(E7)
 		tmc_stealth_enabled.E7 = stepperE7.get_stored_stealthChop();
 #	endif
+*/
 		EEPROM_WRITE(tmc_stealth_enabled);
 	}
 
@@ -1545,9 +1574,9 @@ bool MarlinSettings::_load() {
 			// Get only the number of E stepper parameters previously stored
 			// Any steppers added later are set to their defaults
 
-			uint32_t tmp1[XYZE_N];
-			float tmp2[XYZE_N];
-			feedRate_t tmp3[XYZE_N];
+			uint32_t tmp1[4];
+			float tmp2[4];
+			feedRate_t tmp3[4];
 
 			EEPROM_READ(tmp1); // max_acceleration_mm_per_s2
 			EEPROM_READ(planner.settings.min_segment_time_us);
@@ -1555,18 +1584,18 @@ bool MarlinSettings::_load() {
 			EEPROM_READ(tmp3); // max_feedrate_mm_s
 
 			if (!validating)
-				LOOP_XYZE_N(i) {
+				for (auto i = 0; i < 4; i++) {
 					const bool in = (i < esteppers + XYZ);
-					planner.settings.max_acceleration_mm_per_s2[i] = in ? tmp1[i] : pgm_read_dword(&_DMA[ALIM(i, _DMA)]);
-					planner.settings.axis_steps_per_mm[i] = in ? tmp2[i] : pgm_read_float(&_DASU[ALIM(i, _DASU)]);
-					planner.settings.max_feedrate_mm_s[i] = in ? tmp3[i] : pgm_read_float(&_DMF[ALIM(i, _DMF)]);
+					planner.settings.max_acceleration_unit_per_s2[i] = in ? tmp1[i] : pgm_read_dword(&_DMA[ALIM(i, _DMA)]);
+					planner.settings.axis_steps_per_unit[i] = in ? tmp2[i] : pgm_read_float(&_DASU[ALIM(i, _DASU)]);
+					planner.settings.max_feedrate_unit_per_s[i] = in ? tmp3[i] : pgm_read_float(&_DMF[ALIM(i, _DMF)]);
 				}
 
 			EEPROM_READ(planner.settings.acceleration);
 			EEPROM_READ(planner.settings.retract_acceleration);
 			EEPROM_READ(planner.settings.travel_acceleration);
-			EEPROM_READ(planner.settings.min_feedrate_mm_s);
-			EEPROM_READ(planner.settings.min_travel_feedrate_mm_s);
+			EEPROM_READ(planner.settings.min_feedrate_unit_per_s);
+			EEPROM_READ(planner.settings.min_travel_feedrate_unit_per_s);
 
 #	if HAS_CLASSIC_JERK
 			EEPROM_READ(planner.max_jerk);
@@ -2653,18 +2682,18 @@ bool MarlinSettings::save() {
 void MarlinSettings::reset() {
 	// Controller::getInstance().reset();
 
-	LOOP_XYZE_N(i) {
-		planner.settings.max_acceleration_mm_per_s2[i] = pgm_read_dword(&_DMA[ALIM(i, _DMA)]);
-		planner.settings.axis_steps_per_mm[i] = pgm_read_float(&_DASU[ALIM(i, _DASU)]);
-		planner.settings.max_feedrate_mm_s[i] = pgm_read_float(&_DMF[ALIM(i, _DMF)]);
+	for (auto i = 0; i < 4; i++) {
+		planner.settings.max_acceleration_unit_per_s2[i] = pgm_read_dword(&_DMA[ALIM(i, _DMA)]);
+		planner.settings.axis_steps_per_unit[i] = pgm_read_float(&_DASU[ALIM(i, _DASU)]);
+		planner.settings.max_feedrate_unit_per_s[i] = pgm_read_float(&_DMF[ALIM(i, _DMF)]);
 	}
 
 	planner.settings.min_segment_time_us = DEFAULT_MINSEGMENTTIME;
 	planner.settings.acceleration = DEFAULT_ACCELERATION;
 	planner.settings.retract_acceleration = DEFAULT_RETRACT_ACCELERATION;
 	planner.settings.travel_acceleration = DEFAULT_TRAVEL_ACCELERATION;
-	planner.settings.min_feedrate_mm_s = feedRate_t(DEFAULT_MINIMUMFEEDRATE);
-	planner.settings.min_travel_feedrate_mm_s = feedRate_t(DEFAULT_MINTRAVELFEEDRATE);
+	planner.settings.min_feedrate_unit_per_s = feedRate_t(DEFAULT_MINIMUMFEEDRATE);
+	planner.settings.min_travel_feedrate_unit_per_s = feedRate_t(DEFAULT_MINTRAVELFEEDRATE);
 
 #if HAS_CLASSIC_JERK
 #	ifndef DEFAULT_XJERK
@@ -3147,7 +3176,7 @@ inline void say_units(const bool colon) {
 		SERIAL_ECHOLNPGM(":");
 }
 
-void report_M92(const bool echo = true, const int8_t e = -1);
+void report_M92(const bool echo = true);
 
 /**
  * M503 - Report current settings in RAM
@@ -3230,10 +3259,9 @@ void MarlinSettings::report(const bool forReplay) {
 	CONFIG_ECHO_HEADING("Maximum feedrates (units/m):");
 	CONFIG_ECHO_START();
 	SERIAL_ECHOLNPAIR_P(
-			PSTR("  M203 X"), LINEAR_UNIT(MMS_TO_MMM(planner.settings.max_feedrate_mm_s[X_AXIS])), SP_Y_STR, LINEAR_UNIT(MMS_TO_MMM(planner.settings.max_feedrate_mm_s[Y_AXIS])), SP_Z_STR, LINEAR_UNIT(MMS_TO_MMM(planner.settings.max_feedrate_mm_s[Z_AXIS]))
+			PSTR("  M203 X"), LINEAR_UNIT(MMS_TO_MMM(planner.settings.max_feedrate_unit_per_s[Axis::X()])), SP_Y_STR, LINEAR_UNIT(MMS_TO_MMM(planner.settings.max_feedrate_unit_per_s[Axis::Y()])), SP_Z_STR, LINEAR_UNIT(MMS_TO_MMM(planner.settings.max_feedrate_unit_per_s[Axis::Z()]))
 #	if DISABLED(DISTINCT_E_FACTORS)
-																																																																																													,
-			SP_E_STR, VOLUMETRIC_UNIT(planner.settings.max_feedrate_mm_s[E_AXIS])
+, SP_A_STR, parser.radial_value_to_degrees(planner.settings.max_feedrate_unit_per_s[Axis::A()])
 #	endif
 	);
 #	if ENABLED(DISTINCT_E_FACTORS)
@@ -3247,10 +3275,10 @@ void MarlinSettings::report(const bool forReplay) {
 	CONFIG_ECHO_HEADING("Maximum Acceleration (units/s2):");
 	CONFIG_ECHO_START();
 	SERIAL_ECHOLNPAIR_P(
-			PSTR("  M201 X"), LINEAR_UNIT(planner.settings.max_acceleration_mm_per_s2[X_AXIS]), SP_Y_STR, LINEAR_UNIT(planner.settings.max_acceleration_mm_per_s2[Y_AXIS]), SP_Z_STR, LINEAR_UNIT(planner.settings.max_acceleration_mm_per_s2[Z_AXIS])
+			PSTR("  M201 X"), parser.linear_value_to_mm(planner.settings.max_acceleration_unit_per_s2[Axis::X()]), SP_Y_STR, parser.linear_value_to_mm(planner.settings.max_acceleration_unit_per_s2[Axis::Y()]), SP_Z_STR, parser.linear_value_to_mm(planner.settings.max_acceleration_unit_per_s2[Axis::Z()])
 #	if DISABLED(DISTINCT_E_FACTORS)
 																																																																																										,
-			SP_E_STR, VOLUMETRIC_UNIT(planner.settings.max_acceleration_mm_per_s2[E_AXIS])
+			SP_A_STR, parser.radial_value_to_degrees(planner.settings.max_acceleration_unit_per_s2[Axis::A()])
 #	endif
 	);
 #	if ENABLED(DISTINCT_E_FACTORS)
@@ -3277,10 +3305,10 @@ void MarlinSettings::report(const bool forReplay) {
 	);
 	CONFIG_ECHO_START();
 	SERIAL_ECHOLNPAIR_P(
-			PSTR("  M205 B"), LINEAR_UNIT(planner.settings.min_segment_time_us), PSTR(" S"), LINEAR_UNIT(planner.settings.min_feedrate_mm_s), SP_T_STR, LINEAR_UNIT(planner.settings.min_travel_feedrate_mm_s)
+			PSTR("  M205 B"), parser.linear_value_to_mm(planner.settings.min_segment_time_us), PSTR(" S"), parser.linear_value_to_mm(planner.settings.min_feedrate_unit_per_s), SP_T_STR, parser.linear_value_to_mm(planner.settings.min_travel_feedrate_unit_per_s)
 #	if HAS_JUNCTION_DEVIATION
 																																																																											,
-			PSTR(" J"), LINEAR_UNIT(planner.junction_deviation_mm)
+			PSTR(" J"), parser.linear_value_to_mm(planner.junction_deviation_mm)
 #	endif
 #	if HAS_CLASSIC_JERK
 											,
@@ -3299,12 +3327,12 @@ void MarlinSettings::report(const bool forReplay) {
 	CONFIG_ECHO_START();
 	SERIAL_ECHOLNPAIR_P(
 #		if IS_CARTESIAN
-			PSTR("  M206 X"), LINEAR_UNIT(homeOffset(X)), SP_Y_STR, LINEAR_UNIT(homeOffset(Y)), SP_Z_STR
+			PSTR("  M206 X"), parser.linear_value_to_mm(homeOffset.x()), SP_Y_STR, parser.linear_value_to_mm(homeOffset.y()), SP_Z_STR
 #		else
 			PSTR("  M206 Z")
 #		endif
 			,
-			LINEAR_UNIT(homeOffset(Z)));
+			parser.linear_value_to_mm(homeOffset.z()));
 #	endif
 
 #	if HAS_HOTEND_OFFSET
diff --git a/src/marlin/module/stepper.cpp b/src/marlin/module/stepper.cpp
index 9c19dd5e44..9623d1448d 100644
--- a/src/marlin/module/stepper.cpp
+++ b/src/marlin/module/stepper.cpp
@@ -99,6 +99,7 @@ Stepper stepper; // Singleton
 
 #include <swordfish/Controller.h>
 
+using namespace swordfish::math;
 using namespace swordfish::status;
 
 #if ENABLED(INTEGRATED_BABYSTEPPING)
@@ -188,9 +189,9 @@ uint8_t Stepper::steps_per_isr;
 IF_DISABLED(ADAPTIVE_STEP_SMOOTHING, constexpr)
 uint8_t Stepper::oversampling_factor;
 
-xyze_long_t Stepper::delta_error { 0, 0, 0 };
+Vector6i32 Stepper::delta_error { 0, 0, 0, 0, 0, 0 };
 
-xyze_ulong_t Stepper::advance_dividend { 0, 0, 0 };
+Vector6u32 Stepper::advance_dividend { 0, 0, 0, 0, 0, 0 };
 uint32_t Stepper::advance_divisor = 0,
 				 Stepper::step_events_completed = 0, // The number of step events executed in the current block
 		Stepper::accelerate_until, // The count at which to stop accelerating
@@ -242,9 +243,9 @@ int32_t Stepper::ticks_nominal = -1;
 uint32_t Stepper::acc_step_rate; // needed for deceleration start point
 #endif
 
-xyz_long_t Stepper::endstops_trigsteps;
-xyze_long_t Stepper::count_position { 0, 0, 0 };
-xyze_int8_t Stepper::count_direction { 0, 0, 0 };
+Vector6i32 Stepper::endstops_trigsteps { 0, 0, 0, 0, 0, 0 };
+Vector6i32 Stepper::count_position { 0, 0, 0, 0, 0, 0 };
+Vector6i8 Stepper::count_direction { 0, 0, 0, 0, 0, 0 };
 
 #if ENABLED(LASER_POWER_INLINE_TRAPEZOID)
 Stepper::stepper_laser_t Stepper::laser_trap = {
@@ -494,9 +495,14 @@ Stepper::stepper_laser_t Stepper::laser_trap = {
 #	define Z_APPLY_STEP(v, Q) Z_STEP_WRITE(v)
 #endif
 
-#if DISABLED(MIXING_EXTRUDER)
-#	define E_APPLY_STEP(v, Q) E_STEP_WRITE(stepper_extruder, v)
-#endif
+#define A_APPLY_DIR(v, Q) A_DIR_WRITE(v)
+#define A_APPLY_STEP(v, Q) A_STEP_WRITE(v)
+
+#define B_APPLY_DIR(v, Q) B_DIR_WRITE(v)
+#define B_APPLY_STEP(v, Q) B_STEP_WRITE(v)
+
+#define C_APPLY_DIR(v, Q) C_DIR_WRITE(v)
+#define C_APPLY_STEP(v, Q) C_STEP_WRITE(v)
 
 #define CYCLES_TO_NS(CYC)           (1000UL * (CYC) / ((F_CPU) / 1000000))
 #define NS_PER_PULSE_TIMER_TICK     (1000000000UL / (STEPPER_TIMER_RATE))
@@ -552,60 +558,23 @@ void Stepper::set_directions() {
 	}
 
 #if HAS_X_DIR
-	SET_STEP_DIR(X); // A
+	SET_STEP_DIR(X);
 #endif
 #if HAS_Y_DIR
-	SET_STEP_DIR(Y); // B
+	SET_STEP_DIR(Y);
 #endif
 #if HAS_Z_DIR
-	SET_STEP_DIR(Z); // C
+	SET_STEP_DIR(Z);
 #endif
-
-#if DISABLED(LIN_ADVANCE)
-#	if ENABLED(MIXING_EXTRUDER)
-	                 // Because this is valid for the whole block we don't know
-	                 // what e-steppers will step. Likely all. Set all.
-	if (motor_direction(E_AXIS)) {
-		MIXER_STEPPER_LOOP(j)
-		REV_E_DIR(j);
-		count_direction.e = -1;
-	} else {
-		MIXER_STEPPER_LOOP(j)
-		NORM_E_DIR(j);
-		count_direction.e = 1;
-	}
-#	else
-	if (motor_direction(E_AXIS)) {
-		REV_E_DIR(stepper_extruder);
-		count_direction.e = -1;
-	} else {
-		NORM_E_DIR(stepper_extruder);
-		count_direction.e = 1;
-	}
-#	endif
-#endif // !LIN_ADVANCE
-
-#if HAS_L64XX
-	if (L64XX_OK_to_power_up) { // OK to send the direction commands (which powers up the L64XX steppers)
-		if (L64xxManager.spi_active) {
-			L64xxManager.spi_abort = true; // Interrupted SPI transfer needs to shut down gracefully
-			for (uint8_t j = 1; j <= L64XX::chain[0]; j++)
-				L6470_buf[j] = dSPIN_NOP; // Fill buffer with NOOPs
-			L64xxManager.transfer(L6470_buf, L64XX::chain[0]); // Send enough NOOPs to complete any command
-			L64xxManager.transfer(L6470_buf, L64XX::chain[0]);
-			L64xxManager.transfer(L6470_buf, L64XX::chain[0]);
-		}
-
-		// L64xxManager.dir_commands[] is an array that holds direction command for each stepper
-
-		// Scan command array, copy matches into L64xxManager.transfer
-		for (uint8_t j = 1; j <= L64XX::chain[0]; j++)
-			L6470_buf[j] = L64xxManager.dir_commands[L64XX::chain[j]];
-
-		L64xxManager.transfer(L6470_buf, L64XX::chain[0]); // send the command stream to the drivers
-	}
+#if HAS_A_DIR
+	SET_STEP_DIR(A);
+#endif
+#if HAS_B_DIR
+	SET_STEP_DIR(B);
+#endif
+#if HAS_C_DIR
+	SET_STEP_DIR(C);
 #endif
-
 	DIR_WAIT_AFTER();
 }
 
@@ -1618,7 +1587,7 @@ void Stepper::pulse_phase_isr() {
 	bool firstStep = true;
 	USING_TIMED_PULSE();
 #endif
-	xyze_bool_t step_needed { 0, 0, 0 };
+	bool step_needed[6] { false, false, false, false, false, false };
 
 	do {
 		// If we must abort the current block, do so!
@@ -1791,21 +1760,14 @@ void Stepper::pulse_phase_isr() {
 #if HAS_Z_STEP
 			PULSE_PREP(Z);
 #endif
-
-#if EITHER(LIN_ADVANCE, MIXING_EXTRUDER)
-			delta_error.e += advance_dividend.e;
-			if (delta_error.e >= 0) {
-				count_position.e += count_direction.e;
-#	if ENABLED(LIN_ADVANCE)
-				delta_error.e -= advance_divisor;
-				// Don't step E here - But remember the number of steps to perform
-				motor_direction(E_AXIS) ? --LA_steps : ++LA_steps;
-#	else
-				step_needed.e = true;
-#	endif
-			}
-#elif HAS_E0_STEP
-			PULSE_PREP(E);
+#if HAS_A_STEP
+			PULSE_PREP(A);
+#endif
+#if HAS_B_STEP
+			PULSE_PREP(B);
+#endif
+#if HAS_C_STEP
+			PULSE_PREP(C);
 #endif
 		}
 
@@ -1826,14 +1788,14 @@ void Stepper::pulse_phase_isr() {
 #if HAS_Z_STEP
 		PULSE_START(Z);
 #endif
-
-#if DISABLED(LIN_ADVANCE)
-#	if ENABLED(MIXING_EXTRUDER)
-		if (step_needed.e)
-			E_STEP_WRITE(mixer.get_next_stepper(), !INVERT_E_STEP_PIN);
-#	elif HAS_E0_STEP
-		PULSE_START(E);
-#	endif
+#if HAS_A_STEP
+		PULSE_START(A);
+#endif
+#if HAS_B_STEP
+		PULSE_START(B);
+#endif
+#if HAS_C_STEP
+		PULSE_START(C);
 #endif
 
 #if ENABLED(I2S_STEPPER_STREAM)
@@ -1856,16 +1818,14 @@ void Stepper::pulse_phase_isr() {
 #if HAS_Z_STEP
 		PULSE_STOP(Z);
 #endif
-
-#if DISABLED(LIN_ADVANCE)
-#	if ENABLED(MIXING_EXTRUDER)
-		if (delta_error.e >= 0) {
-			delta_error.e -= advance_divisor;
-			E_STEP_WRITE(mixer.get_stepper(), INVERT_E_STEP_PIN);
-		}
-#	elif HAS_E0_STEP
-		PULSE_STOP(E);
-#	endif
+#if HAS_A_STEP
+		PULSE_STOP(A);
+#endif
+#if HAS_B_STEP
+		PULSE_STOP(B);
+#endif
+#if HAS_C_STEP
+		PULSE_STOP(C);
 #endif
 
 #if ISR_MULTI_STEPS
@@ -1942,15 +1902,6 @@ uint32_t Stepper::block_phase_isr() {
 				interval = calc_timer_interval(acc_step_rate, &steps_per_isr);
 				acceleration_time += interval;
 
-#if ENABLED(LIN_ADVANCE)
-				if (LA_use_advance_lead) {
-					// Fire ISR if final adv_rate is reached
-					if (LA_steps && LA_isr_rate != current_block->advance_speed)
-						nextAdvanceISR = 0;
-				} else if (LA_steps)
-					nextAdvanceISR = 0;
-#endif
-
 // Update laser - Accelerating
 #if ENABLED(LASER_POWER_INLINE_TRAPEZOID)
 				if (laser_trap.enabled) {
@@ -2016,17 +1967,6 @@ uint32_t Stepper::block_phase_isr() {
 				interval = calc_timer_interval(step_rate, &steps_per_isr);
 				deceleration_time += interval;
 
-#if ENABLED(LIN_ADVANCE)
-				if (LA_use_advance_lead) {
-					// Wake up eISR on first deceleration loop and fire ISR if final adv_rate is reached
-					if (step_events_completed <= decelerate_after + steps_per_isr || (LA_steps && LA_isr_rate != current_block->advance_speed)) {
-						initiateLA();
-						LA_isr_rate = current_block->advance_speed;
-					}
-				} else if (LA_steps)
-					nextAdvanceISR = 0;
-#endif // LIN_ADVANCE
-
 // Update laser - Decelerating
 #if ENABLED(LASER_POWER_INLINE_TRAPEZOID)
 				if (laser_trap.enabled) {
@@ -2115,105 +2055,16 @@ uint32_t Stepper::block_phase_isr() {
 			}
 
 			update_state();
-// For non-inline cutter, grossly apply power
-#if ENABLED(LASER_FEATURE) && DISABLED(LASER_POWER_INLINE)
-			cutter.apply_power(current_block->cutter_power);
-#endif
-
-			TERN_(POWER_LOSS_RECOVERY, recovery.info.sdpos = current_block->sdpos);
-
-#if ENABLED(DIRECT_STEPPING)
-			if (IS_PAGE(current_block)) {
-				page_step_state.segment_steps = 0;
-				page_step_state.segment_idx = 0;
-				page_step_state.page = page_manager.get_page(current_block->page_idx);
-				page_step_state.bd.reset();
 
-				if (DirectStepping::Config::DIRECTIONAL)
-					current_block->direction_bits = last_direction_bits;
+			// Flag all moving axes for proper endstop handling
+			uint8_t axis_bits = 0;
 
-				if (!page_step_state.page) {
-					discard_current_block();
-					return interval;
+			for (auto axis : all_axes) {
+				if (!!current_block->steps[axis]) {
+					SBI(axis_bits, axis);
 				}
 			}
-#endif
-
-			// Flag all moving axes for proper endstop handling
-
-#if IS_CORE
-// Define conditions for checking endstops
-#	define S_(N) current_block->steps[CORE_AXIS_##N]
-#	define D_(N) TEST(current_block->direction_bits, CORE_AXIS_##N)
-#endif
-
-#if CORE_IS_XY || CORE_IS_XZ
-/**
- * Head direction in -X axis for CoreXY and CoreXZ bots.
- *
- * If steps differ, both axes are moving.
- * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Y or Z, handled below)
- * If DeltaA ==  DeltaB, the movement is only in the 1st axis (X)
- */
-#	if EITHER(COREXY, COREXZ)
-#		define X_CMP(A, B) ((A) == (B))
-#	else
-#		define X_CMP(A, B) ((A) != (B))
-#	endif
-#	define X_MOVE_TEST (S_(1) != S_(2) || (S_(1) > 0 && X_CMP(D_(1), D_(2))))
-#elif ENABLED(MARKFORGED_XY)
-#	define X_MOVE_TEST (current_block->steps.a != current_block->steps.b)
-#else
-#	define X_MOVE_TEST !!current_block->steps.a
-#endif
-
-#if CORE_IS_XY || CORE_IS_YZ
-/**
- * Head direction in -Y axis for CoreXY / CoreYZ bots.
- *
- * If steps differ, both axes are moving
- * If DeltaA ==  DeltaB, the movement is only in the 1st axis (X or Y)
- * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Y or Z)
- */
-#	if EITHER(COREYX, COREYZ)
-#		define Y_CMP(A, B) ((A) == (B))
-#	else
-#		define Y_CMP(A, B) ((A) != (B))
-#	endif
-#	define Y_MOVE_TEST (S_(1) != S_(2) || (S_(1) > 0 && Y_CMP(D_(1), D_(2))))
-#else
-#	define Y_MOVE_TEST !!current_block->steps.b
-#endif
-
-#if CORE_IS_XZ || CORE_IS_YZ
-/**
- * Head direction in -Z axis for CoreXZ or CoreYZ bots.
- *
- * If steps differ, both axes are moving
- * If DeltaA ==  DeltaB, the movement is only in the 1st axis (X or Y, already handled above)
- * If DeltaA == -DeltaB, the movement is only in the 2nd axis (Z)
- */
-#	if EITHER(COREZX, COREZY)
-#		define Z_CMP(A, B) ((A) == (B))
-#	else
-#		define Z_CMP(A, B) ((A) != (B))
-#	endif
-#	define Z_MOVE_TEST (S_(1) != S_(2) || (S_(1) > 0 && Z_CMP(D_(1), D_(2))))
-#else
-#	define Z_MOVE_TEST !!current_block->steps.c
-#endif
 
-			uint8_t axis_bits = 0;
-			if (X_MOVE_TEST)
-				SBI(axis_bits, A_AXIS);
-			if (Y_MOVE_TEST)
-				SBI(axis_bits, B_AXIS);
-			if (Z_MOVE_TEST)
-				SBI(axis_bits, C_AXIS);
-			// if (!!current_block->steps.e) SBI(axis_bits, E_AXIS);
-			// if (!!current_block->steps.a) SBI(axis_bits, X_HEAD);
-			// if (!!current_block->steps.b) SBI(axis_bits, Y_HEAD);
-			// if (!!current_block->steps.c) SBI(axis_bits, Z_HEAD);
 			axis_did_move = axis_bits;
 
 			// No acceleration / deceleration time elapsed so far
@@ -2236,11 +2087,14 @@ uint32_t Stepper::block_phase_isr() {
 			// Based on the oversampling factor, do the calculations
 			step_event_count = current_block->step_event_count << oversampling;
 
-			// Initialize Bresenham delta errors to 1/2
-			delta_error = -int32_t(step_event_count);
+			for (auto axis : all_axes) {
+				// Initialize Bresenham delta errors to 1/2
+				delta_error[axis] = -i32(step_event_count);
+
+				// Calculate Bresenham dividends and divisors
+				advance_dividend[axis] = current_block->steps[axis] << 1;
+			}
 
-			// Calculate Bresenham dividends and divisors
-			advance_dividend = current_block->steps << 1;
 			advance_divisor = step_event_count << 1;
 
 			// No step events completed so far
@@ -2371,103 +2225,6 @@ uint32_t Stepper::block_phase_isr() {
 	return interval;
 }
 
-#if ENABLED(LIN_ADVANCE)
-
-// Timer interrupt for E. LA_steps is set in the main routine
-uint32_t Stepper::advance_isr() {
-	uint32_t interval;
-
-	if (LA_use_advance_lead) {
-		if (step_events_completed > decelerate_after && LA_current_adv_steps > LA_final_adv_steps) {
-			LA_steps--;
-			LA_current_adv_steps--;
-			interval = LA_isr_rate;
-		} else if (step_events_completed < decelerate_after && LA_current_adv_steps < LA_max_adv_steps) {
-			LA_steps++;
-			LA_current_adv_steps++;
-			interval = LA_isr_rate;
-		} else
-			interval = LA_isr_rate = LA_ADV_NEVER;
-	} else
-		interval = LA_ADV_NEVER;
-
-	if (!LA_steps)
-		return interval; // Leave pins alone if there are no steps!
-
-	DIR_WAIT_BEFORE();
-
-#	if ENABLED(MIXING_EXTRUDER)
-	// We don't know which steppers will be stepped because LA loop follows,
-	// with potentially multiple steps. Set all.
-	if (LA_steps > 0)
-		MIXER_STEPPER_LOOP(j)
-	NORM_E_DIR(j);
-	else if (LA_steps < 0)
-			MIXER_STEPPER_LOOP(j)
-					REV_E_DIR(j);
-#	else
-	if (LA_steps > 0)
-		NORM_E_DIR(stepper_extruder);
-	else if (LA_steps < 0)
-		REV_E_DIR(stepper_extruder);
-#	endif
-
-	DIR_WAIT_AFTER();
-
-// const hal_timer_t added_step_ticks = hal_timer_t(ADDED_STEP_TICKS);
-
-// Step E stepper if we have steps
-#	if ISR_MULTI_STEPS
-	bool firstStep = true;
-	USING_TIMED_PULSE();
-#	endif
-
-	while (LA_steps) {
-#	if ISR_MULTI_STEPS
-		if (firstStep)
-			firstStep = false;
-		else
-			AWAIT_LOW_PULSE();
-#	endif
-
-// Set the STEP pulse ON
-#	if ENABLED(MIXING_EXTRUDER)
-		E_STEP_WRITE(mixer.get_next_stepper(), !INVERT_E_STEP_PIN);
-#	else
-		E_STEP_WRITE(stepper_extruder, !INVERT_E_STEP_PIN);
-#	endif
-
-// Enforce a minimum duration for STEP pulse ON
-#	if ISR_PULSE_CONTROL
-		START_HIGH_PULSE();
-#	endif
-
-		LA_steps < 0 ? ++LA_steps : --LA_steps;
-
-#	if ISR_PULSE_CONTROL
-		AWAIT_HIGH_PULSE();
-#	endif
-
-// Set the STEP pulse OFF
-#	if ENABLED(MIXING_EXTRUDER)
-		E_STEP_WRITE(mixer.get_stepper(), INVERT_E_STEP_PIN);
-#	else
-		E_STEP_WRITE(stepper_extruder, INVERT_E_STEP_PIN);
-#	endif
-
-// For minimum pulse time wait before looping
-// Just wait for the requested pulse duration
-#	if ISR_PULSE_CONTROL
-		if (LA_steps)
-			START_LOW_PULSE();
-#	endif
-	} // LA_steps
-
-	return interval;
-}
-
-#endif // LIN_ADVANCE
-
 #if ENABLED(INTEGRATED_BABYSTEPPING)
 
 // Timer interrupt for baby-stepping
@@ -2544,32 +2301,20 @@ void Stepper::init() {
 	Z4_DIR_INIT();
 #	endif
 #endif
-#if HAS_E0_DIR
-	E0_DIR_INIT();
-#endif
-#if HAS_E1_DIR
-	E1_DIR_INIT();
-#endif
-#if HAS_E2_DIR
-	E2_DIR_INIT();
+#if HAS_A_DIR
+	A_DIR_INIT();
 #endif
-#if HAS_E3_DIR
-	E3_DIR_INIT();
+#if HAS_B_DIR
+	B_DIR_INIT();
 #endif
-#if HAS_E4_DIR
-	E4_DIR_INIT();
-#endif
-#if HAS_E5_DIR
-	E5_DIR_INIT();
-#endif
-#if HAS_E6_DIR
-	E6_DIR_INIT();
-#endif
-#if HAS_E7_DIR
-	E7_DIR_INIT();
+#if HAS_C_DIR
+	C_DIR_INIT();
 #endif
 
+//enable_all_steppers();
+
 // Init Enable Pins - steppers default to disabled.
+
 #if HAS_X_ENABLE
 	X_ENABLE_INIT();
 	if (!X_ENABLE_ON)
@@ -2614,47 +2359,23 @@ void Stepper::init() {
 	Z_BRAKE_INIT();
 	BRAKE_Z();
 #endif
-#if HAS_E0_ENABLE
-	E0_ENABLE_INIT();
-	if (!E_ENABLE_ON)
-		E0_ENABLE_WRITE(HIGH);
-#endif
-#if HAS_E1_ENABLE
-	E1_ENABLE_INIT();
-	if (!E_ENABLE_ON)
-		E1_ENABLE_WRITE(HIGH);
+#if HAS_A_ENABLE
+	A_ENABLE_INIT();
+	if (!A_ENABLE_ON)
+		A_ENABLE_WRITE(HIGH);
 #endif
-#if HAS_E2_ENABLE
-	E2_ENABLE_INIT();
-	if (!E_ENABLE_ON)
-		E2_ENABLE_WRITE(HIGH);
+#if HAS_B_ENABLE
+	B_ENABLE_INIT();
+	if (!B_ENABLE_ON)
+		B_ENABLE_WRITE(HIGH);
 #endif
-#if HAS_E3_ENABLE
-	E3_ENABLE_INIT();
-	if (!E_ENABLE_ON)
-		E3_ENABLE_WRITE(HIGH);
-#endif
-#if HAS_E4_ENABLE
-	E4_ENABLE_INIT();
-	if (!E_ENABLE_ON)
-		E4_ENABLE_WRITE(HIGH);
-#endif
-#if HAS_E5_ENABLE
-	E5_ENABLE_INIT();
-	if (!E_ENABLE_ON)
-		E5_ENABLE_WRITE(HIGH);
-#endif
-#if HAS_E6_ENABLE
-	E6_ENABLE_INIT();
-	if (!E_ENABLE_ON)
-		E6_ENABLE_WRITE(HIGH);
-#endif
-#if HAS_E7_ENABLE
-	E7_ENABLE_INIT();
-	if (!E_ENABLE_ON)
-		E7_ENABLE_WRITE(HIGH);
+#if HAS_C_ENABLE
+	C_ENABLE_INIT();
+	if (!C_ENABLE_ON)
+		C_ENABLE_WRITE(HIGH);
 #endif
 
+
 #define _STEP_INIT(AXIS)           AXIS##_STEP_INIT()
 #define _WRITE_STEP(AXIS, HIGHLOW) AXIS##_STEP_WRITE(HIGHLOW)
 #define _DISABLE_AXIS(AXIS)        DISABLE_AXIS_##AXIS()
@@ -2699,29 +2420,17 @@ void Stepper::init() {
 	AXIS_INIT(Z, Z);
 #endif
 
-#if E_STEPPERS && HAS_E0_STEP
-	E_AXIS_INIT(0);
-#endif
-#if E_STEPPERS > 1 && HAS_E1_STEP
-	E_AXIS_INIT(1);
-#endif
-#if E_STEPPERS > 2 && HAS_E2_STEP
-	E_AXIS_INIT(2);
+#if HAS_A_STEP
+	A_STEP_INIT();
+	A_STEP_WRITE(INVERT_A_STEP_PIN);
 #endif
-#if E_STEPPERS > 3 && HAS_E3_STEP
-	E_AXIS_INIT(3);
+#if HAS_B_STEP
+	B_STEP_INIT();
+	B_STEP_WRITE(INVERT_B_STEP_PIN);
 #endif
-#if E_STEPPERS > 4 && HAS_E4_STEP
-	E_AXIS_INIT(4);
-#endif
-#if E_STEPPERS > 5 && HAS_E5_STEP
-	E_AXIS_INIT(5);
-#endif
-#if E_STEPPERS > 6 && HAS_E6_STEP
-	E_AXIS_INIT(6);
-#endif
-#if E_STEPPERS > 7 && HAS_E7_STEP
-	E_AXIS_INIT(7);
+#if HAS_C_STEP
+	C_STEP_INIT();
+	C_STEP_WRITE(INVERT_C_STEP_PIN);
 #endif
 
 #if DISABLED(I2S_STEPPER_STREAM)
@@ -2731,7 +2440,7 @@ void Stepper::init() {
 #endif
 
 	// Init direction bits for first moves
-	set_directions((INVERT_X_DIR ? _BV(X_AXIS) : 0) | (INVERT_Y_DIR ? _BV(Y_AXIS) : 0) | (INVERT_Z_DIR ? _BV(Z_AXIS) : 0));
+	set_directions((INVERT_X_DIR ? _BV(Axis::X()) : 0) | (INVERT_Y_DIR ? _BV(Axis::Y()) : 0) | (INVERT_Z_DIR ? _BV(Axis::Z()) : 0));
 
 #if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM
 	initialized = true;
@@ -2748,71 +2457,30 @@ void Stepper::init() {
  * This allows get_axis_position_mm to correctly
  * derive the current XYZ position later on.
  */
-void Stepper::_set_position(const int32_t& a, const int32_t& b, const int32_t& c, const int32_t& e) {
-#if CORE_IS_XY
-	// corexy positioning
-	// these equations follow the form of the dA and dB equations on https://www.corexy.com/theory.html
-	count_position.set(a + b, CORESIGN(a - b), c);
-#elif CORE_IS_XZ
-	// corexz planning
-	count_position.set(a + c, b, CORESIGN(a - c));
-#elif CORE_IS_YZ
-	// coreyz planning
-	count_position.set(a, b + c, CORESIGN(b - c));
-#elif ENABLED(MARKFORGED_XY)
-	count_position.set(a - b, b, c);
-#else
-	// default non-h-bot planning
-	count_position.set(a, b, c);
-#endif
-	count_position.e = e;
+void Stepper::_set_position(const Vector6i32& position) {
+	count_position = position;
 }
 
 /**
  * Get a stepper's position in steps.
  */
-int32_t Stepper::position(const AxisEnum axis) {
-#ifdef __AVR__
-	// Protect the access to the position. Only required for AVR, as
-	//  any 32bit CPU offers atomic access to 32bit variables
-	const bool was_enabled = suspend();
-#endif
-
-	const int32_t v = count_position[axis];
-
-#ifdef __AVR__
-	// Reenable Stepper ISR
-	if (was_enabled)
-		wake_up();
-#endif
-	return v;
+int32_t Stepper::position(const Axis axis) {
+	return count_position[axis];
 }
 
 // Set the current position in steps
-void Stepper::set_position(const int32_t& a, const int32_t& b, const int32_t& c, const int32_t& e) {
+void Stepper::set_position(const Vector6i32 &target) {
 	planner.synchronize();
 	const bool was_enabled = suspend();
-	_set_position(a, b, c, e);
+	_set_position(target);
 	if (was_enabled)
 		wake_up();
 }
 
-void Stepper::set_axis_position(const AxisEnum a, const int32_t& v) {
+void Stepper::set_axis_position(const Axis a, const int32_t& v) {
 	planner.synchronize();
 
-#ifdef __AVR__
-	// Protect the access to the position. Only required for AVR, as
-	//  any 32bit CPU offers atomic access to 32bit variables
-	const bool was_enabled = suspend();
-#endif
-
 	count_position[a] = v;
-
-#ifdef __AVR__
-	// Reenable Stepper ISR
-	if (was_enabled)
-		wake_up();
-#endif
 }
 
 // Signal endstops were triggered - This function can be called from
@@ -2821,7 +2489,7 @@ void Stepper::set_axis_position(const AxisEnum a, const int32_t& v) {
 // Stepper ISR (this CAN happen with the endstop limits ISR) then
 // when the stepper ISR resumes, we must be very sure that the movement
 // is properly canceled
-void Stepper::endstop_triggered(const AxisEnum axis) {
+void Stepper::endstop_triggered(const Axis axis) {
 
 	const bool was_enabled = suspend();
 	endstops_trigsteps[axis] = (
@@ -2846,7 +2514,7 @@ void Stepper::endstop_triggered(const AxisEnum axis) {
 		wake_up();
 }
 
-int32_t Stepper::triggered_position(const AxisEnum axis) {
+int32_t Stepper::triggered_position(const Axis axis) {
 #ifdef __AVR__
 	// Protect the access to the position. Only required for AVR, as
 	//  any 32bit CPU offers atomic access to 32bit variables
@@ -2864,860 +2532,11 @@ int32_t Stepper::triggered_position(const AxisEnum axis) {
 	return v;
 }
 
-void Stepper::report_a_position(const xyz_long_t& pos) {
-#if ANY(CORE_IS_XY, CORE_IS_XZ, MARKFORGED_XY, DELTA, IS_SCARA)
-	SERIAL_ECHOPAIR(STR_COUNT_A, pos.x, " B:", pos.y);
-#else
-	SERIAL_ECHOPAIR_P(PSTR(STR_COUNT_X), pos.x, SP_Y_LBL, pos.y);
-#endif
-#if ANY(CORE_IS_XZ, CORE_IS_YZ, DELTA)
-	SERIAL_ECHOLNPAIR(" C:", pos.z);
-#else
-	SERIAL_ECHOLNPAIR_P(SP_Z_LBL, pos.z);
-#endif
+void Stepper::report_a_position(const Vector6i32& pos) {
+	SERIAL_ECHOPAIR_P(PSTR(STR_COUNT_X), pos.x(), SP_Y_LBL, pos.y(), SP_Z_LBL, pos.z(), SP_A_LBL, pos.a(), SP_B_LBL, pos.b(), SP_C_LBL, pos.c());
 }
 
 void Stepper::report_positions() {
-
-#ifdef __AVR__
-	// Protect the access to the position.
-	const bool was_enabled = suspend();
-#endif
-
-	const xyz_long_t pos = count_position;
-
-#ifdef __AVR__
-	if (was_enabled)
-		wake_up();
-#endif
-
-	report_a_position(pos);
-}
-
-#if ENABLED(BABYSTEPPING)
-
-#	define _ENABLE_AXIS(AXIS)       ENABLE_AXIS_##AXIS()
-#	define _READ_DIR(AXIS)          AXIS##_DIR_READ()
-#	define _INVERT_DIR(AXIS)        INVERT_##AXIS##_DIR
-#	define _APPLY_DIR(AXIS, INVERT) AXIS##_APPLY_DIR(INVERT, true)
-
-#	if MINIMUM_STEPPER_PULSE
-#		define STEP_PULSE_CYCLES ((MINIMUM_STEPPER_PULSE) *CYCLES_PER_MICROSECOND)
-#	else
-#		define STEP_PULSE_CYCLES 0
-#	endif
-
-#	if ENABLED(DELTA)
-#		define CYCLES_EATEN_BABYSTEP (2 * 15)
-#	else
-#		define CYCLES_EATEN_BABYSTEP 0
-#	endif
-#	define EXTRA_CYCLES_BABYSTEP (STEP_PULSE_CYCLES - (CYCLES_EATEN_BABYSTEP))
-
-#	if EXTRA_CYCLES_BABYSTEP > 20
-#		define _SAVE_START() const hal_timer_t pulse_start = HAL_timer_get_count(PULSE_TIMER_NUM)
-#		define _PULSE_WAIT() \
-			while (EXTRA_CYCLES_BABYSTEP > (uint32_t) (HAL_timer_get_count(PULSE_TIMER_NUM) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ \
-			}
-#	else
-#		define _SAVE_START() NOOP
-#		if EXTRA_CYCLES_BABYSTEP > 0
-#			define _PULSE_WAIT() DELAY_NS(EXTRA_CYCLES_BABYSTEP* NANOSECONDS_PER_CYCLE)
-#		elif ENABLED(DELTA)
-#			define _PULSE_WAIT() DELAY_US(2);
-#		elif STEP_PULSE_CYCLES > 0
-#			define _PULSE_WAIT() NOOP
-#		else
-#			define _PULSE_WAIT() DELAY_US(4);
-#		endif
-#	endif
-
-#	if ENABLED(BABYSTEPPING_EXTRA_DIR_WAIT)
-#		define EXTRA_DIR_WAIT_BEFORE DIR_WAIT_BEFORE
-#		define EXTRA_DIR_WAIT_AFTER  DIR_WAIT_AFTER
-#	else
-#		define EXTRA_DIR_WAIT_BEFORE()
-#		define EXTRA_DIR_WAIT_AFTER()
-#	endif
-
-#	if DISABLED(DELTA)
-
-#		define BABYSTEP_AXIS(AXIS, INV, DIR) \
-			do { \
-				const uint8_t old_dir = _READ_DIR(AXIS); \
-				_ENABLE_AXIS(AXIS); \
-				DIR_WAIT_BEFORE(); \
-				_APPLY_DIR(AXIS, _INVERT_DIR(AXIS) ^ DIR ^ INV); \
-				DIR_WAIT_AFTER(); \
-				_SAVE_START(); \
-				_APPLY_STEP(AXIS, !_INVERT_STEP_PIN(AXIS), true); \
-				_PULSE_WAIT(); \
-				_APPLY_STEP(AXIS, _INVERT_STEP_PIN(AXIS), true); \
-				EXTRA_DIR_WAIT_BEFORE(); \
-				_APPLY_DIR(AXIS, old_dir); \
-				EXTRA_DIR_WAIT_AFTER(); \
-			} while (0)
-
-#	endif
-
-#	if IS_CORE
-
-#		define BABYSTEP_CORE(A, B, INV, DIR, ALT) \
-			do { \
-				const xy_byte_t old_dir = { _READ_DIR(A), _READ_DIR(B) }; \
-				_ENABLE_AXIS(A); \
-				_ENABLE_AXIS(B); \
-				DIR_WAIT_BEFORE(); \
-				_APPLY_DIR(A, _INVERT_DIR(A) ^ DIR ^ INV); \
-				_APPLY_DIR(B, _INVERT_DIR(B) ^ DIR ^ INV ^ ALT); \
-				DIR_WAIT_AFTER(); \
-				_SAVE_START(); \
-				_APPLY_STEP(A, !_INVERT_STEP_PIN(A), true); \
-				_APPLY_STEP(B, !_INVERT_STEP_PIN(B), true); \
-				_PULSE_WAIT(); \
-				_APPLY_STEP(A, _INVERT_STEP_PIN(A), true); \
-				_APPLY_STEP(B, _INVERT_STEP_PIN(B), true); \
-				EXTRA_DIR_WAIT_BEFORE(); \
-				_APPLY_DIR(A, old_dir.a); \
-				_APPLY_DIR(B, old_dir.b); \
-				EXTRA_DIR_WAIT_AFTER(); \
-			} while (0)
-
-#	endif
-
-// MUST ONLY BE CALLED BY AN ISR,
-// No other ISR should ever interrupt this!
-void Stepper::do_babystep(const AxisEnum axis, const bool direction) {
-
-#	if DISABLED(INTEGRATED_BABYSTEPPING)
-	cli();
-#	endif
-
-	switch (axis) {
-
-#	if ENABLED(BABYSTEP_XY)
-
-		case X_AXIS:
-#		if CORE_IS_XY
-			BABYSTEP_CORE(X, Y, 0, direction, 0);
-#		elif CORE_IS_XZ
-			BABYSTEP_CORE(X, Z, 0, direction, 0);
-#		else
-			BABYSTEP_AXIS(X, 0, direction);
-#		endif
-			break;
-
-		case Y_AXIS:
-#		if CORE_IS_XY
-			BABYSTEP_CORE(X, Y, 1, !direction, (CORESIGN(1) > 0));
-#		elif CORE_IS_YZ
-			BABYSTEP_CORE(Y, Z, 0, direction, (CORESIGN(1) < 0));
-#		else
-			BABYSTEP_AXIS(Y, 0, direction);
-#		endif
-			break;
-
-#	endif
-
-		case Z_AXIS: {
-
-#	if CORE_IS_XZ
-			BABYSTEP_CORE(X, Z, BABYSTEP_INVERT_Z, direction, (CORESIGN(1) < 0));
-#	elif CORE_IS_YZ
-			BABYSTEP_CORE(Y, Z, BABYSTEP_INVERT_Z, direction, (CORESIGN(1) < 0));
-#	elif DISABLED(DELTA)
-			BABYSTEP_AXIS(Z, BABYSTEP_INVERT_Z, direction);
-
-#	else // DELTA
-
-			const bool z_direction = direction ^ BABYSTEP_INVERT_Z;
-
-			ENABLE_AXIS_X();
-			ENABLE_AXIS_Y();
-			ENABLE_AXIS_Z();
-
-			DIR_WAIT_BEFORE();
-
-			const xyz_byte_t old_dir = { X_DIR_READ(), Y_DIR_READ(), Z_DIR_READ() };
-
-			X_DIR_WRITE(INVERT_X_DIR ^ z_direction);
-			Y_DIR_WRITE(INVERT_Y_DIR ^ z_direction);
-			Z_DIR_WRITE(INVERT_Z_DIR ^ z_direction);
-
-			DIR_WAIT_AFTER();
-
-			_SAVE_START();
-
-			X_STEP_WRITE(!INVERT_X_STEP_PIN);
-			Y_STEP_WRITE(!INVERT_Y_STEP_PIN);
-			Z_STEP_WRITE(!INVERT_Z_STEP_PIN);
-
-			_PULSE_WAIT();
-
-			X_STEP_WRITE(INVERT_X_STEP_PIN);
-			Y_STEP_WRITE(INVERT_Y_STEP_PIN);
-			Z_STEP_WRITE(INVERT_Z_STEP_PIN);
-
-			// Restore direction bits
-			EXTRA_DIR_WAIT_BEFORE();
-
-			X_DIR_WRITE(old_dir.x);
-			Y_DIR_WRITE(old_dir.y);
-			Z_DIR_WRITE(old_dir.z);
-
-			EXTRA_DIR_WAIT_AFTER();
-
-#	endif
-
-		} break;
-
-		default:
-			break;
-	}
-
-#	if DISABLED(INTEGRATED_BABYSTEPPING)
-	sei();
-#	endif
-}
-
-#endif // BABYSTEPPING
-
-/**
- * Software-controlled Stepper Motor Current
- */
-
-#if HAS_MOTOR_CURRENT_SPI
-
-// From Arduino DigitalPotControl example
-void Stepper::set_digipot_value_spi(const int16_t address, const int16_t value) {
-	WRITE(DIGIPOTSS_PIN, LOW); // Take the SS pin low to select the chip
-	SPI.transfer(address); // Send the address and value via SPI
-	SPI.transfer(value);
-	WRITE(DIGIPOTSS_PIN, HIGH); // Take the SS pin high to de-select the chip
-	// delay(10);
-}
-
-#endif // HAS_MOTOR_CURRENT_SPI
-
-#if HAS_MOTOR_CURRENT_PWM
-
-void Stepper::refresh_motor_power() {
-	if (!initialized)
-		return;
-	LOOP_L_N(i, COUNT(motor_current_setting)) {
-		switch (i) {
-#	if ANY_PIN(MOTOR_CURRENT_PWM_XY, MOTOR_CURRENT_PWM_X, MOTOR_CURRENT_PWM_Y)
-			case 0:
-#	endif
-#	if PIN_EXISTS(MOTOR_CURRENT_PWM_Z)
-			case 1:
-#	endif
-#	if ANY_PIN(MOTOR_CURRENT_PWM_E, MOTOR_CURRENT_PWM_E0, MOTOR_CURRENT_PWM_E1)
-			case 2:
-#	endif
-				set_digipot_current(i, motor_current_setting[i]);
-			default:
-				break;
-		}
-	}
-}
-
-#endif // HAS_MOTOR_CURRENT_PWM
-
-#if !MB(PRINTRBOARD_G2)
-
-#	if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM
-
-void Stepper::set_digipot_current(const uint8_t driver, const int16_t current) {
-	if (WITHIN(driver, 0, MOTOR_CURRENT_COUNT - 1))
-		motor_current_setting[driver] = current; // update motor_current_setting
-
-	if (!initialized)
-		return;
-
-#		if HAS_MOTOR_CURRENT_SPI
-
-	// SERIAL_ECHOLNPAIR("Digipotss current ", current);
-
-	const uint8_t digipot_ch[] = DIGIPOT_CHANNELS;
-	set_digipot_value_spi(digipot_ch[driver], current);
-
-#		elif HAS_MOTOR_CURRENT_PWM
-
-#			define _WRITE_CURRENT_PWM(P) analogWrite(pin_t(MOTOR_CURRENT_PWM_##P##_PIN), 255L * current / (MOTOR_CURRENT_PWM_RANGE))
-	switch (driver) {
-		case 0:
-#			if PIN_EXISTS(MOTOR_CURRENT_PWM_X)
-			_WRITE_CURRENT_PWM(X);
-#			endif
-#			if PIN_EXISTS(MOTOR_CURRENT_PWM_Y)
-			_WRITE_CURRENT_PWM(Y);
-#			endif
-#			if PIN_EXISTS(MOTOR_CURRENT_PWM_XY)
-			_WRITE_CURRENT_PWM(XY);
-#			endif
-			break;
-		case 1:
-#			if PIN_EXISTS(MOTOR_CURRENT_PWM_Z)
-			_WRITE_CURRENT_PWM(Z);
-#			endif
-			break;
-		case 2:
-#			if PIN_EXISTS(MOTOR_CURRENT_PWM_E)
-			_WRITE_CURRENT_PWM(E);
-#			endif
-#			if PIN_EXISTS(MOTOR_CURRENT_PWM_E0)
-			_WRITE_CURRENT_PWM(E0);
-#			endif
-#			if PIN_EXISTS(MOTOR_CURRENT_PWM_E1)
-			_WRITE_CURRENT_PWM(E1);
-#			endif
-			break;
-	}
-#		endif
-}
-
-void Stepper::digipot_init() {
-
-#		if HAS_MOTOR_CURRENT_SPI
-
-	SPI.begin();
-	SET_OUTPUT(DIGIPOTSS_PIN);
-
-	LOOP_L_N(i, COUNT(motor_current_setting))
-	set_digipot_current(i, motor_current_setting[i]);
-
-#		elif HAS_MOTOR_CURRENT_PWM
-
-#			if PIN_EXISTS(MOTOR_CURRENT_PWM_X)
-	SET_PWM(MOTOR_CURRENT_PWM_X_PIN);
-#			endif
-#			if PIN_EXISTS(MOTOR_CURRENT_PWM_Y)
-	SET_PWM(MOTOR_CURRENT_PWM_Y_PIN);
-#			endif
-#			if PIN_EXISTS(MOTOR_CURRENT_PWM_XY)
-	SET_PWM(MOTOR_CURRENT_PWM_XY_PIN);
-#			endif
-#			if PIN_EXISTS(MOTOR_CURRENT_PWM_Z)
-	SET_PWM(MOTOR_CURRENT_PWM_Z_PIN);
-#			endif
-#			if PIN_EXISTS(MOTOR_CURRENT_PWM_E)
-	SET_PWM(MOTOR_CURRENT_PWM_E_PIN);
-#			endif
-#			if PIN_EXISTS(MOTOR_CURRENT_PWM_E0)
-	SET_PWM(MOTOR_CURRENT_PWM_E0_PIN);
-#			endif
-#			if PIN_EXISTS(MOTOR_CURRENT_PWM_E1)
-	SET_PWM(MOTOR_CURRENT_PWM_E1_PIN);
-#			endif
-
-	refresh_motor_power();
-
-// Set Timer5 to 31khz so the PWM of the motor power is as constant as possible. (removes a buzzing noise)
-#			ifdef __AVR__
-	SET_CS5(PRESCALER_1);
-#			endif
-#		endif
-}
-
-#	endif
-
-#else // PRINTRBOARD_G2
-
-#	include HAL_PATH(../ HAL, fastio / G2_PWM.h)
-
-#endif
-
-#if HAS_MICROSTEPS
-
-/**
- * Software-controlled Microstepping
- */
-
-void Stepper::microstep_init() {
-#	if HAS_X_MS_PINS
-	SET_OUTPUT(X_MS1_PIN);
-	SET_OUTPUT(X_MS2_PIN);
-#		if PIN_EXISTS(X_MS3)
-	SET_OUTPUT(X_MS3_PIN);
-#		endif
-#	endif
-#	if HAS_X2_MS_PINS
-	SET_OUTPUT(X2_MS1_PIN);
-	SET_OUTPUT(X2_MS2_PIN);
-#		if PIN_EXISTS(X2_MS3)
-	SET_OUTPUT(X2_MS3_PIN);
-#		endif
-#	endif
-#	if HAS_Y_MS_PINS
-	SET_OUTPUT(Y_MS1_PIN);
-	SET_OUTPUT(Y_MS2_PIN);
-#		if PIN_EXISTS(Y_MS3)
-	SET_OUTPUT(Y_MS3_PIN);
-#		endif
-#	endif
-#	if HAS_Y2_MS_PINS
-	SET_OUTPUT(Y2_MS1_PIN);
-	SET_OUTPUT(Y2_MS2_PIN);
-#		if PIN_EXISTS(Y2_MS3)
-	SET_OUTPUT(Y2_MS3_PIN);
-#		endif
-#	endif
-#	if HAS_Z_MS_PINS
-	SET_OUTPUT(Z_MS1_PIN);
-	SET_OUTPUT(Z_MS2_PIN);
-#		if PIN_EXISTS(Z_MS3)
-	SET_OUTPUT(Z_MS3_PIN);
-#		endif
-#	endif
-#	if HAS_Z2_MS_PINS
-	SET_OUTPUT(Z2_MS1_PIN);
-	SET_OUTPUT(Z2_MS2_PIN);
-#		if PIN_EXISTS(Z2_MS3)
-	SET_OUTPUT(Z2_MS3_PIN);
-#		endif
-#	endif
-#	if HAS_Z3_MS_PINS
-	SET_OUTPUT(Z3_MS1_PIN);
-	SET_OUTPUT(Z3_MS2_PIN);
-#		if PIN_EXISTS(Z3_MS3)
-	SET_OUTPUT(Z3_MS3_PIN);
-#		endif
-#	endif
-#	if HAS_Z4_MS_PINS
-	SET_OUTPUT(Z4_MS1_PIN);
-	SET_OUTPUT(Z4_MS2_PIN);
-#		if PIN_EXISTS(Z4_MS3)
-	SET_OUTPUT(Z4_MS3_PIN);
-#		endif
-#	endif
-#	if HAS_E0_MS_PINS
-	SET_OUTPUT(E0_MS1_PIN);
-	SET_OUTPUT(E0_MS2_PIN);
-#		if PIN_EXISTS(E0_MS3)
-	SET_OUTPUT(E0_MS3_PIN);
-#		endif
-#	endif
-#	if HAS_E1_MS_PINS
-	SET_OUTPUT(E1_MS1_PIN);
-	SET_OUTPUT(E1_MS2_PIN);
-#		if PIN_EXISTS(E1_MS3)
-	SET_OUTPUT(E1_MS3_PIN);
-#		endif
-#	endif
-#	if HAS_E2_MS_PINS
-	SET_OUTPUT(E2_MS1_PIN);
-	SET_OUTPUT(E2_MS2_PIN);
-#		if PIN_EXISTS(E2_MS3)
-	SET_OUTPUT(E2_MS3_PIN);
-#		endif
-#	endif
-#	if HAS_E3_MS_PINS
-	SET_OUTPUT(E3_MS1_PIN);
-	SET_OUTPUT(E3_MS2_PIN);
-#		if PIN_EXISTS(E3_MS3)
-	SET_OUTPUT(E3_MS3_PIN);
-#		endif
-#	endif
-#	if HAS_E4_MS_PINS
-	SET_OUTPUT(E4_MS1_PIN);
-	SET_OUTPUT(E4_MS2_PIN);
-#		if PIN_EXISTS(E4_MS3)
-	SET_OUTPUT(E4_MS3_PIN);
-#		endif
-#	endif
-#	if HAS_E5_MS_PINS
-	SET_OUTPUT(E5_MS1_PIN);
-	SET_OUTPUT(E5_MS2_PIN);
-#		if PIN_EXISTS(E5_MS3)
-	SET_OUTPUT(E5_MS3_PIN);
-#		endif
-#	endif
-#	if HAS_E6_MS_PINS
-	SET_OUTPUT(E6_MS1_PIN);
-	SET_OUTPUT(E6_MS2_PIN);
-#		if PIN_EXISTS(E6_MS3)
-	SET_OUTPUT(E6_MS3_PIN);
-#		endif
-#	endif
-#	if HAS_E7_MS_PINS
-	SET_OUTPUT(E7_MS1_PIN);
-	SET_OUTPUT(E7_MS2_PIN);
-#		if PIN_EXISTS(E7_MS3)
-	SET_OUTPUT(E7_MS3_PIN);
-#		endif
-#	endif
-
-	static const uint8_t microstep_modes[] = MICROSTEP_MODES;
-	for (uint16_t i = 0; i < COUNT(microstep_modes); i++)
-		microstep_mode(i, microstep_modes[i]);
-}
-
-void Stepper::microstep_ms(const uint8_t driver, const int8_t ms1, const int8_t ms2, const int8_t ms3) {
-	if (ms1 >= 0)
-		switch (driver) {
-#	if HAS_X_MS_PINS || HAS_X2_MS_PINS
-			case 0:
-#		if HAS_X_MS_PINS
-				WRITE(X_MS1_PIN, ms1);
-#		endif
-#		if HAS_X2_MS_PINS
-				WRITE(X2_MS1_PIN, ms1);
-#		endif
-				break;
-#	endif
-#	if HAS_Y_MS_PINS || HAS_Y2_MS_PINS
-			case 1:
-#		if HAS_Y_MS_PINS
-				WRITE(Y_MS1_PIN, ms1);
-#		endif
-#		if HAS_Y2_MS_PINS
-				WRITE(Y2_MS1_PIN, ms1);
-#		endif
-				break;
-#	endif
-#	if HAS_SOME_Z_MS_PINS
-			case 2:
-#		if HAS_Z_MS_PINS
-				WRITE(Z_MS1_PIN, ms1);
-#		endif
-#		if HAS_Z2_MS_PINS
-				WRITE(Z2_MS1_PIN, ms1);
-#		endif
-#		if HAS_Z3_MS_PINS
-				WRITE(Z3_MS1_PIN, ms1);
-#		endif
-#		if HAS_Z4_MS_PINS
-				WRITE(Z4_MS1_PIN, ms1);
-#		endif
-				break;
-#	endif
-#	if HAS_E0_MS_PINS
-			case 3:
-				WRITE(E0_MS1_PIN, ms1);
-				break;
-#	endif
-#	if HAS_E1_MS_PINS
-			case 4:
-				WRITE(E1_MS1_PIN, ms1);
-				break;
-#	endif
-#	if HAS_E2_MS_PINS
-			case 5:
-				WRITE(E2_MS1_PIN, ms1);
-				break;
-#	endif
-#	if HAS_E3_MS_PINS
-			case 6:
-				WRITE(E3_MS1_PIN, ms1);
-				break;
-#	endif
-#	if HAS_E4_MS_PINS
-			case 7:
-				WRITE(E4_MS1_PIN, ms1);
-				break;
-#	endif
-#	if HAS_E5_MS_PINS
-			case 8:
-				WRITE(E5_MS1_PIN, ms1);
-				break;
-#	endif
-#	if HAS_E6_MS_PINS
-			case 9:
-				WRITE(E6_MS1_PIN, ms1);
-				break;
-#	endif
-#	if HAS_E7_MS_PINS
-			case 10:
-				WRITE(E7_MS1_PIN, ms1);
-				break;
-#	endif
-		}
-	if (ms2 >= 0)
-		switch (driver) {
-#	if HAS_X_MS_PINS || HAS_X2_MS_PINS
-			case 0:
-#		if HAS_X_MS_PINS
-				WRITE(X_MS2_PIN, ms2);
-#		endif
-#		if HAS_X2_MS_PINS
-				WRITE(X2_MS2_PIN, ms2);
-#		endif
-				break;
-#	endif
-#	if HAS_Y_MS_PINS || HAS_Y2_MS_PINS
-			case 1:
-#		if HAS_Y_MS_PINS
-				WRITE(Y_MS2_PIN, ms2);
-#		endif
-#		if HAS_Y2_MS_PINS
-				WRITE(Y2_MS2_PIN, ms2);
-#		endif
-				break;
-#	endif
-#	if HAS_SOME_Z_MS_PINS
-			case 2:
-#		if HAS_Z_MS_PINS
-				WRITE(Z_MS2_PIN, ms2);
-#		endif
-#		if HAS_Z2_MS_PINS
-				WRITE(Z2_MS2_PIN, ms2);
-#		endif
-#		if HAS_Z3_MS_PINS
-				WRITE(Z3_MS2_PIN, ms2);
-#		endif
-#		if HAS_Z4_MS_PINS
-				WRITE(Z4_MS2_PIN, ms2);
-#		endif
-				break;
-#	endif
-#	if HAS_E0_MS_PINS
-			case 3:
-				WRITE(E0_MS2_PIN, ms2);
-				break;
-#	endif
-#	if HAS_E1_MS_PINS
-			case 4:
-				WRITE(E1_MS2_PIN, ms2);
-				break;
-#	endif
-#	if HAS_E2_MS_PINS
-			case 5:
-				WRITE(E2_MS2_PIN, ms2);
-				break;
-#	endif
-#	if HAS_E3_MS_PINS
-			case 6:
-				WRITE(E3_MS2_PIN, ms2);
-				break;
-#	endif
-#	if HAS_E4_MS_PINS
-			case 7:
-				WRITE(E4_MS2_PIN, ms2);
-				break;
-#	endif
-#	if HAS_E5_MS_PINS
-			case 8:
-				WRITE(E5_MS2_PIN, ms2);
-				break;
-#	endif
-#	if HAS_E6_MS_PINS
-			case 9:
-				WRITE(E6_MS2_PIN, ms2);
-				break;
-#	endif
-#	if HAS_E7_MS_PINS
-			case 10:
-				WRITE(E7_MS2_PIN, ms2);
-				break;
-#	endif
-		}
-	if (ms3 >= 0)
-		switch (driver) {
-#	if HAS_X_MS_PINS || HAS_X2_MS_PINS
-			case 0:
-#		if HAS_X_MS_PINS && PIN_EXISTS(X_MS3)
-				WRITE(X_MS3_PIN, ms3);
-#		endif
-#		if HAS_X2_MS_PINS && PIN_EXISTS(X2_MS3)
-				WRITE(X2_MS3_PIN, ms3);
-#		endif
-				break;
-#	endif
-#	if HAS_Y_MS_PINS || HAS_Y2_MS_PINS
-			case 1:
-#		if HAS_Y_MS_PINS && PIN_EXISTS(Y_MS3)
-				WRITE(Y_MS3_PIN, ms3);
-#		endif
-#		if HAS_Y2_MS_PINS && PIN_EXISTS(Y2_MS3)
-				WRITE(Y2_MS3_PIN, ms3);
-#		endif
-				break;
-#	endif
-#	if HAS_SOME_Z_MS_PINS
-			case 2:
-#		if HAS_Z_MS_PINS && PIN_EXISTS(Z_MS3)
-				WRITE(Z_MS3_PIN, ms3);
-#		endif
-#		if HAS_Z2_MS_PINS && PIN_EXISTS(Z2_MS3)
-				WRITE(Z2_MS3_PIN, ms3);
-#		endif
-#		if HAS_Z3_MS_PINS && PIN_EXISTS(Z3_MS3)
-				WRITE(Z3_MS3_PIN, ms3);
-#		endif
-#		if HAS_Z4_MS_PINS && PIN_EXISTS(Z4_MS3)
-				WRITE(Z4_MS3_PIN, ms3);
-#		endif
-				break;
-#	endif
-#	if HAS_E0_MS_PINS && PIN_EXISTS(E0_MS3)
-			case 3:
-				WRITE(E0_MS3_PIN, ms3);
-				break;
-#	endif
-#	if HAS_E1_MS_PINS && PIN_EXISTS(E1_MS3)
-			case 4:
-				WRITE(E1_MS3_PIN, ms3);
-				break;
-#	endif
-#	if HAS_E2_MS_PINS && PIN_EXISTS(E2_MS3)
-			case 5:
-				WRITE(E2_MS3_PIN, ms3);
-				break;
-#	endif
-#	if HAS_E3_MS_PINS && PIN_EXISTS(E3_MS3)
-			case 6:
-				WRITE(E3_MS3_PIN, ms3);
-				break;
-#	endif
-#	if HAS_E4_MS_PINS && PIN_EXISTS(E4_MS3)
-			case 7:
-				WRITE(E4_MS3_PIN, ms3);
-				break;
-#	endif
-#	if HAS_E5_MS_PINS && PIN_EXISTS(E5_MS3)
-			case 8:
-				WRITE(E5_MS3_PIN, ms3);
-				break;
-#	endif
-#	if HAS_E6_MS_PINS && PIN_EXISTS(E6_MS3)
-			case 9:
-				WRITE(E6_MS3_PIN, ms3);
-				break;
-#	endif
-#	if HAS_E7_MS_PINS && PIN_EXISTS(E7_MS3)
-			case 10:
-				WRITE(E7_MS3_PIN, ms3);
-				break;
-#	endif
-		}
-}
-
-void Stepper::microstep_mode(const uint8_t driver, const uint8_t stepping_mode) {
-	switch (stepping_mode) {
-#	if HAS_MICROSTEP1
-		case 1:
-			microstep_ms(driver, MICROSTEP1);
-			break;
-#	endif
-#	if HAS_MICROSTEP2
-		case 2:
-			microstep_ms(driver, MICROSTEP2);
-			break;
-#	endif
-#	if HAS_MICROSTEP4
-		case 4:
-			microstep_ms(driver, MICROSTEP4);
-			break;
-#	endif
-#	if HAS_MICROSTEP8
-		case 8:
-			microstep_ms(driver, MICROSTEP8);
-			break;
-#	endif
-#	if HAS_MICROSTEP16
-		case 16:
-			microstep_ms(driver, MICROSTEP16);
-			break;
-#	endif
-#	if HAS_MICROSTEP32
-		case 32:
-			microstep_ms(driver, MICROSTEP32);
-			break;
-#	endif
-#	if HAS_MICROSTEP64
-		case 64:
-			microstep_ms(driver, MICROSTEP64);
-			break;
-#	endif
-#	if HAS_MICROSTEP128
-		case 128:
-			microstep_ms(driver, MICROSTEP128);
-			break;
-#	endif
-
-		default:
-			SERIAL_ERROR_MSG("Microsteps unavailable");
-			break;
-	}
-}
-
-void Stepper::microstep_readings() {
-#	define PIN_CHAR(P) SERIAL_CHAR('0' + READ(P##_PIN))
-#	define MS_LINE(A) \
-		do { \
-			SERIAL_ECHOPGM(" " STRINGIFY(A) ":"); \
-			PIN_CHAR(A##_MS1); \
-			PIN_CHAR(A##_MS2); \
-		} while (0)
-	SERIAL_ECHOPGM("MS1|2|3 Pins");
-#	if HAS_X_MS_PINS
-	MS_LINE(X);
-#		if PIN_EXISTS(X_MS3)
-	PIN_CHAR(X_MS3);
-#		endif
-#	endif
-#	if HAS_Y_MS_PINS
-	MS_LINE(Y);
-#		if PIN_EXISTS(Y_MS3)
-	PIN_CHAR(Y_MS3);
-#		endif
-#	endif
-#	if HAS_Z_MS_PINS
-	MS_LINE(Z);
-#		if PIN_EXISTS(Z_MS3)
-	PIN_CHAR(Z_MS3);
-#		endif
-#	endif
-#	if HAS_E0_MS_PINS
-	MS_LINE(E0);
-#		if PIN_EXISTS(E0_MS3)
-	PIN_CHAR(E0_MS3);
-#		endif
-#	endif
-#	if HAS_E1_MS_PINS
-	MS_LINE(E1);
-#		if PIN_EXISTS(E1_MS3)
-	PIN_CHAR(E1_MS3);
-#		endif
-#	endif
-#	if HAS_E2_MS_PINS
-	MS_LINE(E2);
-#		if PIN_EXISTS(E2_MS3)
-	PIN_CHAR(E2_MS3);
-#		endif
-#	endif
-#	if HAS_E3_MS_PINS
-	MS_LINE(E3);
-#		if PIN_EXISTS(E3_MS3)
-	PIN_CHAR(E3_MS3);
-#		endif
-#	endif
-#	if HAS_E4_MS_PINS
-	MS_LINE(E4);
-#		if PIN_EXISTS(E4_MS3)
-	PIN_CHAR(E4_MS3);
-#		endif
-#	endif
-#	if HAS_E5_MS_PINS
-	MS_LINE(E5);
-#		if PIN_EXISTS(E5_MS3)
-	PIN_CHAR(E5_MS3);
-#		endif
-#	endif
-#	if HAS_E6_MS_PINS
-	MS_LINE(E6);
-#		if PIN_EXISTS(E6_MS3)
-	PIN_CHAR(E6_MS3);
-#		endif
-#	endif
-#	if HAS_E7_MS_PINS
-	MS_LINE(E7);
-#		if PIN_EXISTS(E7_MS3)
-	PIN_CHAR(E7_MS3);
-#		endif
-#	endif
-	SERIAL_EOL();
+	report_a_position(count_position);
 }
 
-#endif // HAS_MICROSTEPS
diff --git a/src/marlin/module/stepper.h b/src/marlin/module/stepper.h
index b831de8711..ade54ae969 100644
--- a/src/marlin/module/stepper.h
+++ b/src/marlin/module/stepper.h
@@ -302,8 +302,8 @@ class Stepper {
     #endif
 
     // Delta error variables for the Bresenham line tracer
-    static xyze_long_t delta_error;
-    static xyze_ulong_t advance_dividend;
+    static swordfish::math::Vector6i32 delta_error;
+    static swordfish::math::Vector6u32 advance_dividend;
     static uint32_t advance_divisor,
                     step_events_completed,  // The number of step events executed in the current block
                     accelerate_until,       // The point from where we need to stop acceleration
@@ -351,15 +351,15 @@ class Stepper {
     #endif
 
     // Exact steps at which an endstop was triggered
-    static xyz_long_t endstops_trigsteps;
+    static swordfish::math::Vector6i32 endstops_trigsteps;
 
 	public:
     // Positions of stepper motors, in step units
-    static xyze_long_t count_position;
+    static swordfish::math::Vector6i32 count_position;
 
 	private:
     // Current stepper motor directions (+1 or -1)
-    static xyze_int8_t count_direction;
+    static swordfish::math::Vector6i8 count_direction;
 
     #if ENABLED(LASER_POWER_INLINE_TRAPEZOID)
 
@@ -430,15 +430,14 @@ class Stepper {
     static bool is_block_busy(const block_t* const block);
 
     // Get the position of a stepper, in steps
-    static int32_t position(const AxisEnum axis);
+    static int32_t position(const Axis axis);
 
     // Set the current position in steps
-    static void set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e);
-    static inline void set_position(const xyze_long_t &abce) { set_position(abce.a, abce.b, abce.c, abce.e); }
-    static void set_axis_position(const AxisEnum a, const int32_t &v);
+    static void set_position(const swordfish::math::Vector6i32 &abce);
+    static void set_axis_position(const Axis a, const int32_t &v);
 
     // Report the positions of the steppers, in steps
-    static void report_a_position(const xyz_long_t &pos);
+    static void report_a_position(const swordfish::math::Vector6i32 &pos);
     static void report_positions();
 
     // Discard current block and free any resources
@@ -456,16 +455,16 @@ class Stepper {
     FORCE_INLINE static void quick_stop() { abort_current_block = true; }
 
     // The direction of a single motor
-    FORCE_INLINE static bool motor_direction(const AxisEnum axis) { return TEST(last_direction_bits, axis); }
+    FORCE_INLINE static bool motor_direction(const Axis axis) { return TEST(last_direction_bits, axis); }
 
     // The last movement direction was not null on the specified axis. Note that motor direction is not necessarily the same.
-    FORCE_INLINE static bool axis_is_moving(const AxisEnum axis) { return TEST(axis_did_move, axis); }
+    FORCE_INLINE static bool axis_is_moving(const Axis axis) { return TEST(axis_did_move, axis); }
 
     // Handle a triggered endstop
-    static void endstop_triggered(const AxisEnum axis);
+    static void endstop_triggered(const Axis axis);
 
     // Triggered position of an axis in steps
-    static int32_t triggered_position(const AxisEnum axis);
+    static int32_t triggered_position(const Axis axis);
 
     #if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM
       static void set_digipot_value_spi(const int16_t address, const int16_t value);
@@ -530,8 +529,7 @@ class Stepper {
   private:
 
     // Set the current position in steps
-    static void _set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e);
-    FORCE_INLINE static void _set_position(const abce_long_t &spos) { _set_position(spos.a, spos.b, spos.c, spos.e); }
+    static void _set_position(const swordfish::math::Vector6i32 &spos);
 
     FORCE_INLINE static uint32_t calc_timer_interval(uint32_t step_rate, uint8_t* loops) {
       uint32_t timer;
diff --git a/src/marlin/module/stepper/indirection.cpp b/src/marlin/module/stepper/indirection.cpp
index 0711c8fa58..323d0f7fba 100644
--- a/src/marlin/module/stepper/indirection.cpp
+++ b/src/marlin/module/stepper/indirection.cpp
@@ -45,4 +45,4 @@ void reset_stepper_drivers() {
 }
 
 // Flags to optimize XYZ Enabled state
-xyz_bool_t axis_enabled = { false, false, false };
+bool axis_enabled[6] = { false, false, false, false, false, false };
diff --git a/src/marlin/module/stepper/indirection.h b/src/marlin/module/stepper/indirection.h
index 42cd683b74..4f7a781fd8 100644
--- a/src/marlin/module/stepper/indirection.h
+++ b/src/marlin/module/stepper/indirection.h
@@ -206,400 +206,56 @@ void reset_stepper_drivers();    // Called by settings.load / settings.reset
   #define Z4_DIR_WRITE(STATE) NOOP
 #endif
 
-// E0 Stepper
-#ifndef E0_ENABLE_INIT
-  #define E0_ENABLE_INIT() SET_OUTPUT(E0_ENABLE_PIN)
-  #define E0_ENABLE_WRITE(STATE) WRITE(E0_ENABLE_PIN,STATE)
-  #define E0_ENABLE_READ() bool(READ(E0_ENABLE_PIN))
-#endif
-#ifndef E0_DIR_INIT
-  #define E0_DIR_INIT() SET_OUTPUT(E0_DIR_PIN)
-  #define E0_DIR_WRITE(STATE) WRITE(E0_DIR_PIN,STATE)
-  #define E0_DIR_READ() bool(READ(E0_DIR_PIN))
-#endif
-#define E0_STEP_INIT() SET_OUTPUT(E0_STEP_PIN)
-#ifndef E0_STEP_WRITE
-  #define E0_STEP_WRITE(STATE) WRITE(E0_STEP_PIN,STATE)
-#endif
-#define E0_STEP_READ() bool(READ(E0_STEP_PIN))
-
-// E1 Stepper
-#ifndef E1_ENABLE_INIT
-  #define E1_ENABLE_INIT() SET_OUTPUT(E1_ENABLE_PIN)
-  #define E1_ENABLE_WRITE(STATE) WRITE(E1_ENABLE_PIN,STATE)
-  #define E1_ENABLE_READ() bool(READ(E1_ENABLE_PIN))
-#endif
-#ifndef E1_DIR_INIT
-  #define E1_DIR_INIT() SET_OUTPUT(E1_DIR_PIN)
-  #define E1_DIR_WRITE(STATE) WRITE(E1_DIR_PIN,STATE)
-  #define E1_DIR_READ() bool(READ(E1_DIR_PIN))
-#endif
-#define E1_STEP_INIT() SET_OUTPUT(E1_STEP_PIN)
-#ifndef E1_STEP_WRITE
-  #define E1_STEP_WRITE(STATE) WRITE(E1_STEP_PIN,STATE)
-#endif
-#define E1_STEP_READ() bool(READ(E1_STEP_PIN))
-
-// E2 Stepper
-#ifndef E2_ENABLE_INIT
-  #define E2_ENABLE_INIT() SET_OUTPUT(E2_ENABLE_PIN)
-  #define E2_ENABLE_WRITE(STATE) WRITE(E2_ENABLE_PIN,STATE)
-  #define E2_ENABLE_READ() bool(READ(E2_ENABLE_PIN))
-#endif
-#ifndef E2_DIR_INIT
-  #define E2_DIR_INIT() SET_OUTPUT(E2_DIR_PIN)
-  #define E2_DIR_WRITE(STATE) WRITE(E2_DIR_PIN,STATE)
-  #define E2_DIR_READ() bool(READ(E2_DIR_PIN))
-#endif
-#define E2_STEP_INIT() SET_OUTPUT(E2_STEP_PIN)
-#ifndef E2_STEP_WRITE
-  #define E2_STEP_WRITE(STATE) WRITE(E2_STEP_PIN,STATE)
-#endif
-#define E2_STEP_READ() bool(READ(E2_STEP_PIN))
-
-// E3 Stepper
-#ifndef E3_ENABLE_INIT
-  #define E3_ENABLE_INIT() SET_OUTPUT(E3_ENABLE_PIN)
-  #define E3_ENABLE_WRITE(STATE) WRITE(E3_ENABLE_PIN,STATE)
-  #define E3_ENABLE_READ() bool(READ(E3_ENABLE_PIN))
-#endif
-#ifndef E3_DIR_INIT
-  #define E3_DIR_INIT() SET_OUTPUT(E3_DIR_PIN)
-  #define E3_DIR_WRITE(STATE) WRITE(E3_DIR_PIN,STATE)
-  #define E3_DIR_READ() bool(READ(E3_DIR_PIN))
-#endif
-#define E3_STEP_INIT() SET_OUTPUT(E3_STEP_PIN)
-#ifndef E3_STEP_WRITE
-  #define E3_STEP_WRITE(STATE) WRITE(E3_STEP_PIN,STATE)
-#endif
-#define E3_STEP_READ() bool(READ(E3_STEP_PIN))
-
-// E4 Stepper
-#ifndef E4_ENABLE_INIT
-  #define E4_ENABLE_INIT() SET_OUTPUT(E4_ENABLE_PIN)
-  #define E4_ENABLE_WRITE(STATE) WRITE(E4_ENABLE_PIN,STATE)
-  #define E4_ENABLE_READ() bool(READ(E4_ENABLE_PIN))
-#endif
-#ifndef E4_DIR_INIT
-  #define E4_DIR_INIT() SET_OUTPUT(E4_DIR_PIN)
-  #define E4_DIR_WRITE(STATE) WRITE(E4_DIR_PIN,STATE)
-  #define E4_DIR_READ() bool(READ(E4_DIR_PIN))
-#endif
-#define E4_STEP_INIT() SET_OUTPUT(E4_STEP_PIN)
-#ifndef E4_STEP_WRITE
-  #define E4_STEP_WRITE(STATE) WRITE(E4_STEP_PIN,STATE)
-#endif
-#define E4_STEP_READ() bool(READ(E4_STEP_PIN))
-
-// E5 Stepper
-#ifndef E5_ENABLE_INIT
-  #define E5_ENABLE_INIT() SET_OUTPUT(E5_ENABLE_PIN)
-  #define E5_ENABLE_WRITE(STATE) WRITE(E5_ENABLE_PIN,STATE)
-  #define E5_ENABLE_READ() bool(READ(E5_ENABLE_PIN))
-#endif
-#ifndef E5_DIR_INIT
-  #define E5_DIR_INIT() SET_OUTPUT(E5_DIR_PIN)
-  #define E5_DIR_WRITE(STATE) WRITE(E5_DIR_PIN,STATE)
-  #define E5_DIR_READ() bool(READ(E5_DIR_PIN))
-#endif
-#define E5_STEP_INIT() SET_OUTPUT(E5_STEP_PIN)
-#ifndef E5_STEP_WRITE
-  #define E5_STEP_WRITE(STATE) WRITE(E5_STEP_PIN,STATE)
-#endif
-#define E5_STEP_READ() bool(READ(E5_STEP_PIN))
-
-// E6 Stepper
-#ifndef E6_ENABLE_INIT
-  #define E6_ENABLE_INIT() SET_OUTPUT(E6_ENABLE_PIN)
-  #define E6_ENABLE_WRITE(STATE) WRITE(E6_ENABLE_PIN,STATE)
-  #define E6_ENABLE_READ() bool(READ(E6_ENABLE_PIN))
-#endif
-#ifndef E6_DIR_INIT
-  #define E6_DIR_INIT() SET_OUTPUT(E6_DIR_PIN)
-  #define E6_DIR_WRITE(STATE) WRITE(E6_DIR_PIN,STATE)
-  #define E6_DIR_READ() bool(READ(E6_DIR_PIN))
-#endif
-#define E6_STEP_INIT() SET_OUTPUT(E6_STEP_PIN)
-#ifndef E6_STEP_WRITE
-  #define E6_STEP_WRITE(STATE) WRITE(E6_STEP_PIN,STATE)
-#endif
-#define E6_STEP_READ() bool(READ(E6_STEP_PIN))
-
-// E7 Stepper
-#ifndef E7_ENABLE_INIT
-  #define E7_ENABLE_INIT() SET_OUTPUT(E7_ENABLE_PIN)
-  #define E7_ENABLE_WRITE(STATE) WRITE(E7_ENABLE_PIN,STATE)
-  #define E7_ENABLE_READ() bool(READ(E7_ENABLE_PIN))
-#endif
-#ifndef E7_DIR_INIT
-  #define E7_DIR_INIT() SET_OUTPUT(E7_DIR_PIN)
-  #define E7_DIR_WRITE(STATE) WRITE(E7_DIR_PIN,STATE)
-  #define E7_DIR_READ() bool(READ(E7_DIR_PIN))
+// A Stepper
+#ifndef A_ENABLE_INIT
+  #define A_ENABLE_INIT() SET_OUTPUT(A_ENABLE_PIN)
+  #define A_ENABLE_WRITE(STATE) WRITE(A_ENABLE_PIN,STATE)
+  #define A_ENABLE_READ() bool(READ(A_ENABLE_PIN))
 #endif
-#define E7_STEP_INIT() SET_OUTPUT(E7_STEP_PIN)
-#ifndef E7_STEP_WRITE
-  #define E7_STEP_WRITE(STATE) WRITE(E7_STEP_PIN,STATE)
+#ifndef A_DIR_INIT
+  #define A_DIR_INIT() SET_OUTPUT(A_DIR_PIN)
+  #define A_DIR_WRITE(STATE) WRITE(A_DIR_PIN,STATE)
+  #define A_DIR_READ() bool(READ(A_DIR_PIN))
 #endif
-#define E7_STEP_READ() bool(READ(E7_STEP_PIN))
-
-/**
- * Extruder indirection for the single E axis
- */
-#if ENABLED(SWITCHING_EXTRUDER) // One stepper driver per two extruders, reversed on odd index
-  #if EXTRUDERS > 7
-    #define E_STEP_WRITE(E,V) do{ if (E < 2) { E0_STEP_WRITE(V); } else if (E < 4) { E1_STEP_WRITE(V); } else if (E < 6) { E2_STEP_WRITE(V); } else { E3_STEP_WRITE(V); } }while(0)
-    #define   NORM_E_DIR(E)   do{ switch (E) { \
-        case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E0_DIR_WRITE( INVERT_E0_DIR); break; \
-        case 2: E1_DIR_WRITE(!INVERT_E1_DIR); break; case 3: E1_DIR_WRITE( INVERT_E1_DIR); break; \
-        case 4: E2_DIR_WRITE(!INVERT_E2_DIR); break; case 5: E2_DIR_WRITE( INVERT_E2_DIR); break; \
-        case 6: E3_DIR_WRITE( INVERT_E3_DIR); break; case 7: E3_DIR_WRITE( INVERT_E3_DIR); break; \
-      } }while(0)
-    #define    REV_E_DIR(E)   do{ switch (E) { \
-        case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E0_DIR_WRITE(!INVERT_E0_DIR); break; \
-        case 2: E1_DIR_WRITE( INVERT_E1_DIR); break; case 3: E1_DIR_WRITE(!INVERT_E1_DIR); break; \
-        case 4: E2_DIR_WRITE( INVERT_E2_DIR); break; case 5: E2_DIR_WRITE(!INVERT_E2_DIR); break; \
-        case 6: E3_DIR_WRITE(!INVERT_E3_DIR); break; case 7: E3_DIR_WRITE(!INVERT_E3_DIR); break; \
-      } }while(0)
-  #elif EXTRUDERS > 6
-    #define E_STEP_WRITE(E,V) do{ if (E < 2) { E0_STEP_WRITE(V); } else if (E < 4) { E1_STEP_WRITE(V); } else if (E < 6) { E2_STEP_WRITE(V); } else { E3_STEP_WRITE(V); } }while(0)
-    #define   NORM_E_DIR(E)   do{ switch (E) { \
-        case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E0_DIR_WRITE( INVERT_E0_DIR); break; \
-        case 2: E1_DIR_WRITE(!INVERT_E1_DIR); break; case 3: E1_DIR_WRITE( INVERT_E1_DIR); break; \
-        case 4: E2_DIR_WRITE(!INVERT_E2_DIR); break; case 5: E2_DIR_WRITE( INVERT_E2_DIR); break; \
-        case 6: E3_DIR_WRITE( INVERT_E3_DIR); break; \
-      } }while(0)
-    #define    REV_E_DIR(E)   do{ switch (E) { \
-        case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E0_DIR_WRITE(!INVERT_E0_DIR); break; \
-        case 2: E1_DIR_WRITE( INVERT_E1_DIR); break; case 3: E1_DIR_WRITE(!INVERT_E1_DIR); break; \
-        case 4: E2_DIR_WRITE( INVERT_E2_DIR); break; case 5: E2_DIR_WRITE(!INVERT_E2_DIR); break; \
-        case 6: E3_DIR_WRITE(!INVERT_E3_DIR); } }while(0)
-  #elif EXTRUDERS > 5
-    #define E_STEP_WRITE(E,V) do{ if (E < 2) { E0_STEP_WRITE(V); } else if (E < 4) { E1_STEP_WRITE(V); } else { E2_STEP_WRITE(V); } }while(0)
-    #define   NORM_E_DIR(E)   do{ switch (E) { \
-        case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E0_DIR_WRITE( INVERT_E0_DIR); break; \
-        case 2: E1_DIR_WRITE(!INVERT_E1_DIR); break; case 3: E1_DIR_WRITE( INVERT_E1_DIR); break; \
-        case 4: E2_DIR_WRITE(!INVERT_E2_DIR); break; case 5: E2_DIR_WRITE( INVERT_E2_DIR); break; \
-      } }while(0)
-    #define    REV_E_DIR(E)   do{ switch (E) { \
-        case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E0_DIR_WRITE(!INVERT_E0_DIR); break; \
-        case 2: E1_DIR_WRITE( INVERT_E1_DIR); break; case 3: E1_DIR_WRITE(!INVERT_E1_DIR); break; \
-        case 4: E2_DIR_WRITE( INVERT_E2_DIR); break; case 5: E2_DIR_WRITE(!INVERT_E2_DIR); break; \
-      } }while(0)
-  #elif EXTRUDERS > 4
-    #define E_STEP_WRITE(E,V) do{ if (E < 2) { E0_STEP_WRITE(V); } else if (E < 4) { E1_STEP_WRITE(V); } else { E2_STEP_WRITE(V); } }while(0)
-    #define   NORM_E_DIR(E)   do{ switch (E) { \
-        case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E0_DIR_WRITE( INVERT_E0_DIR); break; \
-        case 2: E1_DIR_WRITE(!INVERT_E1_DIR); break; case 3: E1_DIR_WRITE( INVERT_E1_DIR); break; \
-        case 4: E2_DIR_WRITE(!INVERT_E2_DIR); break; \
-      } }while(0)
-    #define    REV_E_DIR(E)   do{ switch (E) { \
-        case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E0_DIR_WRITE(!INVERT_E0_DIR); break; \
-        case 2: E1_DIR_WRITE( INVERT_E1_DIR); break; case 3: E1_DIR_WRITE(!INVERT_E1_DIR); break; \
-        case 4: E2_DIR_WRITE( INVERT_E2_DIR); break; \
-      } }while(0)
-  #elif EXTRUDERS > 3
-    #define E_STEP_WRITE(E,V) do{ if (E < 2) { E0_STEP_WRITE(V); } else { E1_STEP_WRITE(V); } }while(0)
-    #define   NORM_E_DIR(E)   do{ switch (E) { \
-        case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E0_DIR_WRITE( INVERT_E0_DIR); break; \
-        case 2: E1_DIR_WRITE(!INVERT_E1_DIR); break; case 3: E1_DIR_WRITE( INVERT_E1_DIR); break; \
-      } }while(0)
-    #define    REV_E_DIR(E)   do{ switch (E) { \
-        case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E0_DIR_WRITE(!INVERT_E0_DIR); break; \
-        case 2: E1_DIR_WRITE( INVERT_E1_DIR); break; case 3: E1_DIR_WRITE(!INVERT_E1_DIR); break; \
-      } }while(0)
-  #elif EXTRUDERS > 2
-    #define E_STEP_WRITE(E,V) do{ if (E < 2) { E0_STEP_WRITE(V); } else { E1_STEP_WRITE(V); } }while(0)
-    #define   NORM_E_DIR(E)   do{ switch (E) { \
-        case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E0_DIR_WRITE( INVERT_E0_DIR); break; \
-        case 2: E1_DIR_WRITE(!INVERT_E1_DIR); break; \
-      } }while(0)
-    #define    REV_E_DIR(E)   do{ switch (E) { \
-        case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E0_DIR_WRITE(!INVERT_E0_DIR); break; \
-        case 2: E1_DIR_WRITE( INVERT_E1_DIR); break; \
-      } }while(0)
-  #else
-    #define E_STEP_WRITE(E,V) E0_STEP_WRITE(V)
-    #define   NORM_E_DIR(E)   do{ E0_DIR_WRITE(E ?  INVERT_E0_DIR : !INVERT_E0_DIR); }while(0)
-    #define    REV_E_DIR(E)   do{ E0_DIR_WRITE(E ? !INVERT_E0_DIR :  INVERT_E0_DIR); }while(0)
-  #endif
-
-#elif HAS_PRUSA_MMU2
-
-  #define E_STEP_WRITE(E,V) E0_STEP_WRITE(V)
-  #define   NORM_E_DIR(E)   E0_DIR_WRITE(!INVERT_E0_DIR)
-  #define    REV_E_DIR(E)   E0_DIR_WRITE( INVERT_E0_DIR)
-
-#elif HAS_PRUSA_MMU1  // One multiplexed stepper driver, reversed on odd index
-
-  #define E_STEP_WRITE(E,V) E0_STEP_WRITE(V)
-  #define   NORM_E_DIR(E)   do{ E0_DIR_WRITE(TEST(E, 0) ? !INVERT_E0_DIR:  INVERT_E0_DIR); }while(0)
-  #define    REV_E_DIR(E)   do{ E0_DIR_WRITE(TEST(E, 0) ?  INVERT_E0_DIR: !INVERT_E0_DIR); }while(0)
-
-#elif E_STEPPERS > 1
-
-  #if E_STEPPERS > 7
-
-    #define _E_STEP_WRITE(E,V) do{ switch (E) { \
-        case 0: E0_STEP_WRITE(V); break; case 1: E1_STEP_WRITE(V); break; case 2: E2_STEP_WRITE(V); break; case 3: E3_STEP_WRITE(V); break; \
-        case 4: E4_STEP_WRITE(V); break; case 5: E5_STEP_WRITE(V); break; case 6: E6_STEP_WRITE(V); break; case 7: E7_STEP_WRITE(V); break; \
-      } }while(0)
-    #define   _NORM_E_DIR(E)   do{ switch (E) { \
-        case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E1_DIR_WRITE(!INVERT_E1_DIR); break; \
-        case 2: E2_DIR_WRITE(!INVERT_E2_DIR); break; case 3: E3_DIR_WRITE(!INVERT_E3_DIR); break; \
-        case 4: E4_DIR_WRITE(!INVERT_E4_DIR); break; case 5: E5_DIR_WRITE(!INVERT_E5_DIR); break; \
-        case 6: E6_DIR_WRITE(!INVERT_E6_DIR); break; case 7: E7_DIR_WRITE(!INVERT_E7_DIR); break; \
-      } }while(0)
-    #define    _REV_E_DIR(E)   do{ switch (E) { \
-        case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E1_DIR_WRITE( INVERT_E1_DIR); break; \
-        case 2: E2_DIR_WRITE( INVERT_E2_DIR); break; case 3: E3_DIR_WRITE( INVERT_E3_DIR); break; \
-        case 4: E4_DIR_WRITE( INVERT_E4_DIR); break; case 5: E5_DIR_WRITE( INVERT_E5_DIR); break; \
-        case 6: E6_DIR_WRITE( INVERT_E6_DIR); break; case 7: E7_DIR_WRITE( INVERT_E7_DIR); break; \
-      } }while(0)
-
-  #elif E_STEPPERS > 6
-
-    #define _E_STEP_WRITE(E,V) do{ switch (E) { \
-        case 0: E0_STEP_WRITE(V); break; case 1: E1_STEP_WRITE(V); break; case 2: E2_STEP_WRITE(V); break; case 3: E3_STEP_WRITE(V); break; \
-        case 4: E4_STEP_WRITE(V); break; case 5: E5_STEP_WRITE(V); break; case 6: E6_STEP_WRITE(V); break; \
-      } }while(0)
-    #define   _NORM_E_DIR(E)   do{ switch (E) { \
-        case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E1_DIR_WRITE(!INVERT_E1_DIR); break; \
-        case 2: E2_DIR_WRITE(!INVERT_E2_DIR); break; case 3: E3_DIR_WRITE(!INVERT_E3_DIR); break; \
-        case 4: E4_DIR_WRITE(!INVERT_E4_DIR); break; case 5: E5_DIR_WRITE(!INVERT_E5_DIR); break; \
-        case 6: E6_DIR_WRITE(!INVERT_E6_DIR); break; \
-      } }while(0)
-    #define    _REV_E_DIR(E)   do{ switch (E) { \
-        case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E1_DIR_WRITE( INVERT_E1_DIR); break; \
-        case 2: E2_DIR_WRITE( INVERT_E2_DIR); break; case 3: E3_DIR_WRITE( INVERT_E3_DIR); break; \
-        case 4: E4_DIR_WRITE( INVERT_E4_DIR); break; case 5: E5_DIR_WRITE( INVERT_E5_DIR); break; \
-        case 6: E6_DIR_WRITE( INVERT_E6_DIR); break; \
-      } }while(0)
-
-  #elif E_STEPPERS > 5
-
-    #define _E_STEP_WRITE(E,V) do{ switch (E) { \
-        case 0: E0_STEP_WRITE(V); break; case 1: E1_STEP_WRITE(V); break; case 2: E2_STEP_WRITE(V); break; case 3: E3_STEP_WRITE(V); break; \
-        case 4: E4_STEP_WRITE(V); break; case 5: E5_STEP_WRITE(V); break; \
-      } }while(0)
-    #define   _NORM_E_DIR(E)   do{ switch (E) { \
-        case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E1_DIR_WRITE(!INVERT_E1_DIR); break; \
-        case 2: E2_DIR_WRITE(!INVERT_E2_DIR); break; case 3: E3_DIR_WRITE(!INVERT_E3_DIR); break; \
-        case 4: E4_DIR_WRITE(!INVERT_E4_DIR); break; case 5: E5_DIR_WRITE(!INVERT_E5_DIR); break; \
-      } }while(0)
-    #define    _REV_E_DIR(E)   do{ switch (E) { \
-        case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E1_DIR_WRITE( INVERT_E1_DIR); break; \
-        case 2: E2_DIR_WRITE( INVERT_E2_DIR); break; case 3: E3_DIR_WRITE( INVERT_E3_DIR); break; \
-        case 4: E4_DIR_WRITE( INVERT_E4_DIR); break; case 5: E5_DIR_WRITE( INVERT_E5_DIR); break; \
-      } }while(0)
-
-  #elif E_STEPPERS > 4
-
-    #define _E_STEP_WRITE(E,V) do{ switch (E) { \
-        case 0: E0_STEP_WRITE(V); break; case 1: E1_STEP_WRITE(V); break; case 2: E2_STEP_WRITE(V); break; case 3: E3_STEP_WRITE(V); break; \
-        case 4: E4_STEP_WRITE(V); break; \
-      } }while(0)
-    #define   _NORM_E_DIR(E)   do{ switch (E) { \
-        case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E1_DIR_WRITE(!INVERT_E1_DIR); break; \
-        case 2: E2_DIR_WRITE(!INVERT_E2_DIR); break; case 3: E3_DIR_WRITE(!INVERT_E3_DIR); break; \
-        case 4: E4_DIR_WRITE(!INVERT_E4_DIR); break; \
-      } }while(0)
-    #define    _REV_E_DIR(E)   do{ switch (E) { \
-        case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E1_DIR_WRITE( INVERT_E1_DIR); break; \
-        case 2: E2_DIR_WRITE( INVERT_E2_DIR); break; case 3: E3_DIR_WRITE( INVERT_E3_DIR); break; \
-        case 4: E4_DIR_WRITE( INVERT_E4_DIR); break; \
-      } }while(0)
-
-  #elif E_STEPPERS > 3
-
-    #define _E_STEP_WRITE(E,V) do{ switch (E) { \
-        case 0: E0_STEP_WRITE(V); break; case 1: E1_STEP_WRITE(V); break; case 2: E2_STEP_WRITE(V); break; case 3: E3_STEP_WRITE(V); break; \
-      } }while(0)
-    #define   _NORM_E_DIR(E)   do{ switch (E) { \
-        case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E1_DIR_WRITE(!INVERT_E1_DIR); break; \
-        case 2: E2_DIR_WRITE(!INVERT_E2_DIR); break; case 3: E3_DIR_WRITE(!INVERT_E3_DIR); break; \
-      } }while(0)
-    #define    _REV_E_DIR(E)   do{ switch (E) { \
-        case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E1_DIR_WRITE( INVERT_E1_DIR); break; \
-        case 2: E2_DIR_WRITE( INVERT_E2_DIR); break; case 3: E3_DIR_WRITE( INVERT_E3_DIR); break; \
-      } }while(0)
-
-  #elif E_STEPPERS > 2
-
-    #define _E_STEP_WRITE(E,V) do{ switch (E) { case 0: E0_STEP_WRITE(V); break; case 1: E1_STEP_WRITE(V); break; case 2: E2_STEP_WRITE(V); } }while(0)
-    #define   _NORM_E_DIR(E)   do{ switch (E) { case 0: E0_DIR_WRITE(!INVERT_E0_DIR); break; case 1: E1_DIR_WRITE(!INVERT_E1_DIR); break; case 2: E2_DIR_WRITE(!INVERT_E2_DIR); } }while(0)
-    #define    _REV_E_DIR(E)   do{ switch (E) { case 0: E0_DIR_WRITE( INVERT_E0_DIR); break; case 1: E1_DIR_WRITE( INVERT_E1_DIR); break; case 2: E2_DIR_WRITE( INVERT_E2_DIR); } }while(0)
-
-  #else
-
-    #define _E_STEP_WRITE(E,V) do{ if (E == 0) { E0_STEP_WRITE(V); } else { E1_STEP_WRITE(V); } }while(0)
-    #define   _NORM_E_DIR(E)   do{ if (E == 0) { E0_DIR_WRITE(!INVERT_E0_DIR); } else { E1_DIR_WRITE(!INVERT_E1_DIR); } }while(0)
-    #define    _REV_E_DIR(E)   do{ if (E == 0) { E0_DIR_WRITE( INVERT_E0_DIR); } else { E1_DIR_WRITE( INVERT_E1_DIR); } }while(0)
-  #endif
-
-  #if HAS_DUPLICATION_MODE
-
-    #if ENABLED(MULTI_NOZZLE_DUPLICATION)
-      #define _DUPE(N,T,V)  do{ if (TEST(duplication_e_mask, N)) E##N##_##T##_WRITE(V); }while(0)
-    #else
-      #define _DUPE(N,T,V)  E##N##_##T##_WRITE(V)
-    #endif
-
-    #define NDIR(N) _DUPE(N,DIR,!INVERT_E##N##_DIR)
-    #define RDIR(N) _DUPE(N,DIR, INVERT_E##N##_DIR)
-
-    #define E_STEP_WRITE(E,V) do{ if (extruder_duplication_enabled) { DUPE(STEP,V); } else _E_STEP_WRITE(E,V); }while(0)
-
-    #if E_STEPPERS > 2
-      #if E_STEPPERS > 7
-        #define DUPE(T,V)     do{ _DUPE(0,T,V); _DUPE(1,T,V); _DUPE(2,T,V); _DUPE(3,T,V); _DUPE(4,T,V); _DUPE(5,T,V); _DUPE(6,T,V); _DUPE(7,T,V); }while(0)
-        #define NORM_E_DIR(E) do{ if (extruder_duplication_enabled) { NDIR(0); NDIR(1); NDIR(2); NDIR(3); NDIR(4); NDIR(5); NDIR(6); NDIR(7); } else _NORM_E_DIR(E); }while(0)
-        #define REV_E_DIR(E)  do{ if (extruder_duplication_enabled) { RDIR(0); RDIR(1); RDIR(2); RDIR(3); RDIR(4); RDIR(5); RDIR(6); RDIR(7); } else  _REV_E_DIR(E); }while(0)
-      #elif E_STEPPERS > 6
-        #define DUPE(T,V)     do{ _DUPE(0,T,V); _DUPE(1,T,V); _DUPE(2,T,V); _DUPE(3,T,V); _DUPE(4,T,V); _DUPE(5,T,V); _DUPE(6,T,V); }while(0)
-        #define NORM_E_DIR(E) do{ if (extruder_duplication_enabled) { NDIR(0); NDIR(1); NDIR(2); NDIR(3); NDIR(4); NDIR(5); NDIR(6); } else _NORM_E_DIR(E); }while(0)
-        #define REV_E_DIR(E)  do{ if (extruder_duplication_enabled) { RDIR(0); RDIR(1); RDIR(2); RDIR(3); RDIR(4); RDIR(5); RDIR(6); } else  _REV_E_DIR(E); }while(0)
-      #elif E_STEPPERS > 5
-        #define DUPE(T,V)     do{ _DUPE(0,T,V); _DUPE(1,T,V); _DUPE(2,T,V); _DUPE(3,T,V); _DUPE(4,T,V); _DUPE(5,T,V); }while(0)
-        #define NORM_E_DIR(E) do{ if (extruder_duplication_enabled) { NDIR(0); NDIR(1); NDIR(2); NDIR(3); NDIR(4); NDIR(5); } else _NORM_E_DIR(E); }while(0)
-        #define REV_E_DIR(E)  do{ if (extruder_duplication_enabled) { RDIR(0); RDIR(1); RDIR(2); RDIR(3); RDIR(4); RDIR(5); } else  _REV_E_DIR(E); }while(0)
-      #elif E_STEPPERS > 4
-        #define DUPE(T,V)     do{ _DUPE(0,T,V); _DUPE(1,T,V); _DUPE(2,T,V); _DUPE(3,T,V); _DUPE(4,T,V); }while(0)
-        #define NORM_E_DIR(E) do{ if (extruder_duplication_enabled) { NDIR(0); NDIR(1); NDIR(2); NDIR(3); NDIR(4); } else _NORM_E_DIR(E); }while(0)
-        #define REV_E_DIR(E)  do{ if (extruder_duplication_enabled) { RDIR(0); RDIR(1); RDIR(2); RDIR(3); RDIR(4); } else  _REV_E_DIR(E); }while(0)
-      #elif E_STEPPERS > 3
-        #define DUPE(T,V)     do{ _DUPE(0,T,V); _DUPE(1,T,V); _DUPE(2,T,V); _DUPE(3,T,V); }while(0)
-        #define NORM_E_DIR(E) do{ if (extruder_duplication_enabled) { NDIR(0); NDIR(1); NDIR(2); NDIR(3); } else _NORM_E_DIR(E); }while(0)
-        #define REV_E_DIR(E)  do{ if (extruder_duplication_enabled) { RDIR(0); RDIR(1); RDIR(2); RDIR(3); } else  _REV_E_DIR(E); }while(0)
-      #else
-        #define DUPE(T,V)     do{ _DUPE(0,T,V); _DUPE(1,T,V); _DUPE(2,T,V); }while(0)
-        #define NORM_E_DIR(E) do{ if (extruder_duplication_enabled) { NDIR(0); NDIR(1); NDIR(2); } else _NORM_E_DIR(E); }while(0)
-        #define REV_E_DIR(E)  do{ if (extruder_duplication_enabled) { RDIR(0); RDIR(1); RDIR(2); } else  _REV_E_DIR(E); }while(0)
-      #endif
-    #else
-      #define DUPE(T,V)     do{ _DUPE(0,T,V); _DUPE(1,T,V); }while(0)
-      #define NORM_E_DIR(E) do{ if (extruder_duplication_enabled) { NDIR(0); NDIR(1); } else _NORM_E_DIR(E); }while(0)
-      #define REV_E_DIR(E)  do{ if (extruder_duplication_enabled) { RDIR(0); RDIR(1); } else  _REV_E_DIR(E); }while(0)
-    #endif
-
-  #else
-
-    #define E_STEP_WRITE(E,V) _E_STEP_WRITE(E,V)
-    #define   NORM_E_DIR(E)   _NORM_E_DIR(E)
-    #define    REV_E_DIR(E)   _REV_E_DIR(E)
-
-  #endif
-
-#elif E_STEPPERS
-  #define E_STEP_WRITE(E,V) E0_STEP_WRITE(V)
-  #define   NORM_E_DIR(E)   E0_DIR_WRITE(!INVERT_E0_DIR)
-  #define    REV_E_DIR(E)   E0_DIR_WRITE( INVERT_E0_DIR)
+#define A_STEP_INIT() SET_OUTPUT(A_STEP_PIN)
+#ifndef A_STEP_WRITE
+  #define A_STEP_WRITE(STATE) WRITE(A_STEP_PIN,STATE)
+#endif
+#define A_STEP_READ() bool(READ(A_STEP_PIN))
 
-#else
-  #define E_STEP_WRITE(E,V) NOOP
-  #define   NORM_E_DIR(E)   NOOP
-  #define    REV_E_DIR(E)   NOOP
+// B Stepper
+#ifndef B_ENABLE_INIT
+  #define B_ENABLE_INIT() SET_OUTPUT(B_ENABLE_PIN)
+  #define B_ENABLE_WRITE(STATE) WRITE(B_ENABLE_PIN,STATE)
+  #define B_ENABLE_READ() bool(READ(B_ENABLE_PIN))
+#endif
+#ifndef B_DIR_INIT
+  #define B_DIR_INIT() SET_OUTPUT(B_DIR_PIN)
+  #define B_DIR_WRITE(STATE) WRITE(B_DIR_PIN,STATE)
+  #define B_DIR_READ() bool(READ(B_DIR_PIN))
+#endif
+#define B_STEP_INIT() SET_OUTPUT(B_STEP_PIN)
+#ifndef B_STEP_WRITE
+  #define B_STEP_WRITE(STATE) WRITE(B_STEP_PIN,STATE)
+#endif
+#define B_STEP_READ() bool(READ(B_STEP_PIN))
 
+// C Stepper
+#ifndef C_ENABLE_INIT
+  #define C_ENABLE_INIT() SET_OUTPUT(C_ENABLE_PIN)
+  #define C_ENABLE_WRITE(STATE) WRITE(C_ENABLE_PIN,STATE)
+  #define C_ENABLE_READ() bool(READ(C_ENABLE_PIN))
 #endif
+#ifndef C_DIR_INIT
+  #define C_DIR_INIT() SET_OUTPUT(C_DIR_PIN)
+  #define C_DIR_WRITE(STATE) WRITE(C_DIR_PIN,STATE)
+  #define C_DIR_READ() bool(READ(C_DIR_PIN))
+#endif
+#define C_STEP_INIT() SET_OUTPUT(C_STEP_PIN)
+#ifndef C_STEP_WRITE
+  #define C_STEP_WRITE(STATE) WRITE(C_STEP_PIN,STATE)
+#endif
+#define C_STEP_READ() bool(READ(C_STEP_PIN))
 
 //
 // Individual stepper enable / disable macros
@@ -725,123 +381,48 @@ void reset_stepper_drivers();    // Called by settings.load / settings.reset
   #endif
 #endif
 
-#ifndef ENABLE_STEPPER_E0
-  #if HAS_E0_ENABLE
-    #define  ENABLE_STEPPER_E0() E0_ENABLE_WRITE( E_ENABLE_ON)
-  #else
-    #define  ENABLE_STEPPER_E0() NOOP
-  #endif
-#endif
-#ifndef DISABLE_STEPPER_E0
-  #if HAS_E0_ENABLE
-    #define DISABLE_STEPPER_E0() E0_ENABLE_WRITE(!E_ENABLE_ON)
-  #else
-    #define DISABLE_STEPPER_E0() NOOP
-  #endif
-#endif
-
-#ifndef ENABLE_STEPPER_E1
-  #if E_STEPPERS > 1 && HAS_E1_ENABLE
-    #define  ENABLE_STEPPER_E1() E1_ENABLE_WRITE( E_ENABLE_ON)
-  #else
-    #define  ENABLE_STEPPER_E1() NOOP
-  #endif
-#endif
-#ifndef DISABLE_STEPPER_E1
-  #if E_STEPPERS > 1 && HAS_E1_ENABLE
-    #define DISABLE_STEPPER_E1() E1_ENABLE_WRITE(!E_ENABLE_ON)
-  #else
-    #define DISABLE_STEPPER_E1() NOOP
-  #endif
-#endif
-
-#ifndef ENABLE_STEPPER_E2
-  #if E_STEPPERS > 2 && HAS_E2_ENABLE
-    #define  ENABLE_STEPPER_E2() E2_ENABLE_WRITE( E_ENABLE_ON)
-  #else
-    #define  ENABLE_STEPPER_E2() NOOP
-  #endif
-#endif
-#ifndef DISABLE_STEPPER_E2
-  #if E_STEPPERS > 2 && HAS_E2_ENABLE
-    #define DISABLE_STEPPER_E2() E2_ENABLE_WRITE(!E_ENABLE_ON)
-  #else
-    #define DISABLE_STEPPER_E2() NOOP
-  #endif
-#endif
-
-#ifndef ENABLE_STEPPER_E3
-  #if E_STEPPERS > 3 && HAS_E3_ENABLE
-    #define  ENABLE_STEPPER_E3() E3_ENABLE_WRITE( E_ENABLE_ON)
-  #else
-    #define  ENABLE_STEPPER_E3() NOOP
-  #endif
-#endif
-#ifndef DISABLE_STEPPER_E3
-  #if E_STEPPERS > 3 && HAS_E3_ENABLE
-    #define DISABLE_STEPPER_E3() E3_ENABLE_WRITE(!E_ENABLE_ON)
-  #else
-    #define DISABLE_STEPPER_E3() NOOP
-  #endif
-#endif
-
-#ifndef ENABLE_STEPPER_E4
-  #if E_STEPPERS > 4 && HAS_E4_ENABLE
-    #define  ENABLE_STEPPER_E4() E4_ENABLE_WRITE( E_ENABLE_ON)
-  #else
-    #define  ENABLE_STEPPER_E4() NOOP
-  #endif
-#endif
-#ifndef DISABLE_STEPPER_E4
-  #if E_STEPPERS > 4 && HAS_E4_ENABLE
-    #define DISABLE_STEPPER_E4() E4_ENABLE_WRITE(!E_ENABLE_ON)
-  #else
-    #define DISABLE_STEPPER_E4() NOOP
-  #endif
-#endif
-
-#ifndef ENABLE_STEPPER_E5
-  #if E_STEPPERS > 5 && HAS_E5_ENABLE
-    #define  ENABLE_STEPPER_E5() E5_ENABLE_WRITE( E_ENABLE_ON)
+#ifndef ENABLE_STEPPER_A
+  #if HAS_A_ENABLE
+    #define  ENABLE_STEPPER_A() A_ENABLE_WRITE( A_ENABLE_ON)
   #else
-    #define  ENABLE_STEPPER_E5() NOOP
+    #define  ENABLE_STEPPER_A() NOOP
   #endif
 #endif
-#ifndef DISABLE_STEPPER_E5
-  #if E_STEPPERS > 5 && HAS_E5_ENABLE
-    #define DISABLE_STEPPER_E5() E5_ENABLE_WRITE(!E_ENABLE_ON)
+#ifndef DISABLE_STEPPER_A
+  #if HAS_A_ENABLE
+    #define DISABLE_STEPPER_A() A_ENABLE_WRITE(!A_ENABLE_ON)
   #else
-    #define DISABLE_STEPPER_E5() NOOP
+    #define DISABLE_STEPPER_A() NOOP
   #endif
 #endif
 
-#ifndef ENABLE_STEPPER_E6
-  #if E_STEPPERS > 6 && HAS_E6_ENABLE
-    #define  ENABLE_STEPPER_E6() E6_ENABLE_WRITE( E_ENABLE_ON)
+#ifndef ENABLE_STEPPER_B
+  #if HAS_B_ENABLE
+    #define  ENABLE_STEPPER_B() B_ENABLE_WRITE( B_ENABLE_ON)
   #else
-    #define  ENABLE_STEPPER_E6() NOOP
+    #define  ENABLE_STEPPER_B() NOOP
   #endif
 #endif
-#ifndef DISABLE_STEPPER_E6
-  #if E_STEPPERS > 6 && HAS_E6_ENABLE
-    #define DISABLE_STEPPER_E6() E6_ENABLE_WRITE(!E_ENABLE_ON)
+#ifndef DISABLE_STEPPER_B
+  #if HAS_B_ENABLE
+    #define DISABLE_STEPPER_B() B_ENABLE_WRITE(!B_ENABLE_ON)
   #else
-    #define DISABLE_STEPPER_E6() NOOP
+    #define DISABLE_STEPPER_B() NOOP
   #endif
 #endif
 
-#ifndef ENABLE_STEPPER_E7
-  #if E_STEPPERS > 7 && HAS_E7_ENABLE
-    #define  ENABLE_STEPPER_E7() E7_ENABLE_WRITE( E_ENABLE_ON)
+#ifndef ENABLE_STEPPER_C
+  #if HAS_C_ENABLE
+    #define  ENABLE_STEPPER_C() C_ENABLE_WRITE( C_ENABLE_ON)
   #else
-    #define  ENABLE_STEPPER_E7() NOOP
+    #define  ENABLE_STEPPER_C() NOOP
   #endif
 #endif
-#ifndef DISABLE_STEPPER_E7
-  #if E_STEPPERS > 7 && HAS_E7_ENABLE
-    #define DISABLE_STEPPER_E7() E7_ENABLE_WRITE(!E_ENABLE_ON)
+#ifndef DISABLE_STEPPER_C
+  #if HAS_C_ENABLE
+    #define DISABLE_STEPPER_C() C_ENABLE_WRITE(!C_ENABLE_ON)
   #else
-    #define DISABLE_STEPPER_E7() NOOP
+    #define DISABLE_STEPPER_C() NOOP
   #endif
 #endif
 
@@ -849,11 +430,11 @@ void reset_stepper_drivers();    // Called by settings.load / settings.reset
 // Axis steppers enable / disable macros
 //
 
-extern xyz_bool_t axis_enabled;
+extern bool axis_enabled[6];
 
-#define SHOULD_ENABLE(N)   !axis_enabled.N
-#define SHOULD_DISABLE(N)  axis_enabled.N
-#define AFTER_CHANGE(N,TF) axis_enabled.N = TF
+#define SHOULD_ENABLE(N)   !axis_enabled[Axis::N()]
+#define SHOULD_DISABLE(N)  axis_enabled[Axis::N()]
+#define AFTER_CHANGE(N,TF) axis_enabled[Axis::N()] = TF
 
 #ifdef HAS_Z_BRAKE
 	#define BRAKE_Z() Z_BRAKE_WRITE(LOW);
@@ -863,12 +444,19 @@ extern xyz_bool_t axis_enabled;
 	#define UNBRAKE_Z() NOOP
 #endif
 
-#define  ENABLE_AXIS_X() if (SHOULD_ENABLE(x))  {  ENABLE_STEPPER_X();  ENABLE_STEPPER_X2(); AFTER_CHANGE(x, true); }
-#define DISABLE_AXIS_X() if (SHOULD_DISABLE(x)) { DISABLE_STEPPER_X(); DISABLE_STEPPER_X2(); AFTER_CHANGE(x, false); set_axis_untrusted(X_AXIS); }
-#define  ENABLE_AXIS_Y() if (SHOULD_ENABLE(y))  {  ENABLE_STEPPER_Y();  ENABLE_STEPPER_Y2(); AFTER_CHANGE(y, true); }
-#define DISABLE_AXIS_Y() if (SHOULD_DISABLE(y)) { DISABLE_STEPPER_Y(); DISABLE_STEPPER_Y2(); AFTER_CHANGE(y, false); set_axis_untrusted(Y_AXIS); }
-#define  ENABLE_AXIS_Z() if (SHOULD_ENABLE(z))  {  ENABLE_STEPPER_Z(); safe_delay(100); UNBRAKE_Z(); AFTER_CHANGE(z, true); }
-#define DISABLE_AXIS_Z() if (SHOULD_DISABLE(z)) { BRAKE_Z(); DISABLE_STEPPER_Z(); AFTER_CHANGE(z, false); set_axis_untrusted(Z_AXIS); Z_RESET(); }
+#define  ENABLE_AXIS_X() if (SHOULD_ENABLE(X))  {  ENABLE_STEPPER_X();  ENABLE_STEPPER_X2(); AFTER_CHANGE(X, true); }
+#define DISABLE_AXIS_X() if (SHOULD_DISABLE(X)) { DISABLE_STEPPER_X(); DISABLE_STEPPER_X2(); AFTER_CHANGE(X, false); set_axis_untrusted(Axis::X()); }
+#define  ENABLE_AXIS_Y() if (SHOULD_ENABLE(Y))  {  ENABLE_STEPPER_Y();  ENABLE_STEPPER_Y2(); AFTER_CHANGE(Y, true); }
+#define DISABLE_AXIS_Y() if (SHOULD_DISABLE(Y)) { DISABLE_STEPPER_Y(); DISABLE_STEPPER_Y2(); AFTER_CHANGE(Y, false); set_axis_untrusted(Axis::Y()); }
+#define  ENABLE_AXIS_Z() if (SHOULD_ENABLE(Z))  {  ENABLE_STEPPER_Z(); safe_delay(100); UNBRAKE_Z(); AFTER_CHANGE(Z, true); }
+#define DISABLE_AXIS_Z() if (SHOULD_DISABLE(Z)) { BRAKE_Z(); DISABLE_STEPPER_Z(); AFTER_CHANGE(Z, false); set_axis_untrusted(Axis::Z()); Z_RESET(); }
+#define  ENABLE_AXIS_A() if (SHOULD_ENABLE(A))  {  ENABLE_STEPPER_A(); AFTER_CHANGE(A, true); }
+#define DISABLE_AXIS_A() if (SHOULD_DISABLE(A)) { DISABLE_STEPPER_A(); AFTER_CHANGE(A, false); set_axis_untrusted(Axis::A()); }
+#define  ENABLE_AXIS_B() if (SHOULD_ENABLE(B))  {  ENABLE_STEPPER_B(); AFTER_CHANGE(B, true); }
+#define DISABLE_AXIS_B() if (SHOULD_DISABLE(B)) { DISABLE_STEPPER_B(); AFTER_CHANGE(B, false); set_axis_untrusted(Axis::B()); }
+#define  ENABLE_AXIS_C() if (SHOULD_ENABLE(C))  {  ENABLE_STEPPER_C(); AFTER_CHANGE(C, true); }
+#define DISABLE_AXIS_C() if (SHOULD_DISABLE(C)) { DISABLE_STEPPER_C(); AFTER_CHANGE(C, false); set_axis_untrusted(Axis::C()); }
+
 
 #ifdef Z_AFTER_DEACTIVATE
   #define Z_RESET() do{ current_position.z = Z_AFTER_DEACTIVATE; sync_plan_position(); }while(0)
diff --git a/src/marlin/pins/pins_postprocess.h b/src/marlin/pins/pins_postprocess.h
index 13b0bdad80..b67cbc9ebb 100644
--- a/src/marlin/pins/pins_postprocess.h
+++ b/src/marlin/pins/pins_postprocess.h
@@ -203,6 +203,7 @@
 //
 // Destroy unused CS pins
 //
+/*
 #if !AXIS_HAS_SPI(X)
   #undef X_CS_PIN
 #endif
@@ -236,6 +237,7 @@
 #if E_STEPPERS > 7 && !AXIS_HAS_SPI(E7)
   #undef E7_CS_PIN
 #endif
+*/
 
 #ifndef X_CS_PIN
   #define X_CS_PIN -1
@@ -607,17 +609,6 @@
   #ifndef Y2_MS3_PIN
     #define Y2_MS3_PIN    _EPIN(Y2_E_INDEX, MS3)
   #endif
-  #if AXIS_HAS_SPI(Y2) && !defined(Y2_CS_PIN)
-    #define Y2_CS_PIN     _EPIN(Y2_E_INDEX, CS)
-  #endif
-  #if AXIS_HAS_UART(Y2)
-    #ifndef Y2_SERIAL_TX_PIN
-      #define Y2_SERIAL_TX_PIN _EPIN(Y2_E_INDEX, SERIAL_TX)
-    #endif
-    #ifndef Y2_SERIAL_RX_PIN
-      #define Y2_SERIAL_RX_PIN _EPIN(Y2_E_INDEX, SERIAL_RX)
-    #endif
-  #endif
   #if defined(Y2_STALL_SENSITIVITY) && ENABLED(Y_DUAL_ENDSTOPS) && _PEXI(Y2_E_INDEX, DIAG)
     #define Y2_DIAG_PIN _EPIN(Y2_E_INDEX, DIAG)
     #if   DIAG_REMAPPED(Y2, X_MIN)
diff --git a/src/marlin/pins/sensitive_pins.h b/src/marlin/pins/sensitive_pins.h
index d7eb187245..85c46cc3cc 100644
--- a/src/marlin/pins/sensitive_pins.h
+++ b/src/marlin/pins/sensitive_pins.h
@@ -35,7 +35,7 @@
 #else
   #define _X_MAX
 #endif
-#if PIN_EXISTS(X_CS) && AXIS_HAS_SPI(X)
+#if PIN_EXISTS(X_CS)
   #define _X_CS X_CS_PIN,
 #else
   #define _X_CS
@@ -68,7 +68,7 @@
 #else
   #define _Y_MAX
 #endif
-#if PIN_EXISTS(Y_CS) && AXIS_HAS_SPI(Y)
+#if PIN_EXISTS(Y_CS)
   #define _Y_CS Y_CS_PIN,
 #else
   #define _Y_CS
@@ -101,7 +101,7 @@
 #else
   #define _Z_MAX
 #endif
-#if PIN_EXISTS(Z_CS) && AXIS_HAS_SPI(Z)
+#if PIN_EXISTS(Z_CS)
   #define _Z_CS Z_CS_PIN,
 #else
   #define _Z_CS
@@ -124,337 +124,40 @@
 
 #define _Z_PINS Z_STEP_PIN, Z_DIR_PIN, Z_ENABLE_PIN, _Z_MIN _Z_MAX _Z_MS1 _Z_MS2 _Z_MS3 _Z_CS
 
-//
-// Extruder Chip Select, Digital Micro-steps
-//
-
-// Mixing stepper, Switching stepper, or regular stepper
-#define E_NEEDED(N) (ENABLED(MIXING_EXTRUDER) && MIXING_STEPPERS > N) \
-                 || (ENABLED(SWITCHING_EXTRUDER) && E_STEPPERS > N) \
-                 || (NONE(SWITCHING_EXTRUDER, MIXING_EXTRUDER) && EXTRUDERS > N)
-
-#define _E0_CS
-#define _E0_MS1
-#define _E0_MS2
-#define _E0_MS3
-
-#if E_NEEDED(0)
-  #if PIN_EXISTS(E0_CS) && AXIS_HAS_SPI(E0)
-    #undef _E0_CS
-    #define _E0_CS E0_CS_PIN,
-  #endif
-  #if PIN_EXISTS(E0_MS1)
-    #undef _E0_MS1
-    #define _E0_MS1 E0_MS1_PIN,
-  #endif
-  #if PIN_EXISTS(E0_MS2)
-    #undef _E0_MS2
-    #define _E0_MS2 E0_MS2_PIN,
-  #endif
-  #if PIN_EXISTS(E0_MS3)
-    #undef _E0_MS3
-    #define _E0_MS3 E0_MS3_PIN,
-  #endif
-#endif
-
-#define _E1_CS
-#define _E1_MS1
-#define _E1_MS2
-#define _E1_MS3
 
-#if E_NEEDED(1)
-  #if PIN_EXISTS(E1_CS) && AXIS_HAS_SPI(E1)
-    #undef _E1_CS
-    #define _E1_CS E1_CS_PIN,
-  #endif
-  #if PIN_EXISTS(E1_MS1)
-    #undef _E1_MS1
-    #define _E1_MS1 E1_MS1_PIN,
-  #endif
-  #if PIN_EXISTS(E1_MS2)
-    #undef _E1_MS2
-    #define _E1_MS2 E1_MS2_PIN,
-  #endif
-  #if PIN_EXISTS(E1_MS3)
-    #undef _E1_MS3
-    #define _E1_MS3 E1_MS3_PIN,
-  #endif
-#endif
-
-#define _E2_CS
-#define _E2_MS1
-#define _E2_MS2
-#define _E2_MS3
-
-#if E_NEEDED(2)
-  #if PIN_EXISTS(E2_CS) && AXIS_HAS_SPI(E2)
-    #undef _E2_CS
-    #define _E2_CS E2_CS_PIN,
-  #endif
-  #if PIN_EXISTS(E2_MS1)
-    #undef _E2_MS1
-    #define _E2_MS1 E2_MS1_PIN,
-  #endif
-  #if PIN_EXISTS(E2_MS2)
-    #undef _E2_MS2
-    #define _E2_MS2 E2_MS2_PIN,
-  #endif
-  #if PIN_EXISTS(E2_MS3)
-    #undef _E2_MS3
-    #define _E2_MS3 E2_MS3_PIN,
-  #endif
-#endif
-
-#define _E3_CS
-#define _E3_MS1
-#define _E3_MS2
-#define _E3_MS3
-
-#if E_NEEDED(3)
-  #if PIN_EXISTS(E3_CS) && AXIS_HAS_SPI(E3)
-    #undef _E3_CS
-    #define _E3_CS E3_CS_PIN,
-  #endif
-  #if PIN_EXISTS(E3_MS1)
-    #undef _E3_MS1
-    #define _E3_MS1 E3_MS1_PIN,
-  #endif
-  #if PIN_EXISTS(E3_MS2)
-    #undef _E3_MS2
-    #define _E3_MS2 E3_MS2_PIN,
-  #endif
-  #if PIN_EXISTS(E3_MS3)
-    #undef _E3_MS3
-    #define _E3_MS3 E3_MS3_PIN,
-  #endif
-#endif
-
-#define _E4_CS
-#define _E4_MS1
-#define _E4_MS2
-#define _E4_MS3
-
-#if E_NEEDED(4)
-  #if PIN_EXISTS(E4_CS) && AXIS_HAS_SPI(E4)
-    #undef _E4_CS
-    #define _E4_CS E4_CS_PIN,
-  #endif
-  #if PIN_EXISTS(E4_MS1)
-    #undef _E4_MS1
-    #define _E4_MS1 E4_MS1_PIN,
-  #endif
-  #if PIN_EXISTS(E4_MS2)
-    #undef _E4_MS2
-    #define _E4_MS2 E4_MS2_PIN,
-  #endif
-  #if PIN_EXISTS(E4_MS3)
-    #undef _E4_MS3
-    #define _E4_MS3 E4_MS3_PIN,
-  #endif
-#endif
-
-#define _E5_CS
-#define _E5_MS1
-#define _E5_MS2
-#define _E5_MS3
-
-#if E_NEEDED(5)
-  #if PIN_EXISTS(E5_CS) && AXIS_HAS_SPI(E5)
-    #undef _E5_CS
-    #define _E5_CS E5_CS_PIN,
-  #endif
-  #if PIN_EXISTS(E5_MS1)
-    #undef _E5_MS1
-    #define _E5_MS1 E5_MS1_PIN,
-  #endif
-  #if PIN_EXISTS(E5_MS2)
-    #undef _E5_MS2
-    #define _E5_MS2 E5_MS2_PIN,
-  #endif
-  #if PIN_EXISTS(E5_MS3)
-    #undef _E5_MS3
-    #define _E5_MS3 E5_MS3_PIN,
-  #endif
-#endif
-
-#define _E6_CS
-#define _E6_MS1
-#define _E6_MS2
-#define _E6_MS3
-
-#if E_NEEDED(6)
-  #if PIN_EXISTS(E6_CS) && AXIS_HAS_SPI(E6)
-    #undef _E6_CS
-    #define _E6_CS E6_CS_PIN,
-  #endif
-  #if PIN_EXISTS(E6_MS2)
-    #undef _E6_MS2
-    #define _E6_MS2 E6_MS2_PIN,
-  #endif
-  #if PIN_EXISTS(E6_MS3)
-    #undef _E6_MS3
-    #define _E6_MS3 E6_MS3_PIN,
-  #endif
-  #if PIN_EXISTS(E6_MS4)
-    #undef _E6_MS4
-    #define _E6_MS4 E6_MS4_PIN,
-  #endif
-#endif
-
-#define _E7_CS
-#define _E7_MS1
-#define _E7_MS2
-#define _E7_MS3
-
-#if E_NEEDED(7)
-  #if PIN_EXISTS(E7_CS) && AXIS_HAS_SPI(E7)
-    #undef _E7_CS
-    #define _E7_CS E7_CS_PIN,
-  #endif
-  #if PIN_EXISTS(E7_MS3)
-    #undef _E7_MS3
-    #define _E7_MS3 E7_MS3_PIN,
-  #endif
-  #if PIN_EXISTS(E7_MS4)
-    #undef _E7_MS4
-    #define _E7_MS4 E7_MS4_PIN,
-  #endif
-  #if PIN_EXISTS(E7_MS5)
-    #undef _E7_MS5
-    #define _E7_MS5 E7_MS5_PIN,
-  #endif
-#endif
-
-//
-// E Steppers
-//
-
-#define _E0_PINS
-#define _E1_PINS
-#define _E2_PINS
-#define _E3_PINS
-#define _E4_PINS
-#define _E5_PINS
-#define _E6_PINS
-#define _E7_PINS
-
-#if EXTRUDERS
-  #undef _E0_PINS
-  #define _E0_PINS E0_STEP_PIN, E0_DIR_PIN, E0_ENABLE_PIN, _E0_CS _E0_MS1 _E0_MS2 _E0_MS3
-#endif
-
-#if ENABLED(SWITCHING_EXTRUDER)
-                      // Tools 0 and 1 use E0
-  #if EXTRUDERS > 2   // Tools 2 and 3 use E1
-    #undef _E1_PINS
-    #define _E1_PINS E1_STEP_PIN, E1_DIR_PIN, E1_ENABLE_PIN, _E1_CS _E1_MS1 _E1_MS2 _E1_MS3
-    #if EXTRUDERS > 4 // Tools 4 and 5 use E2
-      #undef _E2_PINS
-      #define _E2_PINS E2_STEP_PIN, E2_DIR_PIN, E2_ENABLE_PIN, _E2_CS _E2_MS1 _E2_MS2 _E2_MS3
-    #endif
-  #endif
-
-#elif EITHER(HAS_MULTI_EXTRUDER, MIXING_EXTRUDER)
-
-  #undef _E1_PINS
-  #define _E1_PINS E1_STEP_PIN, E1_DIR_PIN, E1_ENABLE_PIN, _E1_CS _E1_MS1 _E1_MS2 _E1_MS3
-  #if EXTRUDERS > 2 || (ENABLED(MIXING_EXTRUDER) && MIXING_STEPPERS > 2)
-    #undef _E2_PINS
-    #define _E2_PINS E2_STEP_PIN, E2_DIR_PIN, E2_ENABLE_PIN, _E2_CS _E2_MS1 _E2_MS2 _E2_MS3
-    #if EXTRUDERS > 3 || (ENABLED(MIXING_EXTRUDER) && MIXING_STEPPERS > 3)
-      #undef _E3_PINS
-      #define _E3_PINS E3_STEP_PIN, E3_DIR_PIN, E3_ENABLE_PIN, _E3_CS _E3_MS1 _E3_MS2 _E3_MS3
-      #if EXTRUDERS > 4 || (ENABLED(MIXING_EXTRUDER) && MIXING_STEPPERS > 4)
-        #undef _E4_PINS
-        #define _E4_PINS E4_STEP_PIN, E4_DIR_PIN, E4_ENABLE_PIN, _E4_CS _E4_MS1 _E4_MS2 _E4_MS3
-        #if EXTRUDERS > 5 || (ENABLED(MIXING_EXTRUDER) && MIXING_STEPPERS > 5)
-          #undef _E5_PINS
-          #define _E5_PINS E5_STEP_PIN, E5_DIR_PIN, E5_ENABLE_PIN, _E5_CS _E5_MS1 _E5_MS2 _E5_MS3
-          #if EXTRUDERS > 6 || (ENABLED(MIXING_EXTRUDER) && MIXING_STEPPERS > 6)
-            #undef _E6_PINS
-            #define _E6_PINS E6_STEP_PIN, E6_DIR_PIN, E6_ENABLE_PIN, _E6_CS _E6_MS1 _E6_MS2 _E6_MS3
-            #if EXTRUDERS > 7 || (ENABLED(MIXING_EXTRUDER) && MIXING_STEPPERS > 7)
-              #undef _E7_PINS
-              #define _E7_PINS E7_STEP_PIN, E7_DIR_PIN, E7_ENABLE_PIN, _E7_CS _E7_MS1 _E7_MS2 _E7_MS3
-            #endif // EXTRUDERS > 7 || MIXING_EXTRUDER > 7
-          #endif // EXTRUDERS > 6 || MIXING_EXTRUDER > 6
-        #endif // EXTRUDERS > 5 || MIXING_EXTRUDER > 5
-      #endif // EXTRUDERS > 4 || MIXING_EXTRUDER > 4
-    #endif // EXTRUDERS > 3 || MIXING_EXTRUDER > 3
-  #endif // EXTRUDERS > 2 || MIXING_EXTRUDER > 2
-
-#endif // HAS_MULTI_EXTRUDER || MIXING_EXTRUDER
-
-//
-// Heaters, Fans, Temp Sensors
-//
-
-#ifndef E0_AUTO_FAN_PIN
-  #define E0_AUTO_FAN_PIN -1
-#endif
-#ifndef E1_AUTO_FAN_PIN
-  #define E1_AUTO_FAN_PIN -1
-#endif
-#ifndef E2_AUTO_FAN_PIN
-  #define E2_AUTO_FAN_PIN -1
+#if PIN_EXISTS(A_MIN)
+  #define _A_MIN A_MIN_PIN,
+#else
+  #define _A_MIN
 #endif
-#ifndef E3_AUTO_FAN_PIN
-  #define E3_AUTO_FAN_PIN -1
+#if PIN_EXISTS(A_MAX)
+  #define _A_MAX A_MAX_PIN,
+#else
+  #define _A_MAX
 #endif
-#ifndef E4_AUTO_FAN_PIN
-  #define E4_AUTO_FAN_PIN -1
+#if PIN_EXISTS(A_CS)
+  #define _A_CS A_CS_PIN,
+#else
+  #define _A_CS
 #endif
-#ifndef E5_AUTO_FAN_PIN
-  #define E5_AUTO_FAN_PIN -1
+#if PIN_EXISTS(A_MS1)
+  #define _A_MS1 A_MS1_PIN,
+#else
+  #define _A_MS1
 #endif
-#ifndef E6_AUTO_FAN_PIN
-  #define E6_AUTO_FAN_PIN -1
+#if PIN_EXISTS(A_MS2)
+  #define _A_MS2 A_MS2_PIN,
+#else
+  #define _A_MS2
 #endif
-#ifndef E7_AUTO_FAN_PIN
-  #define E7_AUTO_FAN_PIN -1
+#if PIN_EXISTS(A_MS3)
+  #define _A_MS3 A_MS3_PIN,
+#else
+  #define _A_MS3
 #endif
 
-#define _H0_PINS
-#define _H1_PINS
-#define _H2_PINS
-#define _H3_PINS
-#define _H4_PINS
-#define _H5_PINS
-#define _H6_PINS
-#define _H7_PINS
+#define _A_PINS A_STEP_PIN, A_DIR_PIN, A_ENABLE_PIN, _A_MIN _A_MAX _A_MS1 _A_MS2 _A_MS3 _A_CS
 
-#if HAS_HOTEND
-  #undef _H0_PINS
-  #define _H0_PINS HEATER_0_PIN, E0_AUTO_FAN_PIN, analogInputToDigitalPin(TEMP_0_PIN),
-  #if HAS_MULTI_HOTEND
-    #undef _H1_PINS
-    #define _H1_PINS HEATER_1_PIN, E1_AUTO_FAN_PIN, analogInputToDigitalPin(TEMP_1_PIN),
-    #if HOTENDS > 2
-      #undef _H2_PINS
-      #define _H2_PINS HEATER_2_PIN, E2_AUTO_FAN_PIN, analogInputToDigitalPin(TEMP_2_PIN),
-      #if HOTENDS > 3
-        #undef _H3_PINS
-        #define _H3_PINS HEATER_3_PIN, E3_AUTO_FAN_PIN, analogInputToDigitalPin(TEMP_3_PIN),
-        #if HOTENDS > 4
-          #undef _H4_PINS
-          #define _H4_PINS HEATER_4_PIN, E4_AUTO_FAN_PIN, analogInputToDigitalPin(TEMP_4_PIN),
-          #if HOTENDS > 5
-            #undef _H5_PINS
-            #define _H5_PINS HEATER_5_PIN, E5_AUTO_FAN_PIN, analogInputToDigitalPin(TEMP_5_PIN),
-            #if HOTENDS > 6
-              #undef _H6_PINS
-              #define _H6_PINS HEATER_6_PIN, E6_AUTO_FAN_PIN, analogInputToDigitalPin(TEMP_6_PIN),
-              #if HOTENDS > 7
-                #undef _H7_PINS
-                #define _H7_PINS HEATER_7_PIN, E7_AUTO_FAN_PIN, analogInputToDigitalPin(TEMP_7_PIN),
-              #endif // HOTENDS > 7
-            #endif // HOTENDS > 6
-          #endif // HOTENDS > 5
-        #endif // HOTENDS > 4
-      #endif // HOTENDS > 3
-    #endif // HOTENDS > 2
-  #endif // HAS_MULTI_HOTEND
-#endif // HOTENDS
 
 //
 // Dual X, Dual Y, Multi-Z
@@ -488,7 +191,7 @@
 #endif
 
 #if ENABLED(Y_DUAL_STEPPER_DRIVERS)
-  #if PIN_EXISTS(Y2_CS) && AXIS_HAS_SPI(Y2)
+  #if PIN_EXISTS(Y2_CS)
     #define _Y2_CS Y2_CS_PIN,
   #else
     #define _Y2_CS
@@ -682,8 +385,7 @@
 
 #define SENSITIVE_PINS { \
   _X_PINS _Y_PINS _Z_PINS _X2_PINS _Y2_PINS _Z2_PINS _Z3_PINS _Z4_PINS _Z_PROBE \
-  _E0_PINS _E1_PINS _E2_PINS _E3_PINS _E4_PINS _E5_PINS _E6_PINS _E7_PINS \
-  _H0_PINS _H1_PINS _H2_PINS _H3_PINS _H4_PINS _H5_PINS _H6_PINS _H7_PINS \
+  _A_PINS \
   _PS_ON _FAN0 _FAN1 _FAN2 _FAN3 _FAN4 _FAN5 _FAN6 _FAN7 _FANC \
   _BED_PINS _CHAMBER_TEMP _CHAMBER_HEATER _CHAMBER_FAN HAL_SENSITIVE_PINS \
 }
diff --git a/src/swordfish/CMakeLists.txt b/src/swordfish/CMakeLists.txt
index d02fac0ac4..b9345f655a 100644
--- a/src/swordfish/CMakeLists.txt
+++ b/src/swordfish/CMakeLists.txt
@@ -7,6 +7,7 @@ target_sources(${PROJECT_NAME}.elf
 		Exception.cpp
 		Exception.h
 		macros.h
+		math.h
 		Module.cpp
 		Module.h
 		PersistentStore.h
diff --git a/src/swordfish/Controller.h b/src/swordfish/Controller.h
index b2374396ce..50f89d2382 100644
--- a/src/swordfish/Controller.h
+++ b/src/swordfish/Controller.h
@@ -9,6 +9,7 @@
 
 #include <array>
 
+#include <swordfish/math.h>
 #include <swordfish/utils/NotCopyable.h>
 #include <swordfish/utils/NotMovable.h>
 #include <swordfish/utils/TypeInfo.h>
diff --git a/src/swordfish/core/Vector3.h b/src/swordfish/core/Vector3.h
index de771def67..9d997cd5e8 100644
--- a/src/swordfish/core/Vector3.h
+++ b/src/swordfish/core/Vector3.h
@@ -67,9 +67,9 @@ namespace swordfish::core {
 		}
 
 		void set(Eigen::Vector3f& value) {
-			x(value(X));
-			y(value(Y));
-			z(value(Z));
+			x(value(Axis::X()));
+			y(value(Axis::Y()));
+			z(value(Axis::Z()));
 		}
 
 		inline operator Eigen::Vector3f() {
diff --git a/src/swordfish/math.h b/src/swordfish/math.h
new file mode 100644
index 0000000000..a52bcdfabd
--- /dev/null
+++ b/src/swordfish/math.h
@@ -0,0 +1,54 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/LU>
+
+#include <swordfish/types.h>
+
+namespace swordfish::math {
+#define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \
+	/** \ingroup matrixtypedefs */ \
+	/** \brief `Size`&times;`Size` matrix of type `Type`. */ \
+	typedef Eigen::Matrix<Type, Size, Size> Matrix##SizeSuffix##TypeSuffix; \
+	/** \ingroup matrixtypedefs */ \
+	/** \brief `Size`&times;`1` vector of type `Type`. */ \
+	typedef Eigen::Matrix<Type, Size, 1> Vector##SizeSuffix##TypeSuffix; \
+	/** \ingroup matrixtypedefs */ \
+	/** \brief `1`&times;`Size` vector of type `Type`. */ \
+	typedef Eigen::Matrix<Type, 1, Size> RowVector##SizeSuffix##TypeSuffix;
+
+#define EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, Size) \
+	/** \ingroup matrixtypedefs */ \
+	/** \brief `Size`&times;`Dynamic` matrix of type `Type`. */ \
+	typedef Eigen::Matrix<Type, Size, Eigen::Dynamic> Matrix##Size##X##TypeSuffix; \
+	/** \ingroup matrixtypedefs */ \
+	/** \brief `Dynamic`&times;`Size` matrix of type `Type`. */ \
+	typedef Eigen::Matrix<Type, Eigen::Dynamic, Size> Matrix##X##Size##TypeSuffix;
+
+#define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \
+	EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 2, 2) \
+	EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 3, 3) \
+	EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 4, 4) \
+	EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 5, 5) \
+	EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 6, 6) \
+	EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Eigen::Dynamic, X) \
+	EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 2) \
+	EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 3) \
+	EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 4) \
+	EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 5) \
+	EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 6)
+
+	EIGEN_MAKE_TYPEDEFS_ALL_SIZES(i8, i8)
+	EIGEN_MAKE_TYPEDEFS_ALL_SIZES(u8, u8)
+	EIGEN_MAKE_TYPEDEFS_ALL_SIZES(i16, i16)
+	EIGEN_MAKE_TYPEDEFS_ALL_SIZES(u16, u16)
+	EIGEN_MAKE_TYPEDEFS_ALL_SIZES(i32, i32)
+	EIGEN_MAKE_TYPEDEFS_ALL_SIZES(u32, u32)
+	EIGEN_MAKE_TYPEDEFS_ALL_SIZES(f32, f32)
+	EIGEN_MAKE_TYPEDEFS_ALL_SIZES(f64, f64)
+	EIGEN_MAKE_TYPEDEFS_ALL_SIZES(bool, bool)
+
+#undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES
+#undef EIGEN_MAKE_TYPEDEFS
+#undef EIGEN_MAKE_FIXED_TYPEDEFS
+} // namespace swordfish::math
\ No newline at end of file
diff --git a/src/swordfish/modules/estop/EStopModule.cpp b/src/swordfish/modules/estop/EStopModule.cpp
index 58cd3620c9..7f443bffbc 100644
--- a/src/swordfish/modules/estop/EStopModule.cpp
+++ b/src/swordfish/modules/estop/EStopModule.cpp
@@ -71,9 +71,10 @@ namespace swordfish::estop {
 			queue.clear();
 			gcode.abort_current();
 
-			set_axis_never_homed(X_AXIS);
-			set_axis_never_homed(Y_AXIS);
-			set_axis_never_homed(Z_AXIS);
+			set_axis_never_homed(Axis::X());
+			set_axis_never_homed(Axis::Y());
+			set_axis_never_homed(Axis::Z());
+			set_axis_never_homed(Axis::A());
 
 			auto& statusModule = StatusModule::getInstance();
 
diff --git a/src/swordfish/modules/motion/CMakeLists.txt b/src/swordfish/modules/motion/CMakeLists.txt
index ab06795909..353060d1c9 100644
--- a/src/swordfish/modules/motion/CMakeLists.txt
+++ b/src/swordfish/modules/motion/CMakeLists.txt
@@ -4,6 +4,7 @@ target_sources(${PROJECT_NAME}.elf
 		CoordinateSystem.h
 		CoordinateSystemTable.cpp
 		CoordinateSystemTable.h
+		FeedRate.h
 		LimitException.cpp
 		LimitException.h
 		Limits.cpp
diff --git a/src/swordfish/modules/motion/FeedRate.h b/src/swordfish/modules/motion/FeedRate.h
new file mode 100644
index 0000000000..0713e8a1a2
--- /dev/null
+++ b/src/swordfish/modules/motion/FeedRate.h
@@ -0,0 +1,63 @@
+#pragma once
+
+#include <swordfish/types.h>
+
+namespace swordfish::motion {
+	enum class FeedRateType {
+		InverseTime,
+		UnitsPerSecond,
+	};
+
+	struct FeedRate {
+	private:
+		FeedRateType type_;
+		f32 value_;
+
+		constexpr FeedRate(FeedRateType type, f32 value) :
+				type_(type), value_(value) {
+		}
+
+	public:
+		static constexpr FeedRate InverseTime(f32 value) {
+			return FeedRate(FeedRateType::InverseTime, value);
+		}
+
+		static constexpr FeedRate UnitsPerSecond(f32 value) {
+			return FeedRate(FeedRateType::UnitsPerSecond, value);
+		}
+
+		const FeedRateType type() const {
+			return type_;
+		}
+
+		f32& value() {
+			return value_;
+		}
+
+		const f32 value() const {
+			return value_;
+		}
+
+		/*FeedRate& operator=(FeedRate& other) {
+		  type_ = other.type_;
+		  value_ = other.value_;
+
+		  return *this;
+		}*/
+
+		FeedRate& operator=(FeedRate other) {
+			type_ = other.type_;
+			value_ = other.value_;
+
+			return *this;
+		}
+
+		FeedRate operator*(f32 value) {
+			return FeedRate(type_, value_ * value);
+		}
+
+		FeedRate operator/(f32 value) {
+			return FeedRate(type_, value_ / value);
+		}
+	};
+} // namespace swordfish::motion
diff --git a/src/swordfish/modules/motion/LimitException.cpp b/src/swordfish/modules/motion/LimitException.cpp
index 5f20bc79cc..b9287ae524 100644
--- a/src/swordfish/modules/motion/LimitException.cpp
+++ b/src/swordfish/modules/motion/LimitException.cpp
@@ -3,7 +3,7 @@
  *
  * Created: 30/09/2021 3:03:31 pm
  *  Author: smohekey
- */ 
+ */
 
 #include <algorithm>
 
@@ -11,22 +11,22 @@
 
 #include "LimitException.h"
 
-namespace swordfish::motion {	
+namespace swordfish::motion {
 	void LimitException::writeJson(io::Writer& writer) const {
 		writer << "{\"type\":\"LimitException\",\"message\":\"Limits exceeded\"";
-			
-		if(_target(X) != std::clamp(_target(X), _min(X), _max(X))) {
-			writer << ",\"x\":{\"target\":" << _target(X) << ",\"min\":" << _min(X) << ",\"max\":" << _max(X) << '}';
+
+		if(_target.x() != std::clamp(_target.x(), _min.x(), _max.x())) {
+			writer << ",\"x\":{\"target\":" << _target.x() << ",\"min\":" << _min.x() << ",\"max\":" << _max.x() << '}';
 		}
-		
-		if(_target(Y) != std::clamp(_target(Y), _min(Y), _max(Y))) {
-			writer << ",\"y\":{\"target\":" << _target(Y) << ",\"min\":" << _min(Y) << ",\"max\":" << _max(Y) << '}';
+
+		if(_target.y() != std::clamp(_target.y(), _min.y(), _max.y())) {
+			writer << ",\"y\":{\"target\":" << _target.y() << ",\"min\":" << _min.y() << ",\"max\":" << _max.y() << '}';
 		}
-		
-		if(_target(Z) != std::clamp(_target(Z), _min(Z), _max(Z))) {
-			writer << ",\"z\":{\"target\":" << _target(Z) << ",\"min\":" << _min(Z) << ",\"max\":" << _max(Z) << '}';
+
+		if(_target.z() != std::clamp(_target.z(), _min.z(), _max.z())) {
+			writer << ",\"z\":{\"target\":" << _target.z() << ",\"min\":" << _min.z() << ",\"max\":" << _max.z() << '}';
 		}
-		
+
 		writer << '}';
 	}
 }
\ No newline at end of file
diff --git a/src/swordfish/modules/motion/LimitException.h b/src/swordfish/modules/motion/LimitException.h
index 0849d1fade..554a07962a 100644
--- a/src/swordfish/modules/motion/LimitException.h
+++ b/src/swordfish/modules/motion/LimitException.h
@@ -7,29 +7,29 @@
 
 #pragma once
 
-#include <Eigen/Core>
 
+#include <swordfish/math.h>
 #include <swordfish/Exception.h>
 
 namespace swordfish::motion {
 	class LimitException : public Exception {
 	private:
-		const Eigen::Vector3f _target;
-		const Eigen::Vector3f _min;
-		const Eigen::Vector3f _max;
+		const swordfish::math::Vector3f32 _target;
+		const swordfish::math::Vector3f32 _min;
+		const swordfish::math::Vector3f32 _max;
 
 	protected:
 		void virtual writeType([[maybe_unused]] io::Writer& writer) const override { }
 		void virtual writeMessage([[maybe_unused]] io::Writer& writer) const override { }
 
 	public:
-		LimitException(const Eigen::Vector3f& target, const Eigen::Vector3f& min, const Eigen::Vector3f& max) : _target(target), _min(min), _max(max) {
+		LimitException(const swordfish::math::Vector3f32& target, const swordfish::math::Vector3f32& min, const swordfish::math::Vector3f32& max) : _target(target), _min(min), _max(max) {
 
 		}
 
 		virtual ~LimitException() { }
 
-		const Eigen::Vector3f& getTarget() {
+		const swordfish::math::Vector3f32& getTarget() {
 			return _target;
 		}
 
diff --git a/src/swordfish/modules/motion/Limits.cpp b/src/swordfish/modules/motion/Limits.cpp
index 7690af7141..25e916673f 100644
--- a/src/swordfish/modules/motion/Limits.cpp
+++ b/src/swordfish/modules/motion/Limits.cpp
@@ -33,16 +33,16 @@ namespace swordfish::motion {
 			Vector3f min = getMin();
 			Vector3f max = getMax();
 
-			if (axis_was_homed((AxisEnum) X)) {
-				vector(X) = std::clamp(vector(X), min(X), max(X));
+			if (axis_was_homed(Axis::X())) {
+				vector.x() = std::clamp(vector.x(), min.x(), max.x());
 			}
 
-			if (axis_was_homed((AxisEnum) Y)) {
-				vector(Y) = std::clamp(vector(Y), min(Y), max(Y));
+			if (axis_was_homed(Axis::Y())) {
+				vector.y() = std::clamp(vector.y(), min.y(), max.y());
 			}
 
-			if (axis_was_homed((AxisEnum) Z)) {
-				vector(Z) = std::clamp(vector(Z), min(Z), max(Z));
+			if (axis_was_homed(Axis::Z())) {
+				vector.z() = std::clamp(vector.z(), min.z(), max.z());
 			}
 		}
 	}
@@ -76,9 +76,9 @@ namespace swordfish::motion {
 			// debug()("z: ", vector.z, ", min_z: ", min.z, ", max_z: ", max.z);
 
 			if (
-					(vector(X) != std::clamp(vector(X), min(X), max(X))) ||
-					(vector(Y) != std::clamp(vector(Y), min(Y), max(Y))) ||
-					(vector(Z) != std::clamp(vector(Z), min(Z), max(Z)))) {
+					(vector.x() != std::clamp(vector.x(), min.x(), max.x())) ||
+					(vector.y() != std::clamp(vector.y(), min.y(), max.y())) ||
+					(vector.z() != std::clamp(vector.z(), min.z(), max.z()))) {
 				throw LimitException(vector, min, max);
 			}
 		}
diff --git a/src/swordfish/modules/motion/MotionModule.cpp b/src/swordfish/modules/motion/MotionModule.cpp
index 62196da413..ad78aee470 100644
--- a/src/swordfish/modules/motion/MotionModule.cpp
+++ b/src/swordfish/modules/motion/MotionModule.cpp
@@ -14,23 +14,26 @@
 #include <marlin/module/motion.h>
 #include <marlin/module/planner.h>
 
-extern float32_t rapidrate_mm_s;
+
 
 #include <swordfish/debug.h>
 #include <swordfish/modules/estop/EStopModule.h>
 #include <swordfish/modules/status/StatusModule.h>
 
+extern swordfish::motion::FeedRate rapidrate_mm_s;
+
 namespace swordfish::motion {
-	using namespace Eigen;
 
 	using namespace swordfish::core;
 	using namespace swordfish::estop;
+	using namespace swordfish::math;
 	using namespace swordfish::status;
 	using namespace swordfish::utils;
 
 	MotionModule* MotionModule::__instance = nullptr;
 
 	core::ValueField<int16_t> MotionModule::__activeCoordinateSystemField = { "wcs", 0, 0 };
+	core::ValueField<bool> MotionModule::__shouldHomeFourth = { "shouldHomeFourth", 18, false };
 	core::ObjectField<CoordinateSystemTable> MotionModule::__workCoordinateSystemsField = { "workCoordinateSystems", 0 };
 	core::ObjectField<Limits> MotionModule::__limitsField = { "limits", 1 };
 	core::ObjectField<core::LinearVector3> MotionModule::__homeOffsetField = { "homeOffset", 2 };
@@ -40,7 +43,7 @@ namespace swordfish::motion {
 	core::Schema MotionModule::__schema = {
 		utils::typeName<MotionModule>(),
 		&(Module::__schema),
-		{ __activeCoordinateSystemField },
+		{ __activeCoordinateSystemField, __shouldHomeFourth },
 		{ __workCoordinateSystemsField,
 		                    __limitsField,
 		                    __homeOffsetField,
@@ -107,39 +110,52 @@ namespace swordfish::motion {
 	void MotionModule::move(Movement movement) {
 		gcode.throwIfAborted();
 
-		Vector4f currentNative { current_position.x, current_position.y, current_position.z, 1 };
-		Vector4f currentLogical = _logicalTransform * currentNative;
-		Vector4f targetLogical {
-			isnan(movement.x) ? currentLogical(X) : (movement.relativeAxes & AxisSelector::X ? currentLogical(X) + movement.x : movement.x),
-			isnan(movement.y) ? currentLogical(Y) : (movement.relativeAxes & AxisSelector::Y ? currentLogical(Y) + movement.y : movement.y),
-			isnan(movement.z) ? currentLogical(Z) : (movement.relativeAxes & AxisSelector::Z ? currentLogical(Z) + movement.z : movement.z),
+		Vector4f32 currentNative { current_position.x(), current_position.y(), current_position.z(), 1 };
+		Vector4f32 currentLogical = _logicalTransform * currentNative;
+		Vector4f32 targetLogical {
+			isnan(movement.x) ? currentLogical.x() : (movement.relativeAxes & AxisSelector::X ? currentLogical.x() + movement.x : movement.x),
+			isnan(movement.y) ? currentLogical.y() : (movement.relativeAxes & AxisSelector::Y ? currentLogical.y() + movement.y : movement.y),
+			isnan(movement.z) ? currentLogical.z() : (movement.relativeAxes & AxisSelector::Z ? currentLogical.z() + movement.z : movement.z),
 			1
 		};
 
-		Vector4f targetNative = _nativeTransform * targetLogical;
+		Vector4f32 targetNative = _nativeTransform * targetLogical;
 
 		debug()("===============================================");
 		debug()(
 				"requested -> x(", absoluteOrRelative(AxisSelector::X, movement.relativeAxes), "): ", movement.x,
 				", y(", absoluteOrRelative(AxisSelector::Y, movement.relativeAxes), "): ", movement.y,
 				", z(", absoluteOrRelative(AxisSelector::Z, movement.relativeAxes), "): ", movement.z,
-				", feedRate: ", movement.feedRate);
-		debug()("current native -> x: ", currentNative(X), ", y: ", currentNative(Y), ", z: ", currentNative(Z));
-		debug()("current logical -> x: ", currentLogical(X), ", y: ", currentLogical(Y), ", z: ", currentLogical(Z));
-		debug()("target logical -> x: ", targetLogical(X), ", y: ", targetLogical(Y), ", z: ", targetLogical(Z));
-		debug()("target native -> x: ", targetNative(X), ", y: ", targetNative(Y), ", z: ", targetNative(Z));
-
-		_limits.throwIfOutside({ targetNative(X), targetNative(Y), targetNative(Z) });
-
-		xyz_pos_t target = {
-			targetNative(X),
-			targetNative(Y),
-			targetNative(Z)
+				", feed_rate: ", movement.feed_rate.has_value() ? movement.feed_rate.value().value() : feedrate_mm_s.value());
+		debug()("current native -> x: ", currentNative.x(), ", y: ", currentNative.y(), ", z: ", currentNative.z());
+		debug()("current logical -> x: ", currentLogical.x(), ", y: ", currentLogical.y(), ", z: ", currentLogical.z());
+		debug()("target logical -> x: ", targetLogical.x(), ", y: ", targetLogical.y(), ", z: ", targetLogical.z());
+		debug()("target native -> x: ", targetNative.x(), ", y: ", targetNative.y(), ", z: ", targetNative.z());
+
+		_limits.throwIfOutside({ targetNative.x(), targetNative.y(), targetNative.z() });
+
+		Vector6f32 target = {
+			targetNative.x(),
+			targetNative.y(),
+			targetNative.z(),
+			0,
+			0,
+			0
 		};
 
+		auto feed_rate = movement.feed_rate.has_value() ? movement.feed_rate.value() : feedrate_mm_s;
+
+		debug()("unscaled feed_rate: ", feed_rate.value());
+
+		if (feed_rate.type() == FeedRateType::UnitsPerSecond) {
+			feed_rate = MMS_SCALED(feed_rate);
+		}
+
+		debug()("scaled feed_rate: ", feed_rate.value());
+
 		planner.buffer_line(
 				target,
-				MMS_SCALED(isnan(movement.feedRate) ? feedrate_mm_s : movement.feedRate),
+				feed_rate,
 				active_extruder,
 				movement.state,
 				0.0,
@@ -169,10 +185,10 @@ namespace swordfish::motion {
 	}
 
 	void MotionModule::updateTransforms() {
-		Matrix4f translate {
-			{1.0, 0.0, 0.0, -_offset(X)},
-			{0.0, 1.0, 0.0, -_offset(Y)},
-			{0.0, 0.0, 1.0, -_offset(Z)},
+		Eigen::Matrix4f translate {
+			{1.0, 0.0, 0.0, -_offset.x()},
+			{0.0, 1.0, 0.0, -_offset.y()},
+			{0.0, 0.0, 1.0, -_offset.z()},
 			{0.0, 0.0, 0.0,         1.0}
 		};
 
@@ -181,7 +197,7 @@ namespace swordfish::motion {
 
 		debug()("shearFactor: ", shearFactor);
 
-		Matrix4f shear {
+		Eigen::Matrix4f shear {
 			{				1.0, 0.0, 0.0, 0.0},
 			{shearFactor, 1.0, 0.0, 0.0},
 			{				0.0, 0.0, 1.0, 0.0},
@@ -193,17 +209,17 @@ namespace swordfish::motion {
 	}
 
 	void MotionModule::updateOffset() {
-		Vector3f homeOffset = getHomeOffset();
-		Vector3f workOffset = getWorkOffset();
-		Vector3f toolOffset = getToolOffset();
+		Vector3f32 homeOffset = getHomeOffset();
+		Vector3f32 workOffset = getWorkOffset();
+		Vector3f32 toolOffset = getToolOffset();
 
 		_offset = homeOffset + workOffset + toolOffset;
 
-		debug()("home x: ", homeOffset(X), ", y: ", homeOffset(Y), ", z: ", homeOffset(Z));
-		debug()("work x: ", workOffset(X), ", y: ", workOffset(Y), ", z: ", workOffset(Z));
-		debug()("tool x: ", toolOffset(X), ", y: ", toolOffset(Y), ", z: ", toolOffset(Z));
+		debug()("home x: ", homeOffset.x(), ", y: ", homeOffset.y(), ", z: ", homeOffset.z());
+		debug()("work x: ", workOffset.x(), ", y: ", workOffset.y(), ", z: ", workOffset.z());
+		debug()("tool x: ", toolOffset.x(), ", y: ", toolOffset.y(), ", z: ", toolOffset.z());
 
-		debug()("offset x: ", _offset(X), ", y: ", _offset(Y), ", z: ", _offset(Z));
+		debug()("offset x: ", _offset.x(), ", y: ", _offset.y(), ", z: ", _offset.z());
 
 		updateTransforms();
 	}
@@ -216,32 +232,32 @@ namespace swordfish::motion {
 		return *_activeCoordinateSystem;
 	}
 
-	void MotionModule::setHomeOffset(Vector3f homeOffset_) {
+	void MotionModule::setHomeOffset(Vector3f32 homeOffset_) {
 		auto& homeOffset = __homeOffsetField.get(_pack);
 
-		homeOffset.x(homeOffset_(X));
-		homeOffset.y(homeOffset_(Y));
-		homeOffset.z(homeOffset_(Z));
+		homeOffset.x(homeOffset_.x());
+		homeOffset.y(homeOffset_.y());
+		homeOffset.z(homeOffset_.z());
 
 		updateOffset();
 	}
 
-	void MotionModule::setWorkOffset(Vector3f workOffset_) {
+	void MotionModule::setWorkOffset(Vector3f32 workOffset_) {
 		auto& workOffset = __workOffsetField.get(_pack);
 
-		workOffset.x(workOffset_(X));
-		workOffset.y(workOffset_(Y));
-		workOffset.z(workOffset_(Z));
+		workOffset.x(workOffset_.x());
+		workOffset.y(workOffset_.y());
+		workOffset.z(workOffset_.z());
 
 		updateOffset();
 	}
 
-	void MotionModule::setToolOffset(Vector3f toolOffset_) {
+	void MotionModule::setToolOffset(Vector3f32 toolOffset_) {
 		auto& toolOffset = __toolOffsetField.get(_pack);
 
-		toolOffset.x(toolOffset_(X));
-		toolOffset.y(toolOffset_(Y));
-		toolOffset.z(toolOffset_(Z));
+		toolOffset.x(toolOffset_.x());
+		toolOffset.y(toolOffset_.y());
+		toolOffset.z(toolOffset_.z());
 
 		updateOffset();
 	}
@@ -254,7 +270,7 @@ namespace swordfish::motion {
 		move({ .x = movement.x,
 		       .y = movement.y,
 		       .z = movement.z,
-		       .feedRate = isnan(movement.feedRate) ? rapidrate_mm_s : movement.feedRate,
+		       .feed_rate = movement.feed_rate.has_value() ? movement.feed_rate.value() : rapidrate_mm_s,
 		       .relativeAxes = movement.relativeAxes,
 		       .accel_mm_s2 = isnan(movement.accel_mm_s2) ? planner.settings.travel_acceleration : movement.accel_mm_s2,
 					 .state = MachineState::RapidMove });
@@ -264,7 +280,7 @@ namespace swordfish::motion {
 		move({ .x = movement.x,
 		       .y = movement.y,
 		       .z = movement.z,
-		       .feedRate = isnan(movement.feedRate) ? feedrate_mm_s : movement.feedRate,
+		       .feed_rate = movement.feed_rate.has_value() ? movement.feed_rate.value() : feedrate_mm_s,
 		       .relativeAxes = movement.relativeAxes,
 		       .accel_mm_s2 = isnan(movement.accel_mm_s2) ? planner.settings.acceleration : movement.accel_mm_s2,
 					 .state = MachineState::FeedMove });
diff --git a/src/swordfish/modules/motion/MotionModule.h b/src/swordfish/modules/motion/MotionModule.h
index 734ddb6b74..c185a20cca 100644
--- a/src/swordfish/modules/motion/MotionModule.h
+++ b/src/swordfish/modules/motion/MotionModule.h
@@ -25,6 +25,7 @@
 
 #include "CoordinateSystem.h"
 #include "CoordinateSystemTable.h"
+#include "Feedrate.h"
 #include "Limits.h"
 #include "NotHomedException.h"
 
@@ -48,7 +49,7 @@ namespace swordfish::motion {
 		float32_t x = NaN;
 		float32_t y = NaN;
 		float32_t z = NaN;
-		float32_t feedRate = NaN;
+		std::optional<FeedRate> feed_rate = std::nullopt;
 		utils::Flags<AxisSelector> relativeAxes = AxisSelector::None;
 		float32_t accel_mm_s2 = NaN;
 		swordfish::status::MachineState state;
@@ -59,6 +60,7 @@ namespace swordfish::motion {
 	class MotionModule : public swordfish::Module {
 	private:
 		static core::ValueField<int16_t> __activeCoordinateSystemField;
+		static core::ValueField<bool> __shouldHomeFourth;
 		static core::ObjectField<CoordinateSystemTable> __workCoordinateSystemsField;
 		static core::ObjectField<Limits> __limitsField;
 		static core::ObjectField<core::LinearVector3> __homeOffsetField;
@@ -82,8 +84,8 @@ namespace swordfish::motion {
 		CoordinateSystem* _toolChangeCoordinateSystem = nullptr; // G59.8
 		CoordinateSystem* _toolCoordinateSystem = nullptr; // G59.9
 
-		Eigen::Vector3f _offset;
-		Eigen::Vector3f _rotation;
+		swordfish::math::Vector3f32 _offset;
+		swordfish::math::Vector3f32 _rotation;
 		Eigen::Matrix4f _nativeTransform;
 		Eigen::Matrix4f _logicalTransform;
 
@@ -113,24 +115,28 @@ namespace swordfish::motion {
 			return __homeOffsetField.get(_pack);
 		}
 
-		void setHomeOffset(Eigen::Vector3f homeOffset);
+		void setHomeOffset(swordfish::math::Vector3f32 homeOffset);
 
 		Eigen::Vector3f getWorkOffset() {
 			return __workOffsetField.get(_pack);
 		}
 
-		void setWorkOffset(Eigen::Vector3f workOffset);
+		void setWorkOffset(swordfish::math::Vector3f32 workOffset);
 
 		Eigen::Vector3f getToolOffset() {
 			return __toolOffsetField.get(_pack);
 		}
 
-		void setToolOffset(Eigen::Vector3f toolOffset);
+		void setToolOffset(swordfish::math::Vector3f32 toolOffset);
 
 		CoordinateSystemTable& getWorkCoordinateSystems() {
 			return _workCoordinateSystems;
 		}
 
+		bool shouldHomeFourth() {
+			return __shouldHomeFourth.get(_pack);
+		}
+
 		Limits& getLimits() {
 			return _limits;
 		}
@@ -161,124 +167,110 @@ namespace swordfish::motion {
 
 		void synchronize();
 
-		FORCE_INLINE float32_t toLogical(AxisEnum axis, float32_t value) {
-			return value + _offset(axis);
-		}
-		FORCE_INLINE float32_t toNative(AxisEnum axis, float32_t value) {
-			return value - _offset(axis);
-		}
-
-		FORCE_INLINE void toLogical(xy_pos_t& raw) {
-			raw.x += _offset(X);
-			raw.y += _offset(Y);
-		}
-		FORCE_INLINE void toLogical(xyz_pos_t& raw) {
-			raw.x += _offset(X);
-			raw.y += _offset(Y);
-			raw.z += _offset(Z);
-		}
-		FORCE_INLINE Eigen::Vector2f toLogical(Eigen::Vector2f& raw) {
-			return { raw(X) += _offset(X), raw(Y) += _offset(Y) };
-		}
-		FORCE_INLINE Eigen::Vector3f toLogical(Eigen::Vector3f& raw) {
-			return { raw(X) += _offset(X), raw(Y) += _offset(Y), raw(Z) += _offset(Z) };
-		}
-
-		FORCE_INLINE void toNative(xy_pos_t& raw) {
-			raw.x -= _offset(X);
-			raw.y -= _offset(Y);
-		}
-		FORCE_INLINE void toNative(xyz_pos_t& raw) {
-			raw.x -= _offset(X);
-			raw.y -= _offset(Y);
-			raw.z -= _offset(Z);
-		}
-		FORCE_INLINE Eigen::Vector2f toNative(Eigen::Vector2f& raw) {
-			return { raw(X) -= _offset(X), raw(Y) -= _offset(Y) };
-		}
-		FORCE_INLINE Eigen::Vector3f toNative(Eigen::Vector3f& raw) {
-			return { raw(X) -= _offset(X), raw(Y) -= _offset(Y), raw(Z) -= _offset(Z) };
-		}
-
-		/*
-		FORCE_INLINE float32_t toLogical(AxisEnum axis, float32_t value) {
-		  return value + _offset(axis);
-		}
-
-		FORCE_INLINE float32_t toNative(AxisEnum axis, float32_t value) {
-		  return value - _offset(axis);
-		}
-
-		FORCE_INLINE void toLogical(xy_pos_t &raw) {
-		  Eigen::Vector4f v = { raw.x, raw.y, 0, 1 };
-
-		  auto r = _logicalTransform * v;
-
-		  raw.x = r(X);
-		  raw.y = r(Y);
-		}
-
-		FORCE_INLINE void toLogical(xyz_pos_t &raw) {
-		  Eigen::Vector4f v = { raw.x, raw.y, raw.z, 1 };
-
-		  auto r = _logicalTransform * v;
-
-		  raw.x = r(X);
-		  raw.y = r(Y);
-		  raw.z = r(Z);
-		}
-
-		FORCE_INLINE Eigen::Vector2f toLogical(Eigen::Vector2f& raw) {
-		  Eigen::Vector4f v = { raw(X), raw(Y), 0, 1 };
-
-		  auto r = _logicalTransform * v;
-
-		  return { r(X), r(Y) };
-		}
-
-		FORCE_INLINE Eigen::Vector3f toLogical(Eigen::Vector3f& raw) {
-		  Eigen::Vector4f v = { raw(X), raw(Y), raw(Z), 1 };
-
-		  auto r = _logicalTransform * v;
-
-		  return { r(X), r(Y), r(Z) };
-		}
-
-		FORCE_INLINE void toNative(xy_pos_t &raw) {
-		  Eigen::Vector4f v = { raw.x, raw.y, 0, 1 };
-
-		  auto r = _nativeTransform * v;
-
-		  raw.x = r(X);
-		  raw.y = r(Y);
-		}
-
-		FORCE_INLINE void toNative(xyz_pos_t &raw) {
-		  Eigen::Vector4f v = { raw.x, raw.y, raw.z, 1 };
-
-		  auto r = _nativeTransform * v;
-
-		  raw.x = r(X);
-		  raw.y = r(Y);
-		  raw.z = r(Z);
-		}
-
-		FORCE_INLINE Eigen::Vector2f toNative(Eigen::Vector2f& raw) {
-		  Eigen::Vector4f v = { raw(X), raw(Y), 0, 1 };
-
-		  auto r = _nativeTransform * v;
-
-		  return { r(X), r(Y) };
-		}
-
-		FORCE_INLINE Eigen::Vector3f toNative(Eigen::Vector3f& raw) {
-		  Eigen::Vector4f v = { raw(X), raw(Y), raw(Z), 1 };
-
-		  auto r = _nativeTransform * v;
-
-		  return { r(X), r(Y), r(Z) };
+		FORCE_INLINE float32_t toLogical(Axis axis, f32 value) {
+			if (axis.is_linear()) {
+				return value + _offset[axis];
+			} else {
+				return value + _rotation[axis - 3];
+			}
+		}
+		FORCE_INLINE float32_t toNative(Axis axis, f32 value) {
+			if (axis.is_linear()) {
+				return value - _offset[axis];
+			} else {
+				return value - _rotation[axis - 3];
+			}
+		}
+
+		FORCE_INLINE swordfish::math::Vector2f32 toLogical(const swordfish::math::Vector2f32& raw) {
+			return {
+				raw.x() + _offset.x(),
+				raw.y() + _offset.y()
+			};
+		}
+
+		FORCE_INLINE swordfish::math::Vector3f32 toLogical(const swordfish::math::Vector3f32& raw) {
+			return {
+				raw.x() + _offset.x(),
+				raw.y() + _offset.y(),
+				raw.z() + _offset.z()
+			};
+		}
+
+		FORCE_INLINE swordfish::math::Vector4f32 toLogical(const swordfish::math::Vector4f32& raw) {
+			return {
+				raw.x() + _offset.x(),
+				raw.y() + _offset.y(),
+				raw.z() + _offset.z(),
+				raw.a() + _rotation.x()
+			};
+		}
+
+		FORCE_INLINE swordfish::math::Vector5f32 toLogical(const swordfish::math::Vector5f32& raw) {
+			return {
+				raw.x() + _offset.x(),
+				raw.y() + _offset.y(),
+				raw.z() + _offset.z(),
+				raw.a() + _rotation.x(),
+				raw.b() + _rotation.y()
+			};
+		}
+
+		FORCE_INLINE swordfish::math::Vector6f32 toLogical(const swordfish::math::Vector6f32& raw) {
+			return {
+				raw.x() + _offset.x(),
+				raw.y() + _offset.y(),
+				raw.z() + _offset.z(),
+				raw.a() + _rotation.x(),
+				raw.b() + _rotation.y(),
+				raw.c() + _rotation.z()
+			};
+		}
+
+FORCE_INLINE swordfish::math::Vector2f32 toNative(const swordfish::math::Vector2f32& raw) {
+			return {
+				raw.x() - _offset.x(),
+				raw.y() - _offset.y()
+			};
+		}
+
+		FORCE_INLINE swordfish::math::Vector3f32 toNative(const swordfish::math::Vector3f32& raw) {
+			return {
+				raw.x() - _offset.x(),
+				raw.y() - _offset.y(),
+				raw.z() - _offset.z()
+			};
+		}
+
+		FORCE_INLINE swordfish::math::Vector4f32 toNative(const swordfish::math::Vector4f32& raw) {
+			return {
+				raw.x() - _offset.x(),
+				raw.y() - _offset.y(),
+				raw.z() - _offset.z(),
+				raw.a() - _rotation.x()
+			};
+		}
+
+		FORCE_INLINE swordfish::math::Vector5f32 toNative(const swordfish::math::Vector5f32& raw) {
+			return {
+				raw.x() - _offset.x(),
+				raw.y() - _offset.y(),
+				raw.z() - _offset.z(),
+				raw.a() - _rotation.x(),
+				raw.b() - _rotation.y()
+			};
+		}
+
+		FORCE_INLINE swordfish::math::Vector6f32 toNative(const swordfish::math::Vector6f32& raw) {
+			return {
+				raw.x() - _offset.x(),
+				raw.y() - _offset.y(),
+				raw.z() - _offset.z(),
+				raw.a() - _rotation.x(),
+				raw.b() - _rotation.y(),
+				raw.c() - _rotation.z()
+			};
 		}
-		*/
 
 		static MotionModule& getInstance(core::Object* parent = nullptr);
 	};
diff --git a/src/swordfish/modules/tools/ToolsModule.cpp b/src/swordfish/modules/tools/ToolsModule.cpp
index 749c8a4016..73909aac40 100644
--- a/src/swordfish/modules/tools/ToolsModule.cpp
+++ b/src/swordfish/modules/tools/ToolsModule.cpp
@@ -7,8 +7,6 @@
 
 #include <charconv>
 
-#include <Eigen/Core>
-
 #include <marlin/gcode/gcode.h>
 #include <marlin/feature/host_actions.h>
 #include <marlin/feature/spindle_laser.h>
@@ -29,14 +27,14 @@
 
 #include "ToolsModule.h"
 
-extern bool run_probe(AxisEnum axis, EndstopEnum endstop, float distance, float retract);
+extern bool run_probe(Axis axis, EndstopValue endstop, float distance, float retract);
 
 namespace swordfish::tools {
-	using namespace Eigen;
 
 	using namespace swordfish::core;
 	using namespace swordfish::io;
 	using namespace swordfish::estop;
+	using namespace swordfish::math;
 	using namespace swordfish::motion;
 	using namespace swordfish::utils;
 	using namespace swordfish::status;
@@ -346,9 +344,9 @@ namespace swordfish::tools {
 		motionModule.setActiveCoordinateSystem(mcs);
 		motionModule.rapidMove({ .z = 0 });
 
-		auto nativeClearanceX = motionModule.toNative(X_AXIS, CaddyClearanceX);
+		auto nativeClearanceX = motionModule.toNative(Axis::X(), CaddyClearanceX);
 
-		if (current_position.x < nativeClearanceX) {
+		if (current_position.x() < nativeClearanceX) {
 			motionModule.setActiveCoordinateSystem(tcs);
 			motionModule.rapidMove({ .x = CaddyClearanceX });
 		}
@@ -402,7 +400,7 @@ namespace swordfish::tools {
 		estopModule.throwIfTriggered();
 
 		// Activate the tool offset
-		motionModule.setToolOffset({ .x = 0, .y = 0, .z = offsetZ });
+		motionModule.setToolOffset({ 0, 0, offsetZ });
 
 		motionModule.setActiveCoordinateSystem(oldWCS);
 
@@ -548,22 +546,22 @@ namespace swordfish::tools {
 		// Run the tool probe
 		remember_feedrate_scaling_off();
 
-		feedrate_mm_s = homing_feedrate(Z_AXIS) * 0.3;
+		feedrate_mm_s = homing_feedrate(Axis::Z()) * 0.3;
 
-		const auto delta = -abs(home_dir(Z_AXIS) > 0 ? Z_MAX_POS - Z_MIN_POS : Z_MIN_POS - Z_MAX_POS);
+		const auto delta = -abs(home_dir(Axis::Z()) > 0 ? Z_MAX_POS - Z_MIN_POS : Z_MIN_POS - Z_MAX_POS);
 
 		endstops.enable_tool_probe(true);
 
-		run_probe(Z_AXIS, TOOL_PROBE, delta, 5);
+		run_probe(Axis::Z(), TOOL_PROBE, delta, 5);
 
 		endstops.enable_tool_probe(false);
 
 		restore_feedrate_and_scaling();
 
-		debug()("z: ", current_position[Z_AXIS]);
+		debug()("z: ", current_position[Axis::Z()]);
 
 		// auto length = motionModule.toNative(Z_AXIS, current_position[Z_AXIS]);
-		auto offsetZ = -homeOffset[Z_AXIS] - workOffset[Z_AXIS] - current_position[Z_AXIS];
+		auto offsetZ = -homeOffset[Axis::Z()] - workOffset[Axis::Z()] - current_position[Axis::Z()];
 		// auto offsetZ = motionModule.toLogical(Z_AXIS, current_position[Z_AXIS]);
 
 		debug()("tool offsetZ: ", offsetZ);
@@ -576,7 +574,7 @@ namespace swordfish::tools {
 			auto& limits = motionModule.getLimits();
 			auto& minObj = limits.getMin();
 
-			motionModule.move({ .x = minObj.x(), .feedRate = homing_feedrate(X_AXIS) });
+			motionModule.move({ .x = minObj.x(), .feed_rate = homing_feedrate(Axis::X()) });
 		}
 
 		return offsetZ;
@@ -623,7 +621,7 @@ namespace swordfish::tools {
 
 			_flags[HomingFlag] = true;
 
-			motionModule.move({ .x = 100, .feedRate = homing_feedrate(X_AXIS), .relativeAxes = AxisSelector::All, .state = MachineState::Homing });
+			motionModule.move({ .x = 100, .feed_rate = homing_feedrate(Axis::X()), .relativeAxes = AxisSelector::All, .state = MachineState::Homing });
 
 			planner.synchronize();
 
@@ -641,7 +639,7 @@ namespace swordfish::tools {
 
 		debug()("storing in pocket: ", pocket.getIndex());
 
-		Vector3f target = pocket.getOffset();
+		Vector3f32 target = pocket.getOffset();
 
 		// move z to top
 		motionModule.setActiveCoordinateSystem(mcs);
@@ -662,16 +660,16 @@ namespace swordfish::tools {
 			caddyClearanceX = CaddyDustShoeClearanceX;
 		}
 
-		auto nativeClearanceX = motionModule.toNative(X_AXIS, caddyClearanceX);
+		auto nativeClearanceX = motionModule.toNative(Axis::X(), caddyClearanceX);
 
-		if (current_position.x >= nativeClearanceX) {
-			motionModule.rapidMove({ .x = caddyClearanceX, .y = target(Y) });
+		if (current_position.x() >= nativeClearanceX) {
+			motionModule.rapidMove({ .x = caddyClearanceX, .y = target.y() });
 			motionModule.synchronize();
 		} else {
 			motionModule.rapidMove({ .x = caddyClearanceX });
 			motionModule.synchronize();
 
-			motionModule.rapidMove({ .y = target(Y) });
+			motionModule.rapidMove({ .y = target.y() });
 			motionModule.synchronize();
 		}
 
@@ -683,15 +681,15 @@ namespace swordfish::tools {
 
 		// move to the tool clip clearance position on the X axis, this is slightly closer to the pocket than
 		// the caddy clearance position.
-		motionModule.move({ .x = target(X) + ToolClipClearanceX, .feedRate = homing_feedrate(X_AXIS) });
+		motionModule.move({ .x = target.x() + ToolClipClearanceX, .feed_rate = homing_feedrate(Axis::X()) });
 		motionModule.synchronize();
 
 		// move z to pocket offset.z
-		motionModule.move({ .z = target(Z), .feedRate = homing_feedrate(Z_AXIS) });
+		motionModule.move({ .z = target.z(), .feed_rate = homing_feedrate(Axis::Z()) });
 		motionModule.synchronize();
 
 		// move to x position of pocket offset
-		motionModule.move({ .x = target(X), .feedRate = homing_feedrate(X_AXIS) });
+		motionModule.move({ .x = target.x(), .feed_rate = homing_feedrate(Axis::X()) });
 		motionModule.synchronize();
 
 		unlock();
@@ -740,24 +738,24 @@ namespace swordfish::tools {
 
 			motionModule.setActiveCoordinateSystem(tcs);
 
-			Vector3f target = sourcePocket->getOffset();
+			Vector3f32 target = sourcePocket->getOffset();
 
-			debug()("target -> x: ", target(X), ", y: ", target(Y));
+			debug()("target -> x: ", target.x(), ", y: ", target.y());
 
 			// move to x y of pocket offset
-			motionModule.rapidMove({ .x = target(X), .y = target(Y) });
+			motionModule.rapidMove({ .x = target.x(), .y = target.y() });
 			motionModule.synchronize();
 
 			unlock();
 
 			// move z to pocket offset.z
-			motionModule.move({ .z = target(Z), .feedRate = homing_feedrate(Z_AXIS) });
+			motionModule.move({ .z = target.z(), .feed_rate = homing_feedrate(Axis::Z()) });
 			motionModule.synchronize();
 
 			lock();
 
 			// remove tool from pocket
-			motionModule.move({ .x = target(X) + ToolClipClearanceX, .feedRate = homing_feedrate(X_AXIS) });
+			motionModule.move({ .x = target.x() + ToolClipClearanceX, .feed_rate = homing_feedrate(Axis::X()) });
 			motionModule.synchronize();
 
 			// move z to top
diff --git a/src/swordfish/types.h b/src/swordfish/types.h
index beab7371ee..8395e0ad91 100644
--- a/src/swordfish/types.h
+++ b/src/swordfish/types.h
@@ -33,11 +33,151 @@ typedef double f64;
 
 typedef int64_t offset_t;
 
-enum {
+
+enum class AxisValue : u8 {
 	X = 0,
 	Y = 1,
-	Z = 2
+	Z = 2,
+	A = 3,
+	B = 4,
+	C = 5,
+	All = 0xFF
+};
+
+enum EndstopValue : u8 {
+  X_MIN       = 0,
+	Y_MIN       = 1,
+	Z_MIN       = 2,
+	Z_MIN_PROBE = 3,
+	WORK_PROBE  = 4,
+	TOOL_PROBE  = 5,
+	UNUSED			= 6,
+  X_MAX       = 7,
+	Y_MAX       = 8,
+	Z_MAX       = 9,
+  X2_MIN      = 10,
+	X2_MAX      = 11,
+  Y2_MIN      = 12,
+	Y2_MAX      = 13,
+  Z2_MIN      = 14,
+	Z2_MAX      = 15,
+	A_MIN       = 16,
+	B_MIN       = 17,
+	C_MIN       = 18,
+	A_MAX       = 19,
+	B_MAX       = 20,
+	C_MAX       = 21
+};
+
+enum class AxisSelector {
+	None = 0,
+	X = 1 << (u8)AxisValue::X,
+	Y = 1 << (u8)AxisValue::Y,
+	Z = 1 << (u8)AxisValue::Z,
+	A = 1 << (u8)AxisValue::A,
+	B = 1 << (u8)AxisValue::B,
+	C = 1 << (u8)AxisValue::C,
+	All = X | Y | Z | A | B | C,
+	__size__ = 6
+};
+
+class Axis {
+private:
+	AxisValue value_;
+
+	constexpr Axis(AxisValue value) : value_(value) {}
+
+public:
+	static constexpr u8 COUNT = 6;
+
+	static constexpr Axis X() {
+		return Axis(AxisValue::X);
+	}
+
+	static constexpr Axis Y() {
+		return Axis(AxisValue::Y);
+	}
+
+	static constexpr Axis Z() {
+		return Axis(AxisValue::Z);
+	}
+
+	static constexpr Axis A() {
+		return Axis(AxisValue::A);
+	}
+
+	static constexpr Axis B() {
+		return Axis(AxisValue::B);
+	}
+
+	static constexpr Axis C() {
+		return Axis(AxisValue::C);
+	}
+
+	constexpr AxisValue value() const {
+		return value_;
+	}
+
+	constexpr operator usize() const {
+		return (usize)value_;
+	}
+
+	constexpr const char to_char() const {
+		constexpr const char chars[] = {
+			'X',
+			'Y',
+			'Z',
+			'A',
+			'B',
+			'C'
+		};
+
+		return chars[*this];
+	}
+
+	constexpr const bool is_linear() const {
+		constexpr const bool is_linear[] = {
+			true,
+			true,
+			true,
+			false,
+			false,
+			false
+		};
+
+		return is_linear[*this];
+	}
+
+	constexpr const bool is_radial() const {
+		return !is_linear();
+	}
+
+	constexpr const u8 bit_value() const {
+		return 1 << *this;
+	}
 };
 
+static constexpr Axis all_axes[] = {
+	Axis::X(),
+	Axis::Y(),
+	Axis::Z(),
+	Axis::A(),
+	Axis::B(),
+	Axis::C()
+};
+
+static constexpr Axis linear_axes[] = {
+	Axis::X(),
+	Axis::Y(),
+	Axis::Z()
+};
+
+static constexpr Axis radial_axes[] = {
+	Axis::A(),
+	Axis::B(),
+	Axis::C()
+};
+
+
 static_assert(std::numeric_limits<float32_t>::has_quiet_NaN);
 static constexpr float32_t NaN = std::numeric_limits<float32_t>::quiet_NaN();
\ No newline at end of file
diff --git "a/\303\260\302\241" "b/\303\260\302\241"
new file mode 100644
index 0000000000..e69de29bb2