diff --git a/exosphere/src/coldboot_main.c b/exosphere/src/coldboot_main.c index 57e0edf02..2d5c1ae8c 100644 --- a/exosphere/src/coldboot_main.c +++ b/exosphere/src/coldboot_main.c @@ -3,20 +3,26 @@ #include "mmu.h" #include "memory_map.h" #include "arm.h" +#include "cpu_context.h" extern uint8_t __pk2ldr_start__[], __pk2ldr_end__[]; /* start.s */ -void __jump_to_lower_el(uint64_t arg, uintptr_t ep, unsigned int el); +void __attribute__((noreturn)) __jump_to_lower_el(uint64_t arg, uintptr_t ep, unsigned int el); void coldboot_main(void) { uintptr_t *mmu_l3_table = (uintptr_t *)TZRAM_GET_SEGMENT_ADDRESS(TZRAM_SEGMENT_ID_L3_TRANSLATION_TABLE); uintptr_t pk2ldr = TZRAM_GET_SEGMENT_ADDRESS(TZRAM_SEGMENT_ID_PK2LDR); + uintptr_t ep; + uint64_t arg; /* Clear and unmap pk2ldr (which is reused as exception entry stacks) */ memset((void *)pk2ldr, 0, __pk2ldr_end__ - __pk2ldr_start__); mmu_unmap_range(3, mmu_l3_table, pk2ldr, __pk2ldr_end__ - __pk2ldr_start__); tlb_invalidate_all_inner_shareable(); - /* TODO: stuff & jump to lower EL */ + use_core_entrypoint_and_argument(get_core_id(), &ep, &arg); + + /* Nintendo jumps to EL1, we jump to EL2. Both are supported with all current packages2. */ + __jump_to_lower_el(arg, ep, 2); } diff --git a/exosphere/src/cpu_context.c b/exosphere/src/cpu_context.c index b47aad282..f964b362b 100644 --- a/exosphere/src/cpu_context.c +++ b/exosphere/src/cpu_context.c @@ -21,12 +21,26 @@ static saved_cpu_context_t g_cpu_contexts[NUM_CPU_CORES] = {0}; -void set_core_entrypoint_and_argument(uint32_t core, uint64_t entrypoint_addr, uint64_t argument) { +void use_core_entrypoint_and_argument(uint32_t core, uintptr_t *entrypoint_addr, uint64_t *argument) { + saved_cpu_context_t *ctx = &g_cpu_contexts[core]; + if(ctx->ELR_EL3 == 0 || ctx->is_active) { + panic(0xFA000007); /* invalid context */ + } + + *entrypoint_addr = ctx->ELR_EL3; + *argument = ctx->argument; + + ctx->ELR_EL3 = 0; + ctx->argument = 0; + ctx->is_active = true; +} + +void set_core_entrypoint_and_argument(uint32_t core, uintptr_t entrypoint_addr, uint64_t argument) { g_cpu_contexts[core].ELR_EL3 = entrypoint_addr; g_cpu_contexts[core].argument = argument; } -uint32_t cpu_on(uint32_t core, uint64_t entrypoint_addr, uint64_t argument) { +uint32_t cpu_on(uint32_t core, uintptr_t entrypoint_addr, uint64_t argument) { /* Is core valid? */ if (core >= NUM_CPU_CORES) { return 0xFFFFFFFE; diff --git a/exosphere/src/cpu_context.h b/exosphere/src/cpu_context.h index d9f8c2617..ca506b3e3 100644 --- a/exosphere/src/cpu_context.h +++ b/exosphere/src/cpu_context.h @@ -50,9 +50,10 @@ void restore_current_core_context(void); void set_current_core_active(void); void set_current_core_inactive(void); -void set_core_entrypoint_and_argument(uint32_t core, uint64_t entrypoint_addr, uint64_t argument); +void use_core_entrypoint_and_argument(uint32_t core, uintptr_t *entrypoint_addr, uint64_t *argument); +void set_core_entrypoint_and_argument(uint32_t core, uintptr_t entrypoint_addr, uint64_t argument); -uint32_t cpu_on(uint32_t core, uint64_t entrypoint_addr, uint64_t argument); +uint32_t cpu_on(uint32_t core, uintptr_t entrypoint_addr, uint64_t argument); uint32_t cpu_off(void);