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