1 files changed, 68 insertions, 0 deletions
diff --git a/ble/src/rtc.rs b/ble/src/rtc.rs
new file mode 100644
index 0000000..8fe581f
--- /dev/null
+++ b/ble/src/rtc.rs
@@ -0,0 +1,68 @@
+use core::sync::atomic::{AtomicUsize, Ordering};
+use log::trace;
+use nrf52840_hal::{
+    clocks::LfOscStopped,
+    rtc::{self, RtcInterrupt},
+    Clocks, Rtc,
+};
+
+struct MilliClock(AtomicUsize);
+impl MilliClock {
+    const fn new() -> Self {
+        Self(AtomicUsize::new(0))
+    }
+
+    #[allow(unused)]
+    fn set(&self, millis: usize) {
+        self.0.store(millis, Ordering::SeqCst)
+    }
+
+    #[allow(unused)]
+    fn add(&self, val: usize) -> usize {
+        self.0.fetch_add(val, 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: MilliClock = MilliClock::new();
+
+pub fn setup<R, H, L>(
+    mut rtc: Rtc<R, rtc::Stopped>,
+    clock: Clocks<H, L, LfOscStopped>,
+) -> impl FnMut()
+where
+    R: rtc::Instance,
+{
+    trace!("setting up clock");
+
+    clock.set_lfclk_src_rc().start_lfclk();
+
+    // Try to set clock to 1kHz.
+    match rtc.set_prescaler(32) {
+        Ok(()) => trace!("set prescaler!"),
+        Err(x) => trace!("couldn't set prescaler: {:?}", x),
+    }
+    // Make sure event is cleared before start.
+    let _ = rtc.get_event_triggered(RtcInterrupt::Tick, true);
+
+    let mut rtc = rtc.enable_counter();
+    move || handler(&mut rtc)
+}
+
+pub fn millis() -> usize {
+    CLOCK.millis()
+}
+
+fn handler<R>(rtc: &mut Rtc<R, rtc::Started>)
+where
+    R: rtc::Instance,
+{
+    let _ = rtc.get_event_triggered(RtcInterrupt::Tick, true);
+    CLOCK.add(1);
+    log::logger().flush();
+}