add cortex_a9::mutex

src/boot.rs

@@ -1,7 +1,8 @@
 use r0::zero_bss;
-use crate::regs::{RegisterR, RegisterW};
-use crate::cortex_a9::{asm, regs::*, mmu};
-use crate::zynq::mpcore;
+use vcell::VolatileCell;
+use crate::regs::{RegisterR, RegisterW, RegisterRW};
+use crate::cortex_a9::{asm, regs::*, cache, mmu};
+use crate::zynq::{slcr, mpcore};
 
 extern "C" {
     static mut __bss_start: u32;
@@ -9,7 +10,8 @@ extern "C" {
     static mut __stack_start: u32;
 }
 
-static mut CORE1_STACK: u32 = 0;
+/// `0` means: wait for initialization by core0
+static mut CORE1_STACK: VolatileCell<u32> = VolatileCell::new(0);
 
 #[link_section = ".text.boot"]
 #[no_mangle]
@@ -23,15 +25,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 +100,45 @@ 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<S: AsMut<[u32]>> {
+    pub stack: S,
+}
+
+impl<S: AsMut<[u32]>> Core1<S> {
+    pub fn stop(&self) {
+        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));
+        });
     }
 
+    /// Reset and start core1
+    ///
+    /// The stack must not be in OCM because core1 still has to
+    /// initialize its MMU before it can access DDR.
+    pub fn start(stack: S) -> Self {
+        let mut core = Core1 { stack };
+        
+        // reset and stop (safe to repeat)
+        core.stop();
+
+        let stack = core.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 to cache
+        asm::dmb();
+        // Flush cache-line
+        cache::dccmvac(unsafe { &CORE1_STACK } as *const _ as u32);
+
         // wake up core1
-    asm::sev();
+        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));
+        });
+
+        core
+    }
 }

src/cortex_a9/cache.rs

@@ -71,6 +71,14 @@ pub fn dccimva(addr: usize) {
     }
 }
 
+/// clear cache line by virtual address to point of coherency (DCCMVAC)
+#[inline]
+pub fn dccmvac(addr: u32) {
+    unsafe {
+        asm!("mcr p15, 0, $0, c7, c10, 1" :: "r" (addr) :: "volatile");
+    }
+}
+
 /// The DCCIVMA (data cache clear and invalidate) applied to the
 /// region of memory occupied by the argument. This does not modify
 /// the argument, but due to the invalidate part (only ever needed if

src/cortex_a9/mod.rs

@@ -2,5 +2,6 @@ pub mod asm;
 pub mod regs;
 pub mod cache;
 pub mod mmu;
+pub mod mutex;
 
 global_asm!(include_str!("exceptions.s"));

src/cortex_a9/mutex.rs

@@ -0,0 +1,73 @@
+use core::ops::{Deref, DerefMut};
+use core::sync::atomic::{AtomicBool, Ordering};
+use core::cell::UnsafeCell;
+use super::asm::*;
+
+#[inline]
+fn wait_for_update() {
+    wfe();
+}
+
+#[inline]
+fn signal_update() {
+    dsb();
+    sev();
+}
+
+pub struct Mutex<T> {
+    locked: AtomicBool,
+    inner: UnsafeCell<T>,
+}
+
+unsafe impl<T: Send> Sync for Mutex<T> {}
+unsafe impl<T: Send> Send for Mutex<T> {}
+
+impl<T> Mutex<T> {
+    pub const fn new(inner: T) -> Self {
+        Mutex{
+            locked: AtomicBool::new(false),
+            inner: UnsafeCell::new(inner),
+        }
+    }
+    
+    pub fn lock(&self) -> MutexGuard<T> {
+        while self.locked.compare_and_swap(false, true, Ordering::Acquire) {
+            while self.locked.load(Ordering::Relaxed) {
+                wait_for_update();
+            }
+        }
+
+        dmb();
+
+        MutexGuard { mutex: self }
+    }
+
+    fn unlock(&self) {
+        dmb();
+        self.locked.store(false, Ordering::Release);
+        signal_update();
+    }
+}
+
+pub struct MutexGuard<'a, T> {
+    mutex: &'a Mutex<T>,
+}
+
+impl<'a, T> Deref for MutexGuard<'a, T> {
+    type Target = T;
+    fn deref(&self) -> &T {
+        unsafe { &*self.mutex.inner.get() }
+    }
+}
+
+impl<'a, T> DerefMut for MutexGuard<'a, T> {
+    fn deref_mut(&mut self) -> &mut T {
+        unsafe { &mut *self.mutex.inner.get() }
+    }
+}
+
+impl<'a, T> Drop for MutexGuard<'a, T> {
+    fn drop(&mut self) {
+        self.mutex.unlock();
+    }
+}

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");

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);