Scheduler implementation (#534)

* Add initial implementation of scheduler

* Try to fix -Wuninitialized warning

* Try to remove Wuninitialized warning temporarily

* Remove unnecessary clear of task element

* fix: Readability and todo for validation

* fix: allocate_slot

* Add explenation of sorted_task_ids_, static_assert for the bitmaps

* Remove long_wait_remaining_us_

* fix: better get_at

* fix: better front_id
Compiler would've probably optimized it but it's better to make it explicit

* fix: endianness in pop_front

* Added unlikely to release_slot conditional

* fix: Correct comment explenation

* No bit field insert, it annoyed me

* No more diagnostic ignore

* fix: loss in accuracy by not adding 1: #534 (comment)

* fix: Moved active_task_count_ modification

* Actually set used_bitmap_ in allocate_slot

* only include sources when cross-compiling

* modify CMakeLists inside Tests

* feat: use ugly ifdef guards for host testing purposes

* add sample tests for Scheduler

* add emulated ticking

* test for execution count

* fix: CI

* fix: expected result in test case + explanation

* add compiler specific and CoreMacros

* copy files from LL and CMSIS for TIM Registers

* bridge between LL MMIO Timer and user defined TIMER

* reduce the number of ugly macros

* compile the new mock file

* update test

* enable debugging of tests with TestMate C++ extension

* feat: add DSB and ISB intructions in x86_64

* feat: add NVIC

* add NVIC logic

* feat: use NVIC and remove print

* fix: move extern"C" block to allow using <iostream>

* add Register class to allow for side effects

* feat: use newly created Register class for side effects, neat trick in CNT write

* feat: be as realistic as possible in test, by using CNT++

* feat: remove unnecessary include

* feat: add an __RBIT impl for x86_64

* fix: compilation

* fix: used_bitmap_ -> free_bitmap_

* Added tests check in CI/CD

* fix(ci/cd): output folder

* Added ARM specific instructions to be able to compile in ARM hosts (Mac)

* Fix: compiler barrier for msvc, hopefully works

* Fix: Try to fix -Wchanges-meaning warning

* Fix typo

* Update Inc/MockedDrivers/common.hpp

#534 (comment)

Co-authored-by: Jorge Sáez <125664643+jorgesg82@users.noreply.github.com>

* Actually do the change github didn't do :/

* Fix typo p2

* Fix another typo

* Fix unsigned long - 64bit when testing using LL functions

* Minor fixes to scheduler
Rebased from remotes/origin/MockTim

* Fix: uint32_t* cast of a word instead of the ptr

* Fix: remove active_task_count_ decrement in pop_front()
#534 (comment)

* Try fix Wstrict-aliasing in front_id

* Try fix Wstrict-aliasing again

* Fix typo in allocate_slot

* fix: test UsedBitmap -> FreeBitmap

* used_bitmap_ -> free_bitmap_ in test part 2

* fixed tests discovery

* fix: set_timeout() test

* Add a test, fix the error (kind of)

* Fix: underflow in schedule_next_interval

* Fix: call schedule_next_interval() in start()

* Fix: invert condition, that was stupid

* feat: add test showcasing possible usage within StateMachine logic

* fix: Calculate correct prescaler & add to HALAL.hpp

* Try to fix tests

* try to fix tests part 2

* Try to fix tests part 3

* part 4 - add the wrapper.c to cmake, will this work?

* part 5 - fix cmake stm32h723xx_wrapper.c path

* fix: try to fix tests part 6

* fix: volatile warnings

* fix: active_PSC is now set when PSC is set

* fix: remove outdated comment

* fix: tests part 7, should work now

* fix: remove unecessary if, return if cancelled in cancel_timeout

* Pop all due tasks, several might be due in the same tick

* Revert force-push

* fix: off by one error + configure_timer_for_interval should use uint32_t

* fix: off by one error in schedule_next_interval()

* All 15 tasks in MultipleTasks test, and it works

* Add SameTaskMultipleTimes test, does all 16 tasks too

* larger NUM_TICKS just in case

* fix bug mentioned in previous cancel_timeout()

* fix: id uint32_t -> uint16_t + bug fix

* fix: remove TimerPeripheral::start and Time::start from HALAL::start

* try to fix ci/cd

* try to fix ci/cd part 2

* try to fix ci/cd part 3

* clean up ci/cd fix

* Remove unnecessary warning ignore

* fix: use wrapper instead of interface for stm32h7xx_ll_tim.h

* fix ci/cd and add the other submodule to be automatically added

* fix: actually add stm32h7xx_ll_tim_wrapper.h

* try to fix ci/cd warning

* try fix warnings again

* fix: add ignore Woverflow to stm32h7xx_ll_tim

* fix: remove need to ignore Woverflow warnings

* fix: Remove tim_register_definitions.hpp
We're already including them in stm32h723xx_wrapper

* Fix overflow calculation and a test

* Add link to wiki

* fix: revert .vscode settings change

* revert .vscode settings change part 2

* finish reverting change

---------

Co-authored-by: Gonzalo Sánchez <gonzalosmoya24@gmail.com>
Co-authored-by: Jorge Sáez <jorgeesg82@gmail.com>
Co-authored-by: Jorge Sáez <125664643+jorgesg82@users.noreply.github.com>
Co-authored-by: StefanCostea <stefanxenadu@gmail.com>

Author: 4 people

.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

@@ -11,5 +11,4 @@
       "tinydir.h": "c",
       "dirent.h": "c"
     }
-  }
-  
+}

CMakeLists.txt

@@ -21,10 +21,28 @@ message(STATUS "${PROJECT_NAME} Ethernet: ${USE_ETHERNET}")
 message(STATUS "${PROJECT_NAME} Nucleo:   ${TARGET_NUCLEO}")
 message(STATUS "${PROJECT_NAME} Crosscompiling: ${CMAKE_CROSSCOMPILING}")
 
+if(NOT EXISTS ${CMAKE_CURRENT_LIST_DIR}/STM32CubeH7/Drivers)
+  execute_process(
+    COMMAND git submodule update --init --depth 1
+    WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}
+  )
+endif()
+
+if(NOT EXISTS ${CMAKE_CURRENT_LIST_DIR}/STM32CubeH7/Drivers/CMSIS/Device/ST/STM32H7xx/Include)
+  execute_process(
+    COMMAND git submodule update --init --depth 1 Drivers/CMSIS/Device/ST/STM32H7xx Drivers/STM32H7xx_HAL_Driver
+    WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/STM32CubeH7
+  )
+endif()
 
 if(NOT CMAKE_CROSSCOMPILING)
   message(STATUS "Compiling for simulator")
   add_subdirectory(Tests)
+else()
+  execute_process(
+    COMMAND git submodule update --init --depth 1 Drivers/BSP/Components/lan8742
+    WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/STM32CubeH7
+  )
 endif()
 
 # ============================
@@ -222,6 +240,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,10 +292,13 @@ 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>
+  ${STM32CUBEH7}/Drivers/STM32H7xx_HAL_Driver/Inc
   $<$<BOOL:${CMAKE_CROSSCOMPILING}>:${STM32CUBEH7}/Drivers/STM32H7xx_HAL_Driver/Inc/Legacy>
 
   # LWIP includes 

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,99 @@
+/*
+ * Scheduler.hpp
+ *
+ * Created on: 17 nov. 2025
+ *     Author: Victor (coauthor Stephan)
+ */
+#pragma once
+
+/* Uso del scheduler, descrito en la wiki: https://wiki.hyperloopupv.com/es/firmware/Timing/Scheduler */
+
+#ifndef TESTING_ENV
+#include "stm32h7xx_ll_tim.h"
+#else
+#include "MockedDrivers/stm32h7xx_ll_tim_wrapper.h"
+#endif
+#include <array>
+#include <cstdint>
+#include <functional>
+
+/* NOTE(vic): Esto cambiará pronto */
+#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 uint16_t register_task(uint32_t period_us, callback_t func);
+    static bool unregister_task(uint16_t id);
+
+    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;
+    static void on_timer_update();
+
+#ifndef TESTING_ENV
+    private:
+#endif
+    struct Task {
+        uint32_t next_fire_us{0};
+        callback_t callback{};
+        uint32_t period_us{0};
+        uint16_t id;
+        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 uint32_t current_interval_us_;
+    static uint16_t timeout_idx_;
+
+    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(uint32_t microseconds);
+
+    // 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);
+    }
+};