add mutex for print and println macros

link.x

@@ -1,5 +1,6 @@
 ENTRY(_boot_cores);
 
+/* Size of stack for core 0 in bytes */
 STACK_SIZE = 0x8000;
 
 /* Provide some defaults */

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() {
@@ -27,3 +33,4 @@ pub fn dsb() {
 pub fn isb() {
     unsafe { asm!("isb" :::: "volatile") }
 }
+

src/cortex_a9/mmu.rs

@@ -124,7 +124,9 @@ impl L1Table {
             tex: 0b101,
             domain: 0b1111,
             exec: true,
-            cacheable: false,
+            // TODO: temporarily turn on cache for SMP testing;
+            //       consider turning it off again for production
+            cacheable: !false, 
             bufferable: true,
         });
         /* (DDR cacheable) */

src/cortex_a9/regs.rs

@@ -115,6 +115,45 @@ register_bit!(sctlr,
               /// Thumb Exception Enable
               te, 30);
 
+impl crate::regs::RegisterRW for SCTLR {
+    fn modify<F: FnOnce(Self::R, Self::W) -> Self::W>(&mut self, f: F) {
+        // todo: this may fail for .nmfi and, in non-secure state,
+        //       also RR (bit 14)
+        let inner = self.read().inner;
+        let inner_w = f(
+            sctlr::Read { inner },
+            sctlr::Write { inner }
+        );
+        self.write(inner_w);
+    }
+}
+
+/// 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 crate::regs::RegisterRW for ACTLR {
+    fn modify<F: FnOnce(Self::R, Self::W) -> Self::W>(&mut self, f: F) {
+        let inner = self.read().inner;
+        let inner_w = f(
+            actlr::Read { inner },
+            actlr::Write { inner }
+        );
+        self.write(inner_w);
+    }
+}
+
 /// Domain Access Control Register
 pub struct DACR;
 def_reg_r!(DACR, u32, "mrc p15, 0, $0, c3, c0, 0");
@@ -163,9 +202,51 @@ pub fn bpiall() {
 
 /// Invalidate D-Cache
 #[inline(always)]
-pub fn dccisw() {
+pub fn dcisw(setway: u32) {
     // TODO: $0 is r11 at what value?
     unsafe {
-        asm!("mcr p15, 0, $0, c7, c5, 6" :: "r" (0) :: "volatile");
+        // steinb: the following is incorrect
+        //asm!("mcr p15, 0, $0, c7, c5, 6" :: "r" (0) :: "volatile");
+
+        // acc. to ARM Architecture Reference Manual, Figure B3-32;
+        // also see example code (for DCCISW, but DCISW will be
+        // analogous) "Example code for cache maintenance operations"
+        // on pages B2-1286 and B2-1287.
+        asm!("mcr p15, 0, $0, c7, c6, 2" :: "r" (setway) :: "volatile");
+    }
+}
+
+/// A made-up "instruction": invalidate all of the L1 D-Cache
+#[inline(always)]
+pub fn dciall() {
+    // the cache associativity could be read from a register, but will
+    // always be 4 in L1 data cache of a cortex a9
+    let ways = 4;
+    let bit_pos_of_way = 30; // 32 - log2(ways)
+    
+    // the cache sets could be read from a register, but are always
+    // 256 for the cores in the zync-7000; in general, 128 or 512 are
+    // also possible for a Cortex-A9.
+    let sets = 256;
+    let bit_pos_of_set = 5; // for a line size of 8 words = 2^5 bytes
+    
+    // select L1 data cache
+    unsafe {
+        asm!("mcr p15, 2, $0, c0, c0, 0" :: "r" (0) :: "volatile");
+    }
+    
+    // Invalidate entire D-Cache by iterating every set and every way    
+    for set in 0..sets {
+        for way in 0..ways {
+            dcisw((set << bit_pos_of_set) | (way << bit_pos_of_way));
+        }
+    }
+}
+
+/// 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");
     }
 }

src/mailbox.rs

@@ -0,0 +1,132 @@
+use crate::cortex_a9::asm;
+use core::ptr::{read_volatile, write_volatile};
+
+/*
+  One-way mailbox:
+
+  All transmissions must originate from one core only,
+  and all receives from the other core only.
+
+  Example transmission (to be executed on core 0):
+  {
+      while (!MAILBOX_FROM_CORE0.acknowledged()) {}
+      println!("ready to send");
+      MAILBOX_FROM_CORE0.send(&data);
+      println!("sent");
+      while (!MAILBOX_FROM_CORE0.acknowledged()) {}
+      println!("got receipt (acknowledgement)");    
+  }
+
+  Example reception (to be executed on core 1):
+  {
+      println("wait for data");
+      while (!MAILBOX_FROM_CORE0.available()) {}
+      let data = MAILBOX_FROM_CORE0.receive();
+      println("data received");
+      MAILBOX_FROM_CORE0.acknowledge(data);
+  }
+  
+  Note that unsafe { ... } blocks must be used around most functions;
+  these have been omitted from the examples for clarity.
+
+*/
+
+pub struct OneWayMailbox {
+    // pointer (data to be transferred): write-only for sending core,
+    // readable and clearable (to 0) for receiving core
+    pointer: usize,
+
+    // helper variable (last pointer value received) for receiving
+    // core
+    echo: usize,
+}
+
+pub static mut MAILBOX_FROM_CORE0: OneWayMailbox = OneWayMailbox::new();
+pub static mut MAILBOX_FROM_CORE1: OneWayMailbox = OneWayMailbox::new();
+
+impl OneWayMailbox {
+    // instantiate a one-way mailbox with no undelivered message
+    pub const fn new() -> OneWayMailbox {
+        OneWayMailbox { pointer: 0, echo: 0 }
+    }
+
+    // recreate pristine condition; may only be called when producers
+    // and consumers are stopped (e.g. when starting core 1 from core
+    // 0).
+    pub fn reset_discard(&mut self) {
+        unsafe {
+            write_volatile(&mut self.pointer, 0);
+            write_volatile(&mut self.echo, 0);
+        }
+        asm::dmb(); 
+    }
+
+    // send a pointer from one core to be received by the other core
+    pub fn send(&mut self, ptr: usize) -> usize {
+        assert!(ptr != 0); // ptr may not be the NULL-like flag        
+        unsafe {
+            write_volatile(&mut self.pointer, ptr);
+        }
+        asm::dmb(); // ensure data at (ptr) has been fully written
+        ptr
+    }
+
+    // receive a pointer from the other core, or 0 if none is present
+    pub fn receive(&self) -> usize {
+        let ptr = unsafe {
+            read_volatile(&self.pointer)
+        };
+        // necessary memory barrier to guarantee that the data at
+        // (ptr) has been fully written before it may be accessed
+        // by the caller of this function
+        asm::dmb(); 
+        ptr
+    }
+
+    // return true if it is guaranteed that the next self.receive()
+    // will return actual data rather than 0
+    pub fn available(&self) -> bool {
+        let ptr = unsafe {
+            read_volatile(&self.pointer)
+        };
+        asm::dmb();
+        ptr != 0
+    }
+
+    // acknowledge receipt of data to the sender (i.e. release it)
+    pub fn acknowledge(&mut self, ptr: usize) {
+        // ensure that the data we release is the data last sent
+        assert_eq!(ptr, unsafe {
+            read_volatile(&self.pointer)
+        });
+        // first possibility for "release" flag:
+        //   pointer and echo are equal
+        unsafe {
+            write_volatile(&mut self.echo, ptr);
+        }
+        asm::dmb(); // write to self.echo before self.pointer
+        // second possibility for "release" flag:
+        //   NULL-like pointer
+        unsafe {
+            write_volatile(&mut self.pointer, 0);
+        }
+        asm::dmb();
+        // reset echo
+        unsafe {
+            write_volatile(&mut self.echo, 0);
+        }
+    }
+
+    // has data been acknowledged?
+    pub fn acknowledged(&self) -> bool {
+        let ptr = unsafe {
+            read_volatile(&self.pointer)
+        };
+        // read self.pointer before self.echo, not after
+        asm::dmb();
+        let echo = unsafe {
+            read_volatile(&self.echo)
+        };
+        (ptr == 0) || (ptr == echo)
+    }
+}

src/main.rs

@@ -9,30 +9,41 @@
 #![allow(dead_code)]
 
 use core::mem::{uninitialized, transmute};
+use core::ptr::write_volatile;
 use r0::zero_bss;
 use compiler_builtins as _;
 use smoltcp::wire::{EthernetAddress, IpAddress, IpCidr};
 use smoltcp::iface::{NeighborCache, EthernetInterfaceBuilder, EthernetInterface};
 use smoltcp::time::Instant;
 use smoltcp::socket::SocketSet;
+use mailbox::{MAILBOX_FROM_CORE0, MAILBOX_FROM_CORE1};
 
 mod regs;
 mod cortex_a9;
 mod clocks;
+mod mailbox;
+mod mpcore;
+mod mutex;
 mod slcr;
 mod uart;
 mod stdio;
 mod eth;
 
-use crate::regs::{RegisterR, RegisterW};
+use crate::regs::{RegisterR, RegisterW, RegisterRW};
 use crate::cortex_a9::{asm, regs::*, mmu};
 
 extern "C" {
     static mut __bss_start: u32;
     static mut __bss_end: u32;
-    static mut __stack_start: u32;
+    static mut __stack_start: u32; // refers to the stack for core 0
+    static mut __stack1_start: u32; // refers to the stack for core 1
 }
 
+// program address as u32, for execution after setting up core 1
+static mut START_ADDR_CORE1: u32 = 0; 
+// initial stack pointer for starting core 1
+static mut INITIAL_SP_CORE1: u32 = 0; // must be zero (as a flag)
+
 #[link_section = ".text.boot"]
 #[no_mangle]
 #[naked]
@@ -41,13 +52,24 @@ pub unsafe extern "C" fn _boot_cores() -> ! {
 
     match MPIDR.read() & CORE_MASK {
         0 => {
+            // executing on core 0
             SP.write(&mut __stack_start as *mut _ as u32);
             boot_core0();
         }
-        _ => loop {
+        _ => {
-            // if not core0, infinitely wait for events
+            // executing on core 1 (as there are only cores 0 and 1)
+            while INITIAL_SP_CORE1 == 0 {
+                // NOTE: This wfe and its loop can be removed as long
+                //       as the regular boot loader remains in place
+                //       (i.e. this program is not written into ROM).
                 asm::wfe();
-        },
+            }
+
+            // the following requires a stack (at least later, for the
+            // function for setting up the MMU)
+            SP.write(INITIAL_SP_CORE1);
+            boot_core1();
+        }
     }
 }
 
@@ -55,16 +77,59 @@ pub unsafe extern "C" fn _boot_cores() -> ! {
 #[inline(never)]
 unsafe fn boot_core0() -> ! {
     l1_cache_init();
+
+    // Invalidate SCU, for all cores
+    mpcore::RegisterBlock::new().scu_invalidate.write(0xffff);
+
     zero_bss(&mut __bss_start, &mut __bss_end);
 
     let mmu_table = mmu::L1Table::get()
         .setup_flat_layout();
     mmu::with_mmu(mmu_table, || {
+        // start SCU
+        mpcore::RegisterBlock::new().scu_control.modify(
+            |_, w| w.enable(true)
+        );
+        // enable SMP (for starting correct SCU operation)
+        ACTLR.modify(|_, w|
+             w.smp(true) // SMP mode
+              .fw(true) // cache and TLB maintenance broadcast on
+        );
+        asm::dmb();
+        asm::dsb();
         main();
         panic!("return from main");
     });
 }
 
+#[naked]
+#[inline(never)]
+unsafe fn boot_core1() -> ! {
+    l1_cache_init();
+
+    // Invalidate SCU, for core1 only
+    mpcore::RegisterBlock::new().scu_invalidate.write(0x00f0);
+
+    // use the MMU L1 Table already set up by core 0
+    let mmu_table = mmu::L1Table::get();
+    mmu::with_mmu(mmu_table, || {
+        // enable SMP (for correct SCU operation)
+        ACTLR.modify(|_, w| 
+                     w.smp(true) // SMP mode
+                     .fw(true) // cache and TLB maintenance broadcast
+        );
+
+        asm::dmb();
+        asm::dsb();
+
+        // now that the MMU is active using the same table as active
+        // on the other core, one can branch to any normal memory
+        // location in which the code may reside
+        asm!("bx r1" :: "{r1}"(START_ADDR_CORE1) :: "volatile");
+        unreachable!();
+    });
+}
+
 fn l1_cache_init() {
     // Invalidate TLBs
     tlbiall();
@@ -73,13 +138,123 @@ fn l1_cache_init() {
     // Invalidate Branch Predictor Array
     bpiall();
     // Invalidate D-Cache
-    dccisw();
+    //
+    // Note: Do use dcisw rather than dccisw to only invalidate rather
+    //       than also clear (which may write values back into the
+    //       underlying L2 cache or memory!)
+    //
+    // use the "made-up instruction" (see definition) dciall()
+    dciall();
+
+    asm::dsb();
+    asm::isb();
 }
 
+fn stop_core1() {
+    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)
+        });
+    });
+}
+
+// Execute f on core 1 using the given stack. Note that these
+// semantics are inherently unsafe as the stack needs to live longer
+// than Rust semantics dictate...hence this method is marked as unsafe
+// to remind the caller to take special care (but also many operations
+// performed would otherwise require `unsafe` blocks).
+unsafe fn run_on_core1(f: fn() -> !, stack: &mut [u32]) {
+    // reset and stop core 1 (this is safe to repeat, if the caller
+    // has already performed this)
+    stop_core1();
+
+    // ensure any mailbox access finishes before the mailbox reset
+    asm::dmb();
+    // reset the mailbox for sending messages
+    MAILBOX_FROM_CORE0.reset_discard();
+    MAILBOX_FROM_CORE1.reset_discard();
+    // determine address of f and save it as start address for core 1
+    write_volatile(
+        &mut START_ADDR_CORE1,
+        f as *const () as u32
+    );
+    write_volatile(
+        &mut INITIAL_SP_CORE1,
+        &mut stack[stack.len() - 1] as *const _ as u32
+    );
+    // ensure the above is written to cache before it is cleaned
+    asm::dmb();
+    // TODO: Is the following necessary, considering that the SCU
+    //       should take care of coherency of all (normal) memory?
+    //
+    // clean cache lines containing START_ADDR_CORE1 and
+    // INITIAL_SP_CORE1
+    dccmvac(&START_ADDR_CORE1 as *const _ as u32);
+    dccmvac(&INITIAL_SP_CORE1 as *const _ as u32);
+    
+    // clean cache lines containing mailboxes
+    dccmvac(&MAILBOX_FROM_CORE0 as *const _ as u32);
+    dccmvac(&MAILBOX_FROM_CORE1 as *const _ as u32);
+
+    // restart core 1
+    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)
+        });
+    });
+}
+
+fn main_core1() -> ! {
+    let mut data: [u32; 2] = [42, 42];
+    println!("Core 1 SP: 0x{:X}", SP.read());
+    loop {
+        // effectively perform something similar to `println!("from
+        // core 1");` by passing a message to core 0 and having core 0
+        // output it via the println! macro
+        unsafe {
+            println!("sending from core 1");
+            MAILBOX_FROM_CORE1.send(&data as *const _ as usize);
+            while !MAILBOX_FROM_CORE1.acknowledged() {
+                println!("core 1 waiting for acknowledgement from core 0");
+            }
+        }
+        
+        // change data to make it more interesting
+        data[1] += 1;
+    }
+}
+
+fn main_core1_program2() -> ! {
+    let mut data: [u32; 2] = [4200, 4200];
+    println!("Core 1 SP: 0x{:X}", SP.read());
+    loop {
+        unsafe {
+            MAILBOX_FROM_CORE1.send(&data as *const _ as usize);
+            while !MAILBOX_FROM_CORE1.acknowledged() {}
+        }
+        // change data to make it more interesting
+        data[0] -= 1;
+        data[1] += 1;
+    }
+}
+
+// reserve some memory as stack for core1
+static mut STACK_CORE1: [u32; 256] = [0; 256];
+
 const HWADDR: [u8; 6] = [0, 0x23, 0xde, 0xea, 0xbe, 0xef];
 
 fn main() {
     println!("Main.");
+    println!("Core 0 SP: 0x{:X}", SP.read());
     let clocks = clocks::CpuClocks::get();
     println!("Clocks: {:?}", clocks);
     println!("CPU speeds: {}/{}/{}/{} MHz",
@@ -92,6 +267,52 @@ fn main() {
     println!("Eth on");
     eth.reset_phy();
 
+    // start executing main_core1() on core 1
+    unsafe {
+        run_on_core1(main_core1, &mut STACK_CORE1[..]);
+    }
+    println!("Started main_core1() on core 1");
+    for _ in 0..5 {
+        // wait for data
+        while unsafe { !MAILBOX_FROM_CORE1.available() } {}
+        // receive data
+        let data_ptr = unsafe { MAILBOX_FROM_CORE1.receive() };
+        println!(
+            "Received via mailbox from core 1: data {} and {} at address 0x{:X}",
+            unsafe { (*(data_ptr as *const [u32; 2]))[0] },
+            unsafe { (*(data_ptr as *const [u32; 2]))[1] },
+            data_ptr
+        );
+        unsafe {
+            MAILBOX_FROM_CORE1.acknowledge(data_ptr);
+        }
+    }
+    stop_core1();
+    println!("Stopped core 1.");
+
+    // start executing main_core1_program2() on core 1
+    unsafe {
+        run_on_core1(main_core1_program2, &mut STACK_CORE1[..]);
+    }
+    println!("Started main_core1_program2() on core 1");
+    for _ in 0..5 {
+        // wait for data
+        while unsafe { !MAILBOX_FROM_CORE1.available() } {}
+        // receive data
+        let data_ptr = unsafe { MAILBOX_FROM_CORE1.receive() };
+        println!(
+            "Received via mailbox from core 1: data {} and {} at address 0x{:X}",
+            unsafe { (*(data_ptr as *const [u32; 2]))[0] },
+            unsafe { (*(data_ptr as *const [u32; 2]))[1] },
+            data_ptr
+        );
+        unsafe {
+            MAILBOX_FROM_CORE1.acknowledge(data_ptr);
+        }
+    }
+    stop_core1();
+    println!("Stopped core 1.");
+
     const RX_LEN: usize = 1;
     let mut rx_descs: [eth::rx::DescEntry; RX_LEN] = unsafe { uninitialized() };
     let mut rx_buffers = [[0u8; eth::MTU]; RX_LEN];

src/mpcore.rs

@@ -0,0 +1,29 @@
+///! Register definitions for Application Processing Unit (mpcore)
+
+use volatile_register::{RO, RW, WO};
+use crate::{register, register_at, register_bit};
+
+#[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>,
+    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);

src/mutex.rs

@@ -0,0 +1,71 @@
+/// Mutex for SMP-safe locking
+
+use crate::cortex_a9::asm;
+
+pub struct Mutex {
+    state: u32,
+}
+
+const UNLOCKED_MUTEX: u32 = 0;
+const LOCKED_MUTEX: u32 = 1;
+
+impl Mutex {
+    pub const fn new_unlocked() -> Mutex {
+        Mutex { state: UNLOCKED_MUTEX }
+    }
+
+    pub const fn new_locked() -> Mutex {
+        Mutex { state: LOCKED_MUTEX }
+    }
+
+    // inlining causes problems with the labels
+    #[inline(never)]
+    pub fn acquire(&mut self) {
+        unsafe {
+            // code adapted from an example by ARM at
+            // http://infocenter.arm.com (Home > ARM Synchronization
+            // Primitives > Practical uses > Implementing a mutex)
+            asm!("
+                  mutex_acquire_label1: 
+                          ldrex r2, [$0];
+                          cmp r2, $1;
+                          beq mutex_acquire_label2;
+                          strexne r2, $1, [$0];
+                          cmpne r2, 1;
+                          beq mutex_acquire_label1;
+                          dmb;
+                          b mutex_acquire_label3;
+                  mutex_acquire_label2:
+                          wfe;
+                          b mutex_acquire_label1;
+                  mutex_acquire_label3: ;
+                 "
+                  ::
+                 // inputs
+                 "r" (&mut self.state as *mut _ as u32), "r" (LOCKED_MUTEX)
+                  :
+                 // clobbers
+                 "r2"
+                  :
+                 "volatile"
+            );
+        }
+    }
+
+    pub fn release(&mut self) {
+        unsafe {
+            asm!("
+                  dmb;
+                  str $1, [$0];
+                  dsb;
+                  sev;
+                 "
+                  ::
+                 // inputs
+                 "r" (&mut self.state as *mut _ as u32), "r" (UNLOCKED_MUTEX)
+                  ::
+                 "volatile"
+            );
+        }
+    }
+}

src/slcr.rs

@@ -90,7 +90,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],
@@ -365,6 +365,13 @@ impl UartRstCtrl {
 register!(pss_rst_ctrl, PssRstCtrl, RW, u32);
 register_bit!(pss_rst_ctrl, soft_rst, 1);
 
+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);
+
 /// Used for MioPin*.io_type
 #[repr(u8)]
 pub enum IoBufferType {

src/stdio.rs

@@ -1,11 +1,12 @@
 use crate::uart::Uart;
+use crate::mutex::Mutex;
 
 const UART_RATE: u32 = 115_200;
 static mut UART: Option<Uart> = None;
+static mut UART_MUTEX: Mutex = Mutex::new_unlocked();
 
-// TODO: locking for SMP
 #[doc(hidden)]
-pub fn get_uart() -> &'static mut Uart {
+fn get_uart() -> &'static mut Uart {
     unsafe {
         match &mut UART {
             None => {
@@ -18,22 +19,35 @@ pub fn get_uart() -> &'static mut Uart {
     }
 }
 
+// call f(UART) with UART locked via UART_MUTEX
+pub fn with_uart<F>(f: F) where F: Fn(&mut Uart) -> () {
+    unsafe {
+        UART_MUTEX.acquire();
+    }
+    f(get_uart());
+    unsafe {
+        UART_MUTEX.release();
+    }
+}
+
 #[macro_export]
 macro_rules! print {
     ($($arg:tt)*) => ({
+        crate::stdio::with_uart(|uart| {
             use core::fmt::Write;
-        let uart = crate::stdio::get_uart();
             let _ = write!(uart, $($arg)*);
+        });
     })
 }
 
 #[macro_export]
 macro_rules! println {
     ($($arg:tt)*) => ({
+        crate::stdio::with_uart(|uart| {
             use core::fmt::Write;
-        let uart = crate::stdio::get_uart();
             let _ = write!(uart, $($arg)*);
             let _ = write!(uart, "\r\n");
             while !uart.tx_fifo_empty() {}
+        });
     })
 }