[U-Boot] [PATCH 09/17] LEON3: added busy wait function, made wait_ms() work when IRQ is disabled.

[Daniel Hellstrom]

Signed-off-by: Daniel Hellstrom <daniel at gaisler.com>
---
 cpu/leon3/cpu_init.c |   30 ++++++++++++++++++++++++++++--
 1 files changed, 28 insertions(+), 2 deletions(-)

diff --git a/cpu/leon3/cpu_init.c b/cpu/leon3/cpu_init.c
index e2e1181..d2f47e1 100644
--- a/cpu/leon3/cpu_init.c
+++ b/cpu/leon3/cpu_init.c
@@ -142,14 +142,40 @@ int cpu_init_r(void)
 	return (0);
 }
 
+/* Busy wait a number of ms */
+void cpu_wait_ms_busy(unsigned long ms)
+{
+	unsigned int ms_delay;
+	volatile unsigned int tmp;
+
+	/* ~10-20 cycles per decrement */
+	ms_delay = leon_cpu_freq / (1000 * 10);
+	do {
+		/* Wait ~1ms */
+		tmp = ms_delay;
+		while ( tmp-- > 0 )
+			;
+	} while ( --ms > 0 );
+}
+
 /* Uses Timer 0 to get accurate
  * pauses. Max 2 raised to 32 ticks
  *
  */
 void cpu_wait_ticks(unsigned long ticks)
 {
-	unsigned long start = get_timer(0);
-	while (get_timer(start) < ticks) ;
+	unsigned long start;
+
+	if ( interrupt_is_enabled() ) {
+		start = get_timer(0);
+		while (get_timer(start) < ticks) ;
+	} else {
+		/* Interrupts disabled, this means that we cannot 
+		 * use get_timer(), it relies on IRQ. Instead the
+		 * CPU frequency is used.
+		 */
+		cpu_wait_ms_busy( ticks2usec(ticks) / 1000 );
+	}
 }
 
 /* initiate and setup timer0 interrupt to 1MHz
-- 
1.5.4