From a11b0b6e0e4acd0f251343a5c6502a4e7255f286 Mon Sep 17 00:00:00 2001
From: TuxSH <1922548+TuxSH@users.noreply.github.com>
Date: Thu, 1 Aug 2019 00:46:16 +0200
Subject: [PATCH] thermosphere: fix x18 init, etc.

---
 thermosphere/Makefile       |  2 +-
 thermosphere/src/core_ctx.c | 14 ++++++++++++++
 thermosphere/src/core_ctx.h |  4 +++-
 thermosphere/src/main.c     | 10 +++++-----
 thermosphere/src/start.s    | 31 ++++++++++++-------------------
 5 files changed, 35 insertions(+), 26 deletions(-)

diff --git a/thermosphere/Makefile b/thermosphere/Makefile
index ca20bad90..b7d655ca2 100644
--- a/thermosphere/Makefile
+++ b/thermosphere/Makefile
@@ -133,7 +133,7 @@ all: $(BUILD)
 
 ifeq ($(PLATFORM), qemu)
 QEMUFLAGS :=	-nographic -machine virt,secure=on,virtualization=on,gic-version=2 -cpu cortex-a57 -smp 4 -m 1024\
-			-bios bl1.bin -d unimp,int -semihosting-config enable,target=native -serial mon:stdio
+			-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 acdea5707..bef3e2506 100644
--- a/thermosphere/src/core_ctx.c
+++ b/thermosphere/src/core_ctx.c
@@ -16,9 +16,23 @@
 
 #include "core_ctx.h"
 
+// start.s
+extern uintptr_t g_initialKernelEntrypoint;
+
+// Prevents it from being put in BSS
 CoreCtx g_coreCtxs[4] = {
     { .coreId = 0 },
     { .coreId = 1 },
     { .coreId = 2 },
     { .coreId = 3 },
 };
+
+void coreCtxInit(u32 coreId, bool isColdbootCore, u64 argument)
+{
+    currentCoreCtx = &g_coreCtxs[coreId];
+    currentCoreCtx->isColdbootCore = isColdbootCore;
+    currentCoreCtx->kernelArgument = argument;
+    if (isColdbootCore) {
+        currentCoreCtx->kernelEntrypoint = g_initialKernelEntrypoint;
+    }
+}
diff --git a/thermosphere/src/core_ctx.h b/thermosphere/src/core_ctx.h
index 39e41065c..c705b0f10 100644
--- a/thermosphere/src/core_ctx.h
+++ b/thermosphere/src/core_ctx.h
@@ -21,8 +21,10 @@ typedef struct CoreCtx {
     u64 kernelArgument;
     uintptr_t kernelEntrypoint;
     u32 coreId;                     // @0x10
-    bool isColdBootCore;            // @0x14
+    bool isColdbootCore;            // @0x14
 } CoreCtx;
 
 extern CoreCtx g_coreCtxs[4];
 register CoreCtx *currentCoreCtx asm("x18");
+
+void coreCtxInit(u32 coreId, bool isColdbootCore, u64 argument);
diff --git a/thermosphere/src/main.c b/thermosphere/src/main.c
index d31f0dc9e..081bdcae1 100644
--- a/thermosphere/src/main.c
+++ b/thermosphere/src/main.c
@@ -13,7 +13,7 @@ static void loadKernelViaSemihosting(void)
     uintptr_t buf = (uintptr_t)__start__ + (1<<20);
     long handle = -1, ret;
 
-    DEBUG("Loading kernel via semihosting file I/O... ");
+    DEBUG("Loading kernel via semihosted file I/O... ");
     handle = semihosting_file_open("test_kernel.bin", FOPEN_MODE_RB);
     if (handle < 0) {
         DEBUG("failed to open file (%ld)!\n", handle);
@@ -25,7 +25,7 @@ static void loadKernelViaSemihosting(void)
         panic();
     }
 
-    DEBUG("OK!");
+    DEBUG("OK!\n");
     semihosting_file_close(handle);
     currentCoreCtx->kernelEntrypoint = buf;
 }
@@ -34,9 +34,9 @@ int main(void)
 {
     enableTraps();
 
-    if (currentCoreCtx->isColdBootCore) {
+    if (currentCoreCtx->isColdbootCore) {
         uartInit(115200);
-        DEBUG("Hello from Thermosphere!\n");
+        DEBUG("EL2: core %u reached main first!\n", currentCoreCtx->coreId);
         if (currentCoreCtx->kernelEntrypoint == 0) {
             if (semihosting_connection_supported()) {
                 loadKernelViaSemihosting();
@@ -48,7 +48,7 @@ int main(void)
         }
     }
     else {
-        DEBUG("Core %u booted\n", currentCoreCtx->coreId);
+        DEBUG("EL2: core %u reached main!\n", currentCoreCtx->coreId);
     }
 
     return 0;
diff --git a/thermosphere/src/start.s b/thermosphere/src/start.s
index 8632ae9c4..c50d24727 100644
--- a/thermosphere/src/start.s
+++ b/thermosphere/src/start.s
@@ -23,7 +23,8 @@ _start:
     b       start
     b       start2
 
-_initialKernelEntrypoint:
+.global g_initialKernelEntrypoint
+g_initialKernelEntrypoint:
     .quad   0
 
 start:
@@ -58,35 +59,27 @@ _startCommon:
     dsb     sy
     isb
 
+    mov     x2, x0
+
     // Get core ID
     // Ensure Aff0 is 4-1 at most (4 cores), and that Aff1, 2 and 3 are 0 (1 cluster only)
-    mrs     x10, mpidr_el1
-    and     x10, x10, #0x00FFFFFF        // Aff0 to 2
-    and     x10, x10, #(0xFF << 32)      // Aff3
-    cmp     x10, #4
+    mrs     x0, mpidr_el1
+    tst     x0, #(0xFF << 32)
+    bne     .
+    and     x0, x0, #0x00FFFFFF        // Aff0 to 2
+    cmp     x0, #4
     bhs     .
 
     // Set tmp stack (__stacks_top__ is aligned)
     adrp    x8, __stacks_top__
-    lsl     x9, x10, #10
+    lsl     x9, x0, #10
     sub     sp, x8, x9
 
     // Set up x18
-    adrp    x18, g_coreCtxs
-    add     x18, x18, #:lo12:g_coreCtxs
-    add     x18, x18, x10, lsl #3
+    mov     w1, w19
+    bl      coreCtxInit
     stp     x18, xzr, [sp, #-0x10]!
 
-    strb    w19, [x18, #0x14]           // isColdbootCore
-
-    // Store entrypoint if first core
-    cbz     x19, _store_arg
-    ldr     x8, _initialKernelEntrypoint
-    str     x8, [x18, #8]
-
-_store_arg:
-    str     x0, [x18, #0]
-
     // Don't call init array to save space?
     // Clear BSS & call main for the first core executing this code
     cbz     x19, _jump_to_main