1 files changed, 29 insertions, 8 deletions

diff --git a/src/atsamd/timer.c b/src/atsamd/timer.c
index dc728055..87dc20fa 100644
--- a/src/atsamd/timer.c
+++ b/src/atsamd/timer.c
@@ -11,19 +11,40 @@
 #include "internal.h" // enable_pclock
 #include "sched.h" // DECL_INIT
 
+#if CONFIG_MACH_SAMC21
+ #define TCp TC0
+ #define TCp_IRQn TC0_IRQn
+ #define TCp_GCLK_ID TC0_GCLK_ID
+ #define ID_TCp ID_TC0
+ #define TCd_GCLK_ID TC1_GCLK_ID
+ #define ID_TCd ID_TC1
+#else
+ #define TCp TC4
+ #define TCp_IRQn TC4_IRQn
+ #define TCp_GCLK_ID TC4_GCLK_ID
+ #define ID_TCp ID_TC4
+ #define TCd_GCLK_ID TC3_GCLK_ID
+ #define ID_TCd ID_TC3
+#endif
+
 // Set the next irq time
 static void
 timer_set(uint32_t value)
 {
-    TC4->COUNT32.CC[0].reg = value;
-    TC4->COUNT32.INTFLAG.reg = TC_INTFLAG_MC0;
+    TCp->COUNT32.CC[0].reg = value;
+    TCp->COUNT32.INTFLAG.reg = TC_INTFLAG_MC0;
 }
 
 // Return the current time (in absolute clock ticks).
 uint32_t
 timer_read_time(void)
 {
-    return TC4->COUNT32.COUNT.reg;
+#if CONFIG_MACH_SAMC21
+    TCp->COUNT32.CTRLBSET.reg |= TC_CTRLBSET_CMD(TC_CTRLBCLR_CMD_READSYNC_Val);
+    while (TCp->COUNT32.SYNCBUSY.reg & TC_SYNCBUSY_COUNT)
+        ;
+#endif
+    return TCp->COUNT32.COUNT.reg;
 }
 
 // Activate timer dispatch as soon as possible
@@ -35,7 +56,7 @@ timer_kick(void)
 
 // IRQ handler
 void __aligned(16) // aligning helps stabilize perf benchmarks
-TC4_Handler(void)
+TCp_Handler(void)
 {
     irq_disable();
     uint32_t next = timer_dispatch_many();
@@ -47,15 +68,15 @@ void
 timer_init(void)
 {
     // Supply power and clock to the timer
-    enable_pclock(TC3_GCLK_ID, ID_TC3);
-    enable_pclock(TC4_GCLK_ID, ID_TC4);
+    enable_pclock(TCd_GCLK_ID, ID_TCd);
+    enable_pclock(TCp_GCLK_ID, ID_TCp);
 
     // Configure the timer
-    TcCount32 *tc = &TC4->COUNT32;
+    TcCount32 *tc = &TCp->COUNT32;
     irqstatus_t flag = irq_save();
     tc->CTRLA.reg = 0;
     tc->CTRLA.reg = TC_CTRLA_MODE_COUNT32;
-    armcm_enable_irq(TC4_Handler, TC4_IRQn, 2);
+    armcm_enable_irq(TCp_Handler, TCp_IRQn, 2);
     tc->INTENSET.reg = TC_INTENSET_MC0;
     tc->COUNT.reg = 0;
     timer_kick();