feat: implement first SVC mailbox instruction (#6)

* refactor: organize code

* feat: move EL0 stack to virtual space

* wip

* feat: Enable EL0 basic mailbox access via SVCs

* refactor: move irq interrupts

README.md

@@ -15,9 +15,10 @@ NovaOS is a expository project where I build a kernel from scratch for a Raspber
 - Frame Buffer ✓
 - Heap Memory allocation ✓
 - MMU ✓
-- SVC instructions
+- SVC instructions ~
-- Kernel Independent Applications
 - Multi Core
+- Multi Applications
 - Dynamic clock speed
+- Kernel Independent Applications
 - Multiprocessing
 - Basic Terminal over UART

link.ld

@@ -5,13 +5,18 @@ SECTIONS {
         KEEP(*(.text._start))
         *(.text .text.*)
     }
+    .vector_table ALIGN(2K) : {
+        KEEP(*(.vector_t))
+    }
+
+    . = ALIGN(4K);
+    __text_end = .;
 
     .rodata : {
         *(.rodata .rodata.*)
     }
 
-    .data ALIGN(2M) : {
+    .data  : {
-        _data = .;
         *(.data .data.*)
     }
 
@@ -21,9 +26,9 @@ SECTIONS {
         __bss_end = .;
     }
 
-    .vector_table ALIGN(2K) : {
+    . = ALIGN(2M);
-        KEEP(*(.vector_table))
+
-    }
+    __share_end = .;
 
     # EL2 Stack
     .stack ALIGN(16): {
@@ -36,15 +41,6 @@ SECTIONS {
     . = ALIGN(2M);
 
     __kernel_end = .;
-
-    .stack_el0 : {
-    __stack_start_el0 = .;
-    . += 10K; #10kB stack
-    __stack_end_el0 = .;
-    }
-
-    . = ALIGN(2M);
-    _end = .;
 }
 
 __bss_size = (__bss_end - __bss_start) >> 3;

src/aarch64/mmu.rs

@@ -1,16 +1,12 @@
-use core::panic;
-
 use core::mem::size_of;
 use nova_error::NovaError;
 
-use crate::get_current_el;
+use crate::{
-
+    aarch64::mmu::physical_mapping::{
-unsafe extern "C" {
+        reserve_block, reserve_block_explicit, reserve_page, reserve_page_explicit,
-    static mut __translation_table_l2_start: u64;
+    },
-    static __stack_start_el0: u64;
+    get_current_el,
-    static __kernel_end: u64;
+};
-    static _data: u64;
-}
 
 const BLOCK: u64 = 0b01;
 const TABLE: u64 = 0b11;
@@ -51,130 +47,149 @@ pub const KERNEL_VIRTUAL_MEM_SPACE: usize = 0xFFFF_FF80_0000_0000;
 
 pub const STACK_START_ADDR: usize = !KERNEL_VIRTUAL_MEM_SPACE & (!0xF);
 
+pub mod physical_mapping;
+
+pub type VirtAddr = usize;
+pub type PhysAddr = usize;
+
+#[derive(Clone, Copy)]
+pub struct TableEntry {
+    value: u64,
+}
+
+impl TableEntry {
+    pub fn invalid() -> Self {
+        Self { value: 0 }
+    }
+
+    fn table_descriptor(addr: PhysAddr) -> Self {
+        Self {
+            value: (addr as u64 & 0x0000_FFFF_FFFF_F000) | TABLE,
+        }
+    }
+
+    fn block_descriptor(physical_address: usize, additional_flags: u64) -> Self {
+        Self {
+            value: (physical_address as u64 & 0x0000_FFFF_FFFF_F000)
+                | BLOCK
+                | ACCESS_FLAG
+                | INNER_SHAREABILITY
+                | additional_flags,
+        }
+    }
+
+    fn page_descriptor(physical_address: usize, additional_flags: u64) -> Self {
+        Self {
+            value: (physical_address as u64 & 0x0000_FFFF_FFFF_F000)
+                | PAGE
+                | ACCESS_FLAG
+                | INNER_SHAREABILITY
+                | additional_flags,
+        }
+    }
+
+    fn is_invalid(self) -> bool {
+        self.value & 0b11 == 0
+    }
+
+    #[inline]
+    fn address(self) -> PhysAddr {
+        self.value as usize & 0x0000_FFFF_FFFF_F000
+    }
+}
+
+pub enum PhysSource {
+    Any,
+    Explicit(PhysAddr),
+}
+
 #[repr(align(4096))]
-pub struct PageTable([u64; TABLE_ENTRY_COUNT]);
+pub struct PageTable([TableEntry; TABLE_ENTRY_COUNT]);
 
 #[no_mangle]
-pub static mut TRANSLATIONTABLE_TTBR0: PageTable = PageTable([0; 512]);
+pub static mut TRANSLATIONTABLE_TTBR0: PageTable = PageTable([TableEntry { value: 0 }; 512]);
 #[no_mangle]
-pub static mut TRANSLATIONTABLE_TTBR1: PageTable = PageTable([0; 512]);
+pub static mut TRANSLATIONTABLE_TTBR1: PageTable = PageTable([TableEntry { value: 0 }; 512]);
-
-static mut PAGING_BITMAP: [u64; MAX_PAGE_COUNT / 64] = [0; MAX_PAGE_COUNT / 64];
 
 /// Allocate a memory block of `size` starting at `virtual_address`.
 pub fn allocate_memory(
-    mut virtual_address: usize,
+    virtual_address: usize,
-    mut size: usize,
+    size_bytes: usize,
-    additional_flags: u64,
+    phys: PhysSource,
+    flags: u64,
 ) -> Result<(), NovaError> {
     if !virtual_address.is_multiple_of(GRANULARITY) {
         return Err(NovaError::Misalignment);
     }
-
+    if !size_bytes.is_multiple_of(GRANULARITY) {
-    let level1_blocks = size / LEVEL1_BLOCK_SIZE;
-    size %= LEVEL1_BLOCK_SIZE;
-    let level2_blocks = size / LEVEL2_BLOCK_SIZE;
-    size %= LEVEL2_BLOCK_SIZE;
-    let level3_pages = size / GRANULARITY;
-    if !size.is_multiple_of(GRANULARITY) {
         return Err(NovaError::InvalidGranularity);
     }
 
-    if level1_blocks > 0 {
-        todo!("Currently not supported");
-    }
-
     let base_table = if virtual_address & KERNEL_VIRTUAL_MEM_SPACE > 0 {
         core::ptr::addr_of_mut!(TRANSLATIONTABLE_TTBR1)
     } else {
         core::ptr::addr_of_mut!(TRANSLATIONTABLE_TTBR0)
     };
 
-    for _ in 0..level2_blocks {
+    match phys {
-        alloc_block_l2(virtual_address, base_table, additional_flags)?;
+        PhysSource::Any => map_range_dynamic(virtual_address, size_bytes, base_table, flags),
-        virtual_address += LEVEL2_BLOCK_SIZE;
+        PhysSource::Explicit(phys_addr) => {
+            map_range_explicit(virtual_address, phys_addr, size_bytes, base_table, flags)
+        }
     }
-    for _ in 0..level3_pages {
+}
-        alloc_page(virtual_address, base_table, additional_flags)?;
+
-        virtual_address += GRANULARITY;
+fn map_range_explicit(
+    mut virt: VirtAddr,
+    mut phys: PhysAddr,
+    size_bytes: usize,
+    base: *mut PageTable,
+    flags: u64,
+) -> Result<(), NovaError> {
+    let mut remaining = size_bytes;
+
+    while !virt.is_multiple_of(LEVEL2_BLOCK_SIZE) && remaining > 0 {
+        map_page(virt, phys, base, flags)?;
+        (virt, _) = virt.overflowing_add(GRANULARITY);
+        phys += GRANULARITY;
+        remaining -= GRANULARITY;
+    }
+
+    while remaining >= LEVEL2_BLOCK_SIZE {
+        map_l2_block(virt, phys, base, flags)?;
+        (virt, _) = virt.overflowing_add(LEVEL2_BLOCK_SIZE);
+        phys += LEVEL2_BLOCK_SIZE;
+        remaining -= LEVEL2_BLOCK_SIZE;
+    }
+
+    while remaining > 0 {
+        map_page(virt, phys, base, flags)?;
+        (virt, _) = virt.overflowing_add(GRANULARITY);
+        phys += GRANULARITY;
+        remaining -= GRANULARITY;
     }
 
     Ok(())
 }
 
-/// Allocate a memory block of `size` starting at `virtual_address`,
+fn map_range_dynamic(
-/// with explicit physical_address.
+    mut virt: PhysAddr,
-///
+    size_bytes: usize,
-/// Note: This can be used when mapping predefined regions.
+    base: *mut PageTable,
-pub fn allocate_memory_explicit(
+    flags: u64,
-    mut virtual_address: usize,
-    mut size: usize,
-    mut physical_address: usize,
-    additional_flags: u64,
 ) -> Result<(), NovaError> {
-    if !virtual_address.is_multiple_of(GRANULARITY) {
+    let mut remaining = size_bytes;
-        return Err(NovaError::Misalignment);
+
-    }
+    while remaining >= LEVEL2_BLOCK_SIZE {
-    if !physical_address.is_multiple_of(GRANULARITY) {
+        map_l2_block(virt, reserve_block(), base, flags)?;
-        return Err(NovaError::Misalignment);
+        (virt, _) = virt.overflowing_add(LEVEL2_BLOCK_SIZE);
+        remaining -= LEVEL2_BLOCK_SIZE;
     }
 
-    let level1_blocks = size / LEVEL1_BLOCK_SIZE;
+    while remaining > 0 {
-    size %= LEVEL1_BLOCK_SIZE;
+        map_page(virt, reserve_page(), base, flags)?;
-    let mut level2_blocks = size / LEVEL2_BLOCK_SIZE;
+        (virt, _) = virt.overflowing_add(GRANULARITY);
-    size %= LEVEL2_BLOCK_SIZE;
+        remaining -= GRANULARITY;
-    let mut level3_pages = size / GRANULARITY;
-    if !size.is_multiple_of(GRANULARITY) {
-        return Err(NovaError::InvalidGranularity);
-    }
-
-    if level1_blocks > 0 {
-        todo!("Currently not supported");
-    }
-
-    let l2_alignment = (physical_address % LEVEL2_BLOCK_SIZE) / GRANULARITY;
-    if l2_alignment != 0 {
-        let l3_diff = LEVEL2_BLOCK_SIZE / GRANULARITY - l2_alignment;
-        if l3_diff > level3_pages {
-            level2_blocks -= 1;
-            level3_pages += TABLE_ENTRY_COUNT;
-        }
-
-        level3_pages -= l3_diff;
-
-        for _ in 0..l3_diff {
-            alloc_page_explicit(
-                virtual_address,
-                physical_address,
-                core::ptr::addr_of_mut!(TRANSLATIONTABLE_TTBR0),
-                additional_flags,
-            )?;
-
-            virtual_address += GRANULARITY;
-            physical_address += GRANULARITY;
-        }
-    }
-
-    for _ in 0..level2_blocks {
-        alloc_block_l2_explicit(
-            virtual_address,
-            physical_address,
-            core::ptr::addr_of_mut!(TRANSLATIONTABLE_TTBR0),
-            additional_flags,
-        )?;
-        virtual_address += LEVEL2_BLOCK_SIZE;
-        physical_address += LEVEL2_BLOCK_SIZE;
-    }
-
-    for _ in 0..level3_pages {
-        alloc_page_explicit(
-            virtual_address,
-            physical_address,
-            core::ptr::addr_of_mut!(TRANSLATIONTABLE_TTBR0),
-            additional_flags,
-        )?;
-        virtual_address += GRANULARITY;
-        physical_address += GRANULARITY;
     }
 
     Ok(())
@@ -210,7 +225,7 @@ pub fn alloc_page_explicit(
     )
 }
 
-fn map_page(
+pub fn map_page(
     virtual_address: usize,
     physical_address: usize,
     base_table_ptr: *mut PageTable,
@@ -220,32 +235,18 @@ fn map_page(
 
     let offsets = [l1_off, l2_off];
 
-    let table_ptr = navigate_table(base_table_ptr, &offsets)?;
+    let table_ptr = navigate_table(base_table_ptr, &offsets, true)?;
     let table = unsafe { &mut *table_ptr };
 
-    if table.0[l3_off] & 0b11 > 0 {
+    if !table.0[l3_off].is_invalid() {
         return Err(NovaError::Paging);
     }
 
-    table.0[l3_off] = create_page_descriptor_entry(physical_address, additional_flags);
+    table.0[l3_off] = TableEntry::page_descriptor(physical_address, additional_flags);
 
     Ok(())
 }
 
-// Allocate a level 2 block.
-pub fn alloc_block_l2(
-    virtual_addr: usize,
-    base_table_ptr: *mut PageTable,
-    additional_flags: u64,
-) -> Result<(), NovaError> {
-    map_l2_block(
-        virtual_addr,
-        reserve_block(),
-        base_table_ptr,
-        additional_flags,
-    )
-}
-
 // Allocate a level 2 block, at a explicit `physical_address`.
 pub fn alloc_block_l2_explicit(
     virtual_addr: usize,
@@ -274,26 +275,26 @@ pub fn map_l2_block(
 ) -> Result<(), NovaError> {
     let (l1_off, l2_off, _) = virtual_address_to_table_offset(virtual_addr);
     let offsets = [l1_off];
-    let table_ptr = navigate_table(base_table_ptr, &offsets)?;
+    let table_ptr = navigate_table(base_table_ptr, &offsets, true)?;
 
     let table = unsafe { &mut *table_ptr };
 
     // Verify virtual address is available.
-    if table.0[l2_off] & 0b11 != 0 {
+    if !table.0[l2_off].is_invalid() {
         return Err(NovaError::Paging);
     }
 
-    let new_entry = create_block_descriptor_entry(physical_address, additional_flags);
+    let new_entry = TableEntry::block_descriptor(physical_address, additional_flags);
 
     table.0[l2_off] = new_entry;
 
     Ok(())
 }
 
-pub fn reserve_range_explicit(
+pub fn reserve_range(
-    start_physical_address: usize,
+    start_physical_address: PhysAddr,
-    end_physical_address: usize,
+    end_physical_address: PhysAddr,
-) -> Result<(), NovaError> {
+) -> Result<PhysAddr, NovaError> {
     let mut size = end_physical_address - start_physical_address;
     let l1_blocks = size / LEVEL1_BLOCK_SIZE;
     size %= LEVEL1_BLOCK_SIZE;
@@ -320,77 +321,7 @@ pub fn reserve_range_explicit(
         addr += GRANULARITY;
     }
 
-    Ok(())
+    Ok(start_physical_address)
-}
-
-fn reserve_page() -> usize {
-    if let Some(address) = find_unallocated_page() {
-        let page = address / GRANULARITY;
-        let word_index = page / 64;
-        unsafe { PAGING_BITMAP[word_index] |= 1 << (page % 64) };
-        return address;
-    }
-    panic!("Out of Memory!");
-}
-
-fn reserve_page_explicit(physical_address: usize) -> Result<(), NovaError> {
-    let page = physical_address / GRANULARITY;
-    let word_index = page / 64;
-
-    if unsafe { PAGING_BITMAP[word_index] } & (1 << (page % 64)) > 0 {
-        return Err(NovaError::Paging);
-    }
-
-    unsafe { PAGING_BITMAP[word_index] |= 1 << (page % 64) };
-    Ok(())
-}
-
-fn reserve_block() -> usize {
-    if let Some(start) = find_contiguous_free_bitmap_words(L2_BLOCK_BITMAP_WORDS) {
-        for j in 0..L2_BLOCK_BITMAP_WORDS {
-            unsafe { PAGING_BITMAP[start + j] = u64::MAX };
-        }
-        return start * 64 * GRANULARITY;
-    }
-
-    panic!("Out of Memory!");
-}
-
-fn reserve_block_explicit(physical_address: usize) -> Result<(), NovaError> {
-    let page = physical_address / GRANULARITY;
-    for i in 0..L2_BLOCK_BITMAP_WORDS {
-        unsafe {
-            if PAGING_BITMAP[(page / 64) + i] != 0 {
-                return Err(NovaError::Paging);
-            }
-        };
-    }
-    for i in 0..L2_BLOCK_BITMAP_WORDS {
-        unsafe {
-            PAGING_BITMAP[(page / 64) + i] = u64::MAX;
-        };
-    }
-    Ok(())
-}
-
-fn create_block_descriptor_entry(physical_address: usize, additional_flags: u64) -> u64 {
-    (physical_address as u64 & 0x0000_FFFF_FFFF_F000)
-        | BLOCK
-        | ACCESS_FLAG
-        | INNER_SHAREABILITY
-        | additional_flags
-}
-
-fn create_page_descriptor_entry(physical_address: usize, additional_flags: u64) -> u64 {
-    (physical_address as u64 & 0x0000_FFFF_FFFF_F000)
-        | PAGE
-        | ACCESS_FLAG
-        | INNER_SHAREABILITY
-        | additional_flags
-}
-
-fn create_table_descriptor_entry(addr: usize) -> u64 {
-    (addr as u64 & 0x0000_FFFF_FFFF_F000) | TABLE
 }
 
 fn virtual_address_to_table_offset(virtual_addr: usize) -> (usize, usize, usize) {
@@ -401,27 +332,16 @@ fn virtual_address_to_table_offset(virtual_addr: usize) -> (usize, usize, usize)
     (l1_off, l2_off, l3_off)
 }
 
-/// Debugging function to navigate the translation tables.
-#[allow(unused_variables)]
-pub fn sim_l3_access(addr: usize) {
-    unsafe {
-        let entry1 = TRANSLATIONTABLE_TTBR0.0[addr / LEVEL1_BLOCK_SIZE];
-        let table2 = &mut *(entry_phys(entry1 as usize) as *mut PageTable);
-        let entry2 = table2.0[(addr % LEVEL1_BLOCK_SIZE) / LEVEL2_BLOCK_SIZE];
-        let table3 = &mut *(entry_phys(entry2 as usize) as *mut PageTable);
-        let _entry3 = table3.0[(addr % LEVEL2_BLOCK_SIZE) / GRANULARITY];
-    }
-}
-
 /// Navigate the table tree, by following given offsets. This function
 /// allocates new tables if required.
 fn navigate_table(
     initial_table_ptr: *mut PageTable,
     offsets: &[usize],
+    create_missing: bool,
 ) -> Result<*mut PageTable, NovaError> {
     let mut table = initial_table_ptr;
     for offset in offsets {
-        table = next_table(table, *offset)?;
+        table = next_table(table, *offset, create_missing)?;
     }
     Ok(table)
 }
@@ -429,13 +349,20 @@ fn navigate_table(
 /// Get the next table one level down.
 ///
 /// If table doesn't exit a page will be allocated for it.
-fn next_table(table_ptr: *mut PageTable, offset: usize) -> Result<*mut PageTable, NovaError> {
+fn next_table(
+    table_ptr: *mut PageTable,
+    offset: usize,
+    create_missing: bool,
+) -> Result<*mut PageTable, NovaError> {
     let table = unsafe { &mut *table_ptr };
-    match table.0[offset] & 0b11 {
+    match table.0[offset].value & 0b11 {
         0 => {
+            if !create_missing {
+                return Err(NovaError::Paging);
+            }
             let new_phys_page_table_address = reserve_page();
 
-            table.0[offset] = create_table_descriptor_entry(new_phys_page_table_address);
+            table.0[offset] = TableEntry::table_descriptor(new_phys_page_table_address);
             map_page(
                 phys_table_to_kernel_space(new_phys_page_table_address),
                 new_phys_page_table_address,
@@ -443,66 +370,34 @@ fn next_table(table_ptr: *mut PageTable, offset: usize) -> Result<*mut PageTable
                 NORMAL_MEM | WRITABLE | PXN | UXN,
             )?;
 
-            Ok(entry_table_addr(table.0[offset] as usize) as *mut PageTable)
+            Ok(resolve_table_addr(table.0[offset].address()) as *mut PageTable)
         }
         1 => Err(NovaError::Paging),
-        3 => Ok(entry_table_addr(table.0[offset] as usize) as *mut PageTable),
+        3 => Ok(resolve_table_addr(table.0[offset].address()) as *mut PageTable),
         _ => unreachable!(),
     }
 }
 
-fn find_unallocated_page() -> Option<usize> {
+/// Converts a physical table address and returns the corresponding virtual address depending on EL.
-    for (i, entry) in unsafe { PAGING_BITMAP }.iter().enumerate() {
+///
-        if *entry != u64::MAX {
+/// - `== EL0` -> panic
-            for offset in 0..64 {
+/// - `== EL1` -> 0xFFFFFF82XXXXXXXX
-                if entry >> offset & 0b1 == 0 {
+/// - `>= EL2` -> physical address
-                    return Some((i * 64 + offset) * GRANULARITY);
-                }
-            }
-        }
-    }
-    None
-}
-
-fn find_contiguous_free_bitmap_words(required_words: usize) -> Option<usize> {
-    let mut run_start = 0;
-    let mut run_len = 0;
-
-    for (i, entry) in unsafe { PAGING_BITMAP }.iter().enumerate() {
-        if *entry == 0 {
-            if run_len == 0 {
-                run_start = i;
-            }
-            run_len += 1;
-
-            if run_len == required_words {
-                return Some(run_start);
-            }
-        } else {
-            run_len = 0;
-        }
-    }
-
-    None
-}
-
-/// Extracts the physical address out of an table entry.
 #[inline]
-fn entry_phys(entry: usize) -> usize {
+fn resolve_table_addr(physical_address: PhysAddr) -> VirtAddr {
-    entry & 0x0000_FFFF_FFFF_F000
+    let current_el = get_current_el();
-}
 
-#[inline]
+    if current_el >= 2 {
-fn entry_table_addr(entry: usize) -> usize {
+        physical_address
-    if get_current_el() == 1 {
+    } else if get_current_el() == 1 {
-        phys_table_to_kernel_space(entry_phys(entry))
+        phys_table_to_kernel_space(physical_address)
     } else {
-        entry_phys(entry)
+        panic!("Access to table entries is forbidden in EL0.")
     }
 }
 
 /// Extracts the physical address out of an table entry.
 #[inline]
-fn phys_table_to_kernel_space(entry: usize) -> usize {
+fn phys_table_to_kernel_space(entry: usize) -> VirtAddr {
     entry | TRANSLATION_TABLE_BASE_ADDR
 }

src/aarch64/mmu/physical_mapping.rs

@@ -0,0 +1,95 @@
+use crate::aarch64::mmu::{PhysAddr, GRANULARITY, L2_BLOCK_BITMAP_WORDS, MAX_PAGE_COUNT};
+use nova_error::NovaError;
+
+struct PagingMap {
+    bitmap: [u64; MAX_PAGE_COUNT / 64],
+}
+
+static mut PAGING_BITMAP: PagingMap = PagingMap {
+    bitmap: [0; MAX_PAGE_COUNT / 64],
+};
+
+pub fn reserve_page() -> PhysAddr {
+    if let Some(address) = find_unallocated_page() {
+        let page = address / GRANULARITY;
+        let word_index = page / 64;
+        unsafe { PAGING_BITMAP.bitmap[word_index] |= 1 << (page % 64) };
+        return address;
+    }
+    panic!("Out of Memory!");
+}
+
+pub fn reserve_page_explicit(physical_address: usize) -> Result<PhysAddr, NovaError> {
+    let page = physical_address / GRANULARITY;
+    let word_index = page / 64;
+
+    if unsafe { PAGING_BITMAP.bitmap[word_index] } & (1 << (page % 64)) > 0 {
+        return Err(NovaError::Paging);
+    }
+
+    unsafe { PAGING_BITMAP.bitmap[word_index] |= 1 << (page % 64) };
+    Ok(physical_address)
+}
+
+pub fn reserve_block() -> usize {
+    if let Some(start) = find_contiguous_free_bitmap_words(L2_BLOCK_BITMAP_WORDS) {
+        for j in 0..L2_BLOCK_BITMAP_WORDS {
+            unsafe { PAGING_BITMAP.bitmap[start + j] = u64::MAX };
+        }
+        return start * 64 * GRANULARITY;
+    }
+
+    panic!("Out of Memory!");
+}
+
+pub fn reserve_block_explicit(physical_address: usize) -> Result<(), NovaError> {
+    let page = physical_address / GRANULARITY;
+    for i in 0..L2_BLOCK_BITMAP_WORDS {
+        unsafe {
+            if PAGING_BITMAP.bitmap[(page / 64) + i] != 0 {
+                return Err(NovaError::Paging);
+            }
+        };
+    }
+    for i in 0..L2_BLOCK_BITMAP_WORDS {
+        unsafe {
+            PAGING_BITMAP.bitmap[(page / 64) + i] = u64::MAX;
+        };
+    }
+    Ok(())
+}
+
+fn find_unallocated_page() -> Option<usize> {
+    for (i, entry) in unsafe { PAGING_BITMAP.bitmap }.iter().enumerate() {
+        if *entry != u64::MAX {
+            for offset in 0..64 {
+                if entry >> offset & 0b1 == 0 {
+                    return Some((i * 64 + offset) * GRANULARITY);
+                }
+            }
+        }
+    }
+    None
+}
+
+fn find_contiguous_free_bitmap_words(required_words: usize) -> Option<usize> {
+    let mut run_start = 0;
+    let mut run_len = 0;
+
+    for (i, entry) in unsafe { PAGING_BITMAP.bitmap }.iter().enumerate() {
+        if *entry == 0 {
+            if run_len == 0 {
+                run_start = i;
+            }
+            run_len += 1;
+
+            if run_len == required_words {
+                return Some(run_start);
+            }
+        } else {
+            run_len = 0;
+        }
+    }
+
+    None
+}

src/config.S

@@ -87,107 +87,11 @@ el1_to_el0:
     msr ELR_EL1, x0
 
     // Set SP_EL1 to stack base
-    ldr x0, =__stack_end_el0
+    adrp x0, EL0_STACK_TOP
-    msr SP_EL0, x0
+    ldr  x1, [x0, :lo12:EL0_STACK_TOP]
+    msr SP_EL0, x1
 
     isb
 
     // Return to EL0
     eret
-
-
-.align 4
-irq_handler:
-    sub sp, sp, #176
-    stp x0, x1, [sp, #0]
-    stp x2, x3, [sp, #16]
-    stp x4, x5, [sp, #32]
-    stp x6, x7, [sp, #48]
-    stp x8, x9, [sp, #64]
-    stp x10, x11, [sp, #80]
-    stp x12, x13, [sp, #96]
-    stp x14, x15, [sp, #112]
-    stp x16, x17, [sp, #128]
-    stp x18, x29, [sp, #144]
-    stp x30, xzr, [sp, #160]
-
-    bl rust_irq_handler
-
-    ldp x0, x1, [sp, #0]
-    ldp x2, x3, [sp, #16]
-    ldp x4, x5, [sp, #32]
-    ldp x6, x7, [sp, #48]
-    ldp x8, x9, [sp, #64]
-    ldp x10, x11, [sp, #80]
-    ldp x12, x13, [sp, #96]
-    ldp x14, x15, [sp, #112]
-    ldp x16, x17, [sp, #128]
-    ldp x18, x29, [sp, #144]
-    ldp x30, xzr, [sp, #160]
-    add sp, sp, #176
-
-    eret
-
-.align 4
-synchronous_interrupt_imm_lower_aarch64:
-    sub sp, sp, #176
-    stp x0, x1, [sp, #0]
-    stp x2, x3, [sp, #16]
-    stp x4, x5, [sp, #32]
-    stp x6, x7, [sp, #48]
-    stp x8, x9, [sp, #64]
-    stp x10, x11, [sp, #80]
-    stp x12, x13, [sp, #96]
-    stp x14, x15, [sp, #112]
-    stp x16, x17, [sp, #128]
-    stp x18, x29, [sp, #144]
-    stp x30, xzr, [sp, #160]
-
-    bl rust_synchronous_interrupt_imm_lower_aarch64
-
-    ldp x0, x1, [sp, #0]
-    ldp x2, x3, [sp, #16]
-    ldp x4, x5, [sp, #32]
-    ldp x6, x7, [sp, #48]
-    ldp x8, x9, [sp, #64]
-    ldp x10, x11, [sp, #80]
-    ldp x12, x13, [sp, #96]
-    ldp x14, x15, [sp, #112]
-    ldp x16, x17, [sp, #128]
-    ldp x18, x29, [sp, #144]
-    ldp x30, xzr, [sp, #160]
-    add sp, sp, #176
-
-    eret
-
-.align 4
-synchronous_interrupt_no_el_change:
-    sub sp, sp, #176
-    stp x0, x1, [sp, #0]
-    stp x2, x3, [sp, #16]
-    stp x4, x5, [sp, #32]
-    stp x6, x7, [sp, #48]
-    stp x8, x9, [sp, #64]
-    stp x10, x11, [sp, #80]
-    stp x12, x13, [sp, #96]
-    stp x14, x15, [sp, #112]
-    stp x16, x17, [sp, #128]
-    stp x18, x29, [sp, #144]
-    stp x30, xzr, [sp, #160]
-
-    bl rust_synchronous_interrupt_no_el_change
-
-    ldp x0, x1, [sp, #0]
-    ldp x2, x3, [sp, #16]
-    ldp x4, x5, [sp, #32]
-    ldp x6, x7, [sp, #48]
-    ldp x8, x9, [sp, #64]
-    ldp x10, x11, [sp, #80]
-    ldp x12, x13, [sp, #96]
-    ldp x14, x15, [sp, #112]
-    ldp x16, x17, [sp, #128]
-    ldp x18, x29, [sp, #144]
-    ldp x30, xzr, [sp, #160]
-    add sp, sp, #176
-
-    eret

src/configuration.rs

@@ -31,80 +31,4 @@ const AS: u64 = 0b1 << 36; // configure an ASID size of 16 bits
 #[no_mangle]
 pub static TCR_EL1_CONF: u64 = IPS | TG0 | TG1 | T0SZ | T1SZ | SH0 | SH1 | AS;
 
-pub mod mmu {
+pub mod memory_mapping;
-
-    use crate::{
-        aarch64::mmu::{
-            alloc_block_l2, alloc_block_l2_explicit, map_l2_block, reserve_range_explicit,
-            DEVICE_MEM, EL0_ACCESSIBLE, KERNEL_VIRTUAL_MEM_SPACE, LEVEL1_BLOCK_SIZE,
-            LEVEL2_BLOCK_SIZE, NORMAL_MEM, PXN, READ_ONLY, STACK_START_ADDR,
-            TRANSLATIONTABLE_TTBR0, TRANSLATIONTABLE_TTBR1, UXN, WRITABLE,
-        },
-        PERIPHERAL_BASE,
-    };
-
-    #[no_mangle]
-    static EL1_STACK_TOP: usize = STACK_START_ADDR | KERNEL_VIRTUAL_MEM_SPACE;
-    const EL1_STACK_BOTTOM: usize = EL1_STACK_TOP - LEVEL2_BLOCK_SIZE * 2;
-
-    extern "C" {
-        static _data: u64;
-        static _end: u64;
-        static __kernel_end: u64;
-    }
-
-    pub fn initialize_mmu_translation_tables() {
-        let shared_segment_end = unsafe { &_data } as *const _ as usize;
-        let kernel_end = unsafe { &__kernel_end } as *const _ as usize;
-        let user_space_end = unsafe { &_end } as *const _ as usize;
-
-        reserve_range_explicit(0x0, user_space_end).unwrap();
-
-        for addr in (0..shared_segment_end).step_by(LEVEL2_BLOCK_SIZE) {
-            let _ = map_l2_block(
-                addr,
-                addr,
-                core::ptr::addr_of_mut!(TRANSLATIONTABLE_TTBR0),
-                EL0_ACCESSIBLE | READ_ONLY | NORMAL_MEM,
-            );
-        }
-
-        for addr in (shared_segment_end..kernel_end).step_by(LEVEL2_BLOCK_SIZE) {
-            let _ = map_l2_block(
-                addr,
-                addr,
-                core::ptr::addr_of_mut!(TRANSLATIONTABLE_TTBR0),
-                WRITABLE | UXN | NORMAL_MEM,
-            );
-        }
-
-        for addr in (kernel_end..user_space_end).step_by(LEVEL2_BLOCK_SIZE) {
-            let _ = map_l2_block(
-                addr,
-                addr,
-                core::ptr::addr_of_mut!(TRANSLATIONTABLE_TTBR0),
-                EL0_ACCESSIBLE | WRITABLE | PXN | NORMAL_MEM,
-            );
-        }
-
-        for addr in (PERIPHERAL_BASE..LEVEL1_BLOCK_SIZE).step_by(LEVEL2_BLOCK_SIZE) {
-            let _ = alloc_block_l2_explicit(
-                addr,
-                addr,
-                core::ptr::addr_of_mut!(TRANSLATIONTABLE_TTBR0),
-                EL0_ACCESSIBLE | WRITABLE | UXN | PXN | DEVICE_MEM,
-            );
-        }
-
-        for addr in (EL1_STACK_BOTTOM..EL1_STACK_TOP)
-            .rev()
-            .step_by(LEVEL2_BLOCK_SIZE)
-        {
-            let _ = alloc_block_l2(
-                addr,
-                core::ptr::addr_of_mut!(TRANSLATIONTABLE_TTBR1),
-                WRITABLE | NORMAL_MEM,
-            );
-        }
-    }
-}

src/configuration/memory_mapping.rs

@@ -0,0 +1,113 @@
+use crate::{
+    aarch64::mmu::{
+        alloc_block_l2_explicit, allocate_memory, map_l2_block, map_page,
+        physical_mapping::reserve_page, reserve_range, PhysAddr, PhysSource, VirtAddr, DEVICE_MEM,
+        EL0_ACCESSIBLE, GRANULARITY, KERNEL_VIRTUAL_MEM_SPACE, LEVEL1_BLOCK_SIZE,
+        LEVEL2_BLOCK_SIZE, NORMAL_MEM, PXN, READ_ONLY, STACK_START_ADDR, TRANSLATIONTABLE_TTBR0,
+        UXN, WRITABLE,
+    },
+    PERIPHERAL_BASE,
+};
+
+#[no_mangle]
+static EL1_STACK_TOP: usize = STACK_START_ADDR | KERNEL_VIRTUAL_MEM_SPACE;
+const EL1_STACK_SIZE: usize = LEVEL2_BLOCK_SIZE * 2;
+#[no_mangle]
+pub static EL0_STACK_TOP: usize = STACK_START_ADDR;
+pub const EL0_STACK_SIZE: usize = LEVEL2_BLOCK_SIZE * 2;
+
+pub const MAILBOX_VIRTUAL_ADDRESS: VirtAddr = 0xFFFF_FF81_FFFF_E000;
+pub static mut MAILBOX_PHYSICAL_ADDRESS: Option<PhysAddr> = None;
+
+extern "C" {
+    static __text_end: u64;
+    static __share_end: u64;
+    static __kernel_end: u64;
+}
+
+pub fn initialize_mmu_translation_tables() {
+    let text_end = unsafe { &__text_end } as *const _ as usize;
+    let shared_segment_end = unsafe { &__share_end } as *const _ as usize;
+    let kernel_end = unsafe { &__kernel_end } as *const _ as usize;
+
+    reserve_range(0x0, kernel_end).unwrap();
+
+    for addr in (0..text_end).step_by(GRANULARITY) {
+        map_page(
+            addr,
+            addr,
+            core::ptr::addr_of_mut!(TRANSLATIONTABLE_TTBR0),
+            EL0_ACCESSIBLE | READ_ONLY | NORMAL_MEM,
+        )
+        .unwrap();
+    }
+
+    for addr in (text_end..shared_segment_end).step_by(GRANULARITY) {
+        map_page(
+            addr,
+            addr,
+            core::ptr::addr_of_mut!(TRANSLATIONTABLE_TTBR0),
+            EL0_ACCESSIBLE | WRITABLE | NORMAL_MEM,
+        )
+        .unwrap();
+    }
+
+    for addr in (shared_segment_end..kernel_end).step_by(LEVEL2_BLOCK_SIZE) {
+        map_l2_block(
+            addr,
+            addr,
+            core::ptr::addr_of_mut!(TRANSLATIONTABLE_TTBR0),
+            WRITABLE | UXN | NORMAL_MEM,
+        )
+        .unwrap();
+    }
+
+    for addr in (PERIPHERAL_BASE..LEVEL1_BLOCK_SIZE).step_by(LEVEL2_BLOCK_SIZE) {
+        alloc_block_l2_explicit(
+            addr,
+            addr,
+            core::ptr::addr_of_mut!(TRANSLATIONTABLE_TTBR0),
+            EL0_ACCESSIBLE | WRITABLE | UXN | PXN | DEVICE_MEM,
+        )
+        .unwrap();
+    }
+
+    // Frame Buffer memory range
+    allocate_memory(
+        0x3c100000,
+        1080 * 1920 * 4,
+        PhysSource::Explicit(0x3c100000),
+        NORMAL_MEM | PXN | UXN | WRITABLE | EL0_ACCESSIBLE,
+    )
+    .unwrap();
+
+    // Allocate EL1 stack
+    allocate_memory(
+        EL1_STACK_TOP - EL1_STACK_SIZE + 0x10,
+        EL1_STACK_SIZE,
+        PhysSource::Any,
+        WRITABLE | NORMAL_MEM,
+    )
+    .unwrap();
+
+    // Allocate EL0 stack
+    allocate_memory(
+        EL0_STACK_TOP - EL0_STACK_SIZE + 0x10,
+        EL0_STACK_SIZE,
+        PhysSource::Any,
+        WRITABLE | EL0_ACCESSIBLE | NORMAL_MEM,
+    )
+    .unwrap();
+
+    {
+        let addr = reserve_page();
+        unsafe { MAILBOX_PHYSICAL_ADDRESS = Some(addr) };
+        allocate_memory(
+            MAILBOX_VIRTUAL_ADDRESS,
+            GRANULARITY,
+            PhysSource::Explicit(addr),
+            WRITABLE | NORMAL_MEM,
+        )
+        .unwrap();
+    }
+}

src/interrupt_handlers.rs

@@ -1,18 +1,8 @@
 use core::arch::asm;
 
-use alloc::vec::Vec;
-
 use crate::{
-    aarch64::registers::{
+    aarch64::registers::{daif::mask_all, read_esr_el1, read_exception_source_el},
-        daif::{mask_all, unmask_irq},
+    get_current_el, println,
-        read_elr_el1, read_esr_el1, read_exception_source_el,
-    },
-    get_current_el,
-    peripherals::{
-        gpio::{read_gpio_event_detect_status, reset_gpio_event_detect_status},
-        uart::clear_uart_interrupt_state,
-    },
-    println, read_address, write_address,
 };
 
 const INTERRUPT_BASE: u32 = 0x3F00_B000;
@@ -22,30 +12,29 @@ const DISABLE_IRQ_BASE: u32 = INTERRUPT_BASE + 0x21C;
 
 const GPIO_PENDING_BIT_OFFSET: u64 = 0b1111 << 49;
 
-struct InterruptHandlers {
+#[repr(C)]
-    source: IRQSource,
+pub struct TrapFrame {
-    function: fn(),
+    pub x0: u64,
-}
+    pub x1: u64,
-
+    pub x2: u64,
-// TODO: replace with hashmap and check for better alternatives for option
+    pub x3: u64,
-static mut INTERRUPT_HANDLERS: Option<Vec<InterruptHandlers>> = None;
+    pub x4: u64,
-
+    pub x5: u64,
-#[derive(Clone)]
+    pub x6: u64,
-#[repr(u32)]
+    pub x7: u64,
-pub enum IRQSource {
+    pub x8: u64,
-    AuxInt = 29,
+    pub x9: u64,
-    I2cSpiSlvInt = 44,
+    pub x10: u64,
-    Pwa0 = 45,
+    pub x11: u64,
-    Pwa1 = 46,
+    pub x12: u64,
-    Smi = 48,
+    pub x13: u64,
-    GpioInt0 = 49,
+    pub x14: u64,
-    GpioInt1 = 50,
+    pub x15: u64,
-    GpioInt2 = 51,
+    pub x16: u64,
-    GpioInt3 = 52,
+    pub x17: u64,
-    I2cInt = 53,
+    pub x18: u64,
-    SpiInt = 54,
+    pub x29: u64,
-    PcmInt = 55,
+    pub x30: u64,
-    UartInt = 57,
 }
 
 /// Representation of the ESR_ELx registers
@@ -69,28 +58,8 @@ impl From<u32> for EsrElX {
     }
 }
 
-#[no_mangle]
+pub mod irq;
-unsafe extern "C" fn rust_irq_handler() {
+pub mod synchronous;
-    mask_all();
-    let pending_irqs = get_irq_pending_sources();
-
-    if pending_irqs & GPIO_PENDING_BIT_OFFSET != 0 {
-        handle_gpio_interrupt();
-        let source_el = read_exception_source_el() >> 2;
-        println!("Source EL: {}", source_el);
-        println!("Current EL: {}", get_current_el());
-        println!("Return register address: {:#x}", read_esr_el1());
-    }
-
-    if let Some(handler_vec) = unsafe { &*core::ptr::addr_of_mut!(INTERRUPT_HANDLERS) } {
-        for handler in handler_vec {
-            if (pending_irqs & (1 << (handler.source.clone() as u32))) != 0 {
-                (handler.function)();
-                clear_interrupt_for_source(handler.source.clone());
-            }
-        }
-    }
-}
 
 #[no_mangle]
 unsafe extern "C" fn rust_synchronous_interrupt_no_el_change() {
@@ -105,124 +74,9 @@ unsafe extern "C" fn rust_synchronous_interrupt_no_el_change() {
     println!("-------------------------------------");
 }
 
-/// Synchronous Exception Handler
-///
-/// Lower Exception level, where the implemented level
-/// immediately lower than the target level is using
-/// AArch64.
-#[no_mangle]
-unsafe extern "C" fn rust_synchronous_interrupt_imm_lower_aarch64() {
-    mask_all();
-
-    let source_el = read_exception_source_el() >> 2;
-    println!("--------Sync Exception in EL{}--------", source_el);
-    println!("Exception escalated to EL {}", get_current_el());
-    println!("Current EL: {}", get_current_el());
-    let esr: EsrElX = EsrElX::from(read_esr_el1());
-    println!("{:?}", esr);
-    println!("Return address: {:#x}", read_elr_el1());
-
-    match esr.ec {
-        0b100100 => {
-            println!("Cause: Data Abort from a lower Exception level");
-        }
-        _ => {
-            println!("Unknown Error Code: {:b}", esr.ec);
-        }
-    }
-    println!("-------------------------------------");
-
-    set_return_to_kernel_main();
-}
-
-fn clear_interrupt_for_source(source: IRQSource) {
-    match source {
-        IRQSource::UartInt => clear_uart_interrupt_state(),
-        _ => {
-            todo!()
-        }
-    }
-}
-
 fn set_return_to_kernel_main() {
     unsafe {
         asm!("ldr x0, =kernel_main", "msr ELR_EL1, x0");
         asm!("mov x0, #(0b0101)", "msr SPSR_EL1, x0");
     }
 }
-
-fn handle_gpio_interrupt() {
-    println!("Interrupt");
-    for i in 0..=53u32 {
-        let val = read_gpio_event_detect_status(i);
-
-        if val {
-            #[allow(clippy::single_match)]
-            match i {
-                26 => {
-                    println!("Button Pressed");
-                }
-                _ => {}
-            }
-            // Reset GPIO Interrupt handler by writing a 1
-            reset_gpio_event_detect_status(i);
-        }
-    }
-    unmask_irq();
-}
-
-/// Enables IRQ Source
-pub fn enable_irq_source(state: IRQSource) {
-    let nr = state as u32;
-    let register = ENABLE_IRQ_BASE + 4 * (nr / 32);
-    let register_offset = nr % 32;
-    let current = unsafe { read_address(register) };
-    let mask = 0b1 << register_offset;
-    let new_val = current | mask;
-    unsafe { write_address(register, new_val) };
-}
-
-/// Disable IRQ Source
-pub fn disable_irq_source(state: IRQSource) {
-    let nr = state as u32;
-    let register = DISABLE_IRQ_BASE + 4 * (nr / 32);
-    let register_offset = nr % 32;
-    let current = unsafe { read_address(register) };
-    let mask = 0b1 << register_offset;
-    let new_val = current | mask;
-    unsafe { write_address(register, new_val) };
-}
-
-/// Read current IRQ Source status
-pub fn read_irq_source_status(state: IRQSource) -> u32 {
-    let nr = state as u32;
-    let register = ENABLE_IRQ_BASE + 4 * (nr / 32);
-    let register_offset = nr % 32;
-    (unsafe { read_address(register) } >> register_offset) & 0b1
-}
-
-/// Status if a IRQ Source is pending
-pub fn is_irq_source_pending(state: IRQSource) -> bool {
-    let nr = state as u32;
-    let register = IRQ_PENDING_BASE + 4 * (nr / 32);
-    let register_offset = nr % 32;
-    ((unsafe { read_address(register) } >> register_offset) & 0b1) != 0
-}
-
-/// Status if a IRQ Source is pending
-pub fn get_irq_pending_sources() -> u64 {
-    let mut pending = unsafe { read_address(IRQ_PENDING_BASE + 4) as u64 } << 32;
-    pending |= unsafe { read_address(IRQ_PENDING_BASE) as u64 };
-    pending
-}
-
-#[inline(always)]
-pub fn initialize_interrupt_handler() {
-    unsafe { INTERRUPT_HANDLERS = Some(Vec::new()) };
-}
-
-pub fn register_interrupt_handler(source: IRQSource, function: fn()) {
-    if let Some(handler_vec) = unsafe { &mut *core::ptr::addr_of_mut!(INTERRUPT_HANDLERS) } {
-        handler_vec.push(InterruptHandlers { source, function });
-    }
-}

src/interrupt_handlers/irq.rs

@@ -0,0 +1,150 @@
+use crate::aarch64::registers::read_esr_el1;
+use crate::{
+    aarch64::registers::{
+        daif::{mask_all, unmask_irq},
+        read_exception_source_el,
+    },
+    get_current_el,
+    interrupt_handlers::{
+        DISABLE_IRQ_BASE, ENABLE_IRQ_BASE, GPIO_PENDING_BIT_OFFSET, IRQ_PENDING_BASE,
+    },
+    peripherals::{
+        gpio::{read_gpio_event_detect_status, reset_gpio_event_detect_status},
+        uart::clear_uart_interrupt_state,
+    },
+    println, read_address, write_address,
+};
+use alloc::vec::Vec;
+struct InterruptHandlers {
+    source: IRQSource,
+    function: fn(),
+}
+
+// TODO: replace with hashmap and check for better alternatives for option
+static mut INTERRUPT_HANDLERS: Option<Vec<InterruptHandlers>> = None;
+
+#[derive(Clone)]
+#[repr(u32)]
+pub enum IRQSource {
+    AuxInt = 29,
+    I2cSpiSlvInt = 44,
+    Pwa0 = 45,
+    Pwa1 = 46,
+    Smi = 48,
+    GpioInt0 = 49,
+    GpioInt1 = 50,
+    GpioInt2 = 51,
+    GpioInt3 = 52,
+    I2cInt = 53,
+    SpiInt = 54,
+    PcmInt = 55,
+    UartInt = 57,
+}
+
+#[inline(always)]
+pub fn initialize_interrupt_handler() {
+    unsafe { INTERRUPT_HANDLERS = Some(Vec::new()) };
+}
+
+pub fn register_interrupt_handler(source: IRQSource, function: fn()) {
+    if let Some(handler_vec) = unsafe { &mut *core::ptr::addr_of_mut!(INTERRUPT_HANDLERS) } {
+        handler_vec.push(InterruptHandlers { source, function });
+    }
+}
+
+#[no_mangle]
+unsafe extern "C" fn rust_irq_handler() {
+    mask_all();
+    let pending_irqs = get_irq_pending_sources();
+
+    if pending_irqs & GPIO_PENDING_BIT_OFFSET != 0 {
+        handle_gpio_interrupt();
+        let source_el = read_exception_source_el() >> 2;
+        println!("Source EL: {}", source_el);
+        println!("Current EL: {}", get_current_el());
+        println!("Return register address: {:#x}", read_esr_el1());
+    }
+
+    if let Some(handler_vec) = unsafe { &*core::ptr::addr_of_mut!(INTERRUPT_HANDLERS) } {
+        for handler in handler_vec {
+            if (pending_irqs & (1 << (handler.source.clone() as u32))) != 0 {
+                (handler.function)();
+                clear_interrupt_for_source(handler.source.clone());
+            }
+        }
+    }
+}
+
+fn handle_gpio_interrupt() {
+    println!("Interrupt");
+    for i in 0..=53u32 {
+        let val = read_gpio_event_detect_status(i);
+
+        if val {
+            #[allow(clippy::single_match)]
+            match i {
+                26 => {
+                    println!("Button Pressed");
+                }
+                _ => {}
+            }
+            // Reset GPIO Interrupt handler by writing a 1
+            reset_gpio_event_detect_status(i);
+        }
+    }
+    unmask_irq();
+}
+
+/// Enables IRQ Source
+pub fn enable_irq_source(state: IRQSource) {
+    let nr = state as u32;
+    let register = ENABLE_IRQ_BASE + 4 * (nr / 32);
+    let register_offset = nr % 32;
+    let current = unsafe { read_address(register) };
+    let mask = 0b1 << register_offset;
+    let new_val = current | mask;
+    unsafe { write_address(register, new_val) };
+}
+
+/// Disable IRQ Source
+pub fn disable_irq_source(state: IRQSource) {
+    let nr = state as u32;
+    let register = DISABLE_IRQ_BASE + 4 * (nr / 32);
+    let register_offset = nr % 32;
+    let current = unsafe { read_address(register) };
+    let mask = 0b1 << register_offset;
+    let new_val = current | mask;
+    unsafe { write_address(register, new_val) };
+}
+
+/// Read current IRQ Source status
+pub fn read_irq_source_status(state: IRQSource) -> u32 {
+    let nr = state as u32;
+    let register = ENABLE_IRQ_BASE + 4 * (nr / 32);
+    let register_offset = nr % 32;
+    (unsafe { read_address(register) } >> register_offset) & 0b1
+}
+
+/// Status if a IRQ Source is pending
+pub fn is_irq_source_pending(state: IRQSource) -> bool {
+    let nr = state as u32;
+    let register = IRQ_PENDING_BASE + 4 * (nr / 32);
+    let register_offset = nr % 32;
+    ((unsafe { read_address(register) } >> register_offset) & 0b1) != 0
+}
+
+/// Status if a IRQ Source is pending
+pub fn get_irq_pending_sources() -> u64 {
+    let mut pending = unsafe { read_address(IRQ_PENDING_BASE + 4) as u64 } << 32;
+    pending |= unsafe { read_address(IRQ_PENDING_BASE) as u64 };
+    pending
+}
+
+fn clear_interrupt_for_source(source: IRQSource) {
+    match source {
+        IRQSource::UartInt => clear_uart_interrupt_state(),
+        _ => {
+            todo!()
+        }
+    }
+}

src/interrupt_handlers/synchronous.rs

@@ -0,0 +1,56 @@
+use crate::{
+    aarch64::registers::{daif::mask_all, read_elr_el1, read_esr_el1, read_exception_source_el},
+    get_current_el,
+    interrupt_handlers::{set_return_to_kernel_main, EsrElX, TrapFrame},
+    pi3::mailbox,
+    println,
+};
+
+/// Synchronous Exception Handler
+///
+/// Source is a lower Exception level, where the implemented level
+/// immediately lower than the target level is using
+/// AArch64.
+#[no_mangle]
+unsafe extern "C" fn rust_synchronous_interrupt_imm_lower_aarch64(frame: &mut TrapFrame) -> usize {
+    mask_all();
+    let esr: EsrElX = EsrElX::from(read_esr_el1());
+    match esr.ec {
+        0b100100 => {
+            log_sync_exception();
+            println!("Cause: Data Abort from a lower Exception level");
+        }
+        0b010101 => {
+            println!("Cause: SVC instruction execution in AArch64");
+            return handle_svc(frame);
+        }
+        _ => {
+            println!("Unknown Error Code: {:b}", esr.ec);
+        }
+    }
+    println!("Returning to kernel main...");
+
+    set_return_to_kernel_main();
+    0
+}
+
+fn handle_svc(frame: &mut TrapFrame) -> usize {
+    match frame.x8 {
+        67 => {
+            let response = mailbox::read_soc_temp([0]).unwrap();
+            response[1] as usize
+        }
+        _ => 0,
+    }
+}
+
+fn log_sync_exception() {
+    let source_el = read_exception_source_el() >> 2;
+    println!("--------Sync Exception in EL{}--------", source_el);
+    println!("Exception escalated to EL {}", get_current_el());
+    println!("Current EL: {}", get_current_el());
+    let esr: EsrElX = EsrElX::from(read_esr_el1());
+    println!("{:?}", esr);
+    println!("Return address: {:#x}", read_elr_el1());
+    println!("-------------------------------------");
+}

src/lib.rs

@@ -14,10 +14,12 @@ use heap::Heap;
 
 use crate::{
     aarch64::mmu::{
-        allocate_memory, KERNEL_VIRTUAL_MEM_SPACE, LEVEL2_BLOCK_SIZE, NORMAL_MEM, UXN, WRITABLE,
+        allocate_memory, PhysSource, KERNEL_VIRTUAL_MEM_SPACE, LEVEL2_BLOCK_SIZE, NORMAL_MEM, UXN,
+        WRITABLE,
     },
-    interrupt_handlers::initialize_interrupt_handler,
+    interrupt_handlers::irq::initialize_interrupt_handler,
     logger::DefaultLogger,
+    pi3::timer::sleep_s,
 };
 
 static PERIPHERAL_BASE: usize = 0x3F00_0000;
@@ -33,7 +35,7 @@ pub unsafe fn init_kernel_heap() {
     let start = core::ptr::addr_of_mut!(__kernel_end) as usize | KERNEL_VIRTUAL_MEM_SPACE;
     let size = LEVEL2_BLOCK_SIZE * 2;
 
-    allocate_memory(start, size, NORMAL_MEM | UXN | WRITABLE).unwrap();
+    allocate_memory(start, size, PhysSource::Any, NORMAL_MEM | UXN | WRITABLE).unwrap();
     let heap = core::ptr::addr_of_mut!(GLOBAL_ALLOCATOR);
     (*heap).init(start, start + size);
 }
@@ -42,6 +44,7 @@ pub unsafe fn init_kernel_heap() {
 fn panic(_panic: &PanicInfo) -> ! {
     loop {
         println!("Panic: {}", _panic.message());
+        sleep_s(1);
     }
 }
 
@@ -77,8 +80,14 @@ pub fn get_current_el() -> u64 {
     el >> 2
 }
 
+static mut KERNEL_INITIALIZED: bool = false;
+
 pub fn initialize_kernel() {
+    if unsafe { KERNEL_INITIALIZED } {
+        return;
+    }
     unsafe { init_kernel_heap() };
     logger::set_logger(Box::new(DefaultLogger));
     initialize_interrupt_handler();
+    unsafe { KERNEL_INITIALIZED = true };
 }

src/main.rs

@@ -11,14 +11,11 @@ extern crate alloc;
 
 use alloc::vec::Vec;
 use nova::{
-    aarch64::{
+    aarch64::registers::{daif, read_id_aa64mmfr0_el1},
-        mmu::{allocate_memory_explicit, EL0_ACCESSIBLE, NORMAL_MEM, PXN, UXN, WRITABLE},
+    configuration::memory_mapping::initialize_mmu_translation_tables,
-        registers::{daif, read_id_aa64mmfr0_el1},
-    },
-    configuration::mmu::initialize_mmu_translation_tables,
     framebuffer::{FrameBuffer, BLUE, GREEN, RED},
     get_current_el,
-    interrupt_handlers::{enable_irq_source, IRQSource},
+    interrupt_handlers::irq::{enable_irq_source, IRQSource},
     peripherals::{
         gpio::{
             blink_gpio, gpio_pull_up, set_falling_edge_detect, set_gpio_function, GPIOFunction,
@@ -26,13 +23,14 @@ use nova::{
         },
         uart::uart_init,
     },
-    pi3::mailbox,
     println,
 };
 
 global_asm!(include_str!("vector.S"));
 global_asm!(include_str!("config.S"));
 
+static mut FRAMEBUFFER: Option<FrameBuffer> = None;
+
 extern "C" {
     fn el2_to_el1();
     fn el1_to_el0();
@@ -66,14 +64,14 @@ pub extern "C" fn main() -> ! {
     println!("Hello World!");
     println!("Exception level: {}", get_current_el());
 
-    unsafe {
+    initialize_mmu_translation_tables();
-        initialize_mmu_translation_tables();
+    unsafe { configure_mmu_el1() };
-        configure_mmu_el1();
+    println!("MMU initialized...");
-        println!("MMU initialized...");
-    };
 
     println!("Register: AA64MMFR0_EL1: {:064b}", read_id_aa64mmfr0_el1());
     println!("Moving El2->EL1");
+    unsafe { FRAMEBUFFER = Some(FrameBuffer::default()) };
+
     unsafe {
         el2_to_el1();
     }
@@ -100,15 +98,6 @@ pub extern "C" fn kernel_main() -> ! {
     }
     println!("heap allocation test: {:?}", test_vector);
 
-    // Frame Buffer memory range
-    // TODO: this is just temporary
-    allocate_memory_explicit(
-        0x3c100000,
-        1080 * 1920 * 4,
-        0x3c100000,
-        NORMAL_MEM | PXN | UXN | WRITABLE | EL0_ACCESSIBLE,
-    )
-    .unwrap();
     println!("Exception Level: {}", get_current_el());
     daif::unmask_all();
 
@@ -132,22 +121,22 @@ pub extern "C" fn el0() -> ! {
 
     enable_irq_source(IRQSource::UartInt);
 
-    let fb = FrameBuffer::default();
+    if let Some(fb) = unsafe { FRAMEBUFFER.as_mut() } {
+        for i in 0..1080 {
+            fb.draw_pixel(50, i, BLUE);
+        }
+        fb.draw_square(500, 500, 600, 700, RED);
+        fb.draw_square_fill(800, 800, 900, 900, GREEN);
+        fb.draw_square_fill(1000, 800, 1200, 700, BLUE);
+        fb.draw_square_fill(900, 100, 800, 150, RED | BLUE);
+        fb.draw_string("Hello World! :D\nTest next Line", 500, 5, 3, BLUE);
 
-    for i in 0..1080 {
+        fb.draw_function(cos, 0, 101, RED);
-        fb.draw_pixel(50, i, BLUE);
     }
-    fb.draw_square(500, 500, 600, 700, RED);
-    fb.draw_square_fill(800, 800, 900, 900, GREEN);
-    fb.draw_square_fill(1000, 800, 1200, 700, BLUE);
-    fb.draw_square_fill(900, 100, 800, 150, RED | BLUE);
-    fb.draw_string("Hello World! :D\nTest next Line", 500, 5, 3, BLUE);
-
-    fb.draw_function(cos, 0, 101, RED);
 
     loop {
-        let temp = mailbox::read_soc_temp([0]).unwrap();
+        let temp = syscall(67);
-        println!("{} °C", temp[1] / 1000);
+        println!("{} °C", temp / 1000);
 
         blink_gpio(SpecificGpio::OnboardLed as u8, 500);
     }
@@ -163,3 +152,17 @@ fn enable_uart() {
     let _ = set_gpio_function(15, GPIOFunction::Alternative0);
     uart_init();
 }
+
+pub fn syscall(nr: u64) -> u64 {
+    let ret: u64;
+
+    unsafe {
+        asm!(
+            "svc #0",
+            in("x8") nr,
+            lateout("x0") ret,
+        );
+    }
+
+    ret
+}

src/pi3/mailbox.rs

@@ -1,4 +1,9 @@
-use crate::{read_address, write_address};
+use core::slice;
+
+use crate::{
+    aarch64::mmu::GRANULARITY, configuration::memory_mapping::MAILBOX_PHYSICAL_ADDRESS,
+    configuration::memory_mapping::MAILBOX_VIRTUAL_ADDRESS, read_address, write_address,
+};
 use nova_error::NovaError;
 
 const MBOX_BASE: u32 = 0x3F00_0000 + 0xB880;
@@ -31,8 +36,9 @@ macro_rules! mailbox_command {
         pub fn $name(
             request_data: [u32; $request_len / 4],
         ) -> Result<[u32; $response_len / 4], NovaError> {
-            let mut mailbox =
+            let mailbox = unsafe {
-                [0u32; (HEADER_LENGTH + max!($request_len, $response_len) + FOOTER_LENGTH) / 4];
+                slice::from_raw_parts_mut(MAILBOX_VIRTUAL_ADDRESS as *mut u32, GRANULARITY / 4)
+            };
             mailbox[0] = (HEADER_LENGTH + max!($request_len, $response_len) + FOOTER_LENGTH) as u32; // Total length in Bytes
             mailbox[1] = 0; // Request
             mailbox[2] = $tag; // Command Tag
@@ -42,9 +48,9 @@ macro_rules! mailbox_command {
             mailbox[5..(5 + ($request_len / 4))].copy_from_slice(&request_data);
             mailbox[(5 + ($request_len / 4))..].fill(0);
 
-            let addr = core::ptr::addr_of!(mailbox[0]) as u32;
+            //let addr = core::ptr::addr_of!(mailbox[0]) as u32;
 
-            write_mailbox(8, addr);
+            write_mailbox(8, unsafe { MAILBOX_PHYSICAL_ADDRESS.unwrap() } as u32);
 
             let _ = read_mailbox(8);
 

src/vector.S

@@ -1,29 +1,133 @@
-.section .vector_table  , "ax"
+.section .vector_t  , "ax"
 .extern irq_handler
 
 .macro    ventry    label
-.align    11
+.align 7
 b    \label
 .endm
 
 .global vector_table
 vector_table:
+    // Exceptions from current EL using SP_EL0
     ventry .
     ventry .
     ventry .
     ventry .
 
-    ventry synchronous_interrupt_no_el_change  // Synchronous Exception  0x200
+    // Exceptions from the current EL using SP_ELx
-    ventry irq_handler                // IRQ(Interrupt Request) 0x280
+    ventry synchronous_interrupt_no_el_change       // Synchronous Exception  0x200
+    ventry irq_handler                              // IRQ(Interrupt Request) 0x280
+    ventry .                                        // FIQ(Fast Interrupt Request) 0x300
+    ventry .                                        // SError 0x580
+
+    // Exceptions from lower EL AArch64
+    ventry synchronous_interrupt_imm_lower_aarch64  // Synchronous Exception 0x400
+    ventry irq_handler                              // IRQ(Interrupt Request) 0x480
+    ventry .                                        //  FIQ(Fast Interrupt Request) 0x500
+    ventry .                                        // SError 0x580
+
+    // Exceptions from lower EL AArch32
+    ventry .
+    ventry .
     ventry .
     ventry .
 
-    ventry synchronous_interrupt_imm_lower_aarch64
+.align 4
-    ventry irq_handler
+irq_handler:
-    ventry .
+    sub sp, sp, #176
-    ventry .
+    stp x0, x1, [sp, #0]
+    stp x2, x3, [sp, #16]
+    stp x4, x5, [sp, #32]
+    stp x6, x7, [sp, #48]
+    stp x8, x9, [sp, #64]
+    stp x10, x11, [sp, #80]
+    stp x12, x13, [sp, #96]
+    stp x14, x15, [sp, #112]
+    stp x16, x17, [sp, #128]
+    stp x18, x29, [sp, #144]
+    stp x30, xzr, [sp, #160]
 
-    ventry .
+    bl rust_irq_handler
-    ventry .
+
-    ventry .
+    ldp x0, x1, [sp, #0]
-    ventry .
+    ldp x2, x3, [sp, #16]
+    ldp x4, x5, [sp, #32]
+    ldp x6, x7, [sp, #48]
+    ldp x8, x9, [sp, #64]
+    ldp x10, x11, [sp, #80]
+    ldp x12, x13, [sp, #96]
+    ldp x14, x15, [sp, #112]
+    ldp x16, x17, [sp, #128]
+    ldp x18, x29, [sp, #144]
+    ldp x30, xzr, [sp, #160]
+    add sp, sp, #176
+
+    eret
+
+.align 4
+synchronous_interrupt_imm_lower_aarch64:
+    sub sp, sp, #176
+    stp x0, x1, [sp, #0]
+    stp x2, x3, [sp, #16]
+    stp x4, x5, [sp, #32]
+    stp x6, x7, [sp, #48]
+    stp x8, x9, [sp, #64]
+    stp x10, x11, [sp, #80]
+    stp x12, x13, [sp, #96]
+    stp x14, x15, [sp, #112]
+    stp x16, x17, [sp, #128]
+    stp x18, x29, [sp, #144]
+    stp x30, xzr, [sp, #160]
+
+    mov x0, sp
+    bl rust_synchronous_interrupt_imm_lower_aarch64
+    str x0, [sp, #0]
+
+    ldp x0, x1, [sp, #0]
+    ldp x2, x3, [sp, #16]
+    ldp x4, x5, [sp, #32]
+    ldp x6, x7, [sp, #48]
+    ldp x8, x9, [sp, #64]
+    ldp x10, x11, [sp, #80]
+    ldp x12, x13, [sp, #96]
+    ldp x14, x15, [sp, #112]
+    ldp x16, x17, [sp, #128]
+    ldp x18, x29, [sp, #144]
+    ldp x30, xzr, [sp, #160]
+    add sp, sp, #176
+
+    eret
+
+.align 4
+synchronous_interrupt_no_el_change:
+    sub sp, sp, #176
+    stp x0, x1, [sp, #0]
+    stp x2, x3, [sp, #16]
+    stp x4, x5, [sp, #32]
+    stp x6, x7, [sp, #48]
+    stp x8, x9, [sp, #64]
+    stp x10, x11, [sp, #80]
+    stp x12, x13, [sp, #96]
+    stp x14, x15, [sp, #112]
+    stp x16, x17, [sp, #128]
+    stp x18, x29, [sp, #144]
+    stp x30, xzr, [sp, #160]
+
+    mov x0, sp
+    bl rust_synchronous_interrupt_no_el_change
+    str x0, [sp, #0]
+
+    ldp x0, x1, [sp, #0]
+    ldp x2, x3, [sp, #16]
+    ldp x4, x5, [sp, #32]
+    ldp x6, x7, [sp, #48]
+    ldp x8, x9, [sp, #64]
+    ldp x10, x11, [sp, #80]
+    ldp x12, x13, [sp, #96]
+    ldp x14, x15, [sp, #112]
+    ldp x16, x17, [sp, #128]
+    ldp x18, x29, [sp, #144]
+    ldp x30, xzr, [sp, #160]
+    add sp, sp, #176
+
+    eret

workspace/heap/src/lib.rs

@@ -39,6 +39,10 @@ impl Heap {
         }
     }
 
+    pub fn size(self) -> usize {
+        self.raw_size
+    }
+
     pub fn init(&mut self, heap_start: usize, heap_end: usize) {
         self.start_address = heap_start as *mut HeapHeader;
         self.end_address = heap_end as *mut HeapHeader;