From b6596d930deb4c1155ab9558ab721f8319325bdb Mon Sep 17 00:00:00 2001
From: Astro <astro@spaceboyz.net>
Date: Sat, 16 Nov 2019 00:12:58 +0100
Subject: [PATCH] boot: ACTLR.enable_smp()

---
 src/boot.rs           |  9 +++++++++
 src/cortex_a9/regs.rs | 32 +++++++++++++++++++++++++++++++-
 2 files changed, 40 insertions(+), 1 deletion(-)

diff --git a/src/boot.rs b/src/boot.rs
index d1e15c7..9d4eb8e 100644
--- a/src/boot.rs
+++ b/src/boot.rs
@@ -52,6 +52,10 @@ unsafe fn boot_core0() -> ! {
         .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");
@@ -68,6 +72,11 @@ unsafe fn boot_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");
     });
diff --git a/src/cortex_a9/regs.rs b/src/cortex_a9/regs.rs
index 70bae68..d4141b8 100644
--- a/src/cortex_a9/regs.rs
+++ b/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");