thermosphere: use x18 but qemu shits the bed

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!

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 },

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");
+extern CoreCtx g_coreCtxs[4];
+register CoreCtx *currentCoreCtx asm("x18");

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

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);
 }

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