2 files changed, 17 insertions, 0 deletions

diff --git a/firmware/target/arm/tcc77x/system-target.h b/firmware/target/arm/tcc77x/system-target.h
index 16cfb9127d..55dd52ef47 100644
--- a/firmware/target/arm/tcc77x/system-target.h
+++ b/firmware/target/arm/tcc77x/system-target.h
@@ -34,4 +34,13 @@
 #define inw(a) (*(volatile unsigned short *) (a))
 #define outw(a,b) (*(volatile unsigned short *) (b) = (a))
 
+/* TC32 is configured to 1MHz in clock_init() */
+#define USEC_TIMER  TC32MCNT
+
+static inline void udelay(unsigned usecs)
+{
+    unsigned stop = USEC_TIMER + usecs;
+    while (TIME_BEFORE(USEC_TIMER, stop));
+}
+
 #endif /* SYSTEM_TARGET_H */
diff --git a/firmware/target/arm/tcc77x/system-tcc77x.c b/firmware/target/arm/tcc77x/system-tcc77x.c
index 90e53afb14..29227b52a3 100644
--- a/firmware/target/arm/tcc77x/system-tcc77x.c
+++ b/firmware/target/arm/tcc77x/system-tcc77x.c
@@ -187,6 +187,14 @@ static void clock_init(void)
         "nop      \n\t"
         "nop      \n\t"
     );
+
+    /* Enable Z-Clock */
+    PCLKCFG5 |= (1<<31) | (4<<28); /* Timer Z-Clock enable, XIN direct*/
+
+    /* Set TC32 timer to be our USEC_TIMER (Xin divided by 12 = 1MHz) */
+    TC32MCNT = 0;
+    TC32LDV = 0;
+    TC32EN = (1<<24) | 11;
 }
 
 static void cpu_init(void)