From 253f4410eef876ec5ee9973a6430db989bc6ad69 Mon Sep 17 00:00:00 2001
From: linuswck <linuswck@m-labs.hk>
Date: Thu, 10 Oct 2024 13:31:20 +0800
Subject: [PATCH] sys_timer: Add fn to retrieve ts in us resolution

---
 src/device/sys_timer.rs | 15 ++++++++++++++-
 1 file changed, 14 insertions(+), 1 deletion(-)

diff --git a/src/device/sys_timer.rs b/src/device/sys_timer.rs
index e4568ca..4c35a87 100644
--- a/src/device/sys_timer.rs
+++ b/src/device/sys_timer.rs
@@ -2,7 +2,7 @@ use core::{cell::RefCell, ops::Deref};
 
 use cortex_m::{interrupt::Mutex, peripheral::syst::SystClkSource};
 use cortex_m_rt::exception;
-use stm32f4xx_hal::{pac::SYST, rcc::Clocks};
+use stm32f4xx_hal::{pac::{SYST, Peripherals}, rcc::Clocks};
 
 /// Rate in Hz
 const TIMER_RATE: u32 = 1000;
@@ -10,9 +10,13 @@ const TIMER_RATE: u32 = 1000;
 const TIMER_DELTA: u32 = 1000 / TIMER_RATE;
 /// Elapsed time in milliseconds
 static TIMER_MS: Mutex<RefCell<u32>> = Mutex::new(RefCell::new(0));
+static mut US_COUNT: u32 = 168;
 
 /// Setup SysTick exception
 pub fn setup(mut syst: SYST, clocks: Clocks) {
+    unsafe  {
+        US_COUNT = clocks.hclk().to_MHz()
+    }
     syst.set_clock_source(SystClkSource::Core);
     syst.set_reload(clocks.hclk().to_Hz() / TIMER_RATE - 1);
     syst.enable_counter();
@@ -32,6 +36,15 @@ pub fn now() -> u32 {
     cortex_m::interrupt::free(|cs| *TIMER_MS.borrow(cs).borrow().deref())
 }
 
+/// Obtain current time in milliseconds + microseconds
+pub fn now_precise() -> (u32, u32) {
+    let ms = now();
+    unsafe {
+        let us = (Peripherals::steal().STK.load.read().bits() - Peripherals::steal().STK.val.read().bits()) / US_COUNT;
+        (ms, us)
+    }
+}
+
 /// block for `amount` milliseconds
 pub fn sleep(amount: u32) {
     if amount == 0 {