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;
 }
 
 /****************************************************************************