Implement Heap allocation (#3)

* Implement Maloc

* Implement Dealloc

* Migrate to a struct based heap implementation

src/framebuffer.rs

@@ -9,7 +9,6 @@ use crate::mailbox::{read_mailbox, write_mailbox};
 struct Mailbox([u32; 36]);
 
 const ALLOCATE_BUFFER: u32 = 0x0004_0001;
-const GET_PHYSICAL_DISPLAY_WH: u32 = 0x0004_0003;
 const SET_PHYSICAL_DISPLAY_WH: u32 = 0x0004_8003;
 const SET_VIRTUAL_DISPLAY_WH: u32 = 0x0004_8004;
 const SET_PIXEL_DEPTH: u32 = 0x0004_8005;
@@ -242,26 +241,3 @@ impl FrameBuffer {
         }
     }
 }
-
-pub fn print_display_resolution() {
-    let mut mailbox: [u32; 8] = [0; 8];
-    mailbox[0] = 8 * 4;
-    mailbox[1] = 0;
-    mailbox[2] = GET_PHYSICAL_DISPLAY_WH;
-    mailbox[3] = 8;
-    mailbox[4] = 0;
-    mailbox[5] = 0;
-    mailbox[6] = 0;
-    mailbox[7] = 0;
-
-    let addr = core::ptr::addr_of!(mailbox[0]) as u32;
-
-    write_mailbox(8, addr);
-
-    let _ = read_mailbox(8);
-    if mailbox[1] == 0 {
-        println!("Failed");
-    }
-
-    println!("Width x Height: {}x{}", mailbox[5], mailbox[6]);
-}

src/heap.rs

@@ -0,0 +1,216 @@
+#![allow(static_mut_refs)]
+
+use core::{
+    alloc::GlobalAlloc,
+    ptr::{self, null_mut, read_volatile},
+};
+
+use crate::NovaError;
+extern crate alloc;
+
+extern "C" {
+    static mut __heap_start: u8;
+    static mut __heap_end: u8;
+}
+
+#[repr(C, align(16))]
+pub struct HeapHeader {
+    pub next: *mut HeapHeader,
+    before: *mut HeapHeader,
+    pub size: usize,
+    free: bool,
+}
+
+const HEAP_HEADER_SIZE: usize = size_of::<HeapHeader>();
+const MIN_BLOCK_SIZE: usize = 16;
+
+// TODO: This implementation has to be reevaluated when implementing multiprocessing
+// Spinlock could be a solution but has its issues:
+// https://matklad.github.io/2020/01/02/spinlocks-considered-harmful.html
+pub static mut HEAP: Heap = Heap {
+    start_address: &raw mut __heap_start as *mut HeapHeader,
+    end_address: &raw mut __heap_end as *mut HeapHeader,
+    raw_size: 0,
+};
+
+// TODO: investigate if there is a better alternative to this
+pub unsafe fn init_global_heap() {
+    HEAP.init();
+}
+
+#[derive(Default)]
+pub struct Novalloc;
+
+unsafe impl GlobalAlloc for Novalloc {
+    unsafe fn alloc(&self, layout: core::alloc::Layout) -> *mut u8 {
+        HEAP.malloc(layout.size()).unwrap()
+    }
+
+    unsafe fn dealloc(&self, ptr: *mut u8, _: core::alloc::Layout) {
+        HEAP.free(ptr).unwrap();
+    }
+}
+
+#[global_allocator]
+static GLOBAL_ALLOCATOR: Novalloc = Novalloc;
+
+pub struct Heap {
+    start_address: *mut HeapHeader,
+    end_address: *mut HeapHeader,
+    raw_size: usize,
+}
+impl Heap {
+    pub fn new(heap_start: usize, heap_end: usize) -> Self {
+        let mut instance = Self {
+            start_address: &raw const heap_start as *mut HeapHeader,
+            end_address: &raw const heap_end as *mut HeapHeader,
+            raw_size: heap_end - heap_start,
+        };
+        instance.init();
+        instance
+    }
+
+    fn init(&mut self) {
+        self.raw_size = self.end_address as usize - self.start_address as usize;
+
+        unsafe {
+            ptr::write(
+                self.start_address,
+                HeapHeader {
+                    next: null_mut(),
+                    before: null_mut(),
+                    size: self.raw_size - HEAP_HEADER_SIZE,
+                    free: true,
+                },
+            );
+        }
+    }
+
+    unsafe fn find_first_fit(&self, size: usize) -> Result<*mut HeapHeader, NovaError> {
+        let mut current = self.start_address;
+        while !fits(size, current) {
+            if (*self.start_address).next.is_null() {
+                return Err(NovaError::HeapFull);
+            }
+            current = (*current).next;
+        }
+        Ok(current)
+    }
+
+    pub fn malloc(&self, mut size: usize) -> Result<*mut u8, NovaError> {
+        if size == 0 {
+            return Err(NovaError::EmptyHeapSegmentNotAllowed);
+        }
+
+        if size < MIN_BLOCK_SIZE {
+            size = MIN_BLOCK_SIZE;
+        }
+
+        // Align size to the next 16 bytes
+        size += (16 - (size % 16)) % 16;
+
+        unsafe {
+            // Find First-Fit memory segment
+            let current = self.find_first_fit(size)?;
+
+            // Return entire block WITHOUT generating a new header
+            // if the current block doesn't have enough space to hold: requested size + HEAP_HEADER_SIZE + MIN_BLOCK_SIZE
+            if (*current).size < size + HEAP_HEADER_SIZE + MIN_BLOCK_SIZE {
+                (*current).free = false;
+                return Ok(current.byte_add(HEAP_HEADER_SIZE) as *mut u8);
+            }
+
+            Self::fragment_segment(current, size);
+
+            let data_start_address = current.byte_add(HEAP_HEADER_SIZE);
+
+            Ok(data_start_address as *mut u8)
+        }
+    }
+
+    unsafe fn fragment_segment(current: *mut HeapHeader, size: usize) {
+        let byte_offset = HEAP_HEADER_SIZE + size;
+        let new_address = current.byte_add(byte_offset);
+
+        // Handle case where fragmenting center free space
+        let next = (*current).next;
+        if !(*current).next.is_null() {
+            (*next).before = new_address;
+        }
+
+        ptr::write(
+            new_address as *mut HeapHeader,
+            HeapHeader {
+                next,
+                before: current,
+                size: (*current).size - size - HEAP_HEADER_SIZE,
+                free: true,
+            },
+        );
+        (*current).next = new_address;
+        (*current).free = false;
+        (*current).size = size;
+    }
+
+    pub fn free(&self, pointer: *mut u8) -> Result<(), NovaError> {
+        let mut segment = unsafe { pointer.sub(HEAP_HEADER_SIZE) as *mut HeapHeader };
+        unsafe {
+            // IF prev is free:
+            // Delete header, add size to previous and fix pointers.
+            // Move Head left
+            if !(*segment).before.is_null() && (*(*segment).before).free {
+                let before_head = (*segment).before;
+                (*before_head).size += (*segment).size + HEAP_HEADER_SIZE;
+                delete_header(segment);
+                segment = before_head;
+            }
+            // IF next is free:
+            // Delete next header and merge size, fix pointers
+            if !(*segment).next.is_null() && (*(*segment).next).free {
+                let next_head = (*segment).next;
+                (*segment).size += (*next_head).size + HEAP_HEADER_SIZE;
+                delete_header(next_head);
+            }
+
+            // Neither: Set free
+            (*segment).free = true;
+        }
+
+        Ok(())
+    }
+
+    pub fn traverse_heap(&self) {
+        let mut pointer_address = self.start_address;
+        loop {
+            let head = unsafe { read_volatile(pointer_address) };
+            println!("Header {:#x}", pointer_address as u32);
+            println!("free: {}", head.free);
+            println!("size: {}", head.size);
+            println!("hasNext: {}", !head.next.is_null());
+            println!("");
+            if !head.next.is_null() {
+                pointer_address = head.next;
+            } else {
+                println!("---------------");
+                return;
+            }
+        }
+    }
+}
+
+unsafe fn fits(size: usize, header: *mut HeapHeader) -> bool {
+    (*header).free && size <= (*header).size
+}
+
+unsafe fn delete_header(header: *mut HeapHeader) {
+    let before = (*header).before;
+    let next = (*header).next;
+
+    if !before.is_null() {
+        (*before).next = next;
+    }
+
+    if !next.is_null() {
+        (*next).before = before;
+    }
+}

src/lib.rs

@@ -23,9 +23,9 @@ pub mod peripherals;
 
 pub mod configuration;
 pub mod framebuffer;
+pub mod heap;
 pub mod irq_interrupt;
 pub mod mailbox;
-pub mod math;
 pub mod timer;
 
 pub fn mmio_read(address: u32) -> u32 {
@@ -39,4 +39,6 @@ pub fn mmio_write(address: u32, data: u32) {
 #[derive(Debug)]
 pub enum NovaError {
     Mailbox,
+    HeapFull,
+    EmptyHeapSegmentNotAllowed,
 }

src/mailbox.rs

@@ -58,7 +58,10 @@ macro_rules! mailbox_command {
     };
 }
 
-mailbox_command!(mb_read_soc_temp, 0x00030006, 4, 8);
+mailbox_command!(mb_read_soc_temp, 0x0003_0006, 4, 8);
+
+// Framebuffer
+mailbox_command!(mb_get_display_resolution, 0x0004_0003, 0, 8);
 
 pub fn read_mailbox(channel: u32) -> u32 {
     // Wait until mailbox is not empty

src/main.rs

@@ -1,18 +1,20 @@
 #![no_main]
 #![no_std]
 #![feature(asm_experimental_arch)]
-
+#![allow(static_mut_refs)]
 use core::{
     arch::{asm, global_asm},
     panic::PanicInfo,
     ptr::write_volatile,
 };
 
+extern crate alloc;
+
 use nova::{
-    framebuffer::{print_display_resolution, FrameBuffer, BLUE, GREEN, ORANGE, RED, YELLOW},
+    framebuffer::{FrameBuffer, BLUE, GREEN, RED},
+    heap::{init_global_heap, HEAP},
     irq_interrupt::enable_irq_source,
     mailbox::mb_read_soc_temp,
-    math::polar_to_cartesian,
     peripherals::{
         gpio::{
             blink_gpio, gpio_pull_up, set_falling_edge_detect, set_gpio_function, GPIOFunction,
@@ -44,7 +46,7 @@ fn panic(_panic: &PanicInfo) -> ! {
 pub unsafe extern "C" fn _start() {
     // Set the stack pointer
     asm!(
-        "ldr x0, =0x8008000",
+        "ldr x0, =__stack_end",
         "mov sp, x0",
         "b main",
         options(noreturn)
@@ -61,8 +63,6 @@ pub extern "C" fn main() -> ! {
     // Set ACT Led to Outout
     let _ = set_gpio_function(21, GPIOFunction::Output);
 
-    print_current_el_str();
-
     // Delay so clock speed can stabilize
     delay_nops(50000);
     println!("Hello World!");
@@ -86,7 +86,9 @@ unsafe fn zero_bss() {
 
 #[no_mangle]
 pub extern "C" fn kernel_main() -> ! {
-    print_current_el_str();
+    println!("EL: {}", get_current_el());
+
+    heap_test();
 
     sleep_us(500_000);
 
@@ -96,20 +98,7 @@ pub extern "C" fn kernel_main() -> ! {
     gpio_pull_up(26);
     set_falling_edge_detect(26, true);
 
-    print_display_resolution();
     let fb = FrameBuffer::new();
-    print_display_resolution();
-
-    for a in 0..360 {
-        let (x, y) = polar_to_cartesian(100.0, a as f32);
-        fb.draw_line(
-            150,
-            150,
-            (150.0 + x) as u32,
-            (150.0 + y) as u32,
-            a * (0x00FFFFFF / 360),
-        );
-    }
 
     fb.draw_square(500, 500, 600, 700, RED);
     fb.draw_square_fill(800, 800, 900, 900, GREEN);
@@ -118,10 +107,6 @@ pub extern "C" fn kernel_main() -> ! {
     fb.draw_string("Hello World! :D\nTest next Line", 500, 5, 3, BLUE);
 
     fb.draw_function(cos, 100, 101, RED);
-    fb.draw_function(cos, 100, 102, ORANGE);
-    fb.draw_function(cos, 100, 103, YELLOW);
-    fb.draw_function(cos, 100, 104, GREEN);
-    fb.draw_function(cos, 100, 105, BLUE);
 
     loop {
         let temp = mb_read_soc_temp([0]).unwrap();
@@ -131,11 +116,28 @@ pub extern "C" fn kernel_main() -> ! {
     }
 }
 
+fn heap_test() {
+    unsafe {
+        init_global_heap();
+        let a = HEAP.malloc(32).unwrap();
+        let b = HEAP.malloc(64).unwrap();
+        let c = HEAP.malloc(128).unwrap();
+        let _ = HEAP.malloc(256).unwrap();
+        HEAP.traverse_heap();
+        HEAP.free(b).unwrap();
+        HEAP.traverse_heap();
+        HEAP.free(a).unwrap();
+        HEAP.traverse_heap();
+        HEAP.free(c).unwrap();
+        HEAP.traverse_heap();
+    }
+}
+
 fn cos(x: u32) -> f64 {
     libm::cos(x as f64 * 0.1) * 20.0
 }
 
-pub fn get_current_el() -> u64 {
+fn get_current_el() -> u64 {
     let el: u64;
     unsafe {
         asm!(
@@ -153,16 +155,3 @@ fn enable_uart() {
     let _ = set_gpio_function(14, GPIOFunction::Alternative0);
     let _ = set_gpio_function(15, GPIOFunction::Alternative0);
 }
-
-fn print_current_el_str() {
-    let el = get_current_el();
-    let el_str = match el {
-        0b11 => "Level 3",
-        0b10 => "Level 2",
-        0b01 => "Level 1",
-        0b00 => "Level 0",
-        _ => "Unknown EL",
-    };
-
-    println!("{}", el_str);
-}

src/math.rs

@@ -1,5 +0,0 @@
-pub fn polar_to_cartesian(r: f32, theta_rad: f32) -> (f32, f32) {
-    let x = r * libm::cosf(theta_rad);
-    let y = r * libm::sinf(theta_rad);
-    (x, y)
-}