From aed9c392835c32bb1d7d48307308c196d7c72859 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alexander=20Neuh=C3=A4user?= Date: Fri, 20 Mar 2026 19:19:16 +0100 Subject: [PATCH] wip --- link.ld | 12 ++-- src/aarch64/mmu.rs | 41 +++++++++--- src/config.S | 97 ----------------------------- src/interrupt_handlers.rs | 80 +++++++++++++++++++----- src/lib.rs | 6 ++ src/main.rs | 20 +++++- src/vector.S | 128 ++++++++++++++++++++++++++++++++++---- workspace/heap/src/lib.rs | 4 ++ 8 files changed, 244 insertions(+), 144 deletions(-) diff --git a/link.ld b/link.ld index 747c275..187a6b0 100644 --- a/link.ld +++ b/link.ld @@ -4,9 +4,13 @@ SECTIONS { .text ALIGN(4) : { KEEP(*(.text._start)) *(.text .text.*) - . = ALIGN(4K); - __text_end = .; } + .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 = .; diff --git a/src/aarch64/mmu.rs b/src/aarch64/mmu.rs index 9063960..a380131 100644 --- a/src/aarch64/mmu.rs +++ b/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]) +} diff --git a/src/config.S b/src/config.S index d544e6d..619f33c 100644 --- a/src/config.S +++ b/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 diff --git a/src/interrupt_handlers.rs b/src/interrupt_handlers.rs index 0f38215..bcba2af 100644 --- a/src/interrupt_handlers.rs +++ b/src/interrupt_handlers.rs @@ -3,15 +3,19 @@ use core::arch::asm; use alloc::vec::Vec; use crate::{ - aarch64::registers::{ - daif::{mask_all, unmask_irq}, - read_elr_el1, read_esr_el1, read_exception_source_el, + 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) { diff --git a/src/lib.rs b/src/lib.rs index f5358c7..8f7d910 100644 --- a/src/lib.rs +++ b/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 }; } diff --git a/src/main.rs b/src/main.rs index 5a82d0b..8dd7119 100644 --- a/src/main.rs +++ b/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 +} diff --git a/src/vector.S b/src/vector.S index 5732b8f..1bfc9b7 100644 --- a/src/vector.S +++ b/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 - ventry irq_handler // IRQ(Interrupt Request) 0x280 + // 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 diff --git a/workspace/heap/src/lib.rs b/workspace/heap/src/lib.rs index 5c1d83f..7a37914 100644 --- a/workspace/heap/src/lib.rs +++ b/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;