1 files changed, 37 insertions, 17 deletions

diff --git a/src/bin/luchie.rs b/src/bin/luchie.rs
index be89fa7..5279f95 100755
--- a/src/bin/luchie.rs
+++ b/src/bin/luchie.rs
@@ -29,6 +29,40 @@ use usbd_human_interface_device::{
 };
 use usbd_serial::{SerialPort, USB_CLASS_CDC};
 
+// Relies on cortex-m
+struct SysTimer {
+    syst: pac::SYST,
+    last: u32,
+    err: u32,
+    reload: u32,
+    ticks_per_microsec: u32,
+}
+
+impl SysTimer {
+    pub fn new(mut syst: pac::SYST, ticks_per_microsec: u32) -> Self {
+        syst.set_clock_source(cortex_m::peripheral::syst::SystClkSource::Core);
+        let reload = 0xffffff;
+        syst.set_reload(reload);
+        syst.clear_current();
+        syst.enable_counter();
+
+        Self { syst, last: 0, err: 0, reload, ticks_per_microsec }
+    }
+
+    pub fn update(&mut self) -> u32 {
+        let mut time = self.reload - cortex_m::peripheral::SYST::get_current();
+        if self.syst.has_wrapped() {
+            time += self.reload + 1;
+        }
+        let elapsed = time.wrapping_sub(self.last) + self.err;
+        self.err = elapsed % self.ticks_per_microsec;
+        let elapsed = elapsed / self.ticks_per_microsec;
+        self.last = time & self.reload; // FIXME: might not be 2^x - 1
+
+        elapsed
+    }
+}
+
 #[entry]
 fn main() -> ! {
     let cp = cortex_m::Peripherals::take().unwrap();
@@ -69,12 +103,7 @@ fn main() -> ! {
     logln!("🐁 luchie starting…");
 
     logln!("⏲️ init timer");
-    let mut syst = cp.SYST;
-    syst.set_clock_source(cortex_m::peripheral::syst::SystClkSource::Core);
-    let syst_reload = 0xffffff;
-    syst.set_reload(syst_reload);
-    syst.clear_current();
-    syst.enable_counter();
+    let mut sys_timer = SysTimer::new(cp.SYST, clocks.sysclk().to_MHz());
 
     // cirque spi connections to spi1:
     //
@@ -150,18 +179,9 @@ fn main() -> ! {
 
     let mut abs_to_rel = AbsToRel::new();
     let mut trackball = TrackBall::new(1);
-    let ticks_per_microsec = clocks.sysclk().to_MHz();
-    let mut last_time = 0;
-    let mut time_error = 0;
     loop {
-        let mut time = syst_reload - cortex_m::peripheral::SYST::get_current();
-        if syst.has_wrapped() {
-            time += syst_reload + 1;
-        }
-        let elapsed = time.wrapping_sub(last_time) + time_error;
-        time_error = elapsed % ticks_per_microsec;
-        let elapsed = elapsed / ticks_per_microsec;
-        last_time = time & syst_reload;
+        let elapsed = sys_timer.update();
+
         usb_dev.poll(&mut [&mut serial, &mut mouse]);
 
         let mut buf = [0u8; 64];