diff --git a/thermosphere/Makefile b/thermosphere/Makefile index 568599615..aafcdb4b5 100644 --- a/thermosphere/Makefile +++ b/thermosphere/Makefile @@ -49,13 +49,12 @@ INCLUDES := include ../common/include #--------------------------------------------------------------------------------- # options for code generation #--------------------------------------------------------------------------------- -ARCH := -march=armv8-a -mtune=cortex-a57 -mgeneral-regs-only #<- important +ARCH := -march=armv8-a -mtune=cortex-a57 -mgeneral-regs-only -ffixed-x18 #<- important DEFINES := -D__CCPLEX__ -DATMOSPHERE_GIT_BRANCH=\"$(AMSBRANCH)\" -DATMOSPHERE_GIT_REV=\"$(AMSREV)\"\ -DATMOSPHERE_RELEASE_VERSION_HASH="0x$(AMSHASH)" $(PLATFORM_DEFINES) CFLAGS := \ -g \ -Os \ - -ffixed-x18 \ -ffunction-sections \ -fdata-sections \ -fomit-frame-pointer \ @@ -133,7 +132,7 @@ export LIBPATHS := $(foreach dir,$(LIBDIRS),-L$(dir)/lib) all: $(BUILD) ifeq ($(PLATFORM), qemu) -QEMUFLAGS := -nographic -machine virt,secure=on,virtualization=on -cpu cortex-a57 -smp 2 -m 1024\ +QEMUFLAGS := -nographic -machine virt,secure=on,virtualization=on,gic-version=2 -cpu cortex-a57 -smp 2 -m 1024\ -bios bl1.bin -d unimp -semihosting-config enable,target=native -serial mon:stdio # NOTE: copy bl1.bin, bl2.bin, bl31.bin from your own build of Arm Trusted Firmware! diff --git a/thermosphere/src/core_ctx.c b/thermosphere/src/core_ctx.c index 42677aa91..acdea5707 100644 --- a/thermosphere/src/core_ctx.c +++ b/thermosphere/src/core_ctx.c @@ -16,7 +16,7 @@ #include "core_ctx.h" -CoreCtx g_coreCtxInstances[4] = { +CoreCtx g_coreCtxs[4] = { { .coreId = 0 }, { .coreId = 1 }, { .coreId = 2 }, diff --git a/thermosphere/src/core_ctx.h b/thermosphere/src/core_ctx.h index 1422788c7..0143eca57 100644 --- a/thermosphere/src/core_ctx.h +++ b/thermosphere/src/core_ctx.h @@ -18,8 +18,10 @@ #include "utils.h" typedef struct CoreCtx { - u32 coreId; + u64 kernelArgument; + u64 kernelEntrypoint; + u32 coreId; // @ 0x0C } CoreCtx; -extern CoreCtx g_coreCtxInstances[4]; -register CoreCtx *currentCoreCtx asm("x18"); \ No newline at end of file +extern CoreCtx g_coreCtxs[4]; +register CoreCtx *currentCoreCtx asm("x18"); diff --git a/thermosphere/src/exception_vectors.s b/thermosphere/src/exception_vectors.s index ce7c38c74..767dadbd7 100644 --- a/thermosphere/src/exception_vectors.s +++ b/thermosphere/src/exception_vectors.s @@ -56,17 +56,17 @@ .endm .macro pivot_stack_for_crash - // Ditch sp_el0 & elr_el1 - // We don't use E2H so that's fine. - msr elr_el1, x0 - mov x0, sp - msr sp_el0, x0 // save stack pointer for the crash - bic x0, x0, #0xFF - bic x0, x0, #0x300 - add x0, x0, #0x1000 - add x0, x0, #0x400 - mov sp, x0 - mrs x0, elr_el1 + // Note: reset x18 assumed uncorrupted + // Note: replace sp_el0 with crashing sp + mrs x18, esr_el2 + mov x18, sp + msr sp_el0, x18 + bic x18, x18, #0xFF + bic x18, x18, #0x300 + add x18, x18, #0x400 + mov sp, x18 + ldp x18, xzr, [sp, #-0x10] + add sp, sp, #0x1000 .endm /* Actual Vectors for Thermosphere. */ @@ -123,6 +123,9 @@ vector_entry irq_sp0 stp x23, xzr, [sp, #0x110] mov x30, x29 + + // Reload our x18 value (currentCoreCtx) + ldp x18, xzr, [sp, #0x120] ret vector_entry fiq_sp0 diff --git a/thermosphere/src/exceptions.c b/thermosphere/src/exceptions.c index 2ed26798f..6cd356018 100644 --- a/thermosphere/src/exceptions.c +++ b/thermosphere/src/exceptions.c @@ -17,6 +17,7 @@ #include "hvc.h" #include "traps.h" #include "sysreg_traps.h" +#include "core_ctx.h" #include "log.h" @@ -121,11 +122,11 @@ void handleLowerElSyncException(ExceptionStackFrame *frame, ExceptionSyndromeReg void handleSameElSyncException(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr) { - serialLog("Same EL sync exception, EC = 0x%02llx IL=%llu ISS=0x%06llx\n", (u64)esr.ec, esr.il, esr.iss); + serialLog("Same EL sync exception on core %x, EC = 0x%02llx IL=%llu ISS=0x%06llx\n", currentCoreCtx->coreId, (u64)esr.ec, esr.il, esr.iss); dumpStackFrame(frame, true); } void handleUnknownException(u32 offset) { - serialLog("Unknown exception! (offset 0x%03lx)\n", offset); + serialLog("Unknown exception on core %x! (offset 0x%03lx)\n", offset, currentCoreCtx->coreId); } diff --git a/thermosphere/src/start.s b/thermosphere/src/start.s index 1b0e11da5..1fe41e3e5 100644 --- a/thermosphere/src/start.s +++ b/thermosphere/src/start.s @@ -25,53 +25,73 @@ _start: b start - nop + b start2 -.global g_kernelEntrypoint -g_kernelEntrypoint: +_initialKernelEntrypoint: .quad 0 start: + mov x19, #1 + b _startCommon +start2: + mov x19, #0 +_startCommon: // Disable interrupts, select sp_el2 msr daifset, 0b1111 msr spsel, #1 - // Save arg, load entrypoint & spsr - mov x19, x0 - ldr x8, g_kernelEntrypoint - msr elr_el2, x8 - mov x8, #(0b1111 << 6 | 0b0101) // EL1h+DAIF - msr spsr_el2, x8 + mrs x20, sctlr_el2 + // Get core ID + mrs x20, mpidr_el1 + and x20, x20, #0xFF - // Make sure the regs have been set - dsb sy - isb + // Set tmp stack + ldr x8, =__stacks_top__ + + /* lsl x9, x20, #10 + sub x8, x8, x9*/ + mov sp, x8 + + // Set up x18 + adrp x18, g_coreCtxs + add x18, x18, #:lo12:g_coreCtxs + add x18, x18, x20, lsl #3 + stp x18, xzr, [sp, #-0x10]! + + // Store entrypoint if first core + cbz x19, _store_arg + ldr x8, _initialKernelEntrypoint + str x8, [x18, #8] + +_store_arg: + str x0, [x18, #0] // Set VBAR ldr x8, =__vectors_start__ msr vbar_el2, x8 - // Set tmp stack - ldr x8, =__stacks_top__ - mov sp, x8 - // Make sure the regs have been set dsb sy isb // Don't call init array to save space? - // Clear BSS + // Clear BSS & call main for the first core executing this code + cbz x20, _jump_to_kernel ldr x0, =__bss_start__ mov w1, #0 ldr x2, =__end__ sub x2, x2, x0 bl memset - // TODO bl main +_jump_to_kernel: // Jump to kernel - mov x0, x19 + mov x8, #(0b1111 << 6 | 0b0101) // EL1h+DAIF + msr spsr_el2, x8 + + ldp x0, x1, [x18] + msr elr_el2, x1 dsb sy isb eret