diff --git a/thermosphere/Makefile b/thermosphere/Makefile index 07cb2db81..0345a446f 100644 --- a/thermosphere/Makefile +++ b/thermosphere/Makefile @@ -70,6 +70,7 @@ CFLAGS := \ -std=gnu11 \ -Werror \ -Wall \ + -Wno-main \ $(ARCH) $(DEFINES) CFLAGS += $(INCLUDE) @@ -140,7 +141,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,mmu -semihosting-config enable,target=native -serial mon:stdio + -bios bl1.bin -d unimp,int -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.h b/thermosphere/src/core_ctx.h index 4f41430c0..2714ec7cf 100644 --- a/thermosphere/src/core_ctx.h +++ b/thermosphere/src/core_ctx.h @@ -23,7 +23,8 @@ typedef struct CoreCtx { u8 *crashStack; // @0x10 u64 scratch; // @0x18 u32 coreId; // @0x20 - bool isBootCore; // @0x24 + bool isBootCore; // @0x24 + bool wasSingleStepping; // @0x25 (for pIRQ handler) } CoreCtx; extern CoreCtx g_coreCtxs[4]; diff --git a/thermosphere/src/exception_vectors.s b/thermosphere/src/exception_vectors.s index 6d9059d55..c09ac4b96 100644 --- a/thermosphere/src/exception_vectors.s +++ b/thermosphere/src/exception_vectors.s @@ -135,6 +135,7 @@ vector_entry fiq_sp0 .endfunc .cfi_endproc /* To save space, insert in an unused vector segment. */ + .global _restore_all_regs _restore_all_regs: ldp x30, x20, [sp, #0xF0] ldp x21, x22, [sp, #0x100] @@ -161,7 +162,7 @@ vector_entry fiq_sp0 ldp x26, x27, [sp, #0xD0] ldp x28, x29, [sp, #0xE0] - add sp, sp, #0x210 + add sp, sp, #0x120 eret vector_entry serror_sp0 diff --git a/thermosphere/src/exceptions.c b/thermosphere/src/exceptions.c index 4f39c5716..7bc209762 100644 --- a/thermosphere/src/exceptions.c +++ b/thermosphere/src/exceptions.c @@ -19,6 +19,7 @@ #include "sysreg_traps.h" #include "smc.h" #include "core_ctx.h" +#include "single_step.h" #include "debug_log.h" @@ -110,6 +111,10 @@ void handleLowerElSyncException(ExceptionStackFrame *frame, ExceptionSyndromeReg handleMsrMrsTrap(frame, esr); break; + case Exception_SoftwareStepLowerEl: + handleSingleStep(frame, esr); + break; + default: DEBUG("Lower EL sync exception, EC = 0x%02llx IL=%llu ISS=0x%06llx\n", (u64)esr.ec, esr.il, esr.iss); dumpStackFrame(frame, false); diff --git a/thermosphere/src/main.c b/thermosphere/src/main.c index 0eb878ad7..e1e19c42c 100644 --- a/thermosphere/src/main.c +++ b/thermosphere/src/main.c @@ -1,3 +1,5 @@ +#include + #include "utils.h" #include "core_ctx.h" #include "debug_log.h" @@ -5,6 +7,8 @@ #include "semihosting.h" #include "traps.h" #include "sysreg.h" +#include "exceptions.h" +#include "single_step.h" extern const u8 __start__[]; @@ -31,9 +35,10 @@ static void loadKernelViaSemihosting(void) currentCoreCtx->kernelEntrypoint = buf; } -int main(void) +void main(ExceptionStackFrame *frame) { enableTraps(); + enableSingleStepExceptions(); if (currentCoreCtx->isBootCore) { uartInit(115200); @@ -50,8 +55,14 @@ int main(void) } else { DEBUG("EL2: core %u reached main!\n", currentCoreCtx->coreId); - DEBUG("Test 0x%08llx %016llx\n", get_physical_address_el1_stage12(0x08010000ull), GET_SYSREG(par_el1)); + //DEBUG("Test 0x%08llx %016llx\n", get_physical_address_el1_stage12(0x08010000ull), GET_SYSREG(par_el1)); } - return 0; + // Set up exception frame: init regs to 0, set spsr, elr, etc. + memset(frame, 0, sizeof(ExceptionStackFrame)); + frame->spsr_el2 = (0xF << 6) | (1 << 2) | 1; // EL1h+DAIF + frame->elr_el2 = currentCoreCtx->kernelEntrypoint; + frame->x[0] = currentCoreCtx->kernelArgument; + + //setSingleStep(frame, false); } diff --git a/thermosphere/src/single_step.c b/thermosphere/src/single_step.c new file mode 100644 index 000000000..a11e3363f --- /dev/null +++ b/thermosphere/src/single_step.c @@ -0,0 +1,48 @@ +/* + * Copyright (c) 2019 Atmosphère-NX + * + * This program is free software; you can redistribute it and/or modify it + * under the terms and conditions of the GNU General Public License, + * version 2, as published by the Free Software Foundation. + * + * This program is distributed in the hope it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "single_step.h" +#include "core_ctx.h" +#include "sysreg.h" +#include "debug_log.h" + +void enableSingleStepExceptions(void) +{ + u64 mdscr = GET_SYSREG(mdscr_el1); + + // Enable Single Step functionality + mdscr |= BIT(0); + + SET_SYSREG(mdscr_el1, mdscr); +} + +void setSingleStep(ExceptionStackFrame *frame, bool singleStep) +{ + // Set or clear SPSR.SS + if (singleStep) { + frame->spsr_el2 |= BITL(22); + } else { + frame->spsr_el2 &= ~BITL(22); + } + + currentCoreCtx->wasSingleStepping = singleStep; +} + +void handleSingleStep(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr) +{ + DEBUG("Single-step exeception ELR = 0x%016llx, ISV = %u, EX = %u\n", frame->elr_el2, (esr.iss >> 24) & 1, (esr.iss >> 6) & 1); + setSingleStep(frame, true); // hehe boi +} diff --git a/thermosphere/src/single_step.h b/thermosphere/src/single_step.h new file mode 100644 index 000000000..8b4f31682 --- /dev/null +++ b/thermosphere/src/single_step.h @@ -0,0 +1,26 @@ +/* + * Copyright (c) 2019 Atmosphère-NX + * + * This program is free software; you can redistribute it and/or modify it + * under the terms and conditions of the GNU General Public License, + * version 2, as published by the Free Software Foundation. + * + * This program is distributed in the hope it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include "utils.h" +#include "exceptions.h" + +void enableSingleStepExceptions(void); + +void setSingleStep(ExceptionStackFrame *frame, bool singleStep); + +void handleSingleStep(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr); diff --git a/thermosphere/src/start.s b/thermosphere/src/start.s index 112b7227e..1f34ab6e2 100644 --- a/thermosphere/src/start.s +++ b/thermosphere/src/start.s @@ -44,7 +44,7 @@ _startCommon: add x8, x8, #:lo12:__vectors_start__ msr vbar_el2, x8 - // Set system to sane defaults, aarch64 for el1 + // Set system to sane defaults, aarch64 for el1, mmu disabled mov x4, #0x0838 movk x4, #0xC5, lsl #16 orr x1, x4, #0x30000000 @@ -70,7 +70,7 @@ _startCommon: cmp x0, #4 bhs . - // Set tmp stack (__stacks_top__ is aligned) + // Set stack pointer adrp x8, __stacks_top__ lsl x9, x0, #10 sub sp, x8, x9 @@ -80,6 +80,9 @@ _startCommon: bl coreCtxInit stp x18, xzr, [sp, #-0x10]! + // Reserve space for exception frame + sub sp, sp, #0x120 + // Don't call init array to save space? // Clear BSS & call main for the first core executing this code cbz x19, _enable_mmu @@ -101,16 +104,13 @@ _enable_mmu: dsb sy isb + mov x0, sp bl main - // Jump to kernel - mov x8, #(0b1111 << 6 | 0b0101) // EL1h+DAIF - msr spsr_el2, x8 - - ldp x0, x1, [x18] - msr elr_el2, x1 dsb sy isb - eret + + // Jump to kernel + b _restore_all_regs .pool