1 files changed, 14 insertions, 1 deletions

diff --git a/firmware/export/system.h b/firmware/export/system.h
index 5886145d7b..038d9567e0 100644
--- a/firmware/export/system.h
+++ b/firmware/export/system.h
@@ -25,7 +25,6 @@
 
 extern void system_reboot (void);
 extern void system_init(void);
-extern int set_irq_level(int level);
 
 #define FREQ CPU_FREQ
 #define BAUDRATE 9600
@@ -73,6 +72,20 @@ extern int set_irq_level(int level);
 
 #ifndef SIMULATOR
 
+/****************************************************************************
+ * Interrupt level setting
+ * The level is left shifted 4 bits
+ ****************************************************************************/
+#define HIGHEST_IRQ_LEVEL (15<<4)
+static inline int set_irq_level(int level)
+{
+    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));
+    return i;
+}
+
 static inline short SWAB16(short value)
   /*
     result[15..8] = value[ 7..0];