diff --git a/.vscode/settings.json b/.vscode/settings.json
index d49e011..c4f7b2a 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -3,6 +3,7 @@
   "rust-analyzer.cargo.target": "thumbv6m-none-eabi",
   "rust-analyzer.cargo.features": [
     "rt",
-    "stm32g081"
+    "stm32g081",
+    "rtic2"
   ]
 }
diff --git a/Cargo.toml b/Cargo.toml
index 1bed7b2..eefcbde 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -23,6 +23,10 @@ fugit = "0.3.7"
 embedded-hal = "1.0.0"
 bare-metal = "1.0.0"
 portable-atomic = { version = "1.10.0", features = ["critical-section"] }
+embedded-hal-async = { version = "1.0.0", optional = true }
+atomic-polyfill = { version = "1.0.3", optional = true }
+rtic-time = { version = "2.0.0", optional = true }
+embedded-time = { version = "0.12.1", optional = true }
 
 [dependencies.stm32g0]
 package = "stm32g0-staging"
@@ -61,6 +65,7 @@ stm32g0x1 = []
 
 i2c-blocking = []
 i2c-nonblocking = []
+rtic2 = ["dep:embedded-hal-async", "dep:atomic-polyfill", "dep:rtic-time"]
 
 [profile.dev]
 incremental = false
diff --git a/src/timer/mod.rs b/src/timer/mod.rs
index 5d90b17..59f8266 100644
--- a/src/timer/mod.rs
+++ b/src/timer/mod.rs
@@ -3,11 +3,13 @@ use crate::rcc::*;
 use crate::stm32::*;
 use crate::time::{Hertz, MicroSecond};
 use core::marker::PhantomData;
-use cortex_m::peripheral::syst::SystClkSource;
-use cortex_m::peripheral::SYST;
+use fugit::HertzU32;
+use fugit::RateExtU32;
 use void::Void;
 
 pub mod delay;
+#[cfg(feature = "rtic2")]
+pub mod monotonics;
 pub mod opm;
 pub mod pins;
 pub mod pwm;
@@ -31,47 +33,59 @@ type Channel2 = Channel<1>;
 type Channel3 = Channel<2>;
 type Channel4 = Channel<3>;
 
-/// System timer
-impl Timer<SYST> {
-    /// Configures the SYST clock as a periodic count down timer
-    pub fn syst(mut syst: SYST, rcc: &mut Rcc) -> Self {
-        syst.set_clock_source(SystClkSource::Core);
-        Timer {
-            tim: syst,
-            clk: rcc.clocks.apb_tim_clk,
-        }
-    }
+/// Timer wrapper for fixed precision timers.
+///
+/// Uses `fugit::TimerDurationU32` for most of operations
+pub struct FTimer<TIM, const FREQ: u32> {
+    tim: TIM,
+}
 
-    /// Starts listening
-    pub fn listen(&mut self) {
-        self.tim.enable_interrupt()
+/// `FTimer` with precision of 1 μs (1 MHz sampling)
+pub type FTimerUs<TIM> = FTimer<TIM, 1_000_000>;
+
+/// `FTimer` with precision of 1 ms (1 kHz sampling)
+///
+/// NOTE: don't use this if your system frequency more than 65 MHz
+pub type FTimerMs<TIM> = FTimer<TIM, 1_000>;
+
+impl<TIM: private::TimerBase, const FREQ: u32> FTimer<TIM, FREQ> {
+    /// Initialize timer
+    pub fn new(mut tim: TIM, rcc: &mut Rcc) -> Self {
+        tim.init(rcc);
+        tim.set_freq(FREQ.Hz(), rcc.clocks.apb_tim_clk);
+
+        Self { tim }
     }
 
-    /// Stops listening
-    pub fn unlisten(&mut self) {
-        self.tim.disable_interrupt()
+    /*/// Creates `Counter` that implements [embedded_hal_02::timer::CountDown]
+    pub fn counter(self) -> Counter<TIM, FREQ> {
+        Counter(self)
     }
 
-    pub fn get_current(&self) -> u32 {
-        SYST::get_current()
+    /// Creates `Delay` that implements [embedded_hal_02::blocking::delay] traits
+    pub fn delay(self) -> Delay<TIM, FREQ> {
+        Delay(self)
+    }*/
+
+    /// Releases the TIM peripheral
+    pub fn release(self) -> TIM {
+        self.tim
     }
 }
 
-impl Timer<SYST> {
-    pub fn start(&mut self, timeout: MicroSecond) {
-        let cycles = crate::time::cycles(timeout, self.clk);
-        assert!(cycles < 0x00ff_ffff);
-        self.tim.set_reload(cycles);
-        self.tim.clear_current();
-        self.tim.enable_counter();
-    }
+pub struct TimerFrequencySettings {
+    psc: u16,
+    arr: u32,
+}
 
-    pub fn wait(&mut self) -> nb::Result<(), Void> {
-        if self.tim.has_wrapped() {
-            Ok(())
-        } else {
-            Err(nb::Error::WouldBlock)
-        }
+impl TimerFrequencySettings {
+    pub(crate) const fn from(target_freq: Hertz, clk: Hertz) -> Self {
+        let ratio = clk.raw() / target_freq.raw();
+        let psc = (ratio - 1) / 0xffff;
+        let arr = ratio / (psc + 1) - 1;
+        let psc = psc as u16;
+
+        Self { psc, arr }
     }
 }
 
@@ -79,115 +93,289 @@ pub trait TimerExt<TIM> {
     fn timer(self, rcc: &mut Rcc) -> Timer<TIM>;
 }
 
-impl TimerExt<SYST> for SYST {
-    fn timer(self, rcc: &mut Rcc) -> Timer<SYST> {
-        Timer::syst(self, rcc)
+pub(super) mod private {
+    use crate::pac::SYST;
+    use crate::timer::MicroSecond;
+    use cortex_m::peripheral::syst::SystClkSource;
+    use fugit::HertzU32;
+
+    use super::{Rcc, TimerFrequencySettings};
+
+    pub trait TimerCommon {
+        type Width: Into<u32> + From<u16>;
+
+        fn init(&mut self, rcc: &mut Rcc);
+
+        fn set_urs(&mut self);
+
+        /// Starts listening
+        fn listen(&mut self);
+
+        /// Stops listening
+        fn unlisten(&mut self);
+
+        /// Gets timer counter current value
+        fn get_current(&self) -> u32;
+
+        fn start(&mut self, timeout: MicroSecond, clk: HertzU32);
+
+        fn has_elapsed(&mut self) -> bool;
+
+        fn clear_irq(&mut self);
+    }
+
+    impl TimerCommon for SYST {
+        type Width = u32;
+
+        fn init(&mut self, _rcc: &mut Rcc) {
+            self.set_clock_source(SystClkSource::Core);
+        }
+
+        fn set_urs(&mut self) {}
+
+        /// Starts listening
+        fn listen(&mut self) {
+            self.enable_interrupt()
+        }
+
+        /// Stops listening
+        fn unlisten(&mut self) {
+            self.disable_interrupt()
+        }
+
+        /// Gets timer counter current value
+        fn get_current(&self) -> u32 {
+            SYST::get_current()
+        }
+
+        fn start(&mut self, timeout: MicroSecond, clk: HertzU32) {
+            let cycles = crate::time::cycles(timeout, clk);
+            assert!(cycles < 0x00ff_ffff);
+            self.set_reload(cycles);
+            self.clear_current();
+            self.enable_counter();
+        }
+
+        /// NOTE This takes &mut self because the read operation maight be side effectful and might clear the bit of the read register for some timers (SYST).
+        fn has_elapsed(&mut self) -> bool {
+            self.has_wrapped()
+        }
+
+        fn clear_irq(&mut self) {}
+    }
+
+    pub trait TimerBase: TimerCommon {
+        /// Pauses timer
+        fn pause(&mut self);
+
+        /// Resumes timer
+        fn resume(&mut self);
+
+        fn set_freq_settings(&mut self, freq_settings: TimerFrequencySettings);
+
+        fn set_freq(&mut self, target_freq: HertzU32, clk: HertzU32);
+
+        /// Resets counter value
+        fn reset(&mut self);
+
+        /// Returns the currently configured frequency
+        fn freq(&self, clk: HertzU32) -> HertzU32;
+
+        /// Generate an update event so that PSC and ARR values are copied into their
+        /// shadow registers.
+        fn force_update(&mut self);
     }
 }
 
 macro_rules! timers {
-    ($($TIM:ident: ($tim:ident, $cnt:ident $(,$cnt_h:ident)*),)+) => {
+    ($($TIM:ident: $width:ty, ($tim:ident, $cnt:ident $(,$cnt_h:ident)*),)+) => {
         $(
-            impl Timer<$TIM> {
-                /// Configures a TIM peripheral as a periodic count down timer
-                pub fn $tim(tim: $TIM, rcc: &mut Rcc) -> Self {
+            impl private::TimerCommon for $TIM {
+                type Width = $width;
+
+                fn init(&mut self, rcc: &mut Rcc) {
                     $TIM::enable(rcc);
                     $TIM::reset(rcc);
-
-                    Timer {
-                        tim,
-                        clk: rcc.clocks.apb_tim_clk,
-                    }
-                }
-
-                /// Pauses timer
-                pub fn pause(&mut self) {
-                    self.tim.cr1().modify(|_, w| w.cen().clear_bit());
                 }
 
-                /// Resumes timer
-                pub fn resume(&mut self) {
-                    self.tim.cr1().modify(|_, w| w.cen().set_bit());
+                fn set_urs(&mut self) {
+                    // Set URS so that when we force_update, it will
+                    // generate an update event *without* triggering an interrupt.
+                    self.cr1().modify(|_, w| w.cen().clear_bit().urs().set_bit());
                 }
 
                 /// Starts listening
-                pub fn listen(&mut self) {
-                    self.tim.dier().write(|w| w.uie().set_bit());
+                fn listen(&mut self) {
+                    self.dier().write(|w| w.uie().set_bit());
                 }
 
                 /// Stops listening
-                pub fn unlisten(&mut self) {
-                    self.tim.dier().write(|w| w.uie().clear_bit());
-                }
-
-                /// Clears interrupt flag
-                pub fn clear_irq(&mut self) {
-                    self.tim.sr().modify(|_, w| w.uif().clear_bit());
-                }
-
-                /// Resets counter value
-                pub fn reset(&mut self) {
-                    self.tim.cnt().reset();
+                fn unlisten(&mut self) {
+                    self.dier().write(|w| w.uie().clear_bit());
                 }
 
                 /// Gets timer counter current value
-                pub fn get_current(&self) -> u32 {
+                fn get_current(&self) -> u32 {
                     let _high = 0;
                     $(
-                        let _high = self.tim.cnt().read().$cnt_h().bits() as u32;
+                        let _high = self.cnt().read().$cnt_h().bits() as u32;
                     )*
-                    let low = self.tim.cnt().read().$cnt().bits() as u32;
+                    let low = self.cnt().read().$cnt().bits() as u32;
                     low | (_high << 16)
                 }
 
-                pub fn start(&mut self, timeout: MicroSecond) {
-                    // Pause the counter. Also set URS so that when we set UG below, it will
-                    // generate an update event *without* triggering an interrupt.
-                    self.tim.cr1().modify(|_, w| w.cen().clear_bit().urs().set_bit());
+                fn start(&mut self, timeout: MicroSecond, clk: HertzU32) {
+                    use private::TimerBase;
+
+                    // Pause the counter.
+                    TimerBase::pause(self);
                     // reset counter
-                    self.tim.cnt().reset();
+                    TimerBase::reset(self);
                     // clear interrupt flag
-                    self.tim.sr().modify(|_, w| w.uif().clear_bit());
+                    self.clear_irq();
 
                     // Calculate counter configuration
-                    let cycles = crate::time::cycles(timeout, self.clk);
+                    let cycles = crate::time::cycles(timeout, clk);
                     let psc = cycles / 0xffff;
                     let arr = cycles / (psc + 1);
+                    let psc = psc as u16;
 
-                    self.tim.psc().write(|w| w.psc().set(psc as u16));
-                    self.tim.arr().write(|w| unsafe { w.bits(arr) });
+                    TimerBase::set_freq_settings(self, TimerFrequencySettings { psc, arr });
 
                     // Generate an update event so that PSC and ARR values are copied into their
                     // shadow registers.
-                    self.tim.egr().write(|w| w.ug().set_bit());
+                    TimerBase::force_update(self);
 
-                    self.tim.cr1().modify(|_, w| w.cen().set_bit());
+                    TimerBase::resume(self);
                 }
 
-                pub fn wait(&mut self) -> nb::Result<(), Void> {
-                    if self.tim.sr().read().uif().bit_is_clear() {
-                        Err(nb::Error::WouldBlock)
-                    } else {
-                        self.tim.sr().modify(|_, w| w.uif().clear_bit());
-                        Ok(())
-                    }
+                fn has_elapsed(&mut self) -> bool {
+                    self.sr().read().uif().bit_is_set()
                 }
 
-                /// Releases the TIM peripheral
-                pub fn release(self) -> $TIM {
-                    self.tim
+                /// Clears interrupt flag
+                fn clear_irq(&mut self) {
+                    self.sr().modify(|_, w| w.uif().clear_bit());
                 }
             }
 
-            impl TimerExt<$TIM> for $TIM {
-                fn timer(self, rcc: &mut Rcc) -> Timer<$TIM> {
-                    Timer::$tim(self, rcc)
+            impl private::TimerBase for $TIM {
+                /// Pauses timer
+                fn pause(&mut self) {
+                    self.cr1().modify(|_, w| w.cen().clear_bit());
+                }
+
+                /// Resumes timer
+                fn resume(&mut self) {
+                    self.cr1().modify(|_, w| w.cen().set_bit());
+                }
+
+                fn set_freq_settings(&mut self, freq_settings: TimerFrequencySettings) {
+                    unsafe {
+                        self.psc().write(|w| w.psc().bits(freq_settings.psc as u16));
+                        self.arr().write(|w| w.arr().bits(freq_settings.arr as $width));
+                    }
+                }
+
+                fn set_freq(&mut self, target_freq: Hertz, clk: Hertz) {
+                    let freq_settings = TimerFrequencySettings::from(target_freq, clk);
+
+                    self.set_freq_settings(freq_settings);
+                }
+
+                /// Resets counter value
+                fn reset(&mut self) {
+                    self.cnt().reset();
+                }
+
+                /// Returns the currently configured frequency
+                fn freq(&self, clk: HertzU32) -> Hertz {
+                    Hertz::from_raw(clk.raw()
+                        / (self.psc().read().bits() + 1)
+                        / (self.arr().read().bits() + 1))
+                }
+
+                fn force_update(&mut self) {
+                    // Generate an update event so that PSC and ARR values are copied into their
+                    // shadow registers.
+                    self.egr().write(|w| w.ug().set_bit());
                 }
             }
         )+
     }
 }
 
+impl<T: private::TimerCommon> TimerExt<T> for T {
+    fn timer(self, rcc: &mut Rcc) -> Timer<T> {
+        Timer::new(self, rcc)
+    }
+}
+
+impl<T: private::TimerCommon> Timer<T> {
+    /// Configures a TIM peripheral as a periodic count down timer
+    pub fn new(mut tim: T, rcc: &mut Rcc) -> Self {
+        tim.init(rcc);
+
+        tim.set_urs();
+
+        Timer {
+            tim,
+            clk: rcc.clocks.apb_tim_clk,
+        }
+    }
+
+    /// Starts listening
+    pub fn listen(&mut self) {
+        self.tim.listen();
+    }
+
+    /// Stops listening
+    pub fn unlisten(&mut self) {
+        self.tim.unlisten();
+    }
+
+    /// Gets timer counter current value
+    pub fn get_current(&self) -> u32 {
+        self.tim.get_current()
+    }
+
+    pub fn wait(&mut self) -> nb::Result<(), Void> {
+        if !self.tim.has_elapsed() {
+            Err(nb::Error::WouldBlock)
+        } else {
+            self.tim.clear_irq();
+            Ok(())
+        }
+    }
+
+    /// Releases the TIM peripheral
+    pub fn release(self) -> T {
+        self.tim
+    }
+}
+
+impl<T: private::TimerBase> Timer<T> {
+    /// Pauses timer
+    pub fn pause(&mut self) {
+        self.tim.pause();
+    }
+
+    /// Resumes timer
+    pub fn resume(&mut self) {
+        self.tim.resume();
+    }
+
+    /// Clears interrupt flag
+    pub fn clear_irq(&mut self) {
+        self.tim.clear_irq();
+    }
+
+    /// Resets counter value
+    pub fn reset(&mut self) {
+        self.tim.reset();
+    }
+}
+
 #[cfg_attr(feature = "defmt", derive(defmt::Format))]
 #[derive(Copy, Clone, Debug, PartialEq, Eq)]
 pub enum ExternalClockMode {
@@ -255,21 +443,21 @@ timers_external_clocks! {
 }
 
 timers! {
-    TIM1: (tim1, cnt),
-    TIM3: (tim3, cnt),
-    TIM14: (tim14, cnt),
-    TIM16: (tim16, cnt),
-    TIM17: (tim17, cnt),
+    TIM1: u16, (tim1, cnt),
+    TIM3: u16, (tim3, cnt),
+    TIM14: u16, (tim14, cnt),
+    TIM16: u16, (tim16, cnt),
+    TIM17: u16, (tim17, cnt),
 }
 
 #[cfg(feature = "stm32g0x1")]
 timers! {
-    TIM2: (tim2, cnt),
+    TIM2: u32, (tim2, cnt),
 }
 
 #[cfg(any(feature = "stm32g070", feature = "stm32g071", feature = "stm32g081"))]
 timers! {
-    TIM6: (tim6, cnt),
-    TIM7: (tim7, cnt),
-    TIM15: (tim15, cnt),
+    TIM6: u16, (tim6, cnt),
+    TIM7: u16, (tim7, cnt),
+    TIM15: u16, (tim15, cnt),
 }
diff --git a/src/timer/monotonics.rs b/src/timer/monotonics.rs
new file mode 100644
index 0000000..9d4e337
--- /dev/null
+++ b/src/timer/monotonics.rs
@@ -0,0 +1,306 @@
+// Copied from stm32f4xx-hal/src/timer.rs
+
+use super::private::TimerCommon;
+use super::Rcc;
+// RTICv2 Monotonic impl
+use super::{private::TimerBase, FTimer};
+use crate::pac;
+use atomic_polyfill::{AtomicU64, Ordering};
+use core::marker::PhantomData;
+use rtic_time::timer_queue::TimerQueueBackend;
+use rtic_time::{
+    half_period_counter::calculate_now, monotonic::TimerQueueBasedMonotonic,
+    timer_queue::TimerQueue, Monotonic,
+};
+
+pub struct MonoTimer<TIM, const FREQ: u32> {
+    _tim: PhantomData<TIM>,
+}
+/// `MonoTimer` with precision of 1 μs (1 MHz sampling)
+pub type MonoTimerUs<TIM> = MonoTimer<TIM, 1_000_000>;
+
+pub struct MonoTimerBackend<TIM> {
+    _tim: PhantomData<TIM>,
+}
+
+impl<TIM: 'static, const FREQ: u32> TimerQueueBasedMonotonic for MonoTimer<TIM, FREQ>
+where
+    MonoTimerBackend<TIM>: TimerQueueBackend<Ticks = u64>,
+{
+    type Backend = MonoTimerBackend<TIM>;
+    type Instant = fugit::TimerInstantU64<FREQ>;
+    type Duration = fugit::TimerDurationU64<FREQ>;
+}
+
+pub trait MonoTimerExt: Sized {
+    fn monotonic<const FREQ: u32>(
+        self,
+        nvic: &mut cortex_m::peripheral::NVIC,
+        rcc: &mut Rcc,
+    ) -> MonoTimer<Self, FREQ>;
+    fn monotonic_us(
+        self,
+        nvic: &mut cortex_m::peripheral::NVIC,
+        rcc: &mut Rcc,
+    ) -> MonoTimer<Self, 1_000_000> {
+        self.monotonic::<1_000_000>(nvic, rcc)
+    }
+}
+
+impl<TIM: 'static, const FREQ: u32> embedded_hal::delay::DelayNs for MonoTimer<TIM, FREQ>
+where
+    Self:
+        Monotonic<Instant = fugit::TimerInstantU64<FREQ>, Duration = fugit::TimerDurationU64<FREQ>>,
+{
+    fn delay_ns(&mut self, ns: u32) {
+        let now = Self::now();
+        let mut done = now + <Self as Monotonic>::Duration::nanos_at_least(ns.into());
+        if now != done {
+            // Compensate for sub-tick uncertainty
+            done += <Self as Monotonic>::Duration::from_ticks(1);
+        }
+
+        while Self::now() < done {}
+    }
+
+    fn delay_us(&mut self, us: u32) {
+        let now = Self::now();
+        let mut done = now + <Self as Monotonic>::Duration::micros_at_least(us.into());
+        if now != done {
+            // Compensate for sub-tick uncertainty
+            done += <Self as Monotonic>::Duration::from_ticks(1);
+        }
+
+        while Self::now() < done {}
+    }
+
+    fn delay_ms(&mut self, ms: u32) {
+        let now = Self::now();
+        let mut done = now + <Self as Monotonic>::Duration::millis_at_least(ms.into());
+        if now != done {
+            // Compensate for sub-tick uncertainty
+            done += <Self as Monotonic>::Duration::from_ticks(1);
+        }
+
+        while Self::now() < done {}
+    }
+}
+
+impl<TIM: 'static, const FREQ: u32> embedded_hal_async::delay::DelayNs for MonoTimer<TIM, FREQ>
+where
+    Self:
+        Monotonic<Instant = fugit::TimerInstantU64<FREQ>, Duration = fugit::TimerDurationU64<FREQ>>,
+{
+    #[inline]
+    async fn delay_ns(&mut self, ns: u32) {
+        Self::delay(<Self as Monotonic>::Duration::nanos_at_least(ns.into())).await;
+    }
+
+    #[inline]
+    async fn delay_us(&mut self, us: u32) {
+        Self::delay(<Self as Monotonic>::Duration::micros_at_least(us.into())).await;
+    }
+
+    #[inline]
+    async fn delay_ms(&mut self, ms: u32) {
+        Self::delay(<Self as Monotonic>::Duration::millis_at_least(ms.into())).await;
+    }
+}
+
+const fn cortex_logical2hw(logical: u8, nvic_prio_bits: u8) -> u8 {
+    ((1 << nvic_prio_bits) - logical) << (8 - nvic_prio_bits)
+}
+
+pub(crate) unsafe fn set_monotonic_prio(
+    nvic: &mut cortex_m::peripheral::NVIC,
+    prio_bits: u8,
+    interrupt: impl cortex_m::interrupt::InterruptNumber,
+) {
+    extern "C" {
+        static RTIC_ASYNC_MAX_LOGICAL_PRIO: u8;
+    }
+
+    let max_prio = RTIC_ASYNC_MAX_LOGICAL_PRIO.max(1).min(1 << prio_bits);
+
+    let hw_prio = cortex_logical2hw(max_prio, prio_bits);
+
+    nvic.set_priority(interrupt, hw_prio);
+}
+
+#[doc(hidden)]
+#[macro_export]
+macro_rules! __internal_create_stm32_timer_interrupt {
+    ($timer:ident) => {
+        #[no_mangle]
+        #[allow(non_snake_case)]
+        unsafe extern "C" fn $timer() {
+            use monomod::TimerQueueBackend;
+            use $crate::timer::monotonics as monomod;
+            monomod::MonoTimerBackend::<$crate::pac::$timer>::timer_queue()
+                .on_monotonic_interrupt();
+        }
+    };
+}
+
+macro_rules! make_timer {
+    ($tim: ident, $timer:ident, $overflow:ident, $tq:ident$(, doc: ($($doc:tt)*))?) => {
+        static $overflow: AtomicU64 = AtomicU64::new(0);
+        static $tq: TimerQueue<MonoTimerBackend<pac::$timer>> = TimerQueue::new();
+
+        impl MonoTimerExt for pac::$timer {
+            fn monotonic<const FREQ: u32>(
+                self,
+                nvic: &mut cortex_m::peripheral::NVIC,
+                rcc: &mut Rcc,
+            ) -> MonoTimer<Self, FREQ> {
+                FTimer::new(self, rcc).monotonic(nvic)
+            }
+        }
+
+        impl<const FREQ: u32> FTimer<pac::$timer, FREQ> {
+            pub fn monotonic(
+                mut self,
+                nvic: &mut cortex_m::peripheral::NVIC,
+            ) -> MonoTimer<pac::$timer, FREQ> {
+                __internal_create_stm32_timer_interrupt!($timer);
+
+                // Enable full-period interrupt.
+                self.tim.dier().modify(|_, w| w.uie().set_bit());
+
+                // Configure and enable half-period interrupt
+                self.tim.ccr1().write(|w| unsafe {
+                    w.ccr().bits(
+                        (<pac::$timer as TimerCommon>::Width::MAX
+                            - (<pac::$timer as TimerCommon>::Width::MAX >> 1))
+                            .into(),
+                    )
+                });
+                self.tim.dier().modify(|_, w| w.cc1ie().set_bit());
+
+                // Trigger an update event to load the prescaler value to the clock.
+                self.tim.egr().write(|w| w.ug().set_bit());
+
+                // The above line raises an update event which will indicate that the timer is already finished.
+                // Since this is not the case, it should be cleared.
+                self.tim.sr().write(|w| w.uif().clear_bit());
+
+                $tq.initialize(MonoTimerBackend::<pac::$timer> { _tim: PhantomData });
+                $overflow.store(0, Ordering::SeqCst);
+
+                // Start the counter.
+                self.tim.resume();
+
+                // SAFETY: We take full ownership of the peripheral and interrupt vector,
+                // plus we are not using any external shared resources so we won't impact
+                // basepri/source masking based critical sections.
+                unsafe {
+                    set_monotonic_prio(nvic, pac::NVIC_PRIO_BITS, <pac::$timer>::IRQ);
+                    cortex_m::peripheral::NVIC::unmask(<pac::$timer>::IRQ);
+                }
+                MonoTimer { _tim: PhantomData }
+            }
+        }
+
+        impl MonoTimerBackend<pac::$timer> {
+            #[inline(always)]
+            fn tim() -> &'static pac::$tim::RegisterBlock {
+                unsafe { &*<pac::$timer>::ptr() }
+            }
+        }
+
+        impl TimerQueueBackend for MonoTimerBackend<pac::$timer> {
+            type Ticks = u64;
+
+            fn now() -> Self::Ticks {
+                calculate_now(
+                    || $overflow.load(Ordering::Relaxed),
+                    || Self::tim().cnt().read().cnt().bits(),
+                )
+            }
+
+            fn set_compare(instant: Self::Ticks) {
+                let now = Self::now();
+
+                // Since the timer may or may not overflow based on the requested compare val, we check how many ticks are left.
+                // `wrapping_sub` takes care of the u64 integer overflow special case.
+                let val = if instant.wrapping_sub(now)
+                    <= (<pac::$timer as TimerCommon>::Width::MAX as u64)
+                {
+                    instant as <pac::$timer as TimerCommon>::Width
+                } else {
+                    // In the past or will overflow
+                    0
+                };
+
+                unsafe {
+                    Self::tim().ccr2().write(|r| r.ccr().bits(val.into()));
+                }
+            }
+
+            fn clear_compare_flag() {
+                Self::tim().sr().write(|w| w.cc2if().clear_bit());
+            }
+
+            fn pend_interrupt() {
+                cortex_m::peripheral::NVIC::pend(<pac::$timer>::IRQ);
+            }
+
+            fn enable_timer() {
+                Self::tim().dier().modify(|_, w| w.cc2ie().set_bit());
+            }
+
+            fn disable_timer() {
+                Self::tim().dier().modify(|_, w| w.cc2ie().clear_bit());
+            }
+
+            fn on_interrupt() {
+                // Full period
+                if Self::tim().sr().read().uif().bit_is_set() {
+                    Self::tim().sr().write(|w| w.uif().clear_bit());
+                    let prev = $overflow.fetch_add(1, Ordering::Relaxed);
+                    assert!(prev % 2 == 1, "Monotonic must have missed an interrupt!");
+                }
+                // Half period
+                if Self::tim().sr().read().cc1if().bit_is_set() {
+                    Self::tim().sr().write(|w| w.cc1if().clear_bit());
+                    let prev = $overflow.fetch_add(1, Ordering::Relaxed);
+                    assert!(prev % 2 == 0, "Monotonic must have missed an interrupt!");
+                }
+            }
+
+            fn timer_queue() -> &'static TimerQueue<Self> {
+                &$tq
+            }
+        }
+    };
+}
+
+#[cfg(feature = "stm32g0x1")]
+make_timer!(tim2, TIM2, TIMER2_OVERFLOWS, TIMER2_TQ);
+
+make_timer!(tim3, TIM3, TIMER3_OVERFLOWS, TIMER3_TQ);
+
+#[cfg(any(feature = "stm32g070", feature = "stm32g071", feature = "stm32g081"))]
+make_timer!(tim15, TIM15, TIMER15_OVERFLOWS, TIMER15_TQ);
+
+pub trait Irq {
+    const IRQ: pac::Interrupt;
+}
+
+#[cfg(feature = "stm32g0x1")]
+impl Irq for pac::TIM2 {
+    const IRQ: pac::Interrupt = pac::Interrupt::TIM2;
+}
+
+impl Irq for pac::TIM3 {
+    const IRQ: pac::Interrupt = pac::Interrupt::TIM3;
+}
+
+#[cfg(any(feature = "stm32g070", feature = "stm32g071", feature = "stm32g081"))]
+impl Irq for pac::TIM15 {
+    const IRQ: pac::Interrupt = pac::Interrupt::TIM15;
+}
+
+impl Irq for pac::TIM17 {
+    const IRQ: pac::Interrupt = pac::Interrupt::TIM17;
+}
diff --git a/src/timer/pwm.rs b/src/timer/pwm.rs
index 745000b..eb1090d 100644
--- a/src/timer/pwm.rs
+++ b/src/timer/pwm.rs
@@ -70,83 +70,61 @@ impl<TIM> Pwm<TIM> {
     }
 }
 
-macro_rules! pwm {
-    ($($TIMX:ident: ($timX:ident, $arr:ident $(,$arr_h:ident)*),)+) => {
-        $(
-            impl PwmExt for $TIMX {
-                fn pwm(self, freq: Hertz, rcc: &mut Rcc) -> Pwm<Self> {
-                    $timX(self, freq, rcc, ClockSource::ApbTim)
-                }
-            }
+impl<T: super::private::TimerBase> PwmExt for T {
+    fn pwm(self, freq: Hertz, rcc: &mut Rcc) -> Pwm<Self> {
+        Pwm::new(self, freq, rcc, ClockSource::ApbTim)
+    }
+}
 
-            fn $timX(tim: $TIMX, freq: Hertz, rcc: &mut Rcc, clock_source: ClockSource) -> Pwm<$TIMX> {
-                $TIMX::enable(rcc);
-                $TIMX::reset(rcc);
+impl<T: super::private::TimerBase> Pwm<T> {
+    fn new(mut tim: T, freq: Hertz, rcc: &mut Rcc, clock_source: ClockSource) -> Pwm<T> {
+        tim.init(rcc);
 
-                let clk = match clock_source {
-                    ClockSource::ApbTim => {
-                        rcc.ccipr().modify(|_, w| w.tim1sel().clear_bit());
-                        rcc.clocks.apb_tim_clk
-                    }
-                    ClockSource::Pllq => {
-                        rcc.ccipr().modify(|_, w| w.tim1sel().set_bit());
-                        rcc.clocks.pll_clk.q.unwrap()
-                    }
-                };
-
-                let mut pwm = Pwm::<$TIMX> {
-                    clk,
-                    tim,
-                };
-                pwm.set_freq(freq);
-                pwm
+        let clk = match clock_source {
+            ClockSource::ApbTim => {
+                rcc.ccipr().modify(|_, w| w.tim1sel().clear_bit());
+                rcc.clocks.apb_tim_clk
+            }
+            ClockSource::Pllq => {
+                rcc.ccipr().modify(|_, w| w.tim1sel().set_bit());
+                rcc.clocks.pll_clk.q.unwrap()
             }
+        };
 
-            impl Pwm<$TIMX> {
-                /// Set the PWM frequency. Actual frequency may differ from
-                /// requested due to precision of input clock. To check actual
-                /// frequency, call freq.
-                pub fn set_freq(&mut self, freq: Hertz) {
-                    let ratio = self.clk / freq;
-                    let psc = (ratio - 1) / 0xffff;
+        tim.set_freq(freq, clk);
+        tim.resume();
 
-                    unsafe {
-                        let arr = ratio / (psc + 1) - 1;
-                        self.tim.psc().write(|w| w.psc().bits(psc as u16));
-                        self.tim.arr().write(|w| w.$arr().bits((arr as u16).into()));
-                        $(
-                            self.tim.arr().modify(|_, w| w.$arr_h().bits((arr >> 16) as u16));
-                        )*
-                        self.tim.cr1().write(|w| w.cen().set_bit());
-                    }
-                }
-                /// Starts listening
-                pub fn listen(&mut self) {
-                    self.tim.dier().write(|w| w.uie().set_bit());
-                }
+        Self { clk, tim }
+    }
 
-                /// Stops listening
-                pub fn unlisten(&mut self) {
-                    self.tim.dier().write(|w| w.uie().clear_bit());
-                }
-                /// Clears interrupt flag
-                pub fn clear_irq(&mut self) {
-                    self.tim.sr().modify(|_, w| w.uif().clear_bit());
-                }
+    /// Set the PWM frequency. Actual frequency may differ from
+    /// requested due to precision of input clock. To check actual
+    /// frequency, call freq.
+    pub fn set_freq(&mut self, freq: Hertz) {
+        self.tim.set_freq(freq, self.clk);
+    }
+    /// Starts listening
+    pub fn listen(&mut self) {
+        self.tim.listen();
+    }
 
-                /// Resets counter value
-                pub fn reset(&mut self) {
-                    self.tim.cnt().reset();
-                }
+    /// Stops listening
+    pub fn unlisten(&mut self) {
+        self.tim.unlisten();
+    }
+    /// Clears interrupt flag
+    pub fn clear_irq(&mut self) {
+        self.tim.clear_irq();
+    }
 
-                /// Returns the currently configured frequency
-                pub fn freq(&self) -> Hertz {
-                    Hertz::from_raw(self.clk.raw()
-                        / (self.tim.psc().read().bits() + 1)
-                        / (self.tim.arr().read().bits() + 1))
-                }
-            }
-        )+
+    /// Resets counter value
+    pub fn reset(&mut self) {
+        self.tim.reset();
+    }
+
+    /// Returns the currently configured frequency
+    pub fn freq(&self) -> Hertz {
+        self.tim.freq(self.clk)
     }
 }
 
@@ -156,7 +134,7 @@ macro_rules! pwm_q {
         $(
             impl PwmQExt for $TIMX {
                 fn pwm_q(self, freq: Hertz, rcc: &mut Rcc) -> Pwm<Self> {
-                    $timX(self, freq, rcc, ClockSource::Pllq)
+                    Pwm::new(self, freq, rcc, ClockSource::Pllq)
                 }
             }
         )+
@@ -185,17 +163,20 @@ macro_rules! pwm_hal {
                 }
 
 
-                pub fn get_duty(&self) -> u32 {
-                    unsafe { (*$TIMX::ptr()).$ccrx().read().bits() }
+                pub fn get_duty(&self) -> <$TIMX as super::private::TimerCommon>::Width {
+                    // TODO: Remove this cast once stm32g0-staging > 0.16.0 is released
+                    unsafe {
+                        (*$TIMX::ptr()).$ccrx().read().ccr().bits() as <$TIMX as super::private::TimerCommon>::Width
+                    }
                 }
 
-                pub fn get_max_duty(&self) -> u32 {
-                    unsafe { (*$TIMX::ptr()).arr().read().bits() }
-
+                pub fn get_max_duty(&self) -> <$TIMX as super::private::TimerCommon>::Width {
+                    unsafe { (*$TIMX::ptr()).arr().read().arr().bits() }
                 }
 
-                pub fn set_duty(&mut self, duty: u32) {
-                    unsafe { (*$TIMX::ptr()).$ccrx().write(|w| w.bits(duty)); }
+                pub fn set_duty(&mut self, duty: <$TIMX as super::private::TimerCommon>::Width) {
+                    // TODO: Remove this cast once stm32g0-staging > 0.16.0 is released
+                    unsafe { (*$TIMX::ptr()).$ccrx().write(|w| w.ccr().bits(duty as _)); }
                 }
             }
 
@@ -209,7 +190,7 @@ macro_rules! pwm_hal {
                 }
 
                 fn set_duty_cycle(&mut self, duty: u16) -> Result<(), Self::Error> {
-                    self.set_duty(duty as u32);
+                    self.set_duty(duty.into());
                     Ok(())
                 }
             }
@@ -324,31 +305,3 @@ pwm_hal! {
     TIM3: (Channel3, cc3e, ccmr2_output, oc3pe, oc3m, ccr3, ccr3_l, ccr3_h),
     TIM3: (Channel4, cc4e, ccmr2_output, oc4pe, oc4m, ccr4, ccr4_l, ccr4_h),
 }
-
-pwm! {
-    TIM1: (tim1, arr),
-    TIM3: (tim3, arr),
-    TIM14: (tim14, arr),
-    TIM16: (tim16, arr),
-    TIM17: (tim17, arr),
-}
-
-#[cfg(feature = "stm32g0x1")]
-pwm! {
-    TIM2: (tim2, arr),
-}
-
-#[cfg(any(feature = "stm32g070", feature = "stm32g071", feature = "stm32g081"))]
-pwm! {
-    TIM15: (tim15, arr),
-}
-
-#[cfg(feature = "stm32g0x1")]
-pwm_q! {
-    TIM1: tim1,
-}
-
-#[cfg(any(feature = "stm32g071", feature = "stm32g081"))]
-pwm_q! {
-    TIM15: tim15,
-}