From 5c85271e48b4f2be7da47d1728ad1ddb95364ad7 Mon Sep 17 00:00:00 2001
From: Dasky <32983009+daskygit@users.noreply.github.com>
Date: Fri, 25 Oct 2024 18:10:17 +0100
Subject: [PATCH] Add timer_save and _restore functions. (#23887)

Co-authored-by: Sergey Vlasov <sigprof@gmail.com>
Co-authored-by: Nick Brassel <nick@tzarc.org>
---
 platforms/avr/timer.c     | 19 +++++++++++++++++++
 platforms/chibios/timer.c | 21 +++++++++++++++++++++
 platforms/timer.h         |  2 ++
 3 files changed, 42 insertions(+)

diff --git a/platforms/avr/timer.c b/platforms/avr/timer.c
index 9fb671ae8d8..26ba0e29fa2 100644
--- a/platforms/avr/timer.c
+++ b/platforms/avr/timer.c
@@ -25,6 +25,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 // counter resolution 1ms
 // NOTE: union { uint32_t timer32; struct { uint16_t dummy; uint16_t timer16; }}
 volatile uint32_t timer_count;
+static uint32_t   saved_ms;
 
 /** \brief timer initialization
  *
@@ -78,6 +79,24 @@ inline void timer_clear(void) {
     }
 }
 
+/** \brief timer save
+ *
+ * Set saved_ms to current time.
+ */
+void timer_save(void) {
+    saved_ms = timer_read32();
+}
+
+/** \brief timer restore
+ *
+ * Set timer_count to saved_ms
+ */
+void timer_restore(void) {
+    ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+        timer_count = saved_ms;
+    }
+}
+
 /** \brief timer read
  *
  * FIXME: needs doc
diff --git a/platforms/chibios/timer.c b/platforms/chibios/timer.c
index 5e01ea63724..4a347b445d8 100644
--- a/platforms/chibios/timer.c
+++ b/platforms/chibios/timer.c
@@ -5,6 +5,7 @@
 static uint32_t ticks_offset = 0;
 static uint32_t last_ticks   = 0;
 static uint32_t ms_offset    = 0;
+static uint32_t saved_ms     = 0;
 #if CH_CFG_ST_RESOLUTION < 32
 static uint32_t last_systime = 0;
 static uint32_t overflow     = 0;
@@ -73,6 +74,26 @@ void timer_clear(void) {
     chSysUnlock();
 }
 
+__attribute__((weak)) void platform_timer_save_value(uint32_t value) {
+    saved_ms = value;
+}
+
+__attribute__((weak)) uint32_t platform_timer_restore_value(void) {
+    return saved_ms;
+}
+
+void timer_restore(void) {
+    chSysLock();
+    ticks_offset = get_system_time_ticks();
+    last_ticks   = 0;
+    ms_offset    = platform_timer_restore_value();
+    chSysUnlock();
+}
+
+void timer_save(void) {
+    platform_timer_save_value(timer_read32());
+}
+
 uint16_t timer_read(void) {
     return (uint16_t)timer_read32();
 }
diff --git a/platforms/timer.h b/platforms/timer.h
index d55f40f0b0b..fb8ff6bc545 100644
--- a/platforms/timer.h
+++ b/platforms/timer.h
@@ -38,6 +38,8 @@ extern volatile uint32_t timer_count;
 
 void     timer_init(void);
 void     timer_clear(void);
+void     timer_save(void);
+void     timer_restore(void);
 uint16_t timer_read(void);
 uint32_t timer_read32(void);
 uint16_t timer_elapsed(uint16_t last);