diff --git a/src/modules/pulse_gen.cpp b/src/modules/pulse_gen.cpp
index 09e4aef..3d38784 100644
--- a/src/modules/pulse_gen.cpp
+++ b/src/modules/pulse_gen.cpp
@@ -1,10 +1,4 @@
 #include "pulse_gen.h"
-using hal::tmc2130::MotorParams;
-using hal::tmc2130::TMC2130;
-using modules::math::mulU24X24toH16;
-using modules::speed_table::calc_timer;
-
-#include "../cmath.h"
 
 namespace modules {
 namespace pulse_gen {
@@ -117,84 +111,5 @@ void PulseGen::Move(pos_t target, steps_t feed_rate) {
     position = target;
 }
 
-st_timer_t PulseGen::Step(const MotorParams &motorParams) {
-    if (!current_block) {
-        // fetch next block
-        if (!block_index.empty())
-            current_block = &block_buffer[block_index.front()];
-        if (!current_block)
-            return 0;
-
-        // Set direction early so that the direction-change delay is accounted for
-        TMC2130::SetDir(motorParams, current_block->direction);
-
-        // Initializes the trapezoid generator from the current block.
-        deceleration_time = 0;
-        acc_step_rate = uint16_t(current_block->initial_rate);
-        acceleration_time = calc_timer(acc_step_rate, step_loops);
-        steps_completed = 0;
-
-        // Set the nominal step loops to zero to indicate, that the timer value is not
-        // known yet. That means, delay the initialization of nominal step rate and step
-        // loops until the steady state is reached.
-        step_loops_nominal = 0;
-    }
-
-    // Step the motor
-    for (uint8_t i = 0; i < step_loops; ++i) {
-        TMC2130::Step(motorParams);
-        if (++steps_completed >= current_block->steps)
-            break;
-    }
-
-    // Calculate new timer value
-    // 13.38-14.63us for steady state,
-    // 25.12us for acceleration / deceleration.
-    st_timer_t timer;
-    if (steps_completed <= current_block->accelerate_until) {
-        // v = t * a   ->   acc_step_rate = acceleration_time * current_block->acceleration_rate
-        acc_step_rate = mulU24X24toH16(acceleration_time, current_block->acceleration_rate);
-        acc_step_rate += uint16_t(current_block->initial_rate);
-        // upper limit
-        if (acc_step_rate > uint16_t(current_block->nominal_rate))
-            acc_step_rate = current_block->nominal_rate;
-        // step_rate to timer interval
-        timer = calc_timer(acc_step_rate, step_loops);
-        acceleration_time += timer;
-    } else if (steps_completed > current_block->decelerate_after) {
-        st_timer_t step_rate = mulU24X24toH16(deceleration_time, current_block->acceleration_rate);
-
-        if (step_rate > acc_step_rate) { // Check step_rate stays positive
-            step_rate = uint16_t(current_block->final_rate);
-        } else {
-            step_rate = acc_step_rate - step_rate; // Decelerate from acceleration end point.
-
-            // lower limit
-            if (step_rate < current_block->final_rate)
-                step_rate = uint16_t(current_block->final_rate);
-        }
-
-        // Step_rate to timer interval.
-        timer = calc_timer(step_rate, step_loops);
-        deceleration_time += timer;
-    } else {
-        if (!step_loops_nominal) {
-            // Calculation of the steady state timer rate has been delayed to the 1st tick
-            // of the steady state to lower the initial interrupt blocking.
-            timer_nominal = calc_timer(uint16_t(current_block->nominal_rate), step_loops);
-            step_loops_nominal = step_loops;
-        }
-        timer = timer_nominal;
-    }
-
-    // If current block is finished, reset pointer
-    if (steps_completed >= current_block->steps) {
-        current_block = nullptr;
-        block_index.pop();
-    }
-
-    return timer;
-}
-
 } // namespace motor
 } // namespace modules
diff --git a/src/modules/pulse_gen.h b/src/modules/pulse_gen.h
index 201dd30..fb143f8 100644
--- a/src/modules/pulse_gen.h
+++ b/src/modules/pulse_gen.h
@@ -3,6 +3,7 @@
 #include "speed_table.h"
 #include "../hal/tmc2130.h"
 #include "../hal/circular_buffer.h"
+#include "../cmath.h"
 
 namespace modules {
 
@@ -10,6 +11,9 @@ namespace modules {
 namespace pulse_gen {
 
 using config::blockBufferSize;
+using hal::tmc2130::TMC2130;
+using math::mulU24X24toH16;
+using speed_table::calc_timer;
 using speed_table::st_timer_t;
 typedef uint32_t steps_t; ///< Absolute step units
 typedef uint32_t rate_t; ///< Type for step rates
@@ -42,7 +46,84 @@ public:
 
     /// Single-step the axis
     /// @returns the interval for the next tick
-    st_timer_t Step(const hal::tmc2130::MotorParams &motorParams);
+    inline st_timer_t Step(const hal::tmc2130::MotorParams &motorParams) {
+        if (!current_block) {
+            // fetch next block
+            if (!block_index.empty())
+                current_block = &block_buffer[block_index.front()];
+            if (!current_block)
+                return 0;
+
+            // Set direction early so that the direction-change delay is accounted for
+            TMC2130::SetDir(motorParams, current_block->direction);
+
+            // Initializes the trapezoid generator from the current block.
+            deceleration_time = 0;
+            acc_step_rate = uint16_t(current_block->initial_rate);
+            acceleration_time = calc_timer(acc_step_rate, step_loops);
+            steps_completed = 0;
+
+            // Set the nominal step loops to zero to indicate, that the timer value is not
+            // known yet. That means, delay the initialization of nominal step rate and step
+            // loops until the steady state is reached.
+            step_loops_nominal = 0;
+        }
+
+        // Step the motor
+        for (uint8_t i = 0; i < step_loops; ++i) {
+            TMC2130::Step(motorParams);
+            if (++steps_completed >= current_block->steps)
+                break;
+        }
+
+        // Calculate new timer value
+        // 13.38-14.63us for steady state,
+        // 25.12us for acceleration / deceleration.
+        st_timer_t timer;
+        if (steps_completed <= current_block->accelerate_until) {
+            // v = t * a   ->   acc_step_rate = acceleration_time * current_block->acceleration_rate
+            acc_step_rate = mulU24X24toH16(acceleration_time, current_block->acceleration_rate);
+            acc_step_rate += uint16_t(current_block->initial_rate);
+            // upper limit
+            if (acc_step_rate > uint16_t(current_block->nominal_rate))
+                acc_step_rate = current_block->nominal_rate;
+            // step_rate to timer interval
+            timer = calc_timer(acc_step_rate, step_loops);
+            acceleration_time += timer;
+        } else if (steps_completed > current_block->decelerate_after) {
+            st_timer_t step_rate = mulU24X24toH16(deceleration_time, current_block->acceleration_rate);
+
+            if (step_rate > acc_step_rate) { // Check step_rate stays positive
+                step_rate = uint16_t(current_block->final_rate);
+            } else {
+                step_rate = acc_step_rate - step_rate; // Decelerate from acceleration end point.
+
+                // lower limit
+                if (step_rate < current_block->final_rate)
+                    step_rate = uint16_t(current_block->final_rate);
+            }
+
+            // Step_rate to timer interval.
+            timer = calc_timer(step_rate, step_loops);
+            deceleration_time += timer;
+        } else {
+            if (!step_loops_nominal) {
+                // Calculation of the steady state timer rate has been delayed to the 1st tick
+                // of the steady state to lower the initial interrupt blocking.
+                timer_nominal = calc_timer(uint16_t(current_block->nominal_rate), step_loops);
+                step_loops_nominal = step_loops;
+            }
+            timer = timer_nominal;
+        }
+
+        // If current block is finished, reset pointer
+        if (steps_completed >= current_block->steps) {
+            current_block = nullptr;
+            block_index.pop();
+        }
+
+        return timer;
+    }
 
 private:
     /// Motion parameters for the current planned or executing move