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"]
 

libsupport_zynq/src/ram.rs

@@ -1,55 +1,100 @@
+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) -> ! {
+    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());
 }