summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
-rw-r--r--firmware/kernel.c18
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;
}
/****************************************************************************