1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
|
#include <ch.h>
#include "timer.h"
static uint32_t ticks_offset = 0;
static uint32_t last_ticks = 0;
static uint32_t ms_offset = 0;
#if CH_CFG_ST_RESOLUTION < 32
static uint32_t last_systime = 0;
static uint32_t overflow = 0;
#endif
// Get the current system time in ticks as a 32-bit number.
// This function must be called from within a system lock zone (so that it can safely use and update the static data).
static inline uint32_t get_system_time_ticks(void) {
uint32_t systime = (uint32_t)chVTGetSystemTimeX();
#if CH_CFG_ST_RESOLUTION < 32
// If the real system timer resolution is less than 32 bits, provide the missing bits by checking for the counter
// overflow. For this to work, this function must be called at least once for every overflow of the system timer.
// In the 16-bit case, the corresponding times are:
// - CH_CFG_ST_FREQUENCY = 100000, overflow will occur every ~0.65 seconds
// - CH_CFG_ST_FREQUENCY = 10000, overflow will occur every ~6.5 seconds
// - CH_CFG_ST_FREQUENCY = 1000, overflow will occur every ~65 seconds
if (systime < last_systime) {
overflow += ((uint32_t)1) << CH_CFG_ST_RESOLUTION;
}
last_systime = systime;
systime += overflow;
#endif
return systime;
}
#if CH_CFG_ST_RESOLUTION < 32
static virtual_timer_t update_timer;
// Update the system tick counter every half of the timer overflow period; this should keep the tick counter correct
// even if something blocks timer interrupts for 1/2 of the timer overflow period.
# define UPDATE_INTERVAL (((sysinterval_t)1) << (CH_CFG_ST_RESOLUTION - 1))
// VT callback function to keep the overflow bits of the system tick counter updated.
static void update_fn(void *arg) {
(void)arg;
chSysLockFromISR();
get_system_time_ticks();
chVTSetI(&update_timer, UPDATE_INTERVAL, update_fn, NULL);
chSysUnlockFromISR();
}
#endif
// The highest multiple of CH_CFG_ST_FREQUENCY that fits into uint32_t. This number of ticks will necessarily
// correspond to some integer number of seconds.
#define OVERFLOW_ADJUST_TICKS ((uint32_t)((UINT32_MAX / CH_CFG_ST_FREQUENCY) * CH_CFG_ST_FREQUENCY))
// The time in milliseconds which corresponds to OVERFLOW_ADJUST_TICKS ticks (this is a precise conversion, because
// OVERFLOW_ADJUST_TICKS corresponds to an integer number of seconds).
#define OVERFLOW_ADJUST_MS (TIME_I2MS(OVERFLOW_ADJUST_TICKS))
void timer_init(void) {
timer_clear();
#if CH_CFG_ST_RESOLUTION < 32
chVTObjectInit(&update_timer);
chVTSet(&update_timer, UPDATE_INTERVAL, update_fn, NULL);
#endif
}
void timer_clear(void) {
chSysLock();
ticks_offset = get_system_time_ticks();
last_ticks = 0;
ms_offset = 0;
chSysUnlock();
}
uint16_t timer_read(void) {
return (uint16_t)timer_read32();
}
uint32_t timer_read32(void) {
chSysLock();
uint32_t ticks = get_system_time_ticks() - ticks_offset;
if (ticks < last_ticks) {
// The 32-bit tick counter overflowed and wrapped around. We cannot just extend the counter to 64 bits here,
// because TIME_I2MS() may encounter overflows when handling a 64-bit argument; therefore the solution here is
// to subtract a reasonably large number of ticks from the tick counter to bring its value below the 32-bit
// limit again, and then add the equivalent number of milliseconds to the converted value. (Adjusting just the
// converted value to account for 2**32 ticks is not possible in general, because 2**32 ticks may not correspond
// to an integer number of milliseconds).
ticks -= OVERFLOW_ADJUST_TICKS;
ticks_offset += OVERFLOW_ADJUST_TICKS;
ms_offset += OVERFLOW_ADJUST_MS;
}
last_ticks = ticks;
uint32_t ms_offset_copy = ms_offset; // read while still holding the lock to ensure a consistent value
chSysUnlock();
return (uint32_t)TIME_I2MS(ticks) + ms_offset_copy;
}
uint16_t timer_elapsed(uint16_t last) {
return TIMER_DIFF_16(timer_read(), last);
}
uint32_t timer_elapsed32(uint32_t last) {
return TIMER_DIFF_32(timer_read32(), last);
}
|
