Diff
1diff --git a/platforms/avr/timer.c b/platforms/avr/timer.c
2index 9fb671ae8d88321d01e54b11138ee5d6713fe0a2..26ba0e29fa2da0f4bd16e53f3fa3694613a1f31a 100644
3--- a/platforms/avr/timer.c
4+++ b/platforms/avr/timer.c
5@@ -25,6 +25,7 @@ along with this program. If not, see <http://www.gnu.org/licenses/>.
6 // counter resolution 1ms
7 // NOTE: union { uint32_t timer32; struct { uint16_t dummy; uint16_t timer16; }}
8 volatile uint32_t timer_count;
9+static uint32_t saved_ms;
10
11 /** \brief timer initialization
12 *
13@@ -78,6 +79,24 @@ inline void timer_clear(void) {
14 }
15 }
16
17+/** \brief timer save
18+ *
19+ * Set saved_ms to current time.
20+ */
21+void timer_save(void) {
22+ saved_ms = timer_read32();
23+}
24+
25+/** \brief timer restore
26+ *
27+ * Set timer_count to saved_ms
28+ */
29+void timer_restore(void) {
30+ ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
31+ timer_count = saved_ms;
32+ }
33+}
34+
35 /** \brief timer read
36 *
37 * FIXME: needs doc
38diff --git a/platforms/chibios/timer.c b/platforms/chibios/timer.c
39index 5e01ea63724519cf6bfcb9ab2ff343e41b229315..4a347b445d837745e481e7c200c2da825f0adcf4 100644
40--- a/platforms/chibios/timer.c
41+++ b/platforms/chibios/timer.c
42@@ -5,6 +5,7 @@
43 static uint32_t ticks_offset = 0;
44 static uint32_t last_ticks = 0;
45 static uint32_t ms_offset = 0;
46+static uint32_t saved_ms = 0;
47 #if CH_CFG_ST_RESOLUTION < 32
48 static uint32_t last_systime = 0;
49 static uint32_t overflow = 0;
50@@ -73,6 +74,26 @@ void timer_clear(void) {
51 chSysUnlock();
52 }
53
54+__attribute__((weak)) void platform_timer_save_value(uint32_t value) {
55+ saved_ms = value;
56+}
57+
58+__attribute__((weak)) uint32_t platform_timer_restore_value(void) {
59+ return saved_ms;
60+}
61+
62+void timer_restore(void) {
63+ chSysLock();
64+ ticks_offset = get_system_time_ticks();
65+ last_ticks = 0;
66+ ms_offset = platform_timer_restore_value();
67+ chSysUnlock();
68+}
69+
70+void timer_save(void) {
71+ platform_timer_save_value(timer_read32());
72+}
73+
74 uint16_t timer_read(void) {
75 return (uint16_t)timer_read32();
76 }
77diff --git a/platforms/timer.h b/platforms/timer.h
78index d55f40f0b0b13f40af60a411cbcd8a7e8ac4c640..fb8ff6bc5454f416d5dd8bbf4fcab91701a1ecbe 100644
79--- a/platforms/timer.h
80+++ b/platforms/timer.h
81@@ -38,6 +38,8 @@ extern volatile uint32_t timer_count;
82
83 void timer_init(void);
84 void timer_clear(void);
85+void timer_save(void);
86+void timer_restore(void);
87 uint16_t timer_read(void);
88 uint32_t timer_read32(void);
89 uint16_t timer_elapsed(uint16_t last);