Scheduler implementation

.github/workflows/tests.yml

@@ -0,0 +1,32 @@
+name: Run tests
+
+on:
+  workflow_dispatch:
+  pull_request:
+    branches: [ development ]
+
+jobs:
+  tests:
+    runs-on: ubuntu-latest
+
+    steps:
+      - name: Checkout repository
+        uses: actions/checkout@v4
+
+      - name: Install build dependencies
+        run: |
+          sudo apt-get update
+          sudo apt-get install -y cmake ninja-build build-essential
+
+      - name: Configure (CMake)
+        run: |
+          cmake --preset simulator
+
+      - name: Build
+        run: |
+          cmake --build out/build/simulator
+
+      - name: Run tests (ctest)
+        working-directory: out/build/simulator
+        run: |
+          ctest --output-on-failure

.vscode/settings.json

@@ -9,7 +9,27 @@
     "files.associations": {
       "mem.h": "c",
       "tinydir.h": "c",
-      "dirent.h": "c"
-    }
+      "dirent.h": "c",
+      "compare": "cpp",
+      "cstdint": "cpp",
+      "iostream": "cpp"
+    },
+    "testMate.cpp.debug.configTemplate": {
+    "type": "cppdbg",
+    "MIMode": "gdb",
+    "program": "${exec}",
+    "args": "${args}",
+    "cwd": "${cwd}",
+
+    "externalConsole": false,
+    // This setupCommands block often fixes generic "pause" issues in GDB
+    "setupCommands": [
+        {
+            "description": "Enable pretty-printing for gdb",
+            "text": "-enable-pretty-printing",
+            "ignoreFailures": true
+        }
+    ]
+}
   }
 

CMakeLists.txt

@@ -222,6 +222,12 @@ add_library(${STLIB_LIBRARY} STATIC
   $<$<AND:$<BOOL:${CMAKE_CROSSCOMPILING}>,$<BOOL:${USE_ETHERNET}>>:${STLIB_HIGH_CPP_ETH}>
 
   $<$<BOOL:${CMAKE_CROSSCOMPILING}>:${CMAKE_CURRENT_LIST_DIR}/Src/ST-LIB.cpp>
+
+  $<$<NOT:$<BOOL:${CMAKE_CROSSCOMPILING}>>:${CMAKE_CURRENT_LIST_DIR}/Src/HALAL/Services/Time/Scheduler.cpp>
+  $<$<NOT:$<BOOL:${CMAKE_CROSSCOMPILING}>>:${CMAKE_CURRENT_LIST_DIR}/Src/MockedDrivers/mocked_ll_tim.cpp>
+  $<$<NOT:$<BOOL:${CMAKE_CROSSCOMPILING}>>:${CMAKE_CURRENT_LIST_DIR}/Src/MockedDrivers/mocked_system_stm32h7xx.c>
+  $<$<NOT:$<BOOL:${CMAKE_CROSSCOMPILING}>>:${CMAKE_CURRENT_LIST_DIR}/Src/MockedDrivers/stm32h723xx_wrapper.c>
+  $<$<NOT:$<BOOL:${CMAKE_CROSSCOMPILING}>>:${CMAKE_CURRENT_LIST_DIR}/Src/MockedDrivers/NVIC.cpp>
 )
 
 set_target_properties(${STLIB_LIBRARY} PROPERTIES
@@ -268,8 +274,10 @@ target_compile_options(${STLIB_LIBRARY} PRIVATE
 
 target_include_directories(${STLIB_LIBRARY} PUBLIC
   # CMSIS + HAL
-  $<$<BOOL:${CMAKE_CROSSCOMPILING}>:${STM32CUBEH7}/Drivers/CMSIS/Device/ST/STM32H7xx/Include>
-  $<$<BOOL:${CMAKE_CROSSCOMPILING}>:${STM32CUBEH7}/Drivers/CMSIS/Include>
+  #$<$<BOOL:${CMAKE_CROSSCOMPILING}>:${STM32CUBEH7}/Drivers/CMSIS/Device/ST/STM32H7xx/Include>
+  #$<$<BOOL:${CMAKE_CROSSCOMPILING}>:${STM32CUBEH7}/Drivers/CMSIS/Include>
+  ${STM32CUBEH7}/Drivers/CMSIS/Device/ST/STM32H7xx/Include
+  ${STM32CUBEH7}/Drivers/CMSIS/Include
   $<$<BOOL:${CMAKE_CROSSCOMPILING}>:${STM32CUBEH7}/Drivers/CMSIS/Core_A/Include>
   $<$<BOOL:${CMAKE_CROSSCOMPILING}>:${STM32CUBEH7}/Drivers/STM32H7xx_HAL_Driver/Inc>
   $<$<BOOL:${CMAKE_CROSSCOMPILING}>:${STM32CUBEH7}/Drivers/STM32H7xx_HAL_Driver/Inc/Legacy>

Inc/HALAL/HALAL.hpp

@@ -21,6 +21,7 @@
 #include "HALAL/Services/PWM/DualPhasedPWM/DualPhasedPWM.hpp"
 
 #include "HALAL/Services/Time/Time.hpp"
+#include "HALAL/Services/Time/Scheduler.hpp"
 #include "HALAL/Services/Time/RTC.hpp"
 
 #include "HALAL/Services/InputCapture/InputCapture.hpp"

Inc/HALAL/Services/Time/Scheduler.hpp

@@ -0,0 +1,113 @@
+/*
+ * Scheduler.hpp
+ *
+ * Created on: 17 nov. 2025
+ *     Author: Victor (coauthor Stephan)
+ */
+#pragma once
+
+#ifndef TESTING_ENV
+    #include "stm32h7xx_ll_tim.h"
+#else
+    #include "MockedDrivers/ll_tim_interface.h"
+#endif
+#include <array>
+#include <cstdint>
+#include <functional>
+
+/* NOTE(vic): Pido perdón a Boris pero es la mejor manera que se me ha ocurrido hacer esto
+ *  Cambiar el SCHEDULER_TIMER_IDX si es mejor usar otro timer que no sea TIM2
+ */
+#ifndef SCHEDULER_TIMER_IDX
+# define SCHEDULER_TIMER_IDX 2
+#endif
+
+#define glue_(a,b) a ## b
+#define glue(a,b) glue_(a,b)
+#define SCHEDULER_TIMER_BASE glue(TIM, glue(SCHEDULER_TIMER_IDX, _BASE))
+
+    // Used to reserve a TimerPeripheral
+#ifndef TESTING_ENV
+#include "stm32h7xx_hal_tim.h"
+#define SCHEDULER_HAL_TIM glue(htim, SCHEDULER_TIMER_IDX)
+extern TIM_HandleTypeDef SCHEDULER_HAL_TIM;
+#endif
+
+struct Scheduler {
+    using callback_t = void (*)();
+    static constexpr uint32_t INVALID_ID = 0xFFu;
+
+    static void start();
+    static void update();
+    static inline uint64_t get_global_tick();
+
+    static inline uint8_t register_task(uint32_t period_us, callback_t func) {
+        if(period_us == 0) [[unlikely]] period_us = 1;
+        return register_task(period_us, func, true);
+    }
+    static bool unregister_task(uint8_t id);
+
+    static inline uint8_t set_timeout(uint32_t microseconds, callback_t func) {
+        if(microseconds == 0) [[unlikely]] microseconds = 1;
+        return register_task(microseconds, func, false);
+    }
+    static inline void cancel_timeout(uint8_t id) {
+        /* NOTE: This does not fix this case:
+          1. id = set_timeout(x, func)
+          2. timeout ends, func gets called and removed internally
+          3. id_2 = set_timeout(y, func_2) // id will be equal to id_2
+          4. clear_timeout(id) -> will remove the second timeout
+         */
+        if(tasks_[id].repeating) return;
+        unregister_task(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;
+    static void on_timer_update();
+
+#ifndef TESTING_ENV
+    private:
+#endif
+    struct Task {
+        uint64_t next_fire_us{0};
+        callback_t callback{};
+        uint32_t period_us{0};
+        bool repeating{false};
+    };
+
+    static constexpr std::size_t kMaxTasks = 16;
+    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
+
+    static std::array<Task, kMaxTasks> tasks_;
+    static_assert(kMaxTasks == 16, "kMaxTasks must be 16, if more is needed, sorted_task_ids_ must change");
+    /* sorted_task_ids_ is a sorted queue with 4bits for each id in the scheduler's current ids */
+    static uint64_t sorted_task_ids_;
+
+    static uint32_t active_task_count_;
+    static_assert(kMaxTasks <= 32, "kMaxTasks must be <= 32, if more is needed, the bitmaps must change");
+    static uint32_t ready_bitmap_;
+    static uint32_t free_bitmap_;
+    static uint64_t global_tick_us_;
+    static uint64_t current_interval_us_;
+
+    static inline uint8_t allocate_slot();
+    static inline void release_slot(uint8_t id);
+    static void insert_sorted(uint8_t id);
+    static void remove_sorted(uint8_t id);
+    static void schedule_next_interval();
+    static inline void configure_timer_for_interval(uint64_t microseconds);
+    static uint8_t register_task(uint32_t period_us, callback_t func, bool repeating);
+
+    // 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 inline void global_timer_disable();
+    static inline void global_timer_enable();
+};

Inc/MockedDrivers/NVIC.hpp

@@ -0,0 +1,34 @@
+#pragma once
+
+#include "stm32h723xx_wrapper.h"
+#include "MockedDrivers/common.hpp"
+#include "MockedDrivers/compiler_specific.hpp"
+
+class NVIC_Type
+{
+    public:
+    volatile uint32_t ISER[8U];               /*!< Offset: 0x000 (R/W)  Interrupt Set Enable Register */
+             uint32_t RESERVED0[24U];
+    volatile uint32_t ICER[8U];               /*!< Offset: 0x080 (R/W)  Interrupt Clear Enable Register */
+             uint32_t RESERVED1[24U];
+    volatile uint32_t ISPR[8U];               /*!< Offset: 0x100 (R/W)  Interrupt Set Pending Register */
+             uint32_t RESERVED2[24U];
+    volatile uint32_t ICPR[8U];               /*!< Offset: 0x180 (R/W)  Interrupt Clear Pending Register */
+             uint32_t RESERVED3[24U];
+    volatile uint32_t IABR[8U];               /*!< Offset: 0x200 (R/W)  Interrupt Active bit Register */
+             uint32_t RESERVED4[56U];
+    volatile uint8_t  IP[240U];               /*!< Offset: 0x300 (R/W)  Interrupt Priority Register (8Bit wide) */
+             uint32_t RESERVED5[644U];
+    volatile uint32_t STIR;                   /*!< Offset: 0xE00 ( /W)  Software Trigger Interrupt Register */
+};
+
+extern NVIC_Type* NVIC;
+
+extern "C"{
+void NVIC_EnableIRQ(IRQn_Type IRQn);
+
+uint32_t NVIC_GetEnableIRQ(IRQn_Type IRQn);
+
+void NVIC_DisableIRQ(IRQn_Type IRQn);
+
+}

Inc/MockedDrivers/Register.hpp

@@ -0,0 +1,117 @@
+#pragma once
+#include <cstdint>
+#include <iostream>
+
+
+template<typename EnumType, EnumType Reg>
+struct RegisterTraits {
+    static void write(uint32_t& target, uint32_t val) {
+        target = val; 
+    }
+};
+
+
+template<typename EnumType, EnumType Reg>
+class RegisterBase {
+public:
+    uint32_t reg = 0;
+
+    RegisterBase() = default;
+    RegisterBase(uint32_t val) : reg(val) {}
+
+    RegisterBase& operator=(uint32_t val) {
+        set(val);
+        return *this;
+    }
+    RegisterBase& operator+=(uint32_t val){
+        set(reg+val);
+        return *this;
+    }
+    RegisterBase& operator-=(uint32_t val){
+        set(reg-val);
+        return *this;
+    }
+
+    RegisterBase& operator&=(uint32_t mask) {
+        set(reg & mask); 
+        return *this;
+    }
+
+    RegisterBase& operator|=(uint32_t mask) {
+        set(reg | mask); 
+        return *this;
+    }
+
+    RegisterBase& operator^=(uint32_t mask) {
+        set(reg ^ mask); 
+        return *this;
+    }
+    RegisterBase& operator<<=(int shift) {
+        set(reg << shift); // Triggers Traits
+        return *this;
+    }
+
+    RegisterBase& operator>>=(int shift) {
+        set(reg >> shift); // Triggers Traits
+        return *this;
+    }
+
+    // --- Shift Read (Does NOT modify Register) ---
+    // Usage: uint32_t val = REG >> 4;
+    uint32_t operator<<(int shift) const {
+        return reg << shift;
+    }
+
+    uint32_t operator>>(int shift) const {
+        return reg >> shift;
+    }
+    RegisterBase& operator++() {
+        set(reg + 1);
+        return *this;
+    }
+
+    // --- Postfix Increment (REG++) ---
+    // int argument is a dummy flag for C++ to distinguish postfix
+    uint32_t operator++(int) {
+        uint32_t old_val = reg;
+        set(reg + 1);
+        return old_val; 
+    }
+
+    RegisterBase& operator--() {
+        set(reg - 1);
+        return *this;
+    }
+
+    uint32_t operator--(int) {
+        uint32_t old_val = reg;
+        set(reg - 1);
+        return old_val;
+    }
+    /**
+     * COMPARISON
+     */
+    bool operator!=(uint32_t val) const {
+        return reg != val;
+    }
+    bool operator==(uint32_t val) const {
+        return reg == val;
+    }
+    /**
+     * BITWISE COMPARISONS
+     */
+    operator uint32_t(){
+        return reg;
+    }
+    operator uint32_t() const volatile {
+        return reg;
+    }
+    operator uint32_t() const {
+        return reg;
+    }
+
+protected:
+    void set(uint32_t val) {
+        RegisterTraits<EnumType, Reg>::write(this->reg, val);
+    }
+};