arm: Use DWT CYCCNT for timecounter if available

2 files changed, 74 insertions, 15 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 d481c440a9..16a6b83262 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
@@ -25,19 +25,20 @@ static void Clock_isr(void *arg);
 
 typedef struct {
   rtems_timecounter_simple base;
+  void (*tick)(void);
   bool countflag;
 } ARMV7M_Timecounter;
 
 static ARMV7M_Timecounter _ARMV7M_TC;
 
-static uint32_t _ARMV7M_TC_get(rtems_timecounter_simple *tc)
+static uint32_t _ARMV7M_TC_systick_get(rtems_timecounter_simple *tc)
 {
   volatile ARMV7M_Systick *systick = _ARMV7M_Systick;
 
   return systick->cvr;
 }
 
-static bool _ARMV7M_TC_is_pending(rtems_timecounter_simple *base)
+static bool _ARMV7M_TC_systick_is_pending(rtems_timecounter_simple *base)
 {
   ARMV7M_Timecounter *tc = (ARMV7M_Timecounter *) base;
   rtems_interrupt_level level;
@@ -58,16 +59,16 @@ static bool _ARMV7M_TC_is_pending(rtems_timecounter_simple *base)
   return countflag;
 }
 
-static uint32_t _ARMV7M_TC_get_timecount(struct timecounter *tc)
+static uint32_t _ARMV7M_TC_systick_get_timecount(struct timecounter *tc)
 {
   return rtems_timecounter_simple_downcounter_get(
     tc,
-    _ARMV7M_TC_get,
-    _ARMV7M_TC_is_pending
+    _ARMV7M_TC_systick_get,
+    _ARMV7M_TC_systick_is_pending
   );
 }
 
-static void _ARMV7M_TC_at_tick(rtems_timecounter_simple *base)
+static void _ARMV7M_TC_systick_at_tick(rtems_timecounter_simple *base)
 {
   ARMV7M_Timecounter *tc = (ARMV7M_Timecounter *) base;
   volatile ARMV7M_Systick *systick = _ARMV7M_Systick;
@@ -78,15 +79,32 @@ static void _ARMV7M_TC_at_tick(rtems_timecounter_simple *base)
   systick->csr;
 }
 
-static void _ARMV7M_TC_tick(void)
+static void _ARMV7M_TC_systick_tick(void)
 {
   rtems_timecounter_simple_downcounter_tick(
     &_ARMV7M_TC.base,
-    _ARMV7M_TC_get,
-    _ARMV7M_TC_at_tick
+    _ARMV7M_TC_systick_get,
+    _ARMV7M_TC_systick_at_tick
   );
 }
 
+static uint32_t _ARMV7M_TC_dwt_get_timecount(struct timecounter *tc)
+{
+  volatile ARMV7M_DWT *dwt = _ARMV7M_DWT;
+
+  return dwt->cyccnt;
+}
+
+static void _ARMV7M_TC_dwt_tick(void)
+{
+  rtems_timecounter_tick();
+}
+
+static void _ARMV7M_TC_tick(void)
+{
+  (*_ARMV7M_TC.tick)();
+}
+
 static void _ARMV7M_Systick_handler(void)
 {
   _ARMV7M_Interrupt_service_enter();
@@ -105,6 +123,7 @@ static void _ARMV7M_Systick_handler_install(void)
 
 static void _ARMV7M_Systick_initialize(void)
 {
+  volatile ARMV7M_DWT *dwt = _ARMV7M_DWT;
   volatile ARMV7M_Systick *systick = _ARMV7M_Systick;
   #ifdef BSP_ARMV7M_SYSTICK_FREQUENCY
     uint64_t freq = BSP_ARMV7M_SYSTICK_FREQUENCY;
@@ -113,6 +132,7 @@ static void _ARMV7M_Systick_initialize(void)
   #endif
   uint64_t us_per_tick = rtems_configuration_get_microseconds_per_tick();
   uint64_t interval = (freq * us_per_tick) / 1000000ULL;
+  uint32_t dwt_ctrl;
 
   systick->rvr = (uint32_t) interval;
   systick->cvr = 0;
@@ -120,12 +140,24 @@ static void _ARMV7M_Systick_initialize(void)
     | ARMV7M_SYSTICK_CSR_TICKINT
     | ARMV7M_SYSTICK_CSR_CLKSOURCE;
 
-  rtems_timecounter_simple_install(
-    &_ARMV7M_TC.base,
-    freq,
-    interval,
-    _ARMV7M_TC_get_timecount
-  );
+  dwt_ctrl = dwt->ctrl;
+  if ((dwt_ctrl & ARMV7M_DWT_CTRL_NOCYCCNT) == 0) {
+    dwt->ctrl = dwt_ctrl | ARMV7M_DWT_CTRL_CYCCNTENA;
+    _ARMV7M_TC.base.tc.tc_get_timecount = _ARMV7M_TC_dwt_get_timecount;
+    _ARMV7M_TC.base.tc.tc_counter_mask = 0xffffffff;
+    _ARMV7M_TC.base.tc.tc_frequency = freq;
+    _ARMV7M_TC.base.tc.tc_quality = RTEMS_TIMECOUNTER_QUALITY_CLOCK_DRIVER;
+    _ARMV7M_TC.tick = _ARMV7M_TC_dwt_tick;
+    rtems_timecounter_install(&_ARMV7M_TC.base.tc);
+  } else {
+    _ARMV7M_TC.tick = _ARMV7M_TC_systick_tick;
+    rtems_timecounter_simple_install(
+      &_ARMV7M_TC.base,
+      freq,
+      interval,
+      _ARMV7M_TC_systick_get_timecount
+    );
+  }
 }
 
 static void _ARMV7M_Systick_cleanup(void)
diff --git a/cpukit/score/cpu/arm/rtems/score/armv7m.h b/cpukit/score/cpu/arm/rtems/score/armv7m.h
index c5e473ec0a..251ecdce39 100644
--- a/cpukit/score/cpu/arm/rtems/score/armv7m.h
+++ b/cpukit/score/cpu/arm/rtems/score/armv7m.h
@@ -75,6 +75,30 @@ typedef struct {
 } ARMV7M_Exception_frame;
 
 typedef struct {
+  uint32_t comp;
+  uint32_t mask;
+  uint32_t function;
+  uint32_t reserved;
+} ARMV7M_DWT_comparator;
+
+typedef struct {
+#define ARMV7M_DWT_CTRL_NOCYCCNT (1U << 25)
+#define ARMV7M_DWT_CTRL_CYCCNTENA (1U << 0)
+  uint32_t ctrl;
+  uint32_t cyccnt;
+  uint32_t cpicnt;
+  uint32_t exccnt;
+  uint32_t sleepcnt;
+  uint32_t lsucnt;
+  uint32_t foldcnt;
+  uint32_t pcsr;
+  ARMV7M_DWT_comparator comparator[249];
+  uint32_t reserved_e0001fb0[1];
+  uint32_t lar;
+  uint32_t lsr;
+} ARMV7M_DWT;
+
+typedef struct {
   uint32_t cpuid;
 
 #define ARMV7M_SCB_ICSR_NMIPENDSET (1U << 31)
@@ -323,6 +347,7 @@ typedef struct {
     0 \
   }
 
+#define ARMV7M_DWT_BASE 0xe0001000
 #define ARMV7M_SCS_BASE 0xe000e000
 #define ARMV7M_ICTAC_BASE (ARMV7M_SCS_BASE + 0x0)
 #define ARMV7M_SYSTICK_BASE (ARMV7M_SCS_BASE + 0x10)
@@ -330,6 +355,8 @@ typedef struct {
 #define ARMV7M_SCB_BASE (ARMV7M_SCS_BASE + 0xd00)
 #define ARMV7M_MPU_BASE (ARMV7M_SCS_BASE + 0xd90)
 
+#define _ARMV7M_DWT \
+  ((volatile ARMV7M_DWT *) ARMV7M_DWT_BASE)
 #define _ARMV7M_ICTAC \
   ((volatile ARMV7M_ICTAC *) ARMV7M_ICTAC_BASE)
 #define _ARMV7M_SCB \