From 78d58d17ec7906a6cadd1678576939d20612cf8f Mon Sep 17 00:00:00 2001
From: pca006132 <jl@m-labs.hk>
Date: Thu, 28 Jan 2021 11:57:52 +0800
Subject: [PATCH] libsupport_zynq: fix stack pointer problems

---
 experiments/link.x           | 14 ++++++++++++++
 experiments/src/main.rs      |  2 +-
 libsupport_zynq/src/abort.rs | 14 +++++++-------
 libsupport_zynq/src/boot.rs  |  5 ++---
 szl/link.x                   | 15 +++++++++++++++
 5 files changed, 39 insertions(+), 11 deletions(-)

diff --git a/experiments/link.x b/experiments/link.x
index 0de13ff..71d0ea1 100644
--- a/experiments/link.x
+++ b/experiments/link.x
@@ -34,6 +34,20 @@ SECTIONS
         __bss_end = .;
     } > OCM3
 
+    .irq_stack1 (NOLOAD) : ALIGN(8)
+    {
+        __irq_stack1_end = .;
+        . += 0x100;
+        __irq_stack1_start = .;
+    } > OCM3
+
+    .irq_stack0 (NOLOAD) : ALIGN(8)
+    {
+        __irq_stack0_end = .;
+        . += 0x100;
+        __irq_stack0_start = .;
+    } > OCM3
+
     .stack1 (NOLOAD) : ALIGN(8) {
         __stack1_end = .;
         . += 0x200;
diff --git a/experiments/src/main.rs b/experiments/src/main.rs
index e44ccb1..a75aa83 100644
--- a/experiments/src/main.rs
+++ b/experiments/src/main.rs
@@ -55,7 +55,7 @@ extern "C" {
 
 static CORE1_RESTART: AtomicBool = AtomicBool::new(false);
 
-interrupt_handler!(IRQ, irq, __stack0_start, __stack1_start, {
+interrupt_handler!(IRQ, irq, __irq_stack0_start, __irq_stack1_start, {
     if MPIDR.read().cpu_id() == 1{
         let mpcore = mpcore::RegisterBlock::mpcore();
         let mut gic = gic::InterruptController::gic(mpcore);
diff --git a/libsupport_zynq/src/abort.rs b/libsupport_zynq/src/abort.rs
index 7fd996c..91c8998 100644
--- a/libsupport_zynq/src/abort.rs
+++ b/libsupport_zynq/src/abort.rs
@@ -2,25 +2,25 @@ use libregister::RegisterR;
 use libcortex_a9::{regs::{DFSR, MPIDR}, interrupt_handler};
 use libboard_zynq::{println, stdio};
 
-interrupt_handler!(UndefinedInstruction, undefined_instruction, __stack0_start, __stack1_start, {
+interrupt_handler!(UndefinedInstruction, undefined_instruction, __irq_stack0_start, __irq_stack1_start, {
     stdio::drop_uart();
     println!("UndefinedInstruction");
     loop {}
 });
 
-interrupt_handler!(SoftwareInterrupt, software_interrupt, __stack0_start, __stack1_start, {
+interrupt_handler!(SoftwareInterrupt, software_interrupt, __irq_stack0_start, __irq_stack1_start, {
     stdio::drop_uart();
     println!("SoftwareInterrupt");
     loop {}
 });
 
-interrupt_handler!(PrefetchAbort, prefetch_abort, __stack0_start, __stack1_start, {
+interrupt_handler!(PrefetchAbort, prefetch_abort, __irq_stack0_start, __irq_stack1_start, {
     stdio::drop_uart();
     println!("PrefetchAbort");
     loop {}
 });
 
-interrupt_handler!(DataAbort, data_abort, __stack0_start, __stack1_start, {
+interrupt_handler!(DataAbort, data_abort, __irq_stack0_start, __irq_stack1_start, {
     stdio::drop_uart();
 
     println!("DataAbort on core {}", MPIDR.read().cpu_id());
@@ -29,20 +29,20 @@ interrupt_handler!(DataAbort, data_abort, __stack0_start, __stack1_start, {
     loop {}
 });
 
-interrupt_handler!(ReservedException, reserved_exception, __stack0_start, __stack1_start, {
+interrupt_handler!(ReservedException, reserved_exception, __irq_stack0_start, __irq_stack1_start, {
     stdio::drop_uart();
     println!("ReservedException");
     loop {}
 });
 
 #[cfg(feature = "dummy_irq_handler")]
-interrupt_handler!(IRQ, irq, __stack0_start, __stack1_start, {
+interrupt_handler!(IRQ, irq, __irq_stack0_start, __irq_stack1_start, {
     stdio::drop_uart();
     println!("IRQ");
     loop {}
 });
 
-interrupt_handler!(FIQ, fiq, __stack0_start, __stack1_start, {
+interrupt_handler!(FIQ, fiq, __irq_stack0_start, __irq_stack1_start, {
     stdio::drop_uart();
     println!("FIQ");
     loop {}
diff --git a/libsupport_zynq/src/boot.rs b/libsupport_zynq/src/boot.rs
index 1cc5eef..1389d3d 100644
--- a/libsupport_zynq/src/boot.rs
+++ b/libsupport_zynq/src/boot.rs
@@ -2,7 +2,7 @@ use r0::zero_bss;
 use core::ptr::write_volatile;
 use libregister::{
     VolatileCell,
-    RegisterR, RegisterW, RegisterRW,
+    RegisterR, RegisterRW,
 };
 use libcortex_a9::{asm, l2c, regs::*, cache, mmu, spin_lock_yield, notify_spin_lock, enable_fpu, interrupt_handler};
 use libboard_zynq::{slcr, mpcore};
@@ -19,16 +19,15 @@ extern "C" {
 static mut CORE1_ENABLED: VolatileCell<bool> = VolatileCell::new(false);
 
 interrupt_handler!(Reset, reset_irq, __stack0_start, __stack1_start, {
+    // no need to setup stack here, as we already did when entering the handler
     match MPIDR.read().cpu_id() {
         0 => {
-            SP.write(&mut __stack0_start as *mut _ as u32);
             boot_core0();
         }
         1 => {
             while !CORE1_ENABLED.get() {
                 spin_lock_yield();
             }
-            SP.write(&mut __stack1_start as *mut _ as u32);
             boot_core1();
         }
         _ => unreachable!(),
diff --git a/szl/link.x b/szl/link.x
index 8ed6397..5251904 100644
--- a/szl/link.x
+++ b/szl/link.x
@@ -59,6 +59,21 @@ SECTIONS
         __stack0_start = .;
     } > OCM3
 
+    .irq_stack1 (NOLOAD) : ALIGN(8)
+    {
+        __irq_stack1_end = .;
+        . += 0x100;
+        __irq_stack1_start = .;
+    } > OCM3
+
+    .irq_stack0 (NOLOAD) : ALIGN(8)
+    {
+        __irq_stack0_end = .;
+        . += 0x100;
+        __irq_stack0_start = .;
+    } > OCM3
+
+
     /DISCARD/ :
     {
         /* Unused exception related info that only wastes space */