From 5850401d72037806dcc6d7463f613035809431d9 Mon Sep 17 00:00:00 2001
From: pca006132 <john.lck40@gmail.com>
Date: Mon, 20 Jul 2020 11:09:04 +0800
Subject: [PATCH] libsupport_zynq/ram: split allocators for two cores.

---
 libsupport_zynq/Cargo.toml |  1 +
 libsupport_zynq/src/ram.rs | 91 ++++++++++++++++++++++++++++----------
 2 files changed, 69 insertions(+), 23 deletions(-)

diff --git a/libsupport_zynq/Cargo.toml b/libsupport_zynq/Cargo.toml
index ad215d1..d65faa9 100644
--- a/libsupport_zynq/Cargo.toml
+++ b/libsupport_zynq/Cargo.toml
@@ -10,6 +10,7 @@ target_zc706 = ["libboard_zynq/target_zc706"]
 target_cora_z7_10 = ["libboard_zynq/target_cora_z7_10"]
 panic_handler = []
 dummy_irq_handler = []
+alloc_core = []
 
 default = ["panic_handler", "dummy_irq_handler"]
 
diff --git a/libsupport_zynq/src/ram.rs b/libsupport_zynq/src/ram.rs
index f7721f8..d21747a 100644
--- a/libsupport_zynq/src/ram.rs
+++ b/libsupport_zynq/src/ram.rs
@@ -1,55 +1,100 @@
-use core::alloc::GlobalAlloc;
-use core::ptr::NonNull;
 use alloc::alloc::Layout;
-use linked_list_allocator::Heap;
-use libcortex_a9::mutex::Mutex;
+use core::alloc::GlobalAlloc;
+use core::cell::UnsafeCell;
+use core::ptr::NonNull;
 use libboard_zynq::ddr::DdrRam;
+use libcortex_a9::regs::MPIDR;
+use libregister::RegisterR;
+use linked_list_allocator::Heap;
 
 #[global_allocator]
-static ALLOCATOR: CortexA9Alloc = CortexA9Alloc(Mutex::new(Heap::empty()));
+static mut ALLOCATOR: CortexA9Alloc = CortexA9Alloc(
+    UnsafeCell::new(Heap::empty()),
+    UnsafeCell::new(Heap::empty()),
+);
 
-/// LockedHeap doesn't lock properly
-struct CortexA9Alloc(Mutex<Heap>);
+struct CortexA9Alloc(UnsafeCell<Heap>, UnsafeCell<Heap>);
 
 unsafe impl Sync for CortexA9Alloc {}
 
 unsafe impl GlobalAlloc for CortexA9Alloc {
     unsafe fn alloc(&self, layout: Layout) -> *mut u8 {
-        self.0.lock()
-            .allocate_first_fit(layout)
-            .ok()
-            .map_or(0 as *mut u8, |allocation| allocation.as_ptr())
+        if MPIDR.read().cpu_id() == 0 {
+            self.0.get().as_mut()
+        } else {
+            self.1.get().as_mut()
+        }
+        .unwrap()
+        .allocate_first_fit(layout)
+        .ok()
+        .map_or(0 as *mut u8, |allocation| allocation.as_ptr())
     }
 
     unsafe fn dealloc(&self, ptr: *mut u8, layout: Layout) {
-        self.0.lock()
-            .deallocate(NonNull::new_unchecked(ptr), layout)
+        if MPIDR.read().cpu_id() == 0 {
+            self.0.get().as_mut()
+        } else {
+            self.1.get().as_mut()
+        }
+        .unwrap()
+        .deallocate(NonNull::new_unchecked(ptr), layout)
     }
 }
 
+#[cfg(not(feature = "alloc_core"))]
 pub fn init_alloc_ddr(ddr: &mut DdrRam) {
     unsafe {
-        ALLOCATOR.0.lock()
+        ALLOCATOR
+            .0
+            .get()
+            .as_mut()
+            .unwrap()
             .init(ddr.ptr::<u8>() as usize, ddr.size());
     }
 }
 
+#[cfg(feature = "alloc_core")]
 extern "C" {
-    static __heap_start: usize;
-    static __heap_end: usize;
+    static __heap0_start: usize;
+    static __heap0_end: usize;
+    static __heap1_start: usize;
+    static __heap1_end: usize;
 }
 
-pub fn init_alloc_linker() {
+#[cfg(feature = "alloc_core")]
+pub fn init_alloc_core0() {
     unsafe {
-        let start = &__heap_start as *const usize as usize;
-        let end = &__heap_end as *const usize as usize;
-        ALLOCATOR.0.lock()
-            .init(start, end - start);
+        let start = &__heap0_start as *const usize as usize;
+        let end = &__heap0_end as *const usize as usize;
+        ALLOCATOR.0.get().as_mut().unwrap().init(start, end - start);
     }
 }
 
+#[cfg(feature = "alloc_core")]
+pub fn init_alloc_core1() {
+    unsafe {
+        let start = &__heap1_start as *const usize as usize;
+        let end = &__heap1_end as *const usize as usize;
+        ALLOCATOR.1.get().as_mut().unwrap().init(start, end - start);
+    }
+}
 
 #[alloc_error_handler]
-fn alloc_error(_: core::alloc::Layout) -> ! {
-    panic!("alloc_error")
+fn alloc_error(layout: core::alloc::Layout) -> ! {
+    let id = MPIDR.read().cpu_id();
+    let heap = unsafe {
+        if id == 0 {
+            ALLOCATOR.0.get()
+        } else {
+            ALLOCATOR.1.get()
+        }
+        .as_mut()
+        .unwrap()
+    };
+    panic!(
+        "Core {} alloc_error, layout: {:?}, used memory: {}",
+        id,
+        layout,
+        heap.used()
+    );
 }