Signed-off-by: Daniel Hellstrom daniel@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