feat(firmware/c_board): Replace DWT delay helper with TIM2/TIM9 timer service

firmware/c_board/app/src/app.cpp

@@ -15,6 +15,7 @@
 #include "firmware/c_board/app/src/spi/bmi088/accel.hpp"
 #include "firmware/c_board/app/src/spi/bmi088/gyro.hpp"
 #include "firmware/c_board/app/src/spi/spi.hpp"
+#include "firmware/c_board/app/src/timer/timer.hpp"
 #include "firmware/c_board/app/src/uart/uart.hpp"
 #include "firmware/c_board/app/src/usb/vendor.hpp"
 #include "firmware/c_board/app/src/utility/boot_mailbox.hpp"
@@ -34,9 +35,10 @@ App::App() {
     SystemClock_Config();
     utility::boot_mailbox.clear();
 
-    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
-    DWT->CYCCNT = 0;
-    DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
+    // TIM9 must be initialized before TIM2.
+    MX_TIM9_Init();
+    MX_TIM2_Init();
+    timer::timer.init();
 
     MX_GPIO_Init();
     MX_DMA_Init();

firmware/c_board/app/src/spi/bmi088/accel.hpp

@@ -10,7 +10,7 @@
 #include "core/src/utility/assert.hpp"
 #include "firmware/c_board/app/src/spi/bmi088/base.hpp"
 #include "firmware/c_board/app/src/spi/spi.hpp"
-#include "firmware/c_board/app/src/timer/delay.hpp"
+#include "firmware/c_board/app/src/timer/timer.hpp"
 #include "firmware/c_board/app/src/usb/vendor.hpp"
 #include "firmware/c_board/app/src/utility/lazy.hpp"
 
@@ -75,11 +75,11 @@ class Accelerometer final
 
         // Dummy read to switch accelerometer to SPI mode.
         read_register(RegisterAddress::kAccChipId);
-        timer::delay(1ms);
+        timer::timer->spin_wait(1ms);
 
         // Reset all registers to reset value.
         write_register(RegisterAddress::kAccSoftReset, 0xB6);
-        timer::delay(1ms);
+        timer::timer->spin_wait(1ms);
 
         // "Who am I" check.
         core::utility::assert_always(read_and_confirm(RegisterAddress::kAccChipId, 0x1E));
@@ -100,7 +100,7 @@ class Accelerometer final
         core::utility::assert_always(write_and_confirm(RegisterAddress::kAccPwrConf, 0x00));
         // Turn on the accelerometer.
         core::utility::assert_always(write_and_confirm(RegisterAddress::kAccPwrCtrl, 0x04));
-        timer::delay(1ms); // Datasheet: wait >=450us after entering normal mode
+        timer::timer->spin_wait(1ms); // Datasheet: wait >=450us after entering normal mode
 
         spi_.unlock();
     }

firmware/c_board/app/src/spi/bmi088/base.hpp

@@ -11,7 +11,7 @@
 
 #include "core/src/utility/assert.hpp"
 #include "firmware/c_board/app/src/spi/spi.hpp"
-#include "firmware/c_board/app/src/timer/delay.hpp"
+#include "firmware/c_board/app/src/timer/timer.hpp"
 
 namespace librmcs::firmware::spi::bmi088 {
 
@@ -53,7 +53,7 @@ class Bmi088Base : private SpiModule {
         for (int i = kMaxRetries; i-- > 0;) {
             if (read_register(addr) == expected)
                 return true;
-            timer::delay(1ms);
+            timer::timer->spin_wait(1ms);
         }
         return false;
     }
@@ -63,7 +63,7 @@ class Bmi088Base : private SpiModule {
 
         for (int i = kMaxRetries; i-- > 0;) {
             write_register(addr, val);
-            timer::delay(1ms);
+            timer::timer->spin_wait(1ms);
             if (read_register(addr) == val)
                 return true;
         }

firmware/c_board/app/src/spi/bmi088/gyro.hpp

@@ -10,7 +10,7 @@
 #include "core/src/utility/assert.hpp"
 #include "firmware/c_board/app/src/spi/bmi088/base.hpp"
 #include "firmware/c_board/app/src/spi/spi.hpp"
-#include "firmware/c_board/app/src/timer/delay.hpp"
+#include "firmware/c_board/app/src/timer/timer.hpp"
 #include "firmware/c_board/app/src/usb/vendor.hpp"
 #include "firmware/c_board/app/src/utility/lazy.hpp"
 
@@ -74,7 +74,7 @@ class Gyroscope final
 
         // Reset all registers to reset value.
         write_register(RegisterAddress::kGyroSoftReset, 0xB6);
-        timer::delay(30ms);
+        timer::timer->spin_wait(30ms);
 
         // "Who am I" check.
         core::utility::assert_always(read_and_confirm(RegisterAddress::kGyroChipId, 0x0F));

firmware/c_board/app/src/timer/delay.cpp

@@ -1,17 +1,18 @@
-#include "firmware/c_board/app/src/timer/delay.hpp"
-
 #include <chrono>
 #include <cstdint>
 
 #include <stm32f4xx_hal.h>
 
 #include "firmware/c_board/app/src/led/led.hpp"
+#include "firmware/c_board/app/src/timer/timer.hpp"
 
 namespace librmcs::firmware::timer {
 
-// The STM32 DWT (Data Watchpoint and Trace) unit is used to rewrite the Hal_Delay function to
-// ensure that it works when interrupts are disabled, while significantly improving accuracy.
-extern "C" void HAL_Delay(uint32_t delay) { timer::delay(std::chrono::milliseconds(delay)); }
+// Rewrite the Hal_Delay function to ensure that it works when interrupts are disabled,
+// while significantly improving accuracy.
+extern "C" void HAL_Delay(uint32_t delay) {
+    timer::timer->spin_wait(timer::Timer::to_duration48_checked(std::chrono::milliseconds(delay)));
+}
 
 // Hack this useless function to perform regular low-priority tasks, eliminating the need for a
 // dedicated timer peripheral.

firmware/c_board/app/src/timer/timer.hpp

@@ -0,0 +1,146 @@
+#pragma once
+
+#include <chrono>
+#include <concepts>
+#include <cstdint>
+#include <limits>
+#include <ratio>
+#include <type_traits>
+
+#include <main.h>
+#include <tim.h>
+
+#include "core/src/utility/assert.hpp"
+#include "firmware/c_board/app/src/utility/lazy.hpp"
+
+namespace librmcs::firmware::timer {
+
+class Timer {
+public:
+    using Lazy = utility::Lazy<Timer>;
+
+    static constexpr uint32_t kClockFrequency = 168'000'000 / 2 / 21;
+    using TickPeriod = std::ratio<1, kClockFrequency>;
+
+    // 1/4 us
+    using Duration = std::chrono::duration<uint32_t, TickPeriod>;
+    using Duration48 = std::chrono::duration<uint64_t, TickPeriod>;
+
+    using TimePoint = std::chrono::time_point<uint32_t, Duration>;
+    using TimePoint48 = std::chrono::time_point<uint64_t, Duration48>;
+
+    // Keep the true-window at least half-cycle for stateless expiration checks.
+    static constexpr uint64_t kMaxDurationTicks = uint64_t{1} << 31;
+    static constexpr uint64_t kMaxDuration48Ticks = uint64_t{1} << 47;
+
+    static constexpr uint64_t kCounter48Mask = 0xFFFFFFFFFFFFULL;
+
+    static constexpr TIM_HandleTypeDef* kTimerLow = &htim2;
+    static constexpr TIM_HandleTypeDef* kTimerHigh = &htim9;
+
+    Timer()
+        : timer_counter_high_(kTimerHigh->Instance->CNT)
+        , timer_counter_low_(kTimerLow->Instance->CNT) {
+        core::utility::assert_always(
+            HAL_TIM_Base_Start(kTimerHigh) == HAL_OK && HAL_TIM_Base_Start(kTimerLow) == HAL_OK);
+    }
+
+    TimePoint timepoint() const { return TimePoint{Duration{timer_counter_low_}}; }
+
+    TimePoint48 timepoint48() const {
+        uint32_t hi1, hi2, lo;
+        do {
+            hi1 = timer_counter_high_;
+            lo = timer_counter_low_;
+            hi2 = timer_counter_high_;
+        } while (hi1 != hi2);
+
+        return TimePoint48{Duration48{(uint64_t{hi2} << 32) | lo}};
+    }
+
+    [[nodiscard]] bool check_expired(TimePoint start_point, Duration delay) const {
+        core::utility::assert_debug(delay.count() <= kMaxDurationTicks);
+
+        const uint32_t start_ticks = start_point.time_since_epoch().count();
+        const uint32_t now_ticks = timepoint().time_since_epoch().count();
+        const Duration elapsed_duration{static_cast<uint32_t>(now_ticks - start_ticks)};
+        return elapsed_duration >= delay;
+    }
+
+    [[nodiscard]] bool check_expired(TimePoint48 start_point, Duration48 delay) const {
+        core::utility::assert_debug(delay.count() <= kMaxDuration48Ticks);
+
+        const uint64_t start_ticks = start_point.time_since_epoch().count() & kCounter48Mask;
+        const uint64_t now_ticks = timepoint48().time_since_epoch().count() & kCounter48Mask;
+        const Duration48 elapsed_duration{(now_ticks - start_ticks) & kCounter48Mask};
+        return elapsed_duration >= delay;
+    }
+
+    void spin_wait(Duration48 delay) const {
+        core::utility::assert_debug(delay.count() <= kMaxDuration48Ticks);
+
+        const TimePoint48 start = timepoint48();
+        while (!check_expired(start, delay))
+            ;
+    }
+
+    template <std::integral Rep, typename Period>
+    [[nodiscard]] static constexpr Duration
+        to_duration_checked(std::chrono::duration<Rep, Period> duration) {
+        static_assert(Period::num > 0 && Period::den > 0);
+
+        const uint64_t count = count_to_u64_checked(duration.count());
+        using InputDuration = std::chrono::duration<uint64_t, Period>;
+        const InputDuration duration_u64{count};
+
+        constexpr Duration max_duration{static_cast<uint32_t>(kMaxDurationTicks)};
+        const InputDuration max_input_duration =
+            std::chrono::duration_cast<InputDuration>(max_duration);
+
+        core::utility::assert_debug(duration_u64 <= max_input_duration);
+        const Duration delay_duration = std::chrono::ceil<Duration>(duration_u64);
+
+        core::utility::assert_debug(delay_duration.count() <= kMaxDurationTicks);
+        return delay_duration;
+    }
+
+    template <std::integral Rep, typename Period>
+    [[nodiscard]] static constexpr Duration48
+        to_duration48_checked(std::chrono::duration<Rep, Period> duration) {
+        static_assert(Period::num > 0 && Period::den > 0);
+
+        const uint64_t count = count_to_u64_checked(duration.count());
+        using InputDuration = std::chrono::duration<uint64_t, Period>;
+        const InputDuration duration_u64{count};
+
+        constexpr Duration48 max_duration{kMaxDuration48Ticks};
+        const InputDuration max_input_duration =
+            std::chrono::duration_cast<InputDuration>(max_duration);
+
+        core::utility::assert_debug(duration_u64 <= max_input_duration);
+        const Duration48 delay_duration = std::chrono::ceil<Duration48>(duration_u64);
+
+        core::utility::assert_debug(delay_duration.count() <= kMaxDuration48Ticks);
+        return delay_duration;
+    }
+
+private:
+    template <std::integral Rep>
+    [[nodiscard]] static uint64_t count_to_u64_checked(Rep count) {
+        if constexpr (std::is_signed_v<Rep>)
+            core::utility::assert_debug(count >= 0);
+
+        if constexpr (sizeof(Rep) > sizeof(uint64_t)) {
+            core::utility::assert_debug(
+                count <= static_cast<Rep>(std::numeric_limits<uint64_t>::max()));
+        }
+        return static_cast<uint64_t>(count);
+    }
+
+    const volatile uint32_t& timer_counter_high_;
+    const volatile uint32_t& timer_counter_low_;
+};
+
+inline constinit Timer::Lazy timer;
+
+} // namespace librmcs::firmware::timer

firmware/c_board/bsp/cubemx/Core/Inc/tim.h

@@ -32,13 +32,19 @@ extern "C" {
 #include "stm32f4xx_hal_tim.h" // IWYU pragma: export
 /* USER CODE END Includes */
 
+extern TIM_HandleTypeDef htim2;
+
 extern TIM_HandleTypeDef htim5;
 
+extern TIM_HandleTypeDef htim9;
+
 /* USER CODE BEGIN Private defines */
 
 /* USER CODE END Private defines */
 
+void MX_TIM2_Init(void);
 void MX_TIM5_Init(void);
+void MX_TIM9_Init(void);
 
 void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);