Guard critical sections in modules::motion

While motion queuing is safe, code that relies on the current block
needs to run with the isr disabled.

Protect AbortPlannedMoves and CurPosition from isr' interference by
using a RAII guard.

src/modules/motion.cpp

@@ -6,6 +6,7 @@
 // TODO: use proper timer abstraction
 #ifdef __AVR__
 #include <avr/interrupt.h>
+#include <util/atomic.h>
 #else
 //#include "../hal/timers.h"
 #endif
@@ -15,6 +16,42 @@ namespace motion {
 
 Motion motion;
 
+/// ISR state manipulation
+static inline void IsrSetEnabled(bool state) {
+#ifdef __AVR__
+    if (state)
+        TIMSK1 |= (1 << OCIE1A);
+    else
+        TIMSK1 &= ~(1 << OCIE1A);
+#endif
+}
+
+static inline bool IsrDisable() {
+#ifdef __AVR__
+    bool state;
+    ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+        state = TIMSK1 & (1 << OCIE1A);
+        TIMSK1 &= ~(1 << OCIE1A);
+    }
+    return state;
+#else
+    return false;
+#endif
+}
+
+class SuspendIsr {
+    bool enabled;
+
+public:
+    SuspendIsr() {
+        enabled = IsrDisable();
+    }
+
+    ~SuspendIsr() {
+        IsrSetEnabled(enabled);
+    }
+};
+
 bool Motion::InitAxis(Axis axis) {
     // disable the axis and re-init the driver: this will clear the internal
     // StallGuard data as a result without special handling
@@ -63,6 +100,11 @@ pos_t Motion::Position(Axis axis) const {
     return axisData[axis].ctrl.Position();
 }
 
+pos_t Motion::CurPosition(Axis axis) const {
+    auto guard = SuspendIsr();
+    return axisData[axis].ctrl.CurPosition();
+}
+
 bool Motion::QueueEmpty() const {
     for (uint8_t i = 0; i != NUM_AXIS; ++i)
         if (!axisData[i].ctrl.QueueEmpty())
@@ -71,10 +113,12 @@ bool Motion::QueueEmpty() const {
 }
 
 void Motion::AbortPlannedMoves(Axis axis, bool halt) {
+    auto guard = SuspendIsr();
     axisData[axis].ctrl.AbortPlannedMoves(halt);
 }
 
 void Motion::AbortPlannedMoves(bool halt) {
+    auto guard = SuspendIsr();
     for (uint8_t i = 0; i != NUM_AXIS; ++i)
         AbortPlannedMoves((Axis)i, halt);
 }
@@ -113,10 +157,10 @@ void Init() {
     // Plan the first interrupt after 8ms from now.
     OCR1A = 0x4000;
     TCNT1 = 0;
+#endif
 
     // Enable interrupt
-    TIMSK1 |= (1 << OCIE1A);
+    IsrSetEnabled(true);
-#endif
 }
 
 } // namespace motion

src/modules/motion.h

@@ -179,7 +179,7 @@ public:
     /// probably be used instead.
     /// @param axis axis affected
     /// @returns the current position of the axis
-    pos_t CurPosition(Axis axis) const { return axisData[axis].ctrl.CurPosition(); }
+    pos_t CurPosition(Axis axis) const;
 
     /// Fetch the current axis position, but in AxisUnit. This function is expensive!
     /// The Axis needs to be supplied as the first template argument: CurPosition<axis>().

tests/unit/logic/stubs/stub_motion.cpp

@@ -46,6 +46,10 @@ pos_t Motion::Position(Axis axis) const {
     return axes[axis].pos;
 }
 
+pos_t Motion::CurPosition(Axis axis) const {
+    return axes[axis].pos;
+}
+
 void Motion::SetPosition(Axis axis, pos_t x) {
     axes[axis].pos = x;
     axisData[axis].ctrl.SetPosition(axes[axis].pos);