From 25dc3fb70c19442deee5cade4d94a876912b81b1 Mon Sep 17 00:00:00 2001
From: Astro <astro@spaceboyz.net>
Date: Sun, 8 Sep 2019 02:34:59 +0200
Subject: [PATCH] systick: implement nanosecond precision

---
 firmware/src/board/systick.rs | 28 +++++++++++++++++++---------
 firmware/src/main.rs          |  5 ++---
 2 files changed, 21 insertions(+), 12 deletions(-)

diff --git a/firmware/src/board/systick.rs b/firmware/src/board/systick.rs
index 24bf3db..df04fdb 100644
--- a/firmware/src/board/systick.rs
+++ b/firmware/src/board/systick.rs
@@ -4,15 +4,22 @@ use cortex_m::peripheral::{SYST, syst::SystClkSource};
 use cortex_m_rt::exception;
 use bare_metal::CriticalSection;
 
-const SYSTICK_RATE: u32 = 250;
-
 static mut TIME: Mutex<RefCell<u64>> = Mutex::new(RefCell::new(0));
+/// In HZ
+const RATE: u32 = 10;
+/// Period between to interrupts in ns
+const INTERVAL: u64 = 1_000_000 / RATE as u64;
 
-pub fn init(cs: &CriticalSection) {
+fn syst() -> &'static mut SYST {
     #[allow(mutable_transmutes)]
-    let syst: &mut SYST = unsafe { core::mem::transmute(&*SYST::ptr()) };
+    unsafe { core::mem::transmute(&*SYST::ptr()) }
+}
+
+pub fn init(_cs: &CriticalSection) {
+    let syst = syst();
+    // syst.set_clock_source(SystClkSource::Core);
     syst.set_clock_source(SystClkSource::External);
-    syst.set_reload(100 * SYST::get_ticks_per_10ms() / SYSTICK_RATE);
+    syst.set_reload(100 * SYST::get_ticks_per_10ms() / RATE);
     syst.clear_current();
     syst.enable_interrupt();
     syst.enable_counter();
@@ -20,14 +27,17 @@ pub fn init(cs: &CriticalSection) {
 
 #[exception]
 unsafe fn SysTick() {
-    let interval = u64::from(1000 / SYSTICK_RATE);
     cortex_m::interrupt::free(|cs| {
-        TIME.borrow(cs).replace_with(|time| *time + interval);
+        TIME.borrow(cs).replace_with(|time| *time + INTERVAL);
     });
 }
 
 pub fn get_time() -> u64 {
-    cortex_m::interrupt::free(|cs| {
+    let base = cortex_m::interrupt::free(|cs| {
         *unsafe { &mut TIME }.borrow(cs).borrow()
-    })
+    });
+    let syst_current = u64::from(SYST::get_current());
+    let syst_reload = u64::from(SYST::get_reload());
+    let precise = INTERVAL - (INTERVAL * syst_current / syst_reload);
+    base + u64::from(precise)
 }
diff --git a/firmware/src/main.rs b/firmware/src/main.rs
index eb0f6bb..66fa4fd 100644
--- a/firmware/src/main.rs
+++ b/firmware/src/main.rs
@@ -181,7 +181,7 @@ fn main() -> ! {
     // if a socket has sent the latest data
     let mut socket_pending = [false; 8];
     loop {
-        adc.data_ready()
+        let _ = adc.data_ready()
             .and_then(|channel|
                 channel.map(|channel|
                     adc.read_data().map(|new_data| {
@@ -199,7 +199,6 @@ fn main() -> ! {
                 for p in socket_pending.iter_mut() {
                     *p = true;
                 }
-
             });
         for (&tcp_handle, pending) in handles.iter().zip(socket_pending.iter_mut()) {
             let socket = &mut *sockets.get::<TcpSocket>(tcp_handle);
@@ -223,7 +222,7 @@ fn main() -> ! {
                 *pending = false;
             }
         }
-        match iface.poll(&mut sockets, Instant::from_millis(get_time() as i64)) {
+        match iface.poll(&mut sockets, Instant::from_millis((get_time() / 1000) as i64)) {
             Ok(_) => (),
             Err(e) => println!("poll error: {}", e)
         }