From c0c3bcb6cc3c7d03cad5c77a0e0493a0dc0febdf Mon Sep 17 00:00:00 2001
From: pca006132 <john.lck40@gmail.com>
Date: Fri, 17 Jul 2020 12:01:03 +0800
Subject: [PATCH 1/2] libsupport_zynq/ram: split allocator for each core.

---
 libsupport_zynq/Cargo.toml |  1 +
 libsupport_zynq/src/ram.rs | 77 ++++++++++++++++++++++++++++----------
 2 files changed, 59 insertions(+), 19 deletions(-)

diff --git a/libsupport_zynq/Cargo.toml b/libsupport_zynq/Cargo.toml
index aff8b8b..aa47bb9 100644
--- a/libsupport_zynq/Cargo.toml
+++ b/libsupport_zynq/Cargo.toml
@@ -9,6 +9,7 @@ edition = "2018"
 target_zc706 = ["libboard_zynq/target_zc706"]
 target_cora_z7_10 = ["libboard_zynq/target_cora_z7_10"]
 panic_handler = []
+alloc_core = []
 
 default = ["panic_handler"]
 
diff --git a/libsupport_zynq/src/ram.rs b/libsupport_zynq/src/ram.rs
index f7721f8..7a3a0af 100644
--- a/libsupport_zynq/src/ram.rs
+++ b/libsupport_zynq/src/ram.rs
@@ -1,55 +1,94 @@
+use alloc::alloc::Layout;
 use core::alloc::GlobalAlloc;
 use core::ptr::NonNull;
-use alloc::alloc::Layout;
-use linked_list_allocator::Heap;
-use libcortex_a9::mutex::Mutex;
 use libboard_zynq::ddr::DdrRam;
+use libcortex_a9::{mutex::Mutex, regs::MPIDR};
+use libregister::RegisterR;
+use linked_list_allocator::Heap;
 
 #[global_allocator]
-static ALLOCATOR: CortexA9Alloc = CortexA9Alloc(Mutex::new(Heap::empty()));
+static ALLOCATOR: CortexA9Alloc =
+    CortexA9Alloc(Mutex::new(Heap::empty()), Mutex::new(Heap::empty()));
 
 /// LockedHeap doesn't lock properly
-struct CortexA9Alloc(Mutex<Heap>);
+struct CortexA9Alloc(Mutex<Heap>, Mutex<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.lock()
+        } else {
+            self.1.lock()
+        }
+        .allocate_first_fit(layout)
+        .ok()
+        .map_or(0 as *mut u8, |allocation| allocation.as_ptr())
     }
 
+    #[cfg(feature = "alloc_core")]
+    unsafe fn dealloc(&self, ptr: *mut u8, layout: Layout) {
+        let start0 = &__heap0_start as *const usize as *const u8;
+        let end0   = &__heap0_end   as *const usize as *const u8;
+        let start1 = &__heap1_start as *const usize as *const u8;
+        let end1   = &__heap1_end   as *const usize as *const u8;
+        let const_ptr = ptr as *const u8;
+
+        if start0 <= const_ptr && const_ptr < end0 {
+            self.0.lock()
+        } else if start1 <= const_ptr && const_ptr < end1 {
+            self.1.lock()
+        } else {
+            panic!("Invalid deallocation: {:p} is not in heap0 nor heap1", ptr);
+        }
+            .deallocate(NonNull::new_unchecked(ptr), layout)
+    }
+
+    #[cfg(not(feature = "alloc_core"))]
     unsafe fn dealloc(&self, ptr: *mut u8, layout: Layout) {
         self.0.lock()
             .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
+            .lock()
             .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.lock().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.lock().init(start, end - start);
+    }
+}
 
 #[alloc_error_handler]
-fn alloc_error(_: core::alloc::Layout) -> ! {
-    panic!("alloc_error")
+fn alloc_error(layout: core::alloc::Layout) -> ! {
+    panic!("Core {} alloc_error, layout: {:?}", MPIDR.read().cpu_id(), layout);
 }
-- 
2.44.1


From 1bb01220345e0a54f8b70fa7f6dd73c6a697e291 Mon Sep 17 00:00:00 2001
From: pca006132 <john.lck40@gmail.com>
Date: Fri, 17 Jul 2020 16:16:01 +0800
Subject: [PATCH 2/2] libsupport_zynq/ram: print heap usage on alloc error.

---
 libsupport_zynq/src/ram.rs | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/libsupport_zynq/src/ram.rs b/libsupport_zynq/src/ram.rs
index 7a3a0af..a712374 100644
--- a/libsupport_zynq/src/ram.rs
+++ b/libsupport_zynq/src/ram.rs
@@ -90,5 +90,11 @@ pub fn init_alloc_core1() {
 
 #[alloc_error_handler]
 fn alloc_error(layout: core::alloc::Layout) -> ! {
-    panic!("Core {} alloc_error, layout: {:?}", MPIDR.read().cpu_id(), layout);
+    let id = MPIDR.read().cpu_id();
+    let heap = if id == 0 {
+        ALLOCATOR.0.lock()
+    } else {
+        ALLOCATOR.1.lock()
+    };
+    panic!("Core {} alloc_error, layout: {:?}, used memory: {}", id, layout, heap.used());
 }
-- 
2.44.1