1 files changed, 12 insertions, 1 deletions

diff --git a/firmware/export/system.h b/firmware/export/system.h
index 380c229f1d..e88c793fae 100644
--- a/firmware/export/system.h
+++ b/firmware/export/system.h
@@ -345,7 +345,7 @@ static inline int set_irq_level(int level)
 
 static inline void enable_fiq(void)
 {
-    /* enable FIQ */
+    /* Clear FIQ disable bit */
     asm volatile (
         "mrs     r0, cpsr         \n"\
         "bic     r0, r0, #0x40    \n"\
@@ -354,6 +354,17 @@ static inline void enable_fiq(void)
     );
 }
 
+static inline void disable_fiq(void)
+{
+    /* Set FIQ disable bit */
+    asm volatile (
+        "mrs     r0, cpsr         \n"\
+        "orr     r0, r0, #0x40    \n"\
+        "msr     cpsr_c, r0         "
+        : : : "r0"
+    );
+}
+
 #define invalidate_icache()
 
 #if CONFIG_CPU == PNX0101