diff --git a/src/hal/avr/tmc2130.cpp b/src/hal/avr/tmc2130.cpp
index 75e6f61..15546e2 100644
--- a/src/hal/avr/tmc2130.cpp
+++ b/src/hal/avr/tmc2130.cpp
@@ -84,9 +84,16 @@ void TMC2130::SetCurrents(const MotorParams &params, const MotorCurrents &curren
 
 void TMC2130::SetEnabled(const MotorParams &params, bool enabled) {
     hal::shr16::shr16.SetTMCDir(params.idx, enabled);
+    if (this->enabled != enabled)
+        ClearStallguard(params);
     this->enabled = enabled;
 }
 
+void TMC2130::ClearStallguard(const MotorParams &params) {
+    // @todo: maximum resolution right now is x256/4 (uint8_t / 4)
+    sg_counter = 4 * (1 << (8 - params.uSteps)) - 1; /// one electrical full step (4 steps when fullstepping)
+}
+
 uint32_t TMC2130::ReadRegister(const MotorParams &params, Registers reg) {
     uint8_t pData[5] = { (uint8_t)reg };
     _spi_tx_rx(params, pData);
@@ -102,6 +109,15 @@ void TMC2130::WriteRegister(const MotorParams &params, Registers reg, uint32_t d
     _handle_spi_status(params, pData[0]);
 }
 
+void TMC2130::Isr(const MotorParams &params) {
+    if (sg_counter) {
+        if (SampleDiag(params))
+            sg_counter--;
+        else if (sg_counter < (4 * (1 << (8 - params.uSteps)) - 1))
+            sg_counter++;
+    }
+}
+
 void TMC2130::_spi_tx_rx(const MotorParams &params, uint8_t (&pData)[5]) {
     hal::gpio::WritePin(params.csPin, hal::gpio::Level::low);
     for (uint8_t i = 0; i < sizeof(pData); i++) {
diff --git a/src/hal/tmc2130.h b/src/hal/tmc2130.h
index b92a5a3..add8419 100644
--- a/src/hal/tmc2130.h
+++ b/src/hal/tmc2130.h
@@ -40,6 +40,7 @@ class TMC2130 {
         uint8_t driver_error : 1;
     } errorFlags;
     bool enabled = false;
+    uint8_t sg_counter;
 
 public:
     enum class Registers : uint8_t {
@@ -107,16 +108,25 @@ public:
     }
 
     /// Return SG state
-    static inline bool Stall(const MotorParams &params) {
+    static inline bool SampleDiag(const MotorParams &params) {
         return gpio::ReadPin(params.sgPin) == gpio::Level::low;
     }
 
+    inline bool Stalled() const {
+        return sg_counter == 0;
+    }
+
+    void ClearStallguard(const MotorParams &params);
+
     /// Reads a driver register and updates the status flags
     uint32_t ReadRegister(const MotorParams &params, Registers reg);
 
     /// Writes a driver register and updates the status flags
     void WriteRegister(const MotorParams &params, Registers reg, uint32_t data);
 
+    /// Used for polling the DIAG pin. Should be called from the stepper isr periodically when moving.
+    void Isr(const MotorParams &params);
+
 private:
     void _spi_tx_rx(const MotorParams &params, uint8_t (&pData)[5]);
     void _handle_spi_status(const MotorParams &params, uint8_t status);
diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp
index 9d048ea..0fe2d15 100644
--- a/src/modules/motion.cpp
+++ b/src/modules/motion.cpp
@@ -18,12 +18,6 @@ void Motion::SetEnabled(Axis axis, bool enabled) {
     if (enabled != axisData[axis].enabled) {
         axisData[axis].drv.SetEnabled(axisParams[axis].params, enabled);
         axisData[axis].enabled = enabled;
-
-        if (!axisData[axis].enabled) {
-            // axis is powered off, clear internal StallGuard counters
-            axisData[axis].stall_trig = false;
-            axisData[axis].stall_cnt = 0;
-        }
     } // else skip unnecessary Enable/Disable operations on an axis if already in the desired state
 }
 
@@ -33,11 +27,11 @@ void Motion::SetMode(Axis axis, MotorMode mode) {
 }
 
 bool Motion::StallGuard(Axis axis) {
-    return axisData[axis].stall_trig;
+    return axisData[axis].drv.Stalled();
 }
 
-void Motion::ClearStallGuardFlag(Axis axis) {
-    axisData[axis].stall_trig = false;
+void Motion::StallGuardReset(Axis axis) {
+    axisData[axis].drv.ClearStallguard(axisParams[axis].params);
 }
 
 // TODO: not implemented
@@ -77,13 +71,8 @@ st_timer_t Motion::Step() {
         if (timers[i] <= config::stepTimerQuantum) {
             timers[i] += axisData[i].ctrl.Step(axisParams[i].params);
 
-            // axis has been moved, sample StallGuard
-            if (hal::tmc2130::TMC2130::Stall(axisParams[i].params)) {
-                // TODO: on the MK3 a stall is marked as such as 1/2 of a full step is
-                // lost: this is too simplistic for production
-                ++axisData[i].stall_cnt;
-                axisData[i].stall_trig = true;
-            }
+            // axis has been moved, run the tmc2130 Isr for this axis
+            axisData[i].drv.Isr(axisParams[i].params);
         }
     }
 
diff --git a/src/modules/motion.h b/src/modules/motion.h
index 0fcae91..3a1ad3a 100644
--- a/src/modules/motion.h
+++ b/src/modules/motion.h
@@ -93,7 +93,7 @@ public:
     bool StallGuard(Axis axis);
 
     /// clear stall guard flag reported on an axis
-    void ClearStallGuardFlag(Axis axis);
+    void StallGuardReset(Axis axis);
 
     /// Enqueue performing of homing of an axis
     void Home(Axis axis, bool direction);
@@ -220,8 +220,6 @@ private:
         pulse_gen::PulseGen ctrl; ///< Motor controller
         bool enabled; ///< Axis enabled
         st_timer_t residual; ///< Axis timer residual
-        uint8_t stall_cnt; ///< Underlying StallGuard lost ustep count
-        bool stall_trig; ///< StallGuard trigger flag
     };
 
     /// Helper to initialize AxisData members