From 88218f606c2e47614ac05edcf9f73a6898652af8 Mon Sep 17 00:00:00 2001 From: TuxSH Date: Mon, 5 Aug 2019 23:49:25 +0200 Subject: [PATCH] thermosphere: add breakpoint/watchpoint enable/reset code --- thermosphere/Makefile | 6 ++--- thermosphere/src/arm.h | 3 +++ thermosphere/src/arm.s | 45 +++++++++++++++++++++++++++++++++- thermosphere/src/breakpoints.c | 29 ++++++++++++++++++++++ thermosphere/src/breakpoints.h | 21 ++++++++++++++++ thermosphere/src/main.c | 5 ++++ thermosphere/src/single_step.c | 2 +- thermosphere/src/sysreg.h | 1 + thermosphere/src/watchpoints.c | 29 ++++++++++++++++++++++ thermosphere/src/watchpoints.h | 21 ++++++++++++++++ 10 files changed, 157 insertions(+), 5 deletions(-) create mode 100644 thermosphere/src/breakpoints.c create mode 100644 thermosphere/src/breakpoints.h create mode 100644 thermosphere/src/watchpoints.c create mode 100644 thermosphere/src/watchpoints.h 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);