From c5db272c911e886a0674bbf959b54e7b50c36636 Mon Sep 17 00:00:00 2001
From: Fred Sundvik <fsundvik@gmail.com>
Date: Fri, 22 Jun 2018 20:10:20 +0300
Subject: [PATCH] Make sure the timer wraps around correctly independent of the
 os tick frequency

---
 tmk_core/common/chibios/timer.c | 44 ++++++++++++++++++++++-----------
 1 file changed, 29 insertions(+), 15 deletions(-)

diff --git a/tmk_core/common/chibios/timer.c b/tmk_core/common/chibios/timer.c
index 3de4cc368..473e533ca 100644
--- a/tmk_core/common/chibios/timer.c
+++ b/tmk_core/common/chibios/timer.c
@@ -2,26 +2,40 @@
 
 #include "timer.h"
 
-void timer_init(void) {}
+static systime_t last_systime = 0;
+static systime_t overflow = 0;
+static uint32_t current_time_ms = 0;
 
-void timer_clear(void) {}
-
-uint16_t timer_read(void)
-{
-    return (uint16_t)ST2MS(chVTGetSystemTime());
+void timer_init(void) {
+  timer_clear();
 }
 
-uint32_t timer_read32(void)
-{
-    return ST2MS(chVTGetSystemTime());
+void timer_clear(void) {
+  last_systime = chVTGetSystemTime();
+  overflow = 0;
+  current_time_ms = 0;
 }
 
-uint16_t timer_elapsed(uint16_t last)
-{
-    return (uint16_t)(ST2MS(chVTTimeElapsedSinceX(MS2ST(last))));
+uint16_t timer_read(void) {
+  return (uint16_t)timer_read32();
 }
 
-uint32_t timer_elapsed32(uint32_t last)
-{
-    return ST2MS(chVTTimeElapsedSinceX(MS2ST(last)));
+uint32_t timer_read32(void) {
+  // Note: We assume that the timer update is called at least once betweeen every wrap around of the system time
+  systime_t current_systime = chVTGetSystemTime();
+  systime_t elapsed = current_systime - last_systime + overflow;
+  uint32_t elapsed_ms = ST2MS(elapsed);
+  current_time_ms += elapsed_ms;
+  overflow = elapsed - MS2ST(elapsed_ms);
+  last_systime = current_systime;
+
+  return current_time_ms;
+}
+
+uint16_t timer_elapsed(uint16_t last) {
+  return timer_read() - last;
+}
+
+uint32_t timer_elapsed32(uint32_t last) {
+  return timer_read32() - last;
 }