[PATCH 3/9] pxa2x0_clock.c: Rework the delay() function

[Marek Vasut]

---
 sys/arch/arm/xscale/pxa2x0_clock.c |   44 +++++++++++++----------------------
 1 files changed, 16 insertions(+), 28 deletions(-)

diff --git a/sys/arch/arm/xscale/pxa2x0_clock.c 
b/sys/arch/arm/xscale/pxa2x0_clock.c
index b781edf..8ea5072 100644
--- a/sys/arch/arm/xscale/pxa2x0_clock.c
+++ b/sys/arch/arm/xscale/pxa2x0_clock.c
@@ -241,26 +241,10 @@ cpu_initclocks()
 }
 
 void
-delay(usecs)
-       u_int usecs;
+delay(u_int usecs)
 {
-       u_int32_t clock, oclock, delta, delaycnt;
+       u_int32_t ost, i;
        volatile int j;
-       int csec, usec;
-
-       if (usecs > (0x80000000 / (pxaost_sc->sc_clock_hz))) {
-               csec = usecs / 10000;
-               usec = usecs % 10000;
-
-               delaycnt = (pxaost_sc->sc_clock_hz / 100) * csec +
-                   (pxaost_sc->sc_clock_hz / 100) * usec / 10000;
-       } else {
-               delaycnt = pxaost_sc->sc_clock_hz * usecs / 1000000;
-       }
-
-       if (delaycnt <= 1)
-               for (j = 100; j > 0; j--)
-                       ;
 
        if (!pxaost_sc) {
                /* clock isn't initialized yet */
@@ -270,16 +254,20 @@ delay(usecs)
                return;
        }
 
-       oclock = bus_space_read_4(pxaost_sc->sc_iot, pxaost_sc->sc_ioh,
-           OST_OSCR0);
+       /* 1us == 3.25 ticks on pxa27x */
+       /* 1us == 3.6864 ticks on pxa25x and 26x */
+       i = usecs / 1000000;
+       usecs %= 1000000;
 
-       while (1) {
-               for (j = 100; j > 0; j--)
-                       ;
-               clock = bus_space_read_4(pxaost_sc->sc_iot, pxaost_sc->sc_ioh,
-                   OST_OSCR0);
-               delta = clock - oclock;
-               if (delta > delaycnt)
-                       break;
+       while (i--) {
+               ost = bus_space_read_4(pxaost_sc->sc_iot, pxaost_sc->sc_ioh,
+                   OST_OSCR0) + pxaost_sc->sc_clock_hz;        /* 1s ahead */
+               while((signed)(ost - bus_space_read_4(pxaost_sc->sc_iot,
+                       pxaost_sc->sc_ioh, OST_OSCR0)) > 0);
        }
+
+       ost = bus_space_read_4(pxaost_sc->sc_iot, pxaost_sc->sc_ioh,
+           OST_OSCR0) + ((pxaost_sc->sc_clock_hz * usecs) / 1000000);
+       while((signed)(ost - bus_space_read_4(pxaost_sc->sc_iot,
+               pxaost_sc->sc_ioh, OST_OSCR0)) > 0);
 }
-- 
1.7.0.5