thermosphere: add breakpoint/watchpoint enable/reset code

thermosphere/Makefile

@@ -141,11 +141,11 @@ all: $(BUILD)
 
 ifeq ($(PLATFORM), qemu)
 
-#export QEMU := qemu-system-aarch64
+export QEMU := qemu-system-aarch64
-export QEMU := ~/qemu/aarch64-softmmu/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 -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/arm.h

@@ -13,3 +13,6 @@ void invalidate_icache_all(void);
 
 void set_memory_registers_enable_mmu(uintptr_t ttbr0, u64 tcr, u64 mair);
 void set_memory_registers_enable_stage2(uintptr_t vttbr, u64 vtcr);
+
+void initBreakpointRegs(size_t num);
+void initWatchpointRegs(size_t num);

thermosphere/src/arm.s

@@ -261,4 +261,47 @@ set_memory_registers_enable_stage2:
     dsb     sy
     isb
 
-    ret
+    ret
+
+
+// Precondition: x0 <= 16
+.section    .text.initBreakpointRegs, "ax", %progbits
+.type       initBreakpointRegs, %function
+.global     initBreakpointRegs
+initBreakpointRegs:
+    // x0 = number
+    adr     x16, 1f
+    mov     x1, #(16 * 8)
+    sub     x0, x1, x0,lsl #3
+    add     x16, x16, x0
+    br      x16
+
+    1:
+    .irp    count, 15,14,13,12,11,10,9,8,7,6,5,4,3,2,1,0
+    msr     dbgbcr\count\()_el1, xzr
+    msr     dbgbvr\count\()_el1, xzr
+    .endr
+    dsb     sy
+    isb
+    ret
+
+// Precondition: x0 <= 16
+.section    .text.initBreakpointRegs, "ax", %progbits
+.type       initWatchpointRegs, %function
+.global     initWatchpointRegs
+initWatchpointRegs:
+    // x0 = number
+    adr     x16, 1f
+    mov     x1, #(16 * 8)
+    sub     x0, x1, x0,lsl #3
+    add     x16, x16, x0
+    br      x16
+
+    1:
+    .irp    count, 15,14,13,12,11,10,9,8,7,6,5,4,3,2,1,0
+    msr     dbgwcr\count\()_el1, xzr
+    msr     dbgwvr\count\()_el1, xzr
+    .endr
+    dsb     sy
+    isb
+    ret

thermosphere/src/breakpoints.c

@@ -0,0 +1,29 @@
+/*
+ * 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 "breakpoints.h"
+#include "utils.h"
+#include "sysreg.h"
+#include "arm.h"
+
+void enableAndResetBreakpoints(void)
+{
+    SET_SYSREG(mdscr_el1, GET_SYSREG(mdscr_el1) | MDSCR_EL1_MDE);
+
+    // 20 for watchpoints
+    size_t num = ((GET_SYSREG(id_aa64dfr0_el1) >> 12) & 0xF) + 1;
+    initBreakpointRegs(num);
+}

thermosphere/src/breakpoints.h

@@ -0,0 +1,21 @@
+/*
+ * 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 "types.h"
+
+void enableAndResetBreakpoints(void);

thermosphere/src/main.c

@@ -9,6 +9,8 @@
 #include "sysreg.h"
 #include "exceptions.h"
 #include "single_step.h"
+#include "breakpoints.h"
+#include "watchpoints.h"
 
 extern const u8 __start__[];
 
@@ -38,6 +40,8 @@ static void loadKernelViaSemihosting(void)
 void main(ExceptionStackFrame *frame)
 {
     enableTraps();
+    enableAndResetBreakpoints();
+    enableAndResetWatchpoints();
 
     if (currentCoreCtx->isBootCore) {
         uartInit(115200);
@@ -63,5 +67,6 @@ void main(ExceptionStackFrame *frame)
     frame->elr_el2  = currentCoreCtx->kernelEntrypoint;
     frame->x[0]     = currentCoreCtx->kernelArgument;
 
+    // Test
     singleStepSetNextState(frame, SingleStepState_ActivePending);
 }

thermosphere/src/single_step.c

@@ -66,5 +66,5 @@ 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);
 
     // Hehe boi
-    singleStepSetNextState(frame, SingleStepState_ActivePending);
+    //singleStepSetNextState(frame, SingleStepState_ActivePending);
 }

thermosphere/src/sysreg.h

@@ -403,6 +403,7 @@
 #define MDCR_EL2_TPMCR      BITL(5)
 #define MDCR_EL2_HPMN_MASK  0x1Full
 
+#define MDSCR_EL1_MDE       BITL(15)
 #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))

thermosphere/src/watchpoints.c

@@ -0,0 +1,29 @@
+/*
+ * 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 "breakpoints.h"
+#include "utils.h"
+#include "sysreg.h"
+#include "arm.h"
+
+void enableAndResetWatchpoints(void)
+{
+    SET_SYSREG(mdscr_el1, GET_SYSREG(mdscr_el1) | MDSCR_EL1_MDE);
+
+    // 20 for watchpoints
+    size_t num = ((GET_SYSREG(id_aa64dfr0_el1) >> 12) & 0xF) + 1;
+    initBreakpointRegs(num);
+}

thermosphere/src/watchpoints.h

@@ -0,0 +1,21 @@
+/*
+ * 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 "types.h"
+
+void enableAndResetWatchpoints(void);