1 files changed, 74 insertions, 0 deletions
diff --git a/src/rtc.rs b/src/rtc.rs
new file mode 100644
index 0000000..79e450a
--- /dev/null
+++ b/src/rtc.rs
@@ -0,0 +1,74 @@
+use core::sync::atomic::{AtomicUsize, Ordering};
+use log;
+use trinket_m0::{clock::GenericClockController, RTC};
+
+struct Clock(AtomicUsize);
+impl Clock {
+    const fn new() -> Self {
+        Self(AtomicUsize::new(0))
+    }
+
+    fn set(&self, millis: usize) {
+        self.0.store(millis, Ordering::SeqCst)
+    }
+
+    // Slightly less than 1ms, due to using a 32,768Hz clock, we can't
+    // hit exactly 1ms, so we shoot for a bit under.
+    fn millis(&self) -> usize {
+        self.0.load(Ordering::SeqCst)
+    }
+}
+
+static CLOCK: Clock = Clock::new();
+
+// Set to run every ~500µs.
+static COUNTER: u32 = 16; // 32 ticks requires 1024 cycles at 32,768Hz for 1 second.
+
+pub fn setup(mut rtc: RTC, clocks: &mut GenericClockController) -> impl FnMut() {
+    let rtc_clock = &clocks.gclk1();
+    clocks.rtc(&rtc_clock);
+
+    rtc.mode0().ctrl.write(|w| w.swrst().set_bit());
+    while rtc.mode0().status.read().syncbusy().bit_is_set() {}
+
+    rtc.mode0().ctrl.write(|w| {
+        w.mode().count32();
+
+        // Neither the prescaler nor matchlr values seem to work. Not
+        // sure why.
+        //w.prescaler().div1024();
+        w.matchclr().set_bit() // Reset on match for periodic
+    });
+
+    rtc.mode0().comp[0].write(|w| unsafe { w.bits(COUNTER) });
+    rtc.mode0().intflag.write(|w| w.cmp0().set_bit());
+    rtc.mode0().intenset.write(|w| w.cmp0().set_bit());
+
+    // Enable the RTC and wait for sync.
+    rtc.mode0().ctrl.write(|w| w.enable().set_bit());
+    while rtc.mode0().status.read().syncbusy().bit_is_set() {}
+
+    move || handler(&mut rtc)
+}
+
+pub fn millis() -> usize {
+    CLOCK.millis()
+}
+
+fn handler(rtc: &mut RTC) {
+    // FIXME: matchclr doesn't seem to work to reset the counter?
+    rtc.mode0().count.write(|w| unsafe { w.bits(0) });
+    rtc.mode0().intflag.write(|w| w.cmp0().set_bit());
+
+    static mut TICKS: usize = 0;
+    static mut ADD: bool = false;
+    unsafe {
+        if ADD {
+            TICKS += 1;
+            CLOCK.set(TICKS);
+        }
+        ADD = !ADD;
+
+        log::logger().flush();
+    }
+}