4 files changed, 184 insertions, 29 deletions

diff --git a/embassy-imxrt/Cargo.toml b/embassy-imxrt/Cargo.toml
index d58de6353..f16002a8d 100644
--- a/embassy-imxrt/Cargo.toml
+++ b/embassy-imxrt/Cargo.toml
@@ -12,7 +12,7 @@ documentation = "https://docs.embassy.dev/embassy-imxrt"
 [package.metadata.embassy_docs]
 src_base = "https://github.com/embassy-rs/embassy/blob/embassy-imxrt-v$VERSION/embassy-imxrt/src/"
 src_base_git = "https://github.com/embassy-rs/embassy/blob/$COMMIT/embassy-imxrt/src/"
-features = ["defmt", "unstable-pac", "time", "time-driver"]
+features = ["defmt", "unstable-pac", "time", "time-driver-os-timer"]
 flavors = [
     { regex_feature = "mimxrt6.*", target = "thumbv8m.main-none-eabihf" }
 ]
@@ -37,9 +37,12 @@ defmt = ["dep:defmt", "embassy-hal-internal/defmt", "embassy-sync/defmt", "mimxr
 time = ["dep:embassy-time", "embassy-embedded-hal/time"]
 
 ## Enable custom embassy time-driver implementation, using 32KHz RTC
-time-driver-rtc = ["_time-driver"]
+time-driver-rtc = ["_time-driver", "embassy-time-driver?/tick-hz-1_000"]
 
-_time-driver = ["dep:embassy-time-driver", "embassy-time-driver?/tick-hz-1_000", "dep:embassy-time-queue-utils", "embassy-embedded-hal/time"]
+## Enable custom embassy time-driver implementation, using 1MHz OS Timer
+time-driver-os-timer = ["_time-driver", "embassy-time-driver?/tick-hz-1_000_000"]
+
+_time-driver = ["dep:embassy-time-driver", "dep:embassy-time-queue-utils", "embassy-embedded-hal/time"]
 
 ## Reexport the PAC for the currently enabled chip at `embassy_imxrt::pac` (unstable)
 unstable-pac = []
diff --git a/embassy-imxrt/src/lib.rs b/embassy-imxrt/src/lib.rs
index fdf07b433..ad9f81e88 100644
--- a/embassy-imxrt/src/lib.rs
+++ b/embassy-imxrt/src/lib.rs
@@ -132,12 +132,10 @@ pub fn init(config: config::Config) -> Peripherals {
             error!("unable to initialize Clocks for reason: {:?}", e);
             // Panic here?
         }
-        gpio::init();
     }
-
-    // init RTC time driver
     #[cfg(feature = "_time-driver")]
     time_driver::init(config.time_interrupt_priority);
+    gpio::init();
 
     peripherals
 }
diff --git a/embassy-imxrt/src/time_driver.rs b/embassy-imxrt/src/time_driver.rs
index 56a8f7397..c68679d3e 100644
--- a/embassy-imxrt/src/time_driver.rs
+++ b/embassy-imxrt/src/time_driver.rs
@@ -1,5 +1,6 @@
-//! RTC Time Driver.
+//! Time Driver.
 use core::cell::{Cell, RefCell};
+#[cfg(feature = "time-driver-rtc")]
 use core::sync::atomic::{compiler_fence, AtomicU32, Ordering};
 
 use critical_section::CriticalSection;
@@ -8,9 +9,26 @@ use embassy_sync::blocking_mutex::Mutex;
 use embassy_time_driver::Driver;
 use embassy_time_queue_utils::Queue;
 
+#[cfg(feature = "time-driver-os-timer")]
+use crate::clocks::enable;
 use crate::interrupt::InterruptExt;
 use crate::{interrupt, pac};
 
+struct AlarmState {
+    timestamp: Cell<u64>,
+}
+
+unsafe impl Send for AlarmState {}
+
+impl AlarmState {
+    const fn new() -> Self {
+        Self {
+            timestamp: Cell::new(u64::MAX),
+        }
+    }
+}
+
+#[cfg(feature = "time-driver-rtc")]
 fn rtc() -> &'static pac::rtc::RegisterBlock {
     unsafe { &*pac::Rtc::ptr() }
 }
@@ -25,24 +43,19 @@ fn rtc() -> &'static pac::rtc::RegisterBlock {
 /// the 1kHz RTC counter is 16 bits and RTC doesn't have separate compare channels,
 /// so using a 32 bit GPREG0-2 as counter, compare, and int_en
 /// `period` is a 32bit integer, gpreg 'counter' is 31 bits plus the parity bit for overflow detection
+#[cfg(feature = "time-driver-rtc")]
 fn calc_now(period: u32, counter: u32) -> u64 {
     ((period as u64) << 31) + ((counter ^ ((period & 1) << 31)) as u64)
 }
 
-struct AlarmState {
-    timestamp: Cell<u64>,
-}
-
-unsafe impl Send for AlarmState {}
-
-impl AlarmState {
-    const fn new() -> Self {
-        Self {
-            timestamp: Cell::new(u64::MAX),
-        }
-    }
-}
+#[cfg(feature = "time-driver-rtc")]
+embassy_time_driver::time_driver_impl!(static DRIVER: Rtc = Rtc {
+    period: AtomicU32::new(0),
+    alarms:  Mutex::const_new(CriticalSectionRawMutex::new(), AlarmState::new()),
+    queue: Mutex::new(RefCell::new(Queue::new())),
+});
 
+#[cfg(feature = "time-driver-rtc")]
 struct Rtc {
     /// Number of 2^31 periods elapsed since boot.
     period: AtomicU32,
@@ -51,12 +64,7 @@ struct Rtc {
     queue: Mutex<CriticalSectionRawMutex, RefCell<Queue>>,
 }
 
-embassy_time_driver::time_driver_impl!(static DRIVER: Rtc = Rtc {
-    period: AtomicU32::new(0),
-    alarms:  Mutex::const_new(CriticalSectionRawMutex::new(), AlarmState::new()),
-    queue: Mutex::new(RefCell::new(Queue::new())),
-});
-
+#[cfg(feature = "time-driver-rtc")]
 impl Rtc {
     /// Access the GPREG0 register to use it as a 31-bit counter.
     #[inline]
@@ -219,6 +227,7 @@ impl Rtc {
     }
 }
 
+#[cfg(feature = "time-driver-rtc")]
 impl Driver for Rtc {
     fn now(&self) -> u64 {
         // `period` MUST be read before `counter`, see comment at the top for details.
@@ -242,13 +251,158 @@ impl Driver for Rtc {
     }
 }
 
-#[cfg(feature = "rt")]
+#[cfg(all(feature = "rt", feature = "time-driver-rtc"))]
 #[allow(non_snake_case)]
 #[interrupt]
 fn RTC() {
     DRIVER.on_interrupt()
 }
 
+#[cfg(feature = "time-driver-os-timer")]
+fn os() -> &'static pac::ostimer0::RegisterBlock {
+    unsafe { &*pac::Ostimer0::ptr() }
+}
+
+/// Convert gray to decimal
+///
+/// Os Event provides a 64-bit timestamp gray-encoded. All we have to
+/// do here is read both 32-bit halves of the register and convert
+/// from gray to regular binary.
+#[cfg(feature = "time-driver-os-timer")]
+fn gray_to_dec(gray: u64) -> u64 {
+    let mut dec = gray;
+
+    dec ^= dec >> 1;
+    dec ^= dec >> 2;
+    dec ^= dec >> 4;
+    dec ^= dec >> 8;
+    dec ^= dec >> 16;
+    dec ^= dec >> 32;
+
+    dec
+}
+
+/// Convert decimal to gray
+///
+/// Before writing match value to the target register, we must convert
+/// it back into gray code.
+#[cfg(feature = "time-driver-os-timer")]
+fn dec_to_gray(dec: u64) -> u64 {
+    let gray = dec;
+    gray ^ (gray >> 1)
+}
+
+#[cfg(feature = "time-driver-os-timer")]
+embassy_time_driver::time_driver_impl!(static DRIVER: OsTimer = OsTimer {
+    alarms:  Mutex::const_new(CriticalSectionRawMutex::new(), AlarmState::new()),
+    queue: Mutex::new(RefCell::new(Queue::new())),
+});
+
+#[cfg(feature = "time-driver-os-timer")]
+struct OsTimer {
+    /// Timestamp at which to fire alarm. u64::MAX if no alarm is scheduled.
+    alarms: Mutex<CriticalSectionRawMutex, AlarmState>,
+    queue: Mutex<CriticalSectionRawMutex, RefCell<Queue>>,
+}
+
+#[cfg(feature = "time-driver-os-timer")]
+impl OsTimer {
+    fn init(&'static self, irq_prio: crate::interrupt::Priority) {
+        // init alarms
+        critical_section::with(|cs| {
+            let alarm = DRIVER.alarms.borrow(cs);
+            alarm.timestamp.set(u64::MAX);
+        });
+
+        // Enable clocks. Documentation advises AGAINST resetting this
+        // peripheral.
+        enable::<crate::peripherals::OS_EVENT>();
+
+        interrupt::OS_EVENT.disable();
+
+        // Make sure interrupt is masked
+        os().osevent_ctrl().modify(|_, w| w.ostimer_intena().clear_bit());
+
+        // Default to the end of time
+        os().match_l().write(|w| unsafe { w.bits(0xffff_ffff) });
+        os().match_h().write(|w| unsafe { w.bits(0xffff_ffff) });
+
+        interrupt::OS_EVENT.unpend();
+        interrupt::OS_EVENT.set_priority(irq_prio);
+        unsafe { interrupt::OS_EVENT.enable() };
+    }
+
+    fn set_alarm(&self, cs: CriticalSection, timestamp: u64) -> bool {
+        let alarm = self.alarms.borrow(cs);
+        alarm.timestamp.set(timestamp);
+
+        let t = self.now();
+        if timestamp <= t {
+            os().osevent_ctrl().modify(|_, w| w.ostimer_intena().clear_bit());
+            alarm.timestamp.set(u64::MAX);
+            return false;
+        }
+
+        let gray_timestamp = dec_to_gray(timestamp);
+
+        os().match_l()
+            .write(|w| unsafe { w.bits(gray_timestamp as u32 & 0xffff_ffff) });
+        os().match_h()
+            .write(|w| unsafe { w.bits((gray_timestamp >> 32) as u32) });
+        os().osevent_ctrl().modify(|_, w| w.ostimer_intena().set_bit());
+
+        true
+    }
+
+    #[cfg(feature = "rt")]
+    fn trigger_alarm(&self, cs: CriticalSection) {
+        let mut next = self.queue.borrow(cs).borrow_mut().next_expiration(self.now());
+        while !self.set_alarm(cs, next) {
+            next = self.queue.borrow(cs).borrow_mut().next_expiration(self.now());
+        }
+    }
+
+    #[cfg(feature = "rt")]
+    fn on_interrupt(&self) {
+        critical_section::with(|cs| {
+            if os().osevent_ctrl().read().ostimer_intrflag().bit_is_set() {
+                os().osevent_ctrl().modify(|_, w| w.ostimer_intena().clear_bit());
+                self.trigger_alarm(cs);
+            }
+        });
+    }
+}
+
+#[cfg(feature = "time-driver-os-timer")]
+impl Driver for OsTimer {
+    fn now(&self) -> u64 {
+        let mut t = os().evtimerh().read().bits() as u64;
+        t <<= 32;
+        t |= os().evtimerl().read().bits() as u64;
+        gray_to_dec(t)
+    }
+
+    fn schedule_wake(&self, at: u64, waker: &core::task::Waker) {
+        critical_section::with(|cs| {
+            let mut queue = self.queue.borrow(cs).borrow_mut();
+
+            if queue.schedule_wake(at, waker) {
+                let mut next = queue.next_expiration(self.now());
+                while !self.set_alarm(cs, next) {
+                    next = queue.next_expiration(self.now());
+                }
+            }
+        })
+    }
+}
+
+#[cfg(all(feature = "rt", feature = "time-driver-os-timer"))]
+#[allow(non_snake_case)]
+#[interrupt]
+fn OS_EVENT() {
+    DRIVER.on_interrupt()
+}
+
 pub(crate) fn init(irq_prio: crate::interrupt::Priority) {
     DRIVER.init(irq_prio)
 }
diff --git a/examples/mimxrt6/Cargo.toml b/examples/mimxrt6/Cargo.toml
index 8fc510c47..b0c56f003 100644
--- a/examples/mimxrt6/Cargo.toml
+++ b/examples/mimxrt6/Cargo.toml
@@ -12,8 +12,8 @@ defmt-rtt = "1.0"
 
 embassy-executor = { version = "0.7.0", path = "../../embassy-executor", features = ["arch-cortex-m", "executor-thread", "executor-interrupt", "defmt"] }
 embassy-futures = { version = "0.1.1", path = "../../embassy-futures" }
-embassy-imxrt = { version = "0.1.0", path = "../../embassy-imxrt", features = ["defmt", "mimxrt685s", "unstable-pac", "time", "time-driver-rtc"] }
-embassy-time = { version = "0.4", path = "../../embassy-time" }
+embassy-imxrt = { version = "0.1.0", path = "../../embassy-imxrt", features = ["defmt", "mimxrt685s", "unstable-pac", "time", "time-driver-os-timer"] }
+embassy-time = { version = "0.4", path = "../../embassy-time", features = ["defmt", "defmt-timestamp-uptime"] }
 embassy-sync = { version = "0.6.2", path = "../../embassy-sync", features = ["defmt"] }
 embedded-hal-1 = { package = "embedded-hal", version = "1.0" }
 embedded-hal-async = "1.0.0"