sched fix (tested on PCU, LV-BMS, HV-BMS and LCU) (#594)

* fix (tested on PCU, LV-BMS and LCU)

* get cleanup to compile

* format checks

* fix tests and scheduler (off by one error)

* Add a test for get_at

* formatting

* Add a test for set_at and fix it

* formatting

* formatting

* try to fix issue in LCU

* add a test for front_id and pop_front

* add changeset for this pr

* formatting for changeset :/

* fix (tested on PCU, LV-BMS and LCU)

* get cleanup to compile

* format checks

* fix tests and scheduler (off by one error)

* Add a test for get_at

* formatting

* Add a test for set_at and fix it

* formatting

* formatting

* try to fix issue in LCU

* add a test for front_id and pop_front

* revert merge from development

* searching for case-insensitive STM32 CLT path

* applied formatter

* removed unused include

* making PacketValue destructor virtual so that CLang doesn't cry

* minor fix

* commit old changes

* fix off by one error in allocating a slot

* formatting

* fix old compile error which shouldn't be one

* Add set_limit_value to set arr in TimerWrapper

* fix indentation

* indentation, again

* formatting

* Move errorhandler away from interrupt callback

* ErrorHandler -> WARNING, prescaler calc -> TimerDomain

* formatting

* Add Infowarning to tests, hopefully this compiles

* formatting, and try to compile tests

* formatting

* formatting again

* change changeset to minor

* remove ErrorHandler in TIM_IC_CaptureCallback since it's an interrupt callback

---------

Co-authored-by: Jorge Sáez <jorgeesg82@gmail.com>

Author: victor-Lopez25

.changesets/fix-scheduler-race-condition.md

@@ -0,0 +1,5 @@
+release: minor
+summary: fix remaining scheduler race conditions and add warning when tasks are not ran in time
+
+The mechanism for checking if tasks are not ran in time is very simple but that also means the scheduler can only know if a task has not been called when the waiting time runs out for the second time.
+This means you will know if you're too slow to execute the task in less than 2x its period but not if you're between 1 and 2x its period.

Inc/HALAL/Models/Packets/PacketValue.hpp

@@ -11,7 +11,7 @@ template <> class PacketValue<> {
 public:
     using value_type = empty_type;
     PacketValue() = default;
-    ~PacketValue() = default;
+    virtual ~PacketValue() = default;
     virtual void* get_pointer() = 0;
     virtual void set_pointer(void* pointer) = 0;
     virtual size_t get_size() = 0;

Inc/HALAL/Models/TimerDomain/TimerDomain.hpp

@@ -576,7 +576,8 @@ struct TimerDomain {
                 ST_LIB::compile_error("This timer is used by the scheduler");
             }
 
-            if (requests[i].request != TimerRequest::AnyGeneralPurpose &&
+            if ((requests[i].request != TimerRequest::AnyGeneralPurpose) &&
+                (requests[i].request != TimerRequest::Any32bit) &&
                 (requests[i].request < 1 || requests[i].request > 24 ||
                  (requests[i].request > 17 && requests[i].request < 23))) {
                 ST_LIB::compile_error("Invalid TimerRequest value for timer");
@@ -817,6 +818,7 @@ struct TimerDomain {
         }
     }
 
+    // NOTE: This is a bit slower than timerwrapper.get_clock_frequency(), so preferrably use that
     static inline uint32_t get_timer_frequency(TIM_TypeDef* tim) {
         uint32_t result = 0;
         if ((tim == TIM2) || (tim == TIM3) || (tim == TIM4) || (tim == TIM5) || (tim == TIM6) ||

Inc/HALAL/Services/Encoder/Encoder.hpp

@@ -63,7 +63,7 @@ template <const TimerDomain::Timer& dev> class Encoder {
         }
 
         tim->instance->tim->PSC = 5;
-        if constexpr (tim->is_32bit_instance) {
+        if constexpr (TimerWrapper<dev>::is_32bit_instance) {
             tim->instance->tim->ARR = UINT32_MAX;
         } else {
             tim->instance->tim->ARR = UINT16_MAX;

Inc/HALAL/Services/Time/Scheduler.hpp

@@ -9,7 +9,15 @@
 /* Uso del scheduler, descrito en la wiki:
  * https://wiki.hyperloopupv.com/es/firmware/Timing/Scheduler */
 
+/* To allow debugging of inline functions only when testing */
+#ifdef SIM_ON
+#define HYPER_INLINE inline
+#else
+#define HYPER_INLINE
+#endif
+
 #include "stm32h7xx_ll_tim_wrapper.h"
+#include "HALAL/Models/Packets/Packet.hpp"
 
 #include <array>
 #include <cstdint>
@@ -33,9 +41,7 @@ struct Scheduler {
     // temporary, will be removed
     [[deprecated]] static inline void start() {}
     static void update();
-    static inline uint64_t get_global_tick() {
-        return global_tick_us_ + Scheduler_global_timer->CNT;
-    }
+    static uint64_t get_global_tick();
 
     static uint16_t register_task(uint32_t period_us, callback_t func);
     static bool unregister_task(uint16_t id);
@@ -44,7 +50,7 @@ struct Scheduler {
     static bool cancel_timeout(uint16_t id);
 
     // internal
-    static void on_timer_update();
+    static inline void on_timer_update();
     static void schedule_next_interval();
     static constexpr uint32_t FREQUENCY = 1'000'000u; // 1 MHz -> 1us precision
 #ifndef SIM_ON
@@ -92,10 +98,19 @@ struct Scheduler {
     static void remove_sorted(uint8_t id);
 
     // helpers
-    static inline uint8_t get_at(uint8_t idx);
-    static inline void set_at(uint8_t idx, uint8_t id);
-    static inline void pop_front();
-    static inline uint8_t front_id();
+    static HYPER_INLINE uint8_t get_at(uint8_t idx) {
+        return (uint8_t)((sorted_task_ids_ >> (idx * 4)) & 0xF);
+    }
+    static HYPER_INLINE void set_at(uint8_t idx, uint8_t id) {
+        uint64_t shift = idx * 4;
+        uint64_t clearmask = ~(0x0FULL << shift);
+        Scheduler::sorted_task_ids_ = (sorted_task_ids_ & clearmask) | ((uint64_t)id << shift);
+    }
+    static HYPER_INLINE uint8_t front_id() { return *((uint8_t*)&sorted_task_ids_) & 0xF; }
+    static HYPER_INLINE void pop_front() {
+        Scheduler::active_task_count_--;
+        Scheduler::sorted_task_ids_ >>= 4;
+    }
 
     static inline void global_timer_disable();
     static inline void global_timer_enable();

Inc/HALAL/Services/Time/TimerWrapper.hpp

@@ -420,6 +420,8 @@ template <const TimerDomain::Timer& dev> struct TimerWrapper {
     inline TIM_TypeDef* get_cmsis_handle() { return instance->tim; }
 
     inline void set_prescaler(uint16_t psc) { instance->tim->PSC = psc; }
+    // TODO: 16 bit and 32 bit version (?)
+    inline void set_limit_value(uint32_t arr) { instance->tim->ARR = arr; }
 
     inline void configure32bit(void (*callback)(void*), void* callback_data, uint32_t period) {
         static_assert(

Src/HALAL/Models/TimerDomain/TimerDomain.cpp

@@ -57,7 +57,9 @@ static void TIM_IC_CaptureCallback(const uint32_t timer_idx, uint32_t channel) {
         if (falling_value < info->period)
             info->value_falling = falling_value;
     } else [[unlikely]] {
-        ErrorHandler("TimerDomain::input_capture_info was modified");
+        // TimerDomain::input_capture_info was modified or STM interrupts are all over the place
+        // either way, this is a no-op
+        // ErrorHandler("TimerDomain::input_capture_info was modified");
     }
 }
 

Src/HALAL/Services/Time/Scheduler.cpp

@@ -1,12 +1,12 @@
 /*
- * Scheduler.hpp
+ * Scheduler.cpp
  *
  * Created on: 17 nov. 2025
  *     Author: Victor (coauthor Stephan)
  */
 #include "HALAL/Services/Time/Scheduler.hpp"
 #include "HALAL/Models/TimerDomain/TimerDomain.hpp"
-#include "ErrorHandler/ErrorHandler.hpp"
+#include "HALAL/Services/InfoWarning/InfoWarning.hpp"
 
 #include <stdint.h>
 
@@ -31,22 +31,7 @@ uint64_t Scheduler::global_tick_us_{0};
 uint32_t Scheduler::current_interval_us_{0};
 uint16_t Scheduler::timeout_idx_{1};
 
-inline uint8_t Scheduler::get_at(uint8_t idx) {
-    int word_idx = idx > 7;
-    uint32_t shift = (idx & 7) << 2;
-    return (((uint32_t*)&sorted_task_ids_)[word_idx] & (0x0F << shift)) >> shift;
-}
-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);
-}
-inline uint8_t Scheduler::front_id() { return *((uint8_t*)&sorted_task_ids_) & 0xF; }
-inline void Scheduler::pop_front() {
-    // O(1) remove of logical index 0
-    Scheduler::active_task_count_--;
-    Scheduler::sorted_task_ids_ >>= 4;
-}
+uint16_t failing_id = Scheduler::INVALID_ID;
 
 // ----------------------------
 
@@ -73,107 +58,55 @@ void Scheduler_start(void) {
     ST_LIB::TimerDomain::callbacks[ST_LIB::timer_idxmap[static_cast<uint8_t>(SCHEDULER_TIMER_DOMAIN
     )]] = Scheduler_global_timer_callback;
 
-    // TODO: change this to use TimerDomain::get_timer_clock()?
-    uint32_t prescaler = (SystemCoreClock / Scheduler::FREQUENCY);
-    // setup prescaler
-    {
-        // ref manual: section 8.7.7 RCC domain 1 clock configuration register
-        uint32_t ahb_prescaler = RCC->D1CFGR & RCC_D1CFGR_HPRE_Msk;
-        if ((ahb_prescaler & 0b1000) != 0) {
-            switch (ahb_prescaler) {
-            case 0b1000:
-                prescaler /= 2;
-                break;
-            case 0b1001:
-                prescaler /= 4;
-                break;
-            case 0b1010:
-                prescaler /= 8;
-                break;
-            case 0b1011:
-                prescaler /= 16;
-                break;
-            case 0b1100:
-                prescaler /= 64;
-                break;
-            case 0b1101:
-                prescaler /= 128;
-                break;
-            case 0b1110:
-                prescaler /= 256;
-                break;
-            case 0b1111:
-                prescaler /= 512;
-                break;
-            }
-        }
-
-        // ref manual: section 8.7.8: RCC domain 2 clock configuration register
-        uint32_t apb1_prescaler = (RCC->D2CFGR & RCC_D2CFGR_D2PPRE1_Msk) >> RCC_D2CFGR_D2PPRE1_Pos;
-        if ((apb1_prescaler & 0b100) != 0) {
-            switch (apb1_prescaler) {
-            case 0b100:
-                prescaler /= 2;
-                break;
-            case 0b101:
-                prescaler /= 4;
-                break;
-            case 0b110:
-                prescaler /= 8;
-                break;
-            case 0b111:
-                prescaler /= 16;
-                break;
-            }
-        }
-        // tim2clk = 2 x pclk1 when apb1_prescaler != 1
-        if (apb1_prescaler != 1) {
-            prescaler *= 2;
-        }
-
-        if (prescaler > 1) {
-            prescaler--;
-        }
-    }
-
-    if (prescaler == 0 || prescaler > 0xFFFF) {
-        ErrorHandler("Invalid prescaler value: %u", prescaler);
-        return;
-    }
-
-    Scheduler_global_timer->PSC = (uint16_t)prescaler;
+    uint16_t prescaler =
+        (uint16_t)(ST_LIB::TimerDomain::get_timer_frequency(Scheduler_global_timer) /
+                   Scheduler::FREQUENCY);
+    Scheduler_global_timer->PSC = prescaler;
     Scheduler_global_timer->ARR = 0;
     Scheduler_global_timer->DIER |= LL_TIM_DIER_UIE;
     Scheduler_global_timer->CR1 =
         LL_TIM_CLOCKDIVISION_DIV1 | (Scheduler_global_timer->CR1 & ~TIM_CR1_CKD);
 
     Scheduler_global_timer->CNT = 0; /* Clear counter value */
 
-    NVIC_EnableIRQ(SCHEDULER_GLOBAL_TIMER_IRQn);
     CLEAR_BIT(Scheduler_global_timer->SR, LL_TIM_SR_UIF); /* clear update interrupt flag */
 
     Scheduler::schedule_next_interval();
 }
 
 void Scheduler::update() {
+    // NOTE: Only _one_ id will be shown per call to update()
+    if (failing_id != Scheduler::INVALID_ID) [[unlikely]] {
+        WARNING("Too slow, could not execute task %u in time", failing_id);
+        failing_id = Scheduler::INVALID_ID;
+    }
+
     while (ready_bitmap_ != 0u) {
         uint32_t bit_index = static_cast<uint32_t>(__builtin_ctz(ready_bitmap_));
 
-        CLEAR_BIT(ready_bitmap_, 1u << bit_index);
-
         Task& task = tasks_[bit_index];
+
         task.callback();
+
+        SchedLock();
+        CLEAR_BIT(ready_bitmap_, 1u << bit_index);
         if (!task.repeating) [[unlikely]] {
-            SchedLock();
             release_slot(static_cast<uint8_t>(bit_index));
-            SchedUnlock();
         }
+        SchedUnlock();
     }
 }
 
+uint64_t Scheduler::get_global_tick() {
+    SchedLock();
+    uint64_t val = global_tick_us_ + Scheduler_global_timer->CNT;
+    SchedUnlock();
+    return val;
+}
+
 inline uint8_t Scheduler::allocate_slot() {
     uint32_t idx = __builtin_ffs(Scheduler::free_bitmap_) - 1;
-    if (idx > static_cast<int>(Scheduler::kMaxTasks)) [[unlikely]]
+    if (idx >= Scheduler::kMaxTasks) [[unlikely]]
         return static_cast<uint8_t>(Scheduler::INVALID_ID);
     Scheduler::free_bitmap_ &= ~(1UL << idx);
     return static_cast<uint8_t>(idx);
@@ -272,9 +205,12 @@ void Scheduler::schedule_next_interval() {
         return;
     }
 
+    SchedLock();
     uint8_t next_id = Scheduler::front_id(); // sorted_task_ids_[0]
     Task& next_task = tasks_[next_id];
     int32_t diff = (int32_t)(next_task.next_fire_us - static_cast<uint32_t>(global_tick_us_));
+    SchedUnlock();
+
     if (diff >= -1 && diff <= 1) [[unlikely]] {
         current_interval_us_ = 1;
         SET_BIT(Scheduler_global_timer->EGR, TIM_EGR_UG); // This should cause an interrupt
@@ -284,18 +220,18 @@ void Scheduler::schedule_next_interval() {
         } else {
             current_interval_us_ = static_cast<uint32_t>(diff);
         }
-        Scheduler_global_timer->ARR = static_cast<uint32_t>(current_interval_us_ - 1u);
-        while (Scheduler_global_timer->CNT > Scheduler_global_timer->ARR) [[unlikely]] {
-            uint32_t offset = Scheduler_global_timer->CNT - Scheduler_global_timer->ARR;
-            current_interval_us_ = offset;
-            SET_BIT(Scheduler_global_timer->EGR, TIM_EGR_UG); // This should cause an interrupt
-            Scheduler_global_timer->CNT = Scheduler_global_timer->CNT + offset;
+
+        Scheduler_global_timer->ARR = current_interval_us_ - 1u;
+        if (Scheduler_global_timer->CNT > Scheduler_global_timer->ARR) [[unlikely]] {
+            uint32_t cnt_temp = Scheduler_global_timer->CNT;
+            Scheduler_global_timer->CNT = 0;
+            global_tick_us_ += cnt_temp;
         }
     }
     Scheduler::global_timer_enable();
 }
 
-void Scheduler::on_timer_update() {
+inline void Scheduler::on_timer_update() {
     global_tick_us_ += current_interval_us_;
 
     while (active_task_count_ > 0) { // Pop all due tasks, several might be due in the same tick
@@ -305,15 +241,20 @@ void Scheduler::on_timer_update() {
         if (diff > 0) [[likely]] {
             break; // Task is in the future, stop processing
         }
-        pop_front();
+        uint32_t task_bit = 1u << candidate_id;
 
+        SchedLock();
+        pop_front();
         // mark task as ready
-        SET_BIT(ready_bitmap_, 1u << candidate_id);
-
+        if ((ready_bitmap_ & task_bit) != 0) [[unlikely]] {
+            failing_id = candidate_id;
+        }
+        SET_BIT(ready_bitmap_, task_bit);
         if (task.repeating) [[likely]] {
             task.next_fire_us = static_cast<uint32_t>(global_tick_us_ + task.period_us);
             insert_sorted(candidate_id);
         }
+        SchedUnlock();
     }
 
     schedule_next_interval();

Tests/Time/common_tests.cpp

@@ -1,5 +1,6 @@
 #include <gtest/gtest.h>
 #include "ErrorHandler/ErrorHandler.hpp"
+#include "HALAL/Services/InfoWarning/InfoWarning.hpp"
 
 std::string ErrorHandlerModel::line;
 std::string ErrorHandlerModel::func;
@@ -32,3 +33,33 @@ void ErrorHandlerModel::ErrorHandlerTrigger(string format, ...) {
 }
 
 void ErrorHandlerModel::ErrorHandlerUpdate() {}
+
+std::string InfoWarning::line;
+std::string InfoWarning::func;
+std::string InfoWarning::file;
+
+namespace ST_LIB::TestInfoWarning {
+bool fail_on_error = false;
+int call_count = 0;
+
+void reset() {
+    fail_on_error = false;
+    call_count = 0;
+}
+
+void set_fail_on_error(bool enabled) { fail_on_error = enabled; }
+}; // namespace ST_LIB::TestInfoWarning
+
+void InfoWarning::SetMetaData(int line, const char* func, const char* file) {
+    InfoWarning::line = to_string(line);
+    InfoWarning::func = string(func);
+    InfoWarning::file = string(file);
+}
+
+void InfoWarning::InfoWarningTrigger(string format, ...) {
+    (void)format;
+    ST_LIB::TestInfoWarning::call_count++;
+    if (ST_LIB::TestInfoWarning::fail_on_error) {
+        EXPECT_EQ(1, 0);
+    }
+}