diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp
index e590e0a..fcbe784 100644
--- a/src/modules/motion.cpp
+++ b/src/modules/motion.cpp
@@ -5,6 +5,7 @@ namespace motion {
 
 Motion motion;
 
+// TODO: not implemented
 void Motion::InitAxis(Axis axis) {}
 
 void Motion::SetEnabled(Axis axis, bool enabled) {
@@ -12,17 +13,35 @@ void Motion::SetEnabled(Axis axis, bool enabled) {
     axisData[axis].enabled = enabled;
 }
 
-bool Motion::StallGuard(Axis axis) { return false; }
+void Motion::SetMode(Axis axis, MotorMode mode) {
+    for (uint8_t i = 0; i != NUM_AXIS; ++i)
+        axisData[axis].drv.SetMode(mode);
+}
 
-void Motion::ClearStallGuardFlag(Axis axis) {}
+// TODO: not implemented
+bool Motion::StallGuard(Axis axis) {
+    return false;
+}
 
-void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate) {}
+// TODO: not implemented
+void Motion::ClearStallGuardFlag(Axis axis) {
+}
 
-pos_t Motion::CurrentPos(Axis axis) const { return axisData[axis].ctrl.Position(); }
+// TODO: not implemented
+void Motion::Home(Axis axis, bool direction) {
+}
 
-void Motion::Home(Axis axis, bool direction) {}
+void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate) {
+    if (axisData[axis].ctrl.PlanMoveTo(pos, feedrate)) {
+        // move was queued, prepare the axis
+        if (!axisData[axis].enabled)
+            SetEnabled(axis, true);
+    }
+}
 
-void Motion::SetMode(Axis axis, MotorMode mode) {}
+pos_t Motion::CurrentPos(Axis axis) const {
+    return axisData[axis].ctrl.Position();
+}
 
 bool Motion::QueueEmpty() const {
     for (uint8_t i = 0; i != NUM_AXIS; ++i)
@@ -36,7 +55,7 @@ void Motion::AbortPlannedMoves() {
         axisData[i].ctrl.AbortPlannedMoves();
 }
 
-void Motion::Step() {}
+st_timer_t Motion::Step() {}
 
 void ISR() {}
 
diff --git a/src/modules/motion.h b/src/modules/motion.h
index 8b0ef1a..0b78929 100644
--- a/src/modules/motion.h
+++ b/src/modules/motion.h
@@ -77,18 +77,27 @@ public:
     /// state especially when the TMC may get randomly reset (deinited)
     void InitAxis(Axis axis);
 
-    /// Set axis power status
+    /// Set axis power status. One must manually ensure no moves are currently being
+    /// performed by calling QueueEmpty().
     void SetEnabled(Axis axis, bool enabled);
 
-    /// Disable axis motor
+    /// Disable axis motor. One must manually ensure no moves are currently being
+    /// performed by calling QueueEmpty().
     void Disable(Axis axis) { SetEnabled(axis, false); }
 
+    /// Set mode of TMC/motors operation. One must manually ensure no moves are currently
+    /// being performed by calling QueueEmpty().
+    void SetMode(Axis axis, MotorMode mode);
+
     /// @returns true if a stall guard event occurred recently on the axis
     bool StallGuard(Axis axis);
 
     /// clear stall guard flag reported on an axis
     void ClearStallGuardFlag(Axis axis);
 
+    /// Enqueue performing of homing of an axis
+    void Home(Axis axis, bool direction);
+
     /// Enqueue a single axis move in steps starting and ending at zero speed with maximum
     /// feedrate. Moves can only be enqueued if the axis is not Full().
     /// @param axis axis affected
@@ -116,14 +125,9 @@ public:
         axisData[axis].ctrl.SetAcceleration(accel);
     }
 
-    /// Enqueue performing of homing of an axis
-    void Home(Axis axis, bool direction);
-
-    /// Set mode of TMC/motors operation
-    void SetMode(Axis axis, MotorMode mode);
-
-    /// State machine doing all the planning and stepping preparation based on received commands
-    void Step();
+    /// State machine doing all the planning and stepping. Called by the stepping ISR.
+    /// @returns the interval for the next tick
+    st_timer_t Step();
 
     /// @returns true if all planned moves have been finished for all axes
     bool QueueEmpty() const;
@@ -132,14 +136,6 @@ public:
     /// @param axis requested
     bool QueueEmpty(Axis axis) const { return axisData[axis].ctrl.QueueEmpty(); }
 
-    /// @returns false if new moves can still be planned for _any_ axis
-    bool Full() const {
-        for (uint8_t i = 0; i != NUM_AXIS; ++i)
-            if (axisData[i].ctrl.Full())
-                return true;
-        return false;
-    }
-
     /// @returns false if new moves can still be planned for one axis
     /// @param axis axis requested
     bool Full(Axis axis) const { return axisData[axis].ctrl.Full(); }