1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
diff --git a/firmware/target/arm/as3525/system-target.h b/firmware/target/arm/as3525/system-target.h
index d2cf994..dd8bc07 100644
--- a/firmware/target/arm/as3525/system-target.h
+++ b/firmware/target/arm/as3525/system-target.h
@@ -62,7 +62,8 @@ extern int c200v2_variant;
 static inline void udelay(unsigned usecs) __attribute__((always_inline));
 static inline void udelay(unsigned usecs)
 {
-    int now, end;
+    unsigned now;
+    int end;

     /**
      * we're limited to 1.5us multiplies due to the odd timer frequency (1.5MHz),
@@ -94,9 +95,23 @@ static inline void udelay(unsigned usecs)
         int delay = usecs * 2 / 3; /* us * 1.5 = us*timer_period */
         end = now - delay;
     }
+
     /* underrun ? */
-    if (end < 0)
+    if (end >= 0)
+        while(TIMER2_VALUE > (unsigned)end) ;
+    else
+    {
+        unsigned old = now;
+        while(now <= old)
+        {
+            old = now;
+            now = TIMER2_VALUE;
+        }
+        /* timer wrap */
+
         end += TIMER_PERIOD;
-    while(TIMER2_VALUE != (unsigned)end);
+        while(TIMER2_VALUE > (unsigned)end) ;
+    }
+
 }
 #endif /* SYSTEM_TARGET_H */