wip

link.ld

@@ -4,9 +4,13 @@ SECTIONS {
     .text ALIGN(4) : {
         KEEP(*(.text._start))
         *(.text .text.*)
+    }
+    .vector_table ALIGN(2K) : {
+        KEEP(*(.vector_t))
+    }
+
     . = ALIGN(4K);
     __text_end = .;
-    }
 
     .rodata : {
         *(.rodata .rodata.*)
@@ -26,10 +30,6 @@ SECTIONS {
 
     __share_end = .;
 
-    .vector_table ALIGN(2K) : {
-        KEEP(*(.vector_table))
-    }
-
     # EL2 Stack
     .stack ALIGN(16): {
     __stack_start = .;

src/aarch64/mmu.rs

@@ -47,7 +47,7 @@ pub const KERNEL_VIRTUAL_MEM_SPACE: usize = 0xFFFF_FF80_0000_0000;
 
 pub const STACK_START_ADDR: usize = !KERNEL_VIRTUAL_MEM_SPACE & (!0xF);
 
-mod physical_mapping;
+pub mod physical_mapping;
 
 type VirtAddr = usize;
 type PhysAddr = usize;
@@ -189,7 +189,7 @@ pub 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 {
@@ -229,7 +229,7 @@ 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 };
 
@@ -311,10 +311,11 @@ fn virtual_address_to_table_offset(virtual_addr: usize) -> (usize, usize, usize)
 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)
 }
@@ -322,10 +323,17 @@ 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 {
         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);
@@ -336,22 +344,22 @@ 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(entry_table_addr(table.0[offset]) as *mut PageTable)
         }
         1 => Err(NovaError::Paging),
-        3 => Ok(entry_table_addr(table.0[offset] as usize) as *mut PageTable),
+        3 => Ok(entry_table_addr(table.0[offset]) as *mut PageTable),
         _ => unreachable!(),
     }
 }
 
 /// Extracts the physical address out of an table entry.
 #[inline]
-fn entry_phys(entry: usize) -> PhysAddr {
-    entry & 0x0000_FFFF_FFFF_F000
+fn entry_phys(entry: u64) -> PhysAddr {
+    entry as usize & 0x0000_FFFF_FFFF_F000
 }
 
 #[inline]
-fn entry_table_addr(entry: usize) -> VirtAddr {
+fn entry_table_addr(entry: u64) -> VirtAddr {
     if get_current_el() == 1 {
         phys_table_to_kernel_space(entry_phys(entry))
     } else {
@@ -364,3 +372,16 @@ fn entry_table_addr(entry: usize) -> VirtAddr {
 fn phys_table_to_kernel_space(entry: usize) -> VirtAddr {
     entry | TRANSLATION_TABLE_BASE_ADDR
 }
+
+fn page_address_to_physical_address(mut virtual_address: VirtAddr) -> PhysAddr {
+    let root_table = if virtual_address & KERNEL_VIRTUAL_MEM_SPACE > 0 {
+        &raw mut TRANSLATIONTABLE_TTBR1
+    } else {
+        &raw mut TRANSLATIONTABLE_TTBR0
+    };
+    virtual_address &= !KERNEL_VIRTUAL_MEM_SPACE;
+    let (l1_off, l2_off, l3_off) = virtual_address_to_table_offset(virtual_address);
+    let offsets = [l1_off, l2_off];
+    let table = unsafe { &*navigate_table(root_table, &offsets, false).unwrap() };
+    entry_phys(table.0[l3_off])
+}

src/config.S

@@ -95,100 +95,3 @@ el1_to_el0:
 
     // 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/interrupt_handlers.rs

@@ -3,15 +3,19 @@ use core::arch::asm;
 use alloc::vec::Vec;
 
 use crate::{
-    aarch64::registers::{
+    aarch64::{
+        mmu::{allocate_memory, physical_mapping::reserve_page},
+        registers::{
             daif::{mask_all, unmask_irq},
             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,
     },
+    pi3::mailbox,
     println, read_address, write_address,
 };
 
@@ -22,6 +26,31 @@ const DISABLE_IRQ_BASE: u32 = INTERRUPT_BASE + 0x21C;
 
 const GPIO_PENDING_BIT_OFFSET: u64 = 0b1111 << 49;
 
+#[repr(C)]
+pub struct TrapFrame {
+    pub x0: u64,
+    pub x1: u64,
+    pub x2: u64,
+    pub x3: u64,
+    pub x4: u64,
+    pub x5: u64,
+    pub x6: u64,
+    pub x7: u64,
+    pub x8: u64,
+    pub x9: u64,
+    pub x10: u64,
+    pub x11: u64,
+    pub x12: u64,
+    pub x13: u64,
+    pub x14: u64,
+    pub x15: u64,
+    pub x16: u64,
+    pub x17: u64,
+    pub x18: u64,
+    pub x29: u64,
+    pub x30: u64,
+}
+
 struct InterruptHandlers {
     source: IRQSource,
     function: fn(),
@@ -107,13 +136,43 @@ unsafe extern "C" fn rust_synchronous_interrupt_no_el_change() {
 
 /// Synchronous Exception Handler
 ///
-/// Lower Exception level, where the implemented level
+/// 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() {
+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 => {
+            println!("Cause: Data Abort from a lower Exception level");
+            log_sync_exception();
+        }
+        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();
+    return 0;
+}
+
+fn handle_svc(frame: &mut TrapFrame) -> usize {
+    match frame.x8 {
+        67 => {
+            let response = mailbox::read_soc_temp([0]).unwrap();
+            response[0] 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());
@@ -121,18 +180,7 @@ unsafe extern "C" fn rust_synchronous_interrupt_imm_lower_aarch64() {
     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) {

src/lib.rs

@@ -78,8 +78,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

@@ -4,7 +4,7 @@
 #![allow(clippy::missing_safety_doc)]
 use core::{
     arch::{asm, global_asm},
-    ptr::write_volatile,
+    ptr::{read_volatile, write_volatile},
 };
 
 extern crate alloc;
@@ -140,8 +140,8 @@ pub extern "C" fn el0() -> ! {
         // TODO: Mailbox requires a physical address. The stack is now in VA space causing an issue.
         // Fix with SVCs ?
 
-        // let temp = mailbox::read_soc_temp([0]).unwrap();
-        // println!("{} °C", temp[1] / 1000);
+        let temp = syscall(67);
+        println!("{} °C", temp / 1000);
 
         blink_gpio(SpecificGpio::OnboardLed as u8, 500);
     }
@@ -157,3 +157,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/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 .
 
+    // Exceptions from the current EL using SP_ELx
     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
-    ventry irq_handler
-    ventry .
-    ventry .
+.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]
 
-    ventry .
-    ventry .
-    ventry .
-    ventry .
+    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]
+
+    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;