diff --git a/thermosphere/Makefile b/thermosphere/Makefile
index f89456c7b..4d1c07495 100644
--- a/thermosphere/Makefile
+++ b/thermosphere/Makefile
@@ -141,11 +141,11 @@ all: $(BUILD)
ifeq ($(PLATFORM), qemu)
-#export QEMU := qemu-system-aarch64
-export QEMU := ~/qemu/aarch64-softmmu/qemu-system-aarch64
+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 -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/arm.h b/thermosphere/src/arm.h
index 1f9a8354b..5d88a5b35 100644
--- a/thermosphere/src/arm.h
+++ b/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);
diff --git a/thermosphere/src/arm.s b/thermosphere/src/arm.s
index 3831a816e..587d58724 100644
--- a/thermosphere/src/arm.s
+++ b/thermosphere/src/arm.s
@@ -261,4 +261,47 @@ set_memory_registers_enable_stage2:
dsb sy
isb
- ret
\ No newline at end of file
+ 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
diff --git a/thermosphere/src/breakpoints.c b/thermosphere/src/breakpoints.c
new file mode 100644
index 000000000..5efb71a83
--- /dev/null
+++ b/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 .
+ */
+
+#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);
+}
diff --git a/thermosphere/src/breakpoints.h b/thermosphere/src/breakpoints.h
new file mode 100644
index 000000000..1f8b888b0
--- /dev/null
+++ b/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 .
+ */
+
+#pragma once
+
+#include "types.h"
+
+void enableAndResetBreakpoints(void);
diff --git a/thermosphere/src/main.c b/thermosphere/src/main.c
index d8234dc79..a0d71bc52 100644
--- a/thermosphere/src/main.c
+++ b/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);
}
diff --git a/thermosphere/src/single_step.c b/thermosphere/src/single_step.c
index 09ccd1952..7f7ae3670 100644
--- a/thermosphere/src/single_step.c
+++ b/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);
}
diff --git a/thermosphere/src/sysreg.h b/thermosphere/src/sysreg.h
index be0fa2a24..53b9e7d6d 100644
--- a/thermosphere/src/sysreg.h
+++ b/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))
diff --git a/thermosphere/src/watchpoints.c b/thermosphere/src/watchpoints.c
new file mode 100644
index 000000000..872de8922
--- /dev/null
+++ b/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 .
+ */
+
+#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);
+}
diff --git a/thermosphere/src/watchpoints.h b/thermosphere/src/watchpoints.h
new file mode 100644
index 000000000..d2c7de677
--- /dev/null
+++ b/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 .
+ */
+
+#pragma once
+
+#include "types.h"
+
+void enableAndResetWatchpoints(void);