From ef6d0ff3f176eb94f6c97f0f0a6a936b4ebf76fa Mon Sep 17 00:00:00 2001
From: Astro <astro@spaceboyz.net>
Date: Mon, 18 Nov 2019 00:38:03 +0100
Subject: [PATCH] boot: reset core1 before start

---
 src/boot.rs      | 49 ++++++++++++++++++++++++++++++++----------------
 src/main.rs      |  2 +-
 src/zynq/slcr.rs |  9 ++++++++-
 3 files changed, 42 insertions(+), 18 deletions(-)

diff --git a/src/boot.rs b/src/boot.rs
index cf2f06b..9c7e8bf 100644
--- a/src/boot.rs
+++ b/src/boot.rs
@@ -1,7 +1,8 @@
 use r0::zero_bss;
-use crate::regs::{RegisterR, RegisterW};
+use vcell::VolatileCell;
+use crate::regs::{RegisterR, RegisterW, RegisterRW};
 use crate::cortex_a9::{asm, regs::*, mmu};
-use crate::zynq::mpcore;
+use crate::zynq::{slcr, mpcore};
 
 extern "C" {
     static mut __bss_start: u32;
@@ -9,7 +10,7 @@ extern "C" {
     static mut __stack_start: u32;
 }
 
-static mut CORE1_STACK: u32 = 0;
+static mut CORE1_STACK: VolatileCell<u32> = VolatileCell::new(0);
 
 #[link_section = ".text.boot"]
 #[no_mangle]
@@ -23,15 +24,11 @@ pub unsafe extern "C" fn _boot_cores() -> ! {
             boot_core0();
         }
         1 => {
-            // Wait for a first `sev` so that `CORE1_STACK` is cleared
-            // by `zero_bss()` on core 0.
-            asm::wfe();
-
-            while CORE1_STACK == 0 {
+            while CORE1_STACK.get() == 0 {
                 asm::wfe();
             }
 
-            SP.write(CORE1_STACK);
+            SP.write(CORE1_STACK.get());
             boot_core1();
         }
         _ => unreachable!(),
@@ -102,13 +99,33 @@ fn l1_cache_init() {
     dciall();
 }
 
-pub fn start_core1<T: AsMut<[u32]>>(mut stack: T) {
-    let stack = stack.as_mut();
-    let stack_start = &mut stack[stack.len() - 1];
-    unsafe {
-        CORE1_STACK = stack_start as *mut _ as u32;
+pub struct Core1;
+
+impl Core1 {
+    pub fn stop() {
+        slcr::RegisterBlock::unlocked(|slcr| {
+            slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_rst1(true));
+            slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_clkstop1(true));
+            slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_rst1(false));
+        });
     }
 
-    // wake up core1
-    asm::sev();
+    pub fn start<T: AsMut<[u32]>>(mut stack: T) {
+        // reset and stop (safe to repeat)
+        Self::stop();
+
+        let stack = stack.as_mut();
+        let stack_start = &mut stack[stack.len() - 1];
+        unsafe {
+            CORE1_STACK.set(stack_start as *mut _ as u32);
+        }
+        // Ensure stack pointer has been written
+        asm::dmb();
+
+        // wake up core1
+        slcr::RegisterBlock::unlocked(|slcr| {
+            slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_rst1(false));
+            slcr.a9_cpu_rst_ctrl.modify(|_, w| w.a9_clkstop1(false));
+        });
+    }
 }
diff --git a/src/main.rs b/src/main.rs
index dc3929d..b99fa78 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -38,7 +38,7 @@ pub fn main() {
 
     let core1_stack = vec![0; 2048];
     println!("{} bytes stack for core1", core1_stack.len());
-    boot::start_core1(core1_stack);
+    boot::Core1::start(core1_stack);
 
     let eth = zynq::eth::Eth::default(HWADDR.clone());
     println!("Eth on");
diff --git a/src/zynq/slcr.rs b/src/zynq/slcr.rs
index 9feb2ed..1d49bc3 100644
--- a/src/zynq/slcr.rs
+++ b/src/zynq/slcr.rs
@@ -129,7 +129,7 @@ pub struct RegisterBlock {
     pub ocm_rst_ctrl: RW<u32>,
     reserved4: [u32; 1],
     pub fpga_rst_ctrl: RW<u32>,
-    pub a9_cpu_rst_ctrl: RW<u32>,
+    pub a9_cpu_rst_ctrl: A9CpuRstCtrl,
     reserved5: [u32; 1],
     pub rs_awdt_ctrl: RW<u32>,
     reserved6: [u32; 2],
@@ -440,6 +440,13 @@ impl UartRstCtrl {
     }
 }
 
+register!(a9_cpu_rst_ctrl, A9CpuRstCtrl, RW, u32);
+register_bit!(a9_cpu_rst_ctrl, peri_rst, 8);
+register_bit!(a9_cpu_rst_ctrl, a9_clkstop1, 5);
+register_bit!(a9_cpu_rst_ctrl, a9_clkstop0, 4);
+register_bit!(a9_cpu_rst_ctrl, a9_rst1, 1);
+register_bit!(a9_cpu_rst_ctrl, a9_rst0, 0);
+
 register!(pss_rst_ctrl, PssRstCtrl, RW, u32);
 register_bit!(pss_rst_ctrl, soft_rst, 1);