thermopshere: refactor jump-to-kernel ,add single-step code

not working under qemu yet though

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!
 

thermosphere/src/core_ctx.h

@@ -24,6 +24,7 @@ typedef struct CoreCtx {
     u64 scratch;                    // @0x18
     u32 coreId;                     // @0x20
     bool isBootCore;                // @0x24
+    bool wasSingleStepping;         // @0x25 (for pIRQ handler)
 } CoreCtx;
 
 extern CoreCtx g_coreCtxs[4];

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

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

thermosphere/src/main.c

@@ -1,3 +1,5 @@
+#include <string.h>
+
 #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);
 }

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 <http://www.gnu.org/licenses/>.
+ */
+
+#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
+}

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 <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "utils.h"
+#include "exceptions.h"
+
+void enableSingleStepExceptions(void);
+
+void setSingleStep(ExceptionStackFrame *frame, bool singleStep);
+
+void handleSingleStep(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr);

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