thermopshere: refactor & fix single-stepping code

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
 

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];

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

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

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

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)