From f40c064e806a17bfe81d0194bc1002ccc8483e01 Mon Sep 17 00:00:00 2001
From: TuxSH <1922548+TuxSH@users.noreply.github.com>
Date: Mon, 9 Mar 2020 18:42:53 +0000
Subject: [PATCH] thermosphere: reload debug registers (refactored)

---
 thermosphere/src/hvisor_hw_breakpoint_manager.cpp | 15 ++++++++++++++-
 thermosphere/src/hvisor_watchpoint_manager.cpp    | 15 ++++++++++++++-
 2 files changed, 28 insertions(+), 2 deletions(-)

diff --git a/thermosphere/src/hvisor_hw_breakpoint_manager.cpp b/thermosphere/src/hvisor_hw_breakpoint_manager.cpp
index be5a3bfdc..c3b02e817 100644
--- a/thermosphere/src/hvisor_hw_breakpoint_manager.cpp
+++ b/thermosphere/src/hvisor_hw_breakpoint_manager.cpp
@@ -20,13 +20,26 @@
 #define _REENT_ONLY
 #include <cerrno>
 
+// Can't use two THERMOSPHERE_SAVE_SYSREG as it prevents ldp from being generated
+#define SAVE_BREAKPOINT(i, _)\
+    __asm__ __volatile__ (\
+        "msr " STRINGIZE(dbgbvr##i##_el1) ", %0\n"\
+        "msr " STRINGIZE(dbgbcr##i##_el1) ", %1"\
+        :\
+        : "r"(m_stopPoints[i].vr), "r"(m_stopPoints[i].cr.raw)\
+        : "memory"\
+    );
+
 namespace ams::hvisor {
 
     HwBreakpointManager HwBreakpointManager::instance{};
 
     void HwBreakpointManager::Reload() const
     {
-        // TODO
+        cpu::dmb();
+        EVAL(REPEAT(MAX_BCR, SAVE_BREAKPOINT, ~));
+        cpu::dsb();
+        cpu::isb();
     }
 
     bool HwBreakpointManager::FindPredicate(const cpu::DebugRegisterPair &pair, uintptr_t addr, size_t, cpu::DebugRegisterPair::LoadStoreControl) const
diff --git a/thermosphere/src/hvisor_watchpoint_manager.cpp b/thermosphere/src/hvisor_watchpoint_manager.cpp
index 83e3c4d89..0850be0e0 100644
--- a/thermosphere/src/hvisor_watchpoint_manager.cpp
+++ b/thermosphere/src/hvisor_watchpoint_manager.cpp
@@ -21,6 +21,16 @@
 #define _REENT_ONLY
 #include <cerrno>
 
+// Can't use two THERMOSPHERE_SAVE_SYSREG as it prevents ldp from being generated
+#define SAVE_WATCHPOINT(i, _)\
+    __asm__ __volatile__ (\
+        "msr " STRINGIZE(dbgwvr##i##_el1) ", %0\n"\
+        "msr " STRINGIZE(dbgwcr##i##_el1) ", %1"\
+        :\
+        : "r"(m_stopPoints[i].vr), "r"(m_stopPoints[i].cr.raw)\
+        : "memory"\
+    );
+
 namespace {
 
     constexpr bool IsRangeMaskWatchpoint(uintptr_t addr, size_t size)
@@ -49,7 +59,10 @@ namespace ams::hvisor {
 
     void WatchpointManager::Reload() const
     {
-        // TODO
+        cpu::dmb();
+        EVAL(REPEAT(MAX_WCR, SAVE_WATCHPOINT, ~));
+        cpu::dsb();
+        cpu::isb();
     }
 
     bool WatchpointManager::FindPredicate(const cpu::DebugRegisterPair &pair, uintptr_t addr, size_t size, cpu::DebugRegisterPair::LoadStoreControl direction) const