From 385f73579979b7b7e90966e30d2d05175bcdcc5d Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 8 Aug 2018 16:57:05 +1000
Subject: [PATCH] HAL_ChibiOS: avoid 64 bit maths in AP_HAL::micros()

this saves a few cycles
---
 libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c   | 62 ++++++++++++-------
 libraries/AP_HAL_ChibiOS/hwdef/common/hrt.h   |  4 +-
 .../AP_HAL_ChibiOS/hwdef/common/stm32_util.c  |  4 +-
 libraries/AP_HAL_ChibiOS/system.cpp           |  8 +--
 4 files changed, 50 insertions(+), 28 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c
index 3fe4a2f5e1..da05c697a6 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c
@@ -20,28 +20,48 @@
 #include "hrt.h"
 #include <stdint.h>
 
-static uint64_t timer_base = 0;
+static uint64_t timer_base_us;
+static uint32_t timer_base_ms;
+static volatile systime_t last_systime;
 
-uint64_t hrt_micros()
+static uint32_t get_systime_us32(void)
 {
-	static volatile uint64_t last_micros;
+    systime_t now = chVTGetSystemTimeX();
+    if (now < last_systime) {
+        timer_base_us += TIME_MAX_SYSTIME;
+        timer_base_ms += TIME_MAX_SYSTIME/1000;
+    }
+    last_systime = now;
+    return now;
+}
+
+/*
+  we use chSysGetStatusAndLockX() to prevent an interrupt while
+  allowing this call from any context
+*/
+
+uint64_t hrt_micros64()
+{
+    syssts_t sts = chSysGetStatusAndLockX();
+    uint32_t now = get_systime_us32();
+    uint64_t ret = timer_base_us + now;
+    chSysRestoreStatusX(sts);
+    return ret;
+}
 
-    /*
-      use chSysGetStatusAndLockX() to prevent an interrupt while
-      allowing this call from any context
-     */
-	syssts_t sts = chSysGetStatusAndLockX();
-	uint64_t micros;
-	micros = timer_base + (uint64_t)chVTGetSystemTimeX();
-	// we are doing this to avoid an additional interupt routing
-	// since we are definitely going to get called atleast once in
-	// a full timer period
-	if (last_micros > micros) {
-        const uint64_t step = TIME_MAX_SYSTIME;
-		timer_base += step;
-		micros += step;
-	}
-	last_micros = micros;
-	chSysRestoreStatusX(sts);
-	return micros;
+uint32_t hrt_micros32()
+{
+    syssts_t sts = chSysGetStatusAndLockX();
+    uint32_t ret = get_systime_us32();
+    chSysRestoreStatusX(sts);
+    return ret;
+}
+
+uint32_t hrt_millis32()
+{
+    syssts_t sts = chSysGetStatusAndLockX();
+    uint32_t now = get_systime_us32();
+    uint32_t ret = (now / 1000U) + timer_base_ms;
+    chSysRestoreStatusX(sts);
+    return ret;
 }
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.h b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.h
index 055ec1d5b3..9ecfd9a373 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.h
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.h
@@ -5,7 +5,9 @@
 extern "C" {
 #endif
 void hrt_init(void);
-uint64_t hrt_micros(void);
+uint64_t hrt_micros64(void);
+uint32_t hrt_micros32(void);
+uint32_t hrt_millis32(void);
 #ifdef __cplusplus
 }
 #endif
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
index ac224d2134..c73fb12040 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
@@ -110,7 +110,7 @@ void memory_flush_all(void)
  */
 void stm32_set_utc_usec(uint64_t time_utc_usec)
 {
-    uint64_t now = hrt_micros();
+    uint64_t now = hrt_micros64();
     if (now <= time_utc_usec) {
         utc_time_offset = time_utc_usec - now;
     }
@@ -121,7 +121,7 @@ void stm32_set_utc_usec(uint64_t time_utc_usec)
 */
 uint64_t stm32_get_utc_usec()
 {
-    return hrt_micros() + utc_time_offset;
+    return hrt_micros64() + utc_time_offset;
 }
 
 struct utc_tm {
diff --git a/libraries/AP_HAL_ChibiOS/system.cpp b/libraries/AP_HAL_ChibiOS/system.cpp
index 23f5605f1c..c607e86c67 100644
--- a/libraries/AP_HAL_ChibiOS/system.cpp
+++ b/libraries/AP_HAL_ChibiOS/system.cpp
@@ -151,22 +151,22 @@ void panic(const char *errormsg, ...)
 
 uint32_t micros()
 {
-    return micros64() & 0xFFFFFFFF;
+    return hrt_micros32();
 }
 
 uint32_t millis()
 {
-    return millis64() & 0xFFFFFFFF;
+    return hrt_millis32();
 }
 
 uint64_t micros64()
 {
-    return hrt_micros();
+    return hrt_micros64();
 }
 
 uint64_t millis64()
 {
-    return micros64() / 1000;
+    return hrt_micros64() / 1000U;
 }
 
 } // namespace AP_HAL