diff --git a/Firmware/xyzcal.cpp b/Firmware/xyzcal.cpp
index cae5f0aed..3794eb4c4 100644
--- a/Firmware/xyzcal.cpp
+++ b/Firmware/xyzcal.cpp
@@ -415,22 +415,12 @@ void print_hysteresis(int16_t min_z, int16_t max_z, int16_t step){
 	}
 }
 
-void update_position_1_step(uint8_t axis, uint8_t dir){
-	if (axis & X_AXIS_MASK)
-		_X_ += dir & X_AXIS_MASK ? -1 : 1;
-	if (axis & Y_AXIS_MASK)
-		_Y_ += dir & Y_AXIS_MASK ? -1 : 1;
-	if (axis & Z_AXIS_MASK)
-		_Z_ += dir & Z_AXIS_MASK ? -1 : 1;
+void update_position_1_step(const uint8_t axis, const uint8_t dir) {
+	count_position[axis] += dir & _BV(axis) ? -1L : 1L;
 }
 
-void set_axes_dir(uint8_t axes, uint8_t dir){
-	if (axes & X_AXIS_MASK)
-		sm4_set_dir(X_AXIS, dir & X_AXIS_MASK);
-	if (axes & Y_AXIS_MASK)
-		sm4_set_dir(Y_AXIS, dir & Y_AXIS_MASK);
-	if (axes & Z_AXIS_MASK)
-		sm4_set_dir(Z_AXIS, dir & Z_AXIS_MASK);
+void __attribute__((noinline)) set_axes_dir(const uint8_t axis, const uint8_t dir) {
+	sm4_set_dir(axis, dir & _BV(axis));
 }
 
 /// Accelerate up to max.speed (defined by @min_delay_us)