From 5081174d2726d4427e2e586f9725c6ade30c4e8f Mon Sep 17 00:00:00 2001 From: TuxSH Date: Mon, 5 Aug 2019 04:46:11 +0200 Subject: [PATCH] thermopshere: refactor & fix single-stepping code --- thermosphere/Makefile | 10 +++++-- thermosphere/src/core_ctx.h | 2 +- thermosphere/src/main.c | 3 +- thermosphere/src/single_step.c | 54 ++++++++++++++++++++++++---------- thermosphere/src/single_step.h | 12 ++++++-- thermosphere/src/sysreg.h | 4 +++ 6 files changed, 61 insertions(+), 24 deletions(-) diff --git a/thermosphere/Makefile b/thermosphere/Makefile index 0345a446f..f89456c7b 100644 --- a/thermosphere/Makefile +++ b/thermosphere/Makefile @@ -140,18 +140,22 @@ export LIBPATHS := $(foreach dir,$(LIBDIRS),-L$(dir)/lib) all: $(BUILD) ifeq ($(PLATFORM), qemu) + +#export QEMU := qemu-system-aarch64 +export QEMU := ~/qemu/aarch64-softmmu/qemu-system-aarch64 + 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! qemu: all @cp thermosphere.bin bl33.bin - @qemu-system-aarch64 $(QEMUFLAGS) + @$(QEMU) $(QEMUFLAGS) qemudbg: all @cp thermosphere.bin bl33.bin - @qemu-system-aarch64 $(QEMUFLAGS) -s -S + @$(QEMU) $(QEMUFLAGS) -s -S endif diff --git a/thermosphere/src/core_ctx.h b/thermosphere/src/core_ctx.h index 2714ec7cf..be3b1575c 100644 --- a/thermosphere/src/core_ctx.h +++ b/thermosphere/src/core_ctx.h @@ -24,7 +24,7 @@ typedef struct CoreCtx { u64 scratch; // @0x18 u32 coreId; // @0x20 bool isBootCore; // @0x24 - bool wasSingleStepping; // @0x25 (for pIRQ handler) + //bool wasSingleStepping; // @0x25 (for pIRQ handler) } CoreCtx; extern CoreCtx g_coreCtxs[4]; diff --git a/thermosphere/src/main.c b/thermosphere/src/main.c index e1e19c42c..d8234dc79 100644 --- a/thermosphere/src/main.c +++ b/thermosphere/src/main.c @@ -38,7 +38,6 @@ static void loadKernelViaSemihosting(void) void main(ExceptionStackFrame *frame) { enableTraps(); - enableSingleStepExceptions(); if (currentCoreCtx->isBootCore) { uartInit(115200); @@ -64,5 +63,5 @@ void main(ExceptionStackFrame *frame) frame->elr_el2 = currentCoreCtx->kernelEntrypoint; frame->x[0] = currentCoreCtx->kernelArgument; - //setSingleStep(frame, false); + singleStepSetNextState(frame, SingleStepState_ActivePending); } diff --git a/thermosphere/src/single_step.c b/thermosphere/src/single_step.c index a11e3363f..09ccd1952 100644 --- a/thermosphere/src/single_step.c +++ b/thermosphere/src/single_step.c @@ -19,30 +19,52 @@ #include "sysreg.h" #include "debug_log.h" -void enableSingleStepExceptions(void) +SingleStepState singleStepGetNextState(ExceptionStackFrame *frame) +{ + u64 mdscr = GET_SYSREG(mdscr_el1); + bool mdscrSS = (mdscr & MDSCR_EL1_SS) != 0; + bool pstateSS = (frame->spsr_el2 & PSTATE_SS) != 0; + + if (!mdscrSS) { + return SingleStepState_Inactive; + } else { + return pstateSS ? SingleStepState_ActivePending : SingleStepState_ActiveNotPending; + } +} + +void singleStepSetNextState(ExceptionStackFrame *frame, SingleStepState state) { u64 mdscr = GET_SYSREG(mdscr_el1); - // Enable Single Step functionality - mdscr |= BIT(0); + switch (state) { + case SingleStepState_Inactive: + // Unset mdscr_el1.ss + mdscr &= ~MDSCR_EL1_SS; + break; + case SingleStepState_ActivePending: + // Set mdscr_el1.ss and pstate.ss + mdscr |= MDSCR_EL1_SS; + frame->spsr_el2 |= PSTATE_SS; + break; + case SingleStepState_ActiveNotPending: + // Set mdscr_el1.ss and unset pstate.ss + mdscr |= MDSCR_EL1_SS; + frame->spsr_el2 |= PSTATE_SS; + break; + default: + break; + } 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) { + // Disable single-step ASAP + singleStepSetNextState(NULL, SingleStepState_Inactive); + 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 + + // Hehe boi + singleStepSetNextState(frame, SingleStepState_ActivePending); } diff --git a/thermosphere/src/single_step.h b/thermosphere/src/single_step.h index 8b4f31682..05d93a857 100644 --- a/thermosphere/src/single_step.h +++ b/thermosphere/src/single_step.h @@ -19,8 +19,16 @@ #include "utils.h" #include "exceptions.h" -void enableSingleStepExceptions(void); +typedef enum SingleStepState { + SingleStepState_Inactive = 0, // Single step disabled OR in the debugger + SingleStepState_ActivePending = 1, // Instruction not yet executed + SingleStepState_ActiveNotPending = 2, // Instruction executed, single-step exception is going to be generated soon +} SingleStepState; -void setSingleStep(ExceptionStackFrame *frame, bool singleStep); +/// Get the single-step state machine state (state after eret) +SingleStepState singleStepGetNextState(ExceptionStackFrame *frame); + +/// Set the single-step state machine state (state after eret). Frame can be NULL iff new state is "inactive" +void singleStepSetNextState(ExceptionStackFrame *frame, SingleStepState state); void handleSingleStep(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr); diff --git a/thermosphere/src/sysreg.h b/thermosphere/src/sysreg.h index ffb7e94ff..be0fa2a24 100644 --- a/thermosphere/src/sysreg.h +++ b/thermosphere/src/sysreg.h @@ -403,6 +403,8 @@ #define MDCR_EL2_TPMCR BITL(5) #define MDCR_EL2_HPMN_MASK 0x1Full +#define MDSCR_EL1_SS BITL(0) + #define ENCODE_SYSREG_FIELDS_MOV(op0, op1, crn, crm, op2) (((op0) << 19) | ((op1) << 16) | ((crn) << 12) | ((crm) << 8) | ((op2) << 5)) #define ENCODE_SYSREG_MOV(name) EVAL(ENCODE_SYSREG_FIELDS_MOV CAT(TUP_, name)) #define MAKE_MSR(name, Rt) (0xD5000000 | ENCODE_SYSREG_MOV(name) | ((Rt) & 0x1F)) @@ -431,3 +433,5 @@ #define SYSREG_OP1_EL3 6 #define SYSREG_OP1_AARCH32_JZL 7 #define SYSREG_OP1_AARCH64_SEL1 7 + +#define PSTATE_SS BITL(21)