1 files changed, 13 insertions, 1 deletions

diff --git a/firmware/system.c b/firmware/system.c
index 87d3b9dc7d..3ec56f7c15 100644
--- a/firmware/system.c
+++ b/firmware/system.c
@@ -302,7 +302,7 @@ void (*vbr[]) (void) __attribute__ ((section (".vectors"))) =
 
 void system_reboot (void)
 {
-    cli ();
+    set_irq_level(15);
 
     asm volatile ("ldc\t%0,vbr" : : "r"(0));
 
@@ -318,6 +318,18 @@ void system_reboot (void)
                   "r"(*(int*)0),"r"(4));
 }
 
+/****************************************************************************
+ * Interrupt level setting
+ ****************************************************************************/
+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 << 4));
+    return (i >> 4) & 0x0f;
+}
+
 void UIE (unsigned int pc) /* Unexpected Interrupt or Exception */
 {
     bool state = true;