diff options
Diffstat (limited to 'Kernel/Prekernel')
| -rw-r--r-- | Kernel/Prekernel/Arch/aarch64/init.cpp | 47 | ||||
| -rw-r--r-- | Kernel/Prekernel/Arch/aarch64/vector_table.S | 99 |
2 files changed, 139 insertions, 7 deletions
diff --git a/Kernel/Prekernel/Arch/aarch64/init.cpp b/Kernel/Prekernel/Arch/aarch64/init.cpp index e031e57c3c..8c6f6a2add 100644 --- a/Kernel/Prekernel/Arch/aarch64/init.cpp +++ b/Kernel/Prekernel/Arch/aarch64/init.cpp @@ -23,8 +23,17 @@ static u32 query_firmware_version(); extern "C" void wait_cycles(int n); +struct TrapFrame { + u64 x[31]; // Saved general purpose registers + u64 spsr_el1; // Save Processor Status Register, EL1 + u64 elr_el1; // Exception Link Reigster, EL1 + u64 tpidr_el1; // EL0 thread ID + u64 sp_el0; // EL0 stack pointer +}; + extern "C" [[noreturn]] void halt(); extern "C" [[noreturn]] void init(); +extern "C" void exception_common(TrapFrame const* const trap_frame); extern "C" [[noreturn]] void init() { @@ -50,9 +59,6 @@ extern "C" [[noreturn]] void init() extern uintptr_t vector_table_el1; el1_vector_table_install(&vector_table_el1); - // Set the register - asm("msr sctlr_el1, %[value]" ::[value] "r"(system_control_register_el1)); - uart.print_str("Initialize MMU\r\n"); Prekernel::init_prekernel_page_tables(); @@ -91,6 +97,41 @@ void __stack_chk_fail() Prekernel::halt(); } +extern "C" void exception_common(TrapFrame const* const trap_frame) +{ + static constexpr bool print_stack_frame = true; + + if constexpr (print_stack_frame) { + auto& uart = Prekernel::UART::the(); + + uart.print_str("Exception Generated by processor!\n"); + for (auto reg = 0; reg < 31; reg++) { + uart.print_str("x"); + uart.print_num(reg); + uart.print_str(": "); + uart.print_hex(trap_frame->x[reg]); + uart.print_str("\r\n"); + } + + // Special registers + uart.print_str("spsr_el1: "); + uart.print_hex(trap_frame->spsr_el1); + uart.print_str("\r\n"); + + uart.print_str("elr_el1: "); + uart.print_hex(trap_frame->elr_el1); + uart.print_str("\r\n"); + + uart.print_str("tpidr_el1: "); + uart.print_hex(trap_frame->tpidr_el1); + uart.print_str("\r\n"); + + uart.print_str("sp_el0: "); + uart.print_hex(trap_frame->sp_el0); + uart.print_str("\r\n"); + } +} + class QueryFirmwareVersionMboxMessage : Prekernel::Mailbox::Message { public: u32 version; diff --git a/Kernel/Prekernel/Arch/aarch64/vector_table.S b/Kernel/Prekernel/Arch/aarch64/vector_table.S index 92886818d8..826b38ba56 100644 --- a/Kernel/Prekernel/Arch/aarch64/vector_table.S +++ b/Kernel/Prekernel/Arch/aarch64/vector_table.S @@ -6,6 +6,12 @@ .section .text.vector_table +#define TRAP_FRAME_SIZE 272 +#define SPSR_EL1_SLOT (31 * 8) +#define ELR_EL1_SLOT (32 * 8) +#define TPIDR_EL0_SLOT (33 * 8) +#define SP_EL0_SLOT (34 * 8) + // Vector Table Entry macro. Each entry is aligned at 128 bytes, meaning we have // at most that many instructions. .macro table_entry label @@ -19,6 +25,79 @@ b . .endm +.extern exception_common + +// +// Save all register states to the current stack +// and enter the C++ exception handler +// +.macro save_current_context + // Allocate stack space for Trap Frame + sub sp, sp, #TRAP_FRAME_SIZE + + stp x0, x1, [sp, #(0 * 0)] + stp x2, x3, [sp, #(2 * 8)] + stp x4, x5, [sp, #(4 * 8)] + stp x6, x7, [sp, #(6 * 8)] + stp x8, x9, [sp, #(8 * 8)] + stp x10, x11, [sp, #(10 * 8)] + stp x12, x13, [sp, #(12 * 8)] + stp x14, x15, [sp, #(14 * 8)] + stp x16, x17, [sp, #(16 * 8)] + stp x18, x19, [sp, #(18 * 8)] + stp x20, x21, [sp, #(20 * 8)] + stp x22, x23, [sp, #(22 * 8)] + stp x24, x25, [sp, #(24 * 8)] + stp x26, x27, [sp, #(26 * 8)] + stp x28, x29, [sp, #(28 * 8)] + str x30, [sp, #(30 * 8)] + + // Let's save some special registers + mrs x0, spsr_el1 + str x0, [sp, #SPSR_EL1_SLOT] + mrs x0, elr_el1 + str x0, [sp, #ELR_EL1_SLOT] + mrs x0, tpidr_el0 + str x0, [sp, #TPIDR_EL0_SLOT] + mrs x0, sp_el0 + str x0, [sp, #SP_EL0_SLOT] + + // Move stack pointer into first argument register + // and jump to the C++ exception handler + mov x0, sp +.endm + +.macro restore_previous_context + // Restore special registers first + ldr x0, [sp, #SPSR_EL1_SLOT] + msr spsr_el1, x0 + ldr x0, [sp, #ELR_EL1_SLOT] + msr elr_el1, x0 + ldr x0, [sp, #TPIDR_EL0_SLOT] + msr tpidr_el0, x0 + ldr x0, [sp, #SP_EL0_SLOT] + msr sp_el0, x0 + + ldp x0, x1, [sp, #(0 * 0)] + ldp x2, x3, [sp, #(2 * 8)] + ldp x4, x5, [sp, #(4 * 8)] + ldp x6, x7, [sp, #(6 * 8)] + ldp x8, x9, [sp, #(8 * 8)] + ldp x10, x11, [sp, #(10 * 8)] + ldp x12, x13, [sp, #(12 * 8)] + ldp x14, x15, [sp, #(14 * 8)] + ldp x16, x17, [sp, #(16 * 8)] + ldp x18, x19, [sp, #(18 * 8)] + ldp x20, x21, [sp, #(20 * 8)] + ldp x22, x23, [sp, #(22 * 8)] + ldp x24, x25, [sp, #(24 * 8)] + ldp x26, x27, [sp, #(26 * 8)] + ldp x28, x29, [sp, #(28 * 8)] + ldr x30, [sp, #(30 * 8)] + + add sp, sp, #TRAP_FRAME_SIZE +.endm + .global vector_table_el1 .weak vector_table_el1 // Vector table is weak in case someone wants to hook us in C++ land :^) .type vector_table_el1, @object @@ -51,13 +130,25 @@ vector_table_el1: unimplemented_entry synchronous_current_elsp_elx: - b . + save_current_context + bl exception_common + restore_previous_context + eret irq_current_elsp_elx: - b . + save_current_context + bl exception_common + restore_previous_context + eret fiq_current_elsp_elx: - b . + save_current_context + bl exception_common + restore_previous_context + eret system_error_current_elsp_elx: - b . + save_current_context + bl exception_common + restore_previous_context + eret |