* Patch by Mathias Küster, 23 Nov 2004:

[karo-tx-uboot.git] / lib_m68k / time.c

diff --git a/lib_m68k/time.c b/lib_m68k/time.c
index fb997b5b49c06e66c79e1de6befedd87986fce30..e2c1b060b48c281278bac58f1f6962bf82d9d7c1 100644 (file)
--- a/lib_m68k/time.c
+++ b/lib_m68k/time.c
@@ -131,6 +131,28 @@ void set_timer (ulong t)
 
 void udelay(unsigned long usec)
 {
+       volatile unsigned short *timerp;
+       uint tmp;
+
+       timerp = (volatile unsigned short *) (CFG_MBAR + MCFTIMER_BASE3);
+       
+       while (usec > 0) {
+               if (usec > 65000)
+                       tmp = 65000;
+               else
+                       tmp = usec;
+               usec = usec - tmp;
+
+               /* Set up TIMER 3 as timebase clock */
+               timerp[MCFTIMER_PCSR] = MCFTIMER_PCSR_OVW;
+               timerp[MCFTIMER_PMR] = 0;
+               /* set period to 1 us */
+               timerp[MCFTIMER_PCSR] =
+                       (5 << 8) | MCFTIMER_PCSR_EN | MCFTIMER_PCSR_OVW;
+
+                timerp[MCFTIMER_PMR] = tmp;
+               while (timerp[MCFTIMER_PCNTR] > 0);
+       }
 }
 
 void timer_init (void)