3 files changed, 10 insertions, 8 deletions

diff --git a/firmware/target/arm/tcc780x/system-target.h b/firmware/target/arm/tcc780x/system-target.h
index aa3cc7aa4f..0802bb92e0 100644
--- a/firmware/target/arm/tcc780x/system-target.h
+++ b/firmware/target/arm/tcc780x/system-target.h
@@ -34,4 +34,11 @@
 #define inw(a) (*(volatile unsigned short *) (a))
 #define outw(a,b) (*(volatile unsigned short *) (b) = (a))
 
+/* TC32 is configured to 1MHz in clock_init() */
+static inline void udelay(unsigned usecs)
+{
+    unsigned stop = TC32MCNT + usecs;
+    while (TIME_BEFORE(TC32MCNT, stop));
+}
+
 #endif /* SYSTEM_TARGET_H */
diff --git a/firmware/target/arm/tcc780x/system-tcc780x.c b/firmware/target/arm/tcc780x/system-tcc780x.c
index bf3c64cd7a..77ae3a4cd9 100644
--- a/firmware/target/arm/tcc780x/system-tcc780x.c
+++ b/firmware/target/arm/tcc780x/system-tcc780x.c
@@ -231,6 +231,9 @@ static void clock_init(void)
     
     /* configure PCK_TCT to 2Mhz (clock source 4 (Xin) divided by 6) */
     PCLK_TCT = PCK_EN | (CKSEL_XIN<<24) | 5;
+    
+    /* set TC32 timer to XIN divided by 12 (1MHz) */
+    TC32EN = (1<<24) | 11;
 }
 #endif
 
diff --git a/firmware/target/arm/usb-tcc.c b/firmware/target/arm/usb-tcc.c
index 6f15e68af8..611e4be195 100644
--- a/firmware/target/arm/usb-tcc.c
+++ b/firmware/target/arm/usb-tcc.c
@@ -133,14 +133,6 @@ void usb_drv_release_endpoint(int ep)
     restore_irq(flags);
 }
 
-static void udelay(unsigned long msecs)
-{
-    /* TODO: implement me other way */
-    msecs*=126;
-    while (msecs--)
-        asm("nop;");
-}
-
 static inline void pullup_on(void)
 {
     TCC7xx_USB_PHY_CFG = 0x000c;