add mutex for print and println macros

src/cortex_a9/mmu.rs

@@ -124,8 +124,9 @@ impl L1Table {
             tex: 0b101,
             domain: 0b1111,
             exec: true,
-            // TODO: temporarily turn on cache for SMP testing
-            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

@@ -226,7 +226,7 @@ pub fn dciall() {
     
     // 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.
+    // 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
     

src/main.rs

@@ -23,6 +23,7 @@ mod cortex_a9;
 mod clocks;
 mod mailbox;
 mod mpcore;
+mod mutex;
 mod slcr;
 mod uart;
 mod stdio;
@@ -214,13 +215,17 @@ unsafe fn run_on_core1(f: fn() -> !, stack: &mut [u32]) {
 
 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() {}
+            while !MAILBOX_FROM_CORE1.acknowledged() {
+                println!("core 1 waiting for acknowledgement from core 0");
+            }
         }
         
         // change data to make it more interesting
@@ -230,6 +235,7 @@ fn main_core1() -> ! {
 
 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);

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/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() {}
+        });
     })
 }