From 49901d1b8a626f1b8cb477b2a13588933488d3d3 Mon Sep 17 00:00:00 2001
From: Astro <astro@spaceboyz.net>
Date: Fri, 15 Nov 2019 23:54:26 +0100
Subject: [PATCH] boot: prepare core1 bootup

---
 src/boot.rs          | 39 ++++++++++++++++++++++++++++++++++++---
 src/cortex_a9/asm.rs |  6 ++++++
 src/zynq/mpcore.rs   | 36 +++++++++++++++++++++++++++++++++---
 3 files changed, 75 insertions(+), 6 deletions(-)

diff --git a/src/boot.rs b/src/boot.rs
index 8312699..d1e15c7 100644
--- a/src/boot.rs
+++ b/src/boot.rs
@@ -1,6 +1,7 @@
 use r0::zero_bss;
 use crate::regs::{RegisterR, RegisterW};
 use crate::cortex_a9::{asm, regs::*, mmu};
+use crate::zynq::mpcore;
 
 extern "C" {
     static mut __bss_start: u32;
@@ -8,6 +9,8 @@ extern "C" {
     static mut __stack_start: u32;
 }
 
+static mut CORE1_STACK: u32 = 0;
+
 #[link_section = ".text.boot"]
 #[no_mangle]
 #[naked]
@@ -19,10 +22,19 @@ pub unsafe extern "C" fn _boot_cores() -> ! {
             SP.write(&mut __stack_start as *mut _ as u32);
             boot_core0();
         }
-        _ => loop {
-            // if not core0, infinitely wait for events
+        1 => {
+            // Wait for a first `sev` so that `CORE1_STACK` is cleared
+            // by `zero_bss()` on core 0.
             asm::wfe();
-        },
+
+            while CORE1_STACK == 0 {
+                asm::wfe();
+            }
+
+            SP.write(CORE1_STACK);
+            boot_core1();
+        }
+        _ => unreachable!(),
     }
 }
 
@@ -30,16 +42,37 @@ pub unsafe extern "C" fn _boot_cores() -> ! {
 #[inline(never)]
 unsafe fn boot_core0() -> ! {
     l1_cache_init();
+
+    let mpcore = mpcore::RegisterBlock::new();
+    mpcore.scu_invalidate.invalidate_all_cores();
+
     zero_bss(&mut __bss_start, &mut __bss_end);
 
     let mmu_table = mmu::L1Table::get()
         .setup_flat_layout();
     mmu::with_mmu(mmu_table, || {
+        mpcore.scu_control.start();
+
         crate::main();
         panic!("return from main");
     });
 }
 
+#[naked]
+#[inline(never)]
+unsafe fn boot_core1() -> ! {
+    l1_cache_init();
+
+    let mpcore = mpcore::RegisterBlock::new();
+    mpcore.scu_invalidate.invalidate_core1();
+
+    let mmu_table = mmu::L1Table::get();
+    mmu::with_mmu(mmu_table, || {
+        crate::main_core1();
+        panic!("return from main_core1");
+    });
+}
+
 fn l1_cache_init() {
     use crate::cortex_a9::cache::*;
 
diff --git a/src/cortex_a9/asm.rs b/src/cortex_a9/asm.rs
index 022530e..c7a92b2 100644
--- a/src/cortex_a9/asm.rs
+++ b/src/cortex_a9/asm.rs
@@ -10,6 +10,12 @@ pub fn wfe() {
     unsafe { asm!("wfe" :::: "volatile") }
 }
 
+/// Send Event
+#[inline]
+pub fn sev() {
+    unsafe { asm!("sev" :::: "volatile") }
+}
+
 /// Data Memory Barrier
 #[inline]
 pub fn dmb() {
diff --git a/src/zynq/mpcore.rs b/src/zynq/mpcore.rs
index 36e503c..bb3b3da 100644
--- a/src/zynq/mpcore.rs
+++ b/src/zynq/mpcore.rs
@@ -1,14 +1,15 @@
 ///! Register definitions for Application Processing Unit (mpcore)
 
-use volatile_register::{RO, RW, WO};
-use crate::{register, register_at, register_bit};
+use volatile_register::{RO, RW};
+use crate::{register, register_at, register_bit, register_bits,
+            regs::RegisterW, regs::RegisterRW};
 
 #[repr(C)]
 pub struct RegisterBlock {
     pub scu_control: ScuControl,
     pub scu_config: RO<u32>,
     pub scu_cpu_power: RW<u32>,
-    pub scu_invalidate: WO<u32>,
+    pub scu_invalidate: ScuInvalidate,
     reserved0: [u32; 12],
     pub filter_start: RW<u32>,
     pub filter_end: RW<u32>,
@@ -27,3 +28,32 @@ register_bit!(scu_control, scu_speculative_linefill_enable, 3);
 register_bit!(scu_control, scu_rams_parity_enable, 2);
 register_bit!(scu_control, address_filtering_enable, 1);
 register_bit!(scu_control, enable, 0);
+
+impl ScuControl {
+    pub fn start(&mut self) {
+        self.modify(|_, w| w.enable(true));
+    }
+}
+
+register!(scu_invalidate, ScuInvalidate, WO, u32);
+register_bits!(scu_invalidate, cpu0_ways, u8, 0, 3);
+register_bits!(scu_invalidate, cpu1_ways, u8, 4, 7);
+register_bits!(scu_invalidate, cpu2_ways, u8, 8, 11);
+register_bits!(scu_invalidate, cpu3_ways, u8, 12, 15);
+
+impl ScuInvalidate {
+    pub fn invalidate_all_cores(&mut self) {
+        self.write(ScuInvalidate::zeroed()
+            .cpu0_ways(0xf)
+            .cpu1_ways(0xf)
+            .cpu2_ways(0xf)
+            .cpu3_ways(0xf)
+        );
+    }
+
+    pub fn invalidate_core1(&mut self) {
+        self.write(ScuInvalidate::zeroed()
+            .cpu1_ways(0xf)
+        );
+    }
+}