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 1/2] 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");
-- 
2.44.1


From 5bd336c961ed78eaec518507093789d6d1ac8903 Mon Sep 17 00:00:00 2001
From: morgan <mc@m-labs.hk>
Date: Fri, 2 Feb 2024 17:01:07 +0800
Subject: [PATCH 2/2] add support for using custom FIQ handler cfg: add dummy
 FIQ handler cfg abort: gate dummy FIQ handler using cfg

---
 experiments/Cargo.toml       | 2 +-
 libsupport_zynq/Cargo.toml   | 3 ++-
 libsupport_zynq/src/abort.rs | 1 +
 3 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/experiments/Cargo.toml b/experiments/Cargo.toml
index afa0a64..b8456c8 100644
--- a/experiments/Cargo.toml
+++ b/experiments/Cargo.toml
@@ -18,5 +18,5 @@ embedded-hal = "0.2"
 libregister = { path = "../libregister" }
 libcortex_a9 = { path = "../libcortex_a9" }
 libboard_zynq = { path = "../libboard_zynq" }
-libsupport_zynq = { path = "../libsupport_zynq", default-features = false, features = ["panic_handler"]}
+libsupport_zynq = { path = "../libsupport_zynq", default-features = false, features = ["panic_handler", "dummy_fiq_handler"]}
 libasync = { path = "../libasync" }
diff --git a/libsupport_zynq/Cargo.toml b/libsupport_zynq/Cargo.toml
index 3e3ae86..9adf2f9 100644
--- a/libsupport_zynq/Cargo.toml
+++ b/libsupport_zynq/Cargo.toml
@@ -12,9 +12,10 @@ target_redpitaya = ["libboard_zynq/target_redpitaya"]
 target_kasli_soc = ["libboard_zynq/target_kasli_soc"]
 panic_handler = []
 dummy_irq_handler = []
+dummy_fiq_handler = []
 alloc_core = []
 
-default = ["panic_handler", "dummy_irq_handler"]
+default = ["panic_handler", "dummy_irq_handler", "dummy_fiq_handler"]
 
 [dependencies]
 r0 = "1"
diff --git a/libsupport_zynq/src/abort.rs b/libsupport_zynq/src/abort.rs
index 91c8998..20279f4 100644
--- a/libsupport_zynq/src/abort.rs
+++ b/libsupport_zynq/src/abort.rs
@@ -42,6 +42,7 @@ interrupt_handler!(IRQ, irq, __irq_stack0_start, __irq_stack1_start, {
     loop {}
 });
 
+#[cfg(feature = "dummy_fiq_handler")]
 interrupt_handler!(FIQ, fiq, __irq_stack0_start, __irq_stack1_start, {
     stdio::drop_uart();
     println!("FIQ");
-- 
2.44.1