From 298f64a2f9de2b23e0846f853df5a0112d7e0ea2 Mon Sep 17 00:00:00 2001
From: morgan <mc@m-labs.hk>
Date: Fri, 2 Feb 2024 16:15:46 +0800
Subject: [PATCH] boot: enable FIQ asm: add FIQ enable instruction

---
 libcortex_a9/src/asm.rs     | 6 ++++++
 libsupport_zynq/src/boot.rs | 2 ++
 2 files changed, 8 insertions(+)

diff --git a/libcortex_a9/src/asm.rs b/libcortex_a9/src/asm.rs
index e513275..2243c9e 100644
--- a/libcortex_a9/src/asm.rs
+++ b/libcortex_a9/src/asm.rs
@@ -34,6 +34,12 @@ pub fn isb() {
     unsafe { llvm_asm!("isb" :::: "volatile") }
 }
 
+/// Enable FIQ
+#[inline]
+pub unsafe fn enable_fiq() {
+    llvm_asm!("cpsie f":::: "volatile");
+}
+
 /// Enable IRQ
 #[inline]
 pub unsafe fn enable_irq() {
diff --git a/libsupport_zynq/src/boot.rs b/libsupport_zynq/src/boot.rs
index 1389d3d..f542c89 100644
--- a/libsupport_zynq/src/boot.rs
+++ b/libsupport_zynq/src/boot.rs
@@ -54,6 +54,7 @@ unsafe extern "C" fn boot_core0() -> ! {
         asm::dmb();
         asm::dsb();
 
+        asm::enable_fiq();
         asm::enable_irq();
         main_core0();
         panic!("return from main");
@@ -75,6 +76,7 @@ unsafe extern "C" fn boot_core1() -> ! {
         asm::dmb();
         asm::dsb();
 
+        asm::enable_fiq();
         asm::enable_irq();
         main_core1();
         panic!("return from main_core1");