Fix Scheduler race conditions

.vscode/launch.json

@@ -26,6 +26,27 @@
             "stopAtEntry": false,
             "MIMode": "lldb",
             "preLaunchTask": "CMake: build"
-        }
+        },
+        {
+            "name": "Debug ST-LIB tests (simulator) [gdb]",
+            "type": "cppdbg",
+            "request": "launch",
+            "program": "${workspaceFolder}/out/build/simulator/Tests/st-lib-test",
+            "args": [
+                "--gtest_break_on_failure"
+            ],
+            "cwd": "${workspaceFolder}",
+            "externalConsole": false,
+            "stopAtEntry": false,
+            "MIMode": "gdb",
+            "setupCommands": [
+                {
+                    "description": "Enable pretty-printing for gdb",
+                    "text": "-enable-pretty-printing",
+                    "ignoreFailures": true
+                }
+            ],
+            "preLaunchTask": "CMake: build"
+        },
     ]
 }

Inc/HALAL/Models/TimerDomain/TimerDomain.hpp

@@ -26,8 +26,7 @@
     (SCHEDULER_TIMER_DOMAIN != 23) && (SCHEDULER_TIMER_DOMAIN != 24)
 #error Scheduler timer must be a 32 bit timer
 #endif
-
-#define SCHEDULER_GLOBAL_TIMER_IRQn glue(TIM, glue(SCHEDULER_TIMER_DOMAIN, _IRQn))
+extern TIM_TypeDef* Scheduler_global_timer;
 
 // NOTE: only works for static arrays
 #define ARRAY_LENGTH(a) (sizeof(a) / sizeof(*a))
@@ -676,8 +675,8 @@ TimerXList
             static inline std::array<Instance, N> instances{};
 
             static void init(std::span<const Config, N> cfgs) {
-                TIM_TypeDef* sched_timer = cmsis_timers[timer_idxmap[SCHEDULER_TIMER_DOMAIN]];
-                rcc_enable_timer(sched_timer);
+                Scheduler_global_timer = cmsis_timers[timer_idxmap[SCHEDULER_TIMER_DOMAIN]];
+                rcc_enable_timer(Scheduler_global_timer);
 
                 for (std::size_t i = 0; i < N; i++) {
                     const Config& e = cfgs[i];

Inc/HALAL/Services/Time/Scheduler.hpp

@@ -23,7 +23,10 @@ extern TIM_TypeDef* Scheduler_global_timer;
 
 struct Scheduler {
     using callback_t = void (*)();
-    static constexpr uint32_t INVALID_ID = 0xFFu;
+    static constexpr uint32_t kMaxTasks = 16;
+    // INVALID_ID must be an even multiple of kMaxTasks.
+    // if it isn't it could theoretically be used as an id in set_timeout
+    static constexpr uint32_t INVALID_ID = 2 * kMaxTasks;
 
     static void start();
     static void update();
@@ -37,13 +40,8 @@ struct Scheduler {
     static uint16_t set_timeout(uint32_t microseconds, callback_t func);
     static bool cancel_timeout(uint16_t id);
 
-    // static void global_timer_callback();
-
-    // Have to be public because SCHEDULER_GLOBAL_TIMER_CALLBACK won't work
-    // otherwise
-    // static const uint32_t global_timer_base = SCHEDULER_TIMER_BASE;
+    // internal
     static void on_timer_update();
-
 #ifndef SIM_ON
 private:
 #endif
@@ -55,7 +53,12 @@ struct Scheduler {
         bool repeating{false};
     };
 
-    static constexpr std::size_t kMaxTasks = 16;
+    static_assert(
+        ((INVALID_ID / kMaxTasks) % 2) == 0,
+        "INVALID_ID must be an even multiple of kMaxTasks"
+    );
+    static_assert(INVALID_ID >= kMaxTasks, "INVALID_ID must not be a possible task id");
+
     static_assert((kMaxTasks & (kMaxTasks - 1)) == 0, "kMaxTasks must be a power of two");
     static constexpr uint32_t FREQUENCY = 1'000'000u; // 1 MHz -> 1us precision
 

Src/HALAL/Services/Time/Scheduler.cpp

@@ -10,6 +10,11 @@
 
 #include <stdint.h>
 
+#define SCHEDULER_GLOBAL_TIMER_IRQn glue(TIM, glue(SCHEDULER_TIMER_DOMAIN, _IRQn))
+
+#define SchedLock() NVIC_DisableIRQ(SCHEDULER_GLOBAL_TIMER_IRQn)
+#define SchedUnlock() NVIC_EnableIRQ(SCHEDULER_GLOBAL_TIMER_IRQn)
+
 TIM_TypeDef* Scheduler_global_timer = nullptr;
 
 namespace {
@@ -35,8 +40,6 @@ inline void Scheduler::set_at(uint8_t idx, uint8_t id) {
     uint32_t shift = idx * 4;
     uint64_t clearmask = ~(0xFF << shift);
     Scheduler::sorted_task_ids_ = (sorted_task_ids_ & clearmask) | (id << shift);
-    // sorted_task_ids_ |= ((id & 0x0F) << shift); // This is also an option in case id is
-    // incorrect, I don't think it's necessary though
 }
 inline uint8_t Scheduler::front_id() { return *((uint8_t*)&sorted_task_ids_) & 0xF; }
 inline void Scheduler::pop_front() {
@@ -68,6 +71,7 @@ void Scheduler::start() {
     Scheduler_global_timer =
         ST_LIB::TimerDomain::cmsis_timers[ST_LIB::timer_idxmap[SCHEDULER_TIMER_DOMAIN]];
 
+    // TODO: change this to use TimerDomain::get_timer_clock()?
     uint32_t prescaler = (SystemCoreClock / Scheduler::FREQUENCY);
     // setup prescaler
     {
@@ -155,11 +159,15 @@ void Scheduler::start() {
 void Scheduler::update() {
     while (ready_bitmap_ != 0u) {
         uint32_t bit_index = static_cast<uint32_t>(__builtin_ctz(ready_bitmap_));
-        ready_bitmap_ &= ~(1u << bit_index); // Clear the bit
+
+        CLEAR_BIT(ready_bitmap_, 1u << bit_index);
+
         Task& task = tasks_[bit_index];
         task.callback();
         if (!task.repeating) [[unlikely]] {
+            SchedLock();
             release_slot(static_cast<uint8_t>(bit_index));
+            SchedUnlock();
         }
     }
 }
@@ -299,7 +307,9 @@ void Scheduler::on_timer_update() {
             break; // Task is in the future, stop processing
         }
         pop_front();
-        ready_bitmap_ |= (1u << candidate_id); // mark task as ready
+
+        // mark task as ready
+        SET_BIT(ready_bitmap_, 1u << candidate_id);
 
         if (task.repeating) [[likely]] {
             task.next_fire_us = static_cast<uint32_t>(global_tick_us_ + task.period_us);
@@ -319,18 +329,21 @@ uint16_t Scheduler::register_task(uint32_t period_us, callback_t func) {
         return static_cast<uint8_t>(Scheduler::INVALID_ID);
 
     uint8_t slot = allocate_slot();
-    if (slot == Scheduler::INVALID_ID)
+    if (slot == Scheduler::INVALID_ID) [[unlikely]]
         return slot;
 
     Task& task = tasks_[slot];
     task.callback = func;
     task.period_us = period_us;
     task.repeating = true;
+    task.id = static_cast<uint32_t>(slot);
+
+    SchedLock();
     task.next_fire_us =
         static_cast<uint32_t>(global_tick_us_ + Scheduler_global_timer->CNT + period_us);
-    task.id = static_cast<uint32_t>(slot);
     insert_sorted(slot);
     schedule_next_interval();
+    SchedUnlock();
     return task.id;
 }
 
@@ -343,22 +356,25 @@ uint16_t Scheduler::set_timeout(uint32_t microseconds, callback_t func) {
         return static_cast<uint8_t>(Scheduler::INVALID_ID);
 
     uint8_t slot = allocate_slot();
-    if (slot == Scheduler::INVALID_ID)
+    if (slot == Scheduler::INVALID_ID) [[unlikely]]
         return slot;
 
     Task& task = tasks_[slot];
     task.callback = func;
     task.period_us = microseconds;
     task.repeating = false;
-    task.next_fire_us = static_cast<uint32_t>(global_tick_us_ + microseconds);
     task.id = slot + Scheduler::timeout_idx_ * Scheduler::kMaxTasks;
-
     // Add 2 instead of 1 so overflow doesn't make timeout_idx == 0,
     // we need it to never be 0
     Scheduler::timeout_idx_ += 2;
 
+    SchedLock();
+    task.next_fire_us =
+        static_cast<uint32_t>(global_tick_us_ + Scheduler_global_timer->CNT + microseconds);
     insert_sorted(slot);
     schedule_next_interval();
+    SchedUnlock();
+
     return task.id;
 }
 
@@ -368,9 +384,11 @@ bool Scheduler::unregister_task(uint16_t id) {
     if (free_bitmap_ & (1UL << id))
         return false;
 
+    SchedLock();
     remove_sorted(id);
     release_slot(id);
     schedule_next_interval();
+    SchedUnlock();
     return true;
 }
 
@@ -384,8 +402,10 @@ bool Scheduler::cancel_timeout(uint16_t id) {
     if (free_bitmap_ & (1UL << idx))
         return false;
 
+    SchedLock();
     remove_sorted(idx);
     release_slot(idx);
     schedule_next_interval();
+    SchedUnlock();
     return true;
 }

Tests/StateMachine/state_machine_test.cpp

@@ -1,6 +1,7 @@
 #include <gtest/gtest.h>
 #include "ST-LIB_LOW/StateMachine/StateMachine.hpp"
 #include "HALAL/Services/Time/Scheduler.hpp"
+#include "HALAL/Models/TimerDomain/TimerDomain.hpp"
 
 enum class MasterState { A, B, C };
 
@@ -65,6 +66,9 @@ static void reset_test_state() {
     TIM2_BASE->SR = 0;
     TIM2_BASE->CR1 = 0;
     TIM2_BASE->DIER = 0;
+
+    Scheduler_global_timer =
+        ST_LIB::TimerDomain::cmsis_timers[ST_LIB::timer_idxmap[SCHEDULER_TIMER_DOMAIN]];
 }
 
 static void tick_scheduler(int ticks) {