arm: zynq: U-Boot udelay < 1000 FIX

Rework the __udelay function of U-Boot Zynq Arch to handle
delay < 1000 usec

Signed-off-by: David Andrey <david.andrey@netmodule.com>
Signed-off-by: Michal Simek <michal.simek@xilinx.com>
Reviewed-by: Tom Rini <trini@ti.com>

diff --git a/arch/arm/cpu/armv7/zynq/timer.c b/arch/arm/cpu/armv7/zynq/timer.c
index 45b405a4ba2a764831615a24ecd86d2cb6214466..5d8bb24096e7212f71688c84c160cb16be02efbc 100644 (file)
--- a/arch/arm/cpu/armv7/zynq/timer.c
+++ b/arch/arm/cpu/armv7/zynq/timer.c
@@ -114,15 +114,43 @@ ulong get_timer_masked(void)
 
 void __udelay(unsigned long usec)
 {
-       unsigned long long tmp;
-       ulong tmo;
-
-       tmo = usec / (1000000 / CONFIG_SYS_HZ);
-       tmp = get_ticks() + tmo; /* Get current timestamp */
-
-       while (get_ticks() < tmp) { /* Loop till event */
-                /* NOP */;
-       }
+       u32 countticks;
+       u32 timeend;
+       u32 timediff;
+       u32 timenow;
+
+       if (usec == 0)
+               return;
+
+       countticks = (u32) (((unsigned long long) TIMER_TICK_HZ * usec) /
+                                                               1000000);
+
+       /* decrementing timer */
+       timeend = readl(&timer_base->counter) - countticks;
+
+#if TIMER_LOAD_VAL != 0xFFFFFFFF
+       /* do not manage multiple overflow */
+       if (countticks >= TIMER_LOAD_VAL)
+               countticks = TIMER_LOAD_VAL - 1;
+#endif
+
+       do {
+               timenow = readl(&timer_base->counter);
+
+               if (timenow >= timeend) {
+                       /* normal case */
+                       timediff = timenow - timeend;
+               } else {
+                       if ((TIMER_LOAD_VAL - timeend + timenow) <=
+                                                               countticks) {
+                               /* overflow */
+                               timediff = TIMER_LOAD_VAL - timeend + timenow;
+                       } else {
+                               /* missed the exact match */
+                               break;
+                       }
+               }
+       } while (timediff > 0);
 }
 
 /* Timer without interrupts */