sparc: leon3: Added busy wait function, made wait_ms() work when IRQ is disabled

Signed-off-by: Daniel Hellstrom <daniel@gaisler.com>

diff --git a/arch/sparc/cpu/leon3/cpu_init.c b/arch/sparc/cpu/leon3/cpu_init.c
index 88f82c9..9e294bf 100644 (file)
--- a/arch/sparc/cpu/leon3/cpu_init.c
+++ b/arch/sparc/cpu/leon3/cpu_init.c
@@ -11,6 +11,7 @@
 #include <asm/asi.h>
 #include <asm/leon.h>
 #include <asm/io.h>
+#include <asm/irq.h>
 #include <ambapp.h>
 #include <grlib/irqmp.h>
 #include <grlib/gptimer.h>
@@ -141,14 +142,41 @@ 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);
+       }
 }
 
 int timer_interrupt_init_cpu(void)