boot: ACTLR.enable_smp()

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,46 @@ 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();
+        ACTLR.enable_smp();
+        // TODO: Barriers reqd when core1 is not yet starting?
+        asm::dmb();
+        asm::dsb();
+
         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, || {
+        ACTLR.enable_smp();
+        // TODO: Barriers reqd when core1 is not yet starting?
+        asm::dmb();
+        asm::dsb();
+
+        crate::main_core1();
+        panic!("return from main_core1");
+    });
+}
+
 fn l1_cache_init() {
     use crate::cortex_a9::cache::*;
 

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() {

src/cortex_a9/regs.rs

@@ -1,5 +1,5 @@
 use crate::{register_bit, register_bits};
-use crate::regs::{RegisterR, RegisterW};
+use crate::regs::{RegisterR, RegisterW, RegisterRW};
 
 macro_rules! def_reg_r {
     ($name:tt, $type: ty, $asm_instr:tt) => {
@@ -115,6 +115,36 @@ register_bit!(sctlr,
               /// Thumb Exception Enable
               te, 30);
 
+/// Auxiliary Control Register
+pub struct ACTLR;
+wrap_reg!(actlr);
+def_reg_r!(ACTLR, actlr::Read, "mrc p15, 0, $0, c1, c0, 1");
+def_reg_w!(ACTLR, actlr::Write, "mcr p15, 0, $0, c1, c0, 1");
+// SMP bit
+register_bit!(actlr, parity_on, 9);
+register_bit!(actlr, alloc_one_way, 8);
+register_bit!(actlr, excl, 7);
+register_bit!(actlr, smp, 6);
+register_bit!(actlr, write_full_line_of_zeros, 3);
+register_bit!(actlr, l1_prefetch_enable, 2);
+// Cache/TLB maintenance broadcast
+register_bit!(actlr, fw, 0);
+
+impl RegisterRW for ACTLR {
+    fn modify<F: FnOnce(Self::R, Self::W) -> Self::W>(&mut self, f: F) {
+        let r = self.read();
+        let w = actlr::Write { inner: r.inner };
+        let w = f(r, w);
+        self.write(w);
+    }
+}
+
+impl ACTLR {
+    pub fn enable_smp(&mut self) {
+        self.modify(|_, w| w.smp(true).fw(true));
+    }
+}
+
 /// Domain Access Control Register
 pub struct DACR;
 def_reg_r!(DACR, u32, "mrc p15, 0, $0, c3, c0, 0");

src/zynq/mod.rs

@@ -5,3 +5,4 @@ pub mod eth;
 pub mod axi_hp;
 pub mod axi_gp;
 pub mod ddr;
+pub mod mpcore;

src/zynq/mpcore.rs

@@ -0,0 +1,59 @@
+///! Register definitions for Application Processing Unit (mpcore)
+
+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: ScuInvalidate,
+    reserved0: [u32; 12],
+    pub filter_start: RW<u32>,
+    pub filter_end: RW<u32>,
+    reserved1: [u32; 2],
+    pub scu_access_control: RW<u32>,
+    pub scu_non_secure_access_control: RW<u32>,
+    // there is plenty more (unimplemented)
+}
+register_at!(RegisterBlock, 0xF8F00000, new);
+
+register!(scu_control, ScuControl, RW, u32);
+register_bit!(scu_control, ic_standby_enable, 6);
+register_bit!(scu_control, scu_standby_enable, 5);
+register_bit!(scu_control, force_to_port0_enable, 4);
+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)
+        );
+    }
+}