1 files changed, 37 insertions, 28 deletions

diff --git a/c/src/lib/libbsp/arm/shared/armv7m/clock/armv7m-clock-config.c b/c/src/lib/libbsp/arm/shared/armv7m/clock/armv7m-clock-config.c
index 8e4ae338f8..e78684c8d2 100644
--- a/c/src/lib/libbsp/arm/shared/armv7m/clock/armv7m-clock-config.c
+++ b/c/src/lib/libbsp/arm/shared/armv7m/clock/armv7m-clock-config.c
@@ -13,6 +13,7 @@
  */
 
 #include <rtems.h>
+#include <rtems/timecounter.h>
 #include <rtems/score/armv7m.h>
 
 #include <bsp.h>
@@ -22,15 +23,35 @@
 /* This is defined in clockdrv_shell.h */
 static void Clock_isr(void *arg);
 
-#define _ARMV7M_Systick_get_factor(freq) \
-  ((1000000000ULL << 32) / (freq))
+static rtems_timecounter_simple _ARMV7M_TC;
 
-#ifdef BSP_ARMV7M_SYSTICK_FREQUENCY
-  #define _ARMV7M_Systick_factor \
-    _ARMV7M_Systick_get_factor(BSP_ARMV7M_SYSTICK_FREQUENCY)
-#else
-  static uint64_t _ARMV7M_Systick_factor;
-#endif
+static uint32_t _ARMV7M_TC_get(rtems_timecounter_simple *tc)
+{
+  volatile ARMV7M_Systick *systick = _ARMV7M_Systick;
+
+  return systick->cvr;
+}
+
+static bool _ARMV7M_TC_is_pending(rtems_timecounter_simple *tc)
+{
+  volatile ARMV7M_SCB *scb = _ARMV7M_SCB;
+
+  return ((scb->icsr & ARMV7M_SCB_ICSR_PENDSTSET) != 0);
+}
+
+static uint32_t _ARMV7M_TC_get_timecount(struct timecounter *tc)
+{
+  return rtems_timecounter_simple_downcounter_get(
+    tc,
+    _ARMV7M_TC_get,
+    _ARMV7M_TC_is_pending
+  );
+}
+
+static void _ARMV7M_TC_tick(void)
+{
+  rtems_timecounter_simple_downcounter_tick(&_ARMV7M_TC, _ARMV7M_TC_get);
+}
 
 static void _ARMV7M_Systick_at_tick(void)
 {
@@ -67,15 +88,18 @@ static void _ARMV7M_Systick_initialize(void)
   uint64_t us_per_tick = rtems_configuration_get_microseconds_per_tick();
   uint64_t interval = (freq * us_per_tick) / 1000000ULL;
 
-  #ifndef BSP_ARMV7M_SYSTICK_FREQUENCY
-    _ARMV7M_Systick_factor = _ARMV7M_Systick_get_factor(freq);
-  #endif
-
   systick->rvr = (uint32_t) interval;
   systick->cvr = 0;
   systick->csr = ARMV7M_SYSTICK_CSR_ENABLE
     | ARMV7M_SYSTICK_CSR_TICKINT
     | ARMV7M_SYSTICK_CSR_CLKSOURCE;
+
+  rtems_timecounter_simple_install(
+    &_ARMV7M_TC,
+    freq,
+    interval,
+    _ARMV7M_TC_get_timecount
+  );
 }
 
 static void _ARMV7M_Systick_cleanup(void)
@@ -85,19 +109,7 @@ static void _ARMV7M_Systick_cleanup(void)
   systick->csr = 0;
 }
 
-static uint32_t _ARMV7M_Systick_nanoseconds_since_last_tick(void)
-{
-  volatile ARMV7M_Systick *systick = _ARMV7M_Systick;
-  volatile ARMV7M_SCB *scb = _ARMV7M_SCB;
-  uint32_t rvr = systick->rvr;
-  uint32_t c = rvr - systick->cvr;
-
-  if ((scb->icsr & ARMV7M_SCB_ICSR_PENDSTSET) != 0) {
-    c = rvr - systick->cvr + rvr;
-  }
-
-  return (uint32_t) ((c * _ARMV7M_Systick_factor) >> 32);
-}
+#define Clock_driver_timecounter_tick() _ARMV7M_TC_tick()
 
 #define Clock_driver_support_at_tick() \
   _ARMV7M_Systick_at_tick()
@@ -114,9 +126,6 @@ static uint32_t _ARMV7M_Systick_nanoseconds_since_last_tick(void)
 #define Clock_driver_support_shutdown_hardware() \
   _ARMV7M_Systick_cleanup()
 
-#define Clock_driver_nanoseconds_since_last_tick \
-  _ARMV7M_Systick_nanoseconds_since_last_tick
-
 /* Include shared source clock driver code */
 #include "../../../../shared/clockdrv_shell.h"