samd/mphalport: Use CYCCNT for SAMD51's mp_hal_ticks_cpu().

And use mp_hal_ticks_us() for SAM21's mp_hal_ticks_cpu().  The SAMD21 has
no CYCCNT register, and the SysTick register has only a 1 ms span (== 48000
count range).

2 files changed, 17 insertions, 3 deletions

diff --git a/ports/samd/mphalport.h b/ports/samd/mphalport.h
index c2aca8b0e..b7e65da35 100644
--- a/ports/samd/mphalport.h
+++ b/ports/samd/mphalport.h
@@ -71,11 +71,21 @@ static inline mp_uint_t mp_hal_ticks_us(void) {
     #endif
 }
 
-// ticks_cpu is limited to a 1 ms period, since the CPU SysTick counter
-// is used for the 1 ms SysTick_Handler interrupt.
+#if defined (MCU_SAMD21)
+
+#define mp_hal_ticks_cpu mp_hal_ticks_us
+
+#elif defined (MCU_SAMD51)
+static inline void mp_hal_ticks_cpu_enable(void) {
+    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
+    DWT->CYCCNT = 0;
+    DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
+}
+
 static inline mp_uint_t mp_hal_ticks_cpu(void) {
-    return (system_time_t)SysTick->VAL;
+    return (system_time_t)DWT->CYCCNT;
 }
+#endif
 
 static inline uint64_t mp_hal_time_ns(void) {
     return mp_hal_ticks_ms_64() * 1000000;
diff --git a/ports/samd/samd_soc.c b/ports/samd/samd_soc.c
index 8d6e808f6..aca6df0dd 100644
--- a/ports/samd/samd_soc.c
+++ b/ports/samd/samd_soc.c
@@ -36,6 +36,7 @@
 #include "samd_soc.h"
 #include "sam.h"
 #include "tusb.h"
+#include "mphalport.h"
 
 static void usb_init(void) {
     // Init USB clock
@@ -110,4 +111,7 @@ void samd_init(void) {
     SysTick_Config(get_cpu_freq() / 1000);
     init_us_counter();
     usb_init();
+    #if defined (MCU_SAMD51)
+    mp_hal_ticks_cpu_enable();
+    #endif
 }