diff options
-rw-r--r-- | firmware/kernel.c | 18 |
1 files changed, 5 insertions, 13 deletions
diff --git a/firmware/kernel.c b/firmware/kernel.c index b0c28e56dc..8474126a0d 100644 --- a/firmware/kernel.c +++ b/firmware/kernel.c @@ -53,21 +53,13 @@ void yield(void) /**************************************************************************** * Interrupt level setting ****************************************************************************/ -static int current_irq_level = 15; - int set_irq_level(int level) { - int old; - - /* First raise to highest level and update the shadow */ - asm volatile ("ldc %0, sr" : : "r" (15<<4)); - old = current_irq_level; - current_irq_level = level; - - /* Then set the wanted level */ - asm volatile ("ldc %0, sr" : : "r" ((unsigned int)level<<4)); - - return ((unsigned int)old >> 4) & 0x0f; + int i; + /* Read the old level and set the new one */ + asm volatile ("stc sr, %0" : "=r" (i)); + asm volatile ("ldc %0, sr" : : "r" (level << 4)); + return (i >> 4) & 0x0f; } /**************************************************************************** |