From e862d711121a4920346fadf4b038e72e20f87730 Mon Sep 17 00:00:00 2001
From: Finomnis <finomnis@gmail.com>
Date: Fri, 3 Nov 2023 10:31:50 +0100
Subject: [PATCH] Fixes. Now finally working!

---
 rtic-monotonics/src/imxrt.rs | 33 ++++++++++++++++++++-------------
 1 file changed, 20 insertions(+), 13 deletions(-)

diff --git a/rtic-monotonics/src/imxrt.rs b/rtic-monotonics/src/imxrt.rs
index a0906c66996a..43bb3043e49f 100644
--- a/rtic-monotonics/src/imxrt.rs
+++ b/rtic-monotonics/src/imxrt.rs
@@ -137,6 +137,9 @@ macro_rules! make_timer {
                     FRR: 1,     // Free-Run mode
                 );
 
+                // Reset period
+                $period.store(0, Ordering::Relaxed);
+
                 // Prescaler
                 ral::modify_reg!(ral::gpt, gpt, PR,
                     PRESCALER: (prescaler - 1), // Scale to our desired clock rate
@@ -247,21 +250,22 @@ macro_rules! make_timer {
             }
 
             fn set_compare(instant: Self::Instant) {
-                // let now = Self::now();
-                // let max_ticks = $bits::MAX as u64;
+                let gpt = unsafe{ $timer::instance() };
 
-                // // Since the timer may or may not overflow based on the requested compare val, we check how many ticks are left.
-                // let val = match instant.checked_duration_since(now) {
-                //     None => 0, // In the past
-                //     Some(x) if x.ticks() <= max_ticks => instant.duration_since_epoch().ticks() as $bits, // Will not overflow
-                //     Some(_x) => $timer.cnt().read().cnt().wrapping_add($bits::MAX - 1), // Will overflow
-                // };
+                // Set the timer regardless of whether it is multiple periods in the future,
+                // or even already in the past.
+                // The worst thing that can happen is a spurious wakeup, and with a timer
+                // period of half an hour, this is hardly a problem.
 
-                // $timer.ccr(1).write(|r| r.set_ccr(val));
+                let ticks = instant.duration_since_epoch().ticks();
+                let ticks_wrapped = ticks as u32;
+
+                ral::write_reg!(ral::gpt, gpt, OCR[1], ticks_wrapped);
             }
 
             fn clear_compare_flag() {
-                // $timer.sr().modify(|r| r.set_ccif(1, false));
+                let gpt = unsafe{ $timer::instance() };
+                ral::write_reg!(ral::gpt, gpt, SR, OF2: 1);
             }
 
             fn pend_interrupt() {
@@ -273,12 +277,15 @@ macro_rules! make_timer {
 
                 let (rollover, half_rollover) = ral::read_reg!(ral::gpt, gpt, SR, ROV, OF1);
 
-                if((rollover != 0) || (half_rollover != 0)){
+                if rollover != 0 {
                     $period.fetch_add(1, Ordering::Relaxed);
+                    ral::write_reg!(ral::gpt, gpt, SR, ROV: 1);
                 }
 
-                // Clear all status registers.
-                ral::write_reg!(ral::gpt, gpt, SR, 0b11_1111);
+                if half_rollover != 0 {
+                    $period.fetch_add(1, Ordering::Relaxed);
+                    ral::write_reg!(ral::gpt, gpt, SR, OF1: 1);
+                }
             }
         }
     };