dmnt: first pass at breakpoints/watchpoints

stratosphere/dmnt.gen2/dmnt.gen2.json

@@ -22,7 +22,7 @@
 			"value":	{
 				"highest_thread_priority":	63,
 				"lowest_thread_priority":	24,
-				"lowest_cpu_id":	3,
+				"lowest_cpu_id":	0,
 				"highest_cpu_id":	3
 			}
 		}, {

stratosphere/dmnt.gen2/source/dmnt2_breakpoint_manager.cpp

@@ -0,0 +1,54 @@
+/*
+ * Copyright (c) 2018-2020 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 <stratosphere.hpp>
+#include "dmnt2_breakpoint_manager.hpp"
+#include "dmnt2_debug_process.hpp"
+#include "dmnt2_debug_log.hpp"
+
+namespace ams::dmnt {
+
+    BreakPointManager::BreakPointManager(DebugProcess *debug_process) : BreakPointManagerBase(debug_process) {
+        /* ... */
+    }
+
+    void BreakPointManager::ClearStep() {
+        BreakPoint *bp = nullptr;
+        for (size_t i = 0; (bp = static_cast<BreakPoint *>(this->GetBreakPoint(i))) != nullptr; ++i) {
+            if (bp->m_in_use && bp->m_is_step) {
+                AMS_DMNT2_GDB_LOG_DEBUG("BreakPointManager::ClearStep %p 0x%lx (idx=%zu)\n", bp, bp->m_address, i);
+                bp->Clear(m_debug_process);
+            }
+        }
+    }
+
+    Result BreakPointManager::SetBreakPoint(uintptr_t address, size_t size, bool is_step) {
+        /* Get a free breakpoint. */
+        BreakPoint *bp = static_cast<BreakPoint *>(this->GetFreeBreakPoint());
+
+        /* Set the breakpoint. */
+        Result result = svc::ResultOutOfHandles();
+        if (bp != nullptr) {
+            result = bp->Set(m_debug_process, address, size, is_step);
+        }
+
+        if (R_FAILED(result)) {
+            AMS_DMNT2_GDB_LOG_DEBUG("BreakPointManager::SetBreakPoint %p 0x%lx !!! Fail 0x%08x !!!\n", bp, bp->m_address, result.GetValue());
+        }
+
+        return result;
+    }
+
+}

stratosphere/dmnt.gen2/source/dmnt2_breakpoint_manager.hpp

@@ -0,0 +1,41 @@
+/*
+ * Copyright (c) 2018-2020 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 <stratosphere.hpp>
+#include "dmnt2_breakpoint_manager_base.hpp"
+
+namespace ams::dmnt {
+
+    class DebugProcess;
+
+    struct BreakPoint : public BreakPointBase {
+        bool m_is_step;
+
+        BreakPoint() { /* ... */ }
+
+        virtual Result Set(DebugProcess *debug_process, uintptr_t address, size_t size, bool is_step) = 0;
+    };
+
+    class BreakPointManager : public BreakPointManagerBase {
+        public:
+            explicit BreakPointManager(DebugProcess *debug_process);
+
+            void ClearStep();
+
+            Result SetBreakPoint(uintptr_t address, size_t size, bool is_step);
+    };
+
+}

stratosphere/dmnt.gen2/source/dmnt2_breakpoint_manager_base.cpp

@@ -0,0 +1,64 @@
+/*
+ * Copyright (c) 2018-2020 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 <stratosphere.hpp>
+#include "dmnt2_breakpoint_manager_base.hpp"
+#include "dmnt2_debug_process.hpp"
+#include "dmnt2_debug_log.hpp"
+
+namespace ams::dmnt {
+
+    BreakPointManagerBase::BreakPointManagerBase(DebugProcess *debug_process) : m_debug_process(debug_process) {
+        /* ... */
+    }
+
+    void BreakPointManagerBase::ClearAll() {
+        BreakPointBase *bp = nullptr;
+        for (size_t i = 0; (bp = static_cast<BreakPointBase *>(this->GetBreakPoint(i))) != nullptr; ++i) {
+            if (bp->m_in_use) {
+                bp->Clear(m_debug_process);
+            }
+        }
+    }
+
+    void BreakPointManagerBase::Reset() {
+        BreakPointBase *bp = nullptr;
+        for (size_t i = 0; (bp = static_cast<BreakPointBase *>(this->GetBreakPoint(i))) != nullptr; ++i) {
+            bp->Reset();
+        }
+    }
+
+    Result BreakPointManagerBase::ClearBreakPoint(uintptr_t address, size_t size) {
+        BreakPointBase *bp = nullptr;
+        for (size_t i = 0; (bp = static_cast<BreakPointBase *>(this->GetBreakPoint(i))) != nullptr; ++i) {
+            if (bp->m_in_use && bp->m_address == address) {
+                AMS_ABORT_UNLESS(bp->m_size == size);
+                return bp->Clear(m_debug_process);
+            }
+        }
+        return ResultSuccess();
+    }
+
+    BreakPointBase *BreakPointManagerBase::GetFreeBreakPoint() {
+        BreakPointBase *bp = nullptr;
+        for (size_t i = 0; (bp = static_cast<BreakPointBase *>(this->GetBreakPoint(i))) != nullptr; ++i) {
+            if (!bp->m_in_use) {
+                return bp;
+            }
+        }
+        return nullptr;
+    }
+
+}

stratosphere/dmnt.gen2/source/dmnt2_breakpoint_manager_base.hpp

@@ -0,0 +1,50 @@
+/*
+ * Copyright (c) 2018-2020 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 <stratosphere.hpp>
+
+namespace ams::dmnt {
+
+    class DebugProcess;
+
+    struct BreakPointBase {
+        bool m_in_use;
+        uintptr_t m_address;
+        size_t m_size;
+
+        BreakPointBase() : m_in_use(false) { /* ... */ }
+
+        void Reset() { m_in_use = false; }
+
+        virtual Result Clear(DebugProcess *debug_process) = 0;
+    };
+
+    class BreakPointManagerBase {
+        protected:
+            DebugProcess *m_debug_process;
+        public:
+            explicit BreakPointManagerBase(DebugProcess *debug_process);
+
+            void ClearAll();
+            void Reset();
+
+            Result ClearBreakPoint(uintptr_t address, size_t size);
+        protected:
+            virtual BreakPointBase *GetBreakPoint(size_t index) = 0;
+            BreakPointBase *GetFreeBreakPoint();
+    };
+
+}

stratosphere/dmnt.gen2/source/dmnt2_debug_process.cpp

@@ -19,6 +19,14 @@
 
 namespace ams::dmnt {
 
+    namespace {
+
+        s32 SignExtend(u32 value, u32 bits) {
+            return static_cast<s32>(value << (32 - bits)) >> (32 - bits);
+        }
+
+    }
+
     Result DebugProcess::Attach(os::ProcessId process_id) {
         /* Attach to the process. */
         R_TRY(svc::DebugActiveProcess(std::addressof(m_debug_handle), process_id.value));
@@ -39,6 +47,10 @@ namespace ams::dmnt {
 
     void DebugProcess::Detach() {
         if (m_is_valid) {
+            m_software_breakpoints.ClearAll();
+            m_hardware_breakpoints.ClearAll();
+            m_hardware_watchpoints.ClearAll();
+
             R_ABORT_UNLESS(svc::CloseHandle(m_debug_handle));
             m_debug_handle = svc::InvalidHandle;
         }
@@ -213,6 +225,189 @@ namespace ams::dmnt {
         return svc::WriteDebugProcessMemory(m_debug_handle, reinterpret_cast<uintptr_t>(src), address, size);
     }
 
+    Result DebugProcess::Continue() {
+        AMS_DMNT2_GDB_LOG_DEBUG("DebugProcess::Continue() all\n");
+
+        u64 thread_ids[] = { 0 };
+        R_TRY(svc::ContinueDebugEvent(m_debug_handle, svc::ContinueFlag_ExceptionHandled | svc::ContinueFlag_EnableExceptionEvent | svc::ContinueFlag_ContinueAll, thread_ids, util::size(thread_ids)));
+
+        m_continue_thread_id = 0;
+        m_status             = ProcessStatus_Running;
+
+        this->SetLastThreadId(0);
+        this->SetLastSignal(GdbSignal_Signal0);
+
+        return ResultSuccess();
+    }
+
+    Result DebugProcess::Continue(u64 thread_id) {
+        AMS_DMNT2_GDB_LOG_DEBUG("DebugProcess::Continue() thread_id=%lx\n", thread_id);
+
+        u64 thread_ids[] = { thread_id };
+        R_TRY(svc::ContinueDebugEvent(m_debug_handle, svc::ContinueFlag_ExceptionHandled | svc::ContinueFlag_EnableExceptionEvent, thread_ids, util::size(thread_ids)));
+
+        m_continue_thread_id = thread_id;
+        m_status             = ProcessStatus_Running;
+
+        this->SetLastThreadId(0);
+        this->SetLastSignal(GdbSignal_Signal0);
+
+        return ResultSuccess();
+    }
+
+    Result DebugProcess::Step() {
+        AMS_DMNT2_GDB_LOG_DEBUG("DebugProcess::Step() all\n");
+        return this->Step(this->GetLastThreadId());
+    }
+
+    Result DebugProcess::Step(u64 thread_id) {
+        AMS_DMNT2_GDB_LOG_DEBUG("DebugProcess::Step() thread_id=%lx\n", thread_id);
+
+        /* Get the thread context. */
+        svc::ThreadContext ctx;
+        R_TRY(this->GetThreadContext(std::addressof(ctx), thread_id, svc::ThreadContextFlag_Control));
+
+        /* Note that we're stepping. */
+        m_stepping = true;
+
+        /* Determine where we're stepping to. */
+        u64 current_pc  = ctx.pc;
+        u64 step_target = 0;
+        this->GetBranchTarget(ctx, thread_id, current_pc, step_target);
+
+        /* Ensure we end with valid breakpoints. */
+        auto bp_guard = SCOPE_GUARD { this->ClearStep(); };
+
+        /* Set step breakpoint on current pc. */
+        /* TODO: aarch32 breakpoints. */
+        if (current_pc) {
+            R_TRY(m_step_breakpoints.SetBreakPoint(current_pc, sizeof(u32), true));
+        }
+
+        if (step_target) {
+            R_TRY(m_step_breakpoints.SetBreakPoint(step_target, sizeof(u32), true));
+        }
+
+        bp_guard.Cancel();
+        return ResultSuccess();
+    }
+
+    void DebugProcess::ClearStep() {
+        /* If we should, clear our step breakpoints. */
+        if (m_stepping) {
+            m_step_breakpoints.ClearStep();
+            m_stepping = false;
+        }
+    }
+
+    Result DebugProcess::Break() {
+        if (this->GetStatus() == ProcessStatus_Running) {
+            AMS_DMNT2_GDB_LOG_DEBUG("DebugProcess::Break\n");
+            return svc::BreakDebugProcess(m_debug_handle);
+        } else {
+            AMS_DMNT2_GDB_LOG_ERROR("DebugProcess::Break called on non-running process!\n");
+            return ResultSuccess();
+        }
+    }
+
+    void DebugProcess::GetBranchTarget(svc::ThreadContext &ctx, u64 thread_id, u64 &current_pc, u64 &target) {
+        /* Save pc, in case we modify it. */
+        const u64 pc = current_pc;
+
+        /* Clear the target. */
+        target = 0;
+
+        /* By default, we advance by four. */
+        current_pc += 4;
+
+        /* Get the instruction where we were. */
+        u32 insn = 0;
+        this->ReadMemory(std::addressof(insn), pc, sizeof(insn));
+
+        /* Handle by architecture. */
+        bool is_call = false;
+        if (this->Is64Bit()) {
+            if ((insn & 0x7C000000) == 0x14000000) {
+                /* Unconditional branch (b/bl) */
+                if (insn != 0x14000001) {
+                    is_call    = (insn & 0x80000000) == 0x80000000;
+                    current_pc = 0;
+                    target     = SignExtend(((insn & 0x03FFFFFF) << 2), 28) + pc;
+                }
+            } else if ((insn & 0x7E000000) == 0x34000000) {
+                /* Compare/Branch (cbz/cbnz) */
+                target = SignExtend(((insn & 0x00FFFFE0) >> 3), 21) + pc;
+            } else if ((insn & 0x7E000000) == 0x36000000) {
+                /* Test and branch (tbz/tbnz) */
+                target = SignExtend(((insn & 0x0007FFE0) >> 3), 16) + pc;
+            } else if ((insn & 0xFF000010) == 0x54000000) {
+                /* Conditional branch (b.*) */
+                if ((insn & 0xF) == 0xE) {
+                    /* Unconditional. */
+                    current_pc = 0;
+                }
+                target = SignExtend(((insn & 0x00FFFFE0) >> 3), 21) + pc;
+            } else if ((insn & 0xFF8FFC1F) == 0xD60F0000) {
+                /* Unconditional branch */
+                is_call = (insn & 0x00F00000) == 0x00300000;
+                if (!is_call) {
+                    current_pc = 0;
+                }
+
+                /* Get the register. */
+                svc::ThreadContext new_ctx;
+                if (R_SUCCEEDED(this->GetThreadContext(std::addressof(new_ctx), thread_id, svc::ThreadContextFlag_Control | svc::ThreadContextFlag_General))) {
+                    const int reg = (insn & 0x03E0) >> 5;
+                    if (reg < 29) {
+                        target = new_ctx.r[reg];
+                    } else if (reg == 29) {
+                        target = new_ctx.fp;
+                    } else if (reg == 30) {
+                        target = new_ctx.lr;
+                    } else if (reg == 31) {
+                        target = new_ctx.sp;
+                    }
+                }
+            }
+        } else {
+            /* TODO aarch32 branch decoding */
+        }
+    }
+
+    Result DebugProcess::SetBreakPoint(uintptr_t address, size_t size, bool is_step) {
+        return m_software_breakpoints.SetBreakPoint(address, size, is_step);
+    }
+
+    Result DebugProcess::ClearBreakPoint(uintptr_t address, size_t size) {
+        m_software_breakpoints.ClearBreakPoint(address, size);
+        return ResultSuccess();
+    }
+
+    Result DebugProcess::SetHardwareBreakPoint(uintptr_t address, size_t size, bool is_step) {
+        return m_hardware_breakpoints.SetBreakPoint(address, size, is_step);
+    }
+
+    Result DebugProcess::ClearHardwareBreakPoint(uintptr_t address, size_t size) {
+        m_hardware_breakpoints.ClearBreakPoint(address, size);
+        return ResultSuccess();
+    }
+
+    Result DebugProcess::SetWatchPoint(u64 address, u64 size, bool read, bool write) {
+        return m_hardware_watchpoints.SetWatchPoint(address, size, read, write);
+    }
+
+    Result DebugProcess::ClearWatchPoint(u64 address, u64 size) {
+        return m_hardware_watchpoints.ClearBreakPoint(address, size);
+    }
+
+    Result DebugProcess::GetWatchPointInfo(u64 address, bool &read, bool &write) {
+        return m_hardware_watchpoints.GetWatchPointInfo(address, read, write);
+    }
+
+    bool DebugProcess::IsValidWatchPoint(u64 address, u64 size) {
+        return HardwareWatchPointManager::IsValidWatchPoint(address, size);
+    }
+
     Result DebugProcess::GetThreadCurrentCore(u32 *out, u64 thread_id) {
         u64 dummy_value;
         u32 val32 = 0;

stratosphere/dmnt.gen2/source/dmnt2_debug_process.hpp

@@ -17,6 +17,9 @@
 #include <stratosphere.hpp>
 #include "dmnt2_gdb_signal.hpp"
 #include "dmnt2_module_definition.hpp"
+#include "dmnt2_software_breakpoint.hpp"
+#include "dmnt2_hardware_breakpoint.hpp"
+#include "dmnt2_hardware_watchpoint.hpp"
 
 namespace ams::dmnt {
 
@@ -45,17 +48,24 @@ namespace ams::dmnt {
             ProcessStatus m_status{ProcessStatus_DebugBreak};
             os::ProcessId m_process_id{os::InvalidProcessId};
             u64 m_last_thread_id{};
-            u64 m_thread_id_override;
+            u64 m_thread_id_override{};
+            u64 m_continue_thread_id{};
             GdbSignal m_last_signal{};
+            bool m_stepping{false};
             bool m_thread_valid[ThreadCountMax]{};
             u64 m_thread_ids[ThreadCountMax]{};
             osdbg::ThreadInfo m_thread_infos[ThreadCountMax]{};
             svc::DebugInfoCreateProcess m_create_process_info{};
+            SoftwareBreakPointManager m_software_breakpoints;
+            HardwareBreakPointManager m_hardware_breakpoints;
+            HardwareWatchPointManager m_hardware_watchpoints;
+            BreakPointManager &m_step_breakpoints;
             ModuleDefinition m_module_definitions[ModuleCountMax]{};
             size_t m_module_count{};
             size_t m_main_module{};
         public:
-            constexpr DebugProcess() = default;
+            /* TODO: ifdef for hardware breakpoints. */
+            DebugProcess() : m_software_breakpoints(this), m_hardware_breakpoints(this), m_hardware_watchpoints(this), m_step_breakpoints(m_software_breakpoints) { /* ... */ }
             ~DebugProcess() { this->Detach(); }
 
             svc::Handle GetHandle() const { return m_debug_handle; }
@@ -102,9 +112,31 @@ namespace ams::dmnt {
             Result ReadMemory(void *dst, uintptr_t address, size_t size);
             Result WriteMemory(const void *src, uintptr_t address, size_t size);
 
+            Result Continue();
+            Result Continue(u64 thread_id);
+            Result Step();
+            Result Step(u64 thread_id);
+            void ClearStep();
+
+            Result Break();
+
+            Result SetBreakPoint(uintptr_t address, size_t size, bool is_step);
+            Result ClearBreakPoint(uintptr_t address, size_t size);
+
+            Result SetHardwareBreakPoint(uintptr_t address, size_t size, bool is_step);
+            Result ClearHardwareBreakPoint(uintptr_t address, size_t size);
+
+            Result SetWatchPoint(u64 address, u64 size, bool read, bool write);
+            Result ClearWatchPoint(u64 address, u64 size);
+            Result GetWatchPointInfo(u64 address, bool &read, bool &write);
+
+            static bool IsValidWatchPoint(u64 address, u64 size);
+
             Result GetThreadCurrentCore(u32 *out, u64 thread_id);
 
             Result GetProcessDebugEvent(svc::DebugEventInfo *out);
+
+            void GetBranchTarget(svc::ThreadContext &ctx, u64 thread_id, u64 &current_pc, u64 &target);
         private:
             Result Start();
 

stratosphere/dmnt.gen2/source/dmnt2_gdb_server_impl.cpp

@@ -20,6 +20,25 @@
 namespace ams::dmnt {
 
     namespace {
+
+        constexpr const u32 SdkBreakPoint     = 0xE7FFFFFF;
+        constexpr const u32 SdkBreakPointMask = 0xFFFFFFFF;
+
+        constexpr const u32 ArmBreakPoint     = 0xE7FFDEFE;
+        constexpr const u32 ArmBreakPointMask = 0xFFFFFFFF;
+
+        constexpr const u32 A64BreakPoint     = 0xD4200000;
+        constexpr const u32 A64BreakPointMask = 0xFFE0001F;
+
+        constexpr const u32 A64Halt           = 0xD4400000;
+        constexpr const u32 A64HaltMask       = 0xFFE0001F;
+
+        constexpr const u32 A32BreakPoint     = 0xE1200070;
+        constexpr const u32 A32BreakPointMask = 0xFFF000F0;
+
+        constexpr const u32 T16BreakPoint     = 0x0000BE00;
+        constexpr const u32 T16BreakPointMask = 0x0000FF00;
+
         constexpr const char TargetXmlAarch64[] =
             "l<?xml version=\"1.0\"?>"
             "<!DOCTYPE target SYSTEM \"gdb-target.dtd\">"
@@ -349,6 +368,15 @@ namespace ams::dmnt {
             AMS_DMNT2_GDB_LOG_DEBUG("Offset/Length %x/%x\n", offset, length);
         }
 
+        s32 FindThreadIdIndex(u64 *thread_ids, s32 num_threads, u64 thread_id) {
+            for (auto i = 0; i < num_threads; ++i) {
+                if (thread_ids[i] == thread_id) {
+                    return i;
+                }
+            }
+            return 0;
+        }
+
         void SetGdbRegister32(char *dst, u32 value) {
             if (value != 0) {
                 AppendReply(dst, "%08x", util::ConvertToBigEndian(value));
@@ -570,10 +598,189 @@ namespace ams::dmnt {
             }
 
             /* Process the event. */
+            bool reply = false;
+            GdbSignal signal;
+            char send_buffer[GdbPacketBufferSize];
+            u64 thread_id = d.thread_id;
+            m_debug_process.ClearStep();
+
+            send_buffer[0] = 0;
             switch (d.type) {
-                default:
+                case svc::DebugEvent_Exception:
-                    AMS_DMNT2_GDB_LOG_DEBUG("Unhandled ProcessEvent %u\n", static_cast<u32>(d.type));
+                    {
+                        switch (d.info.exception.type) {
+                            case svc::DebugException_BreakPoint:
+                                {
+                                    signal = GdbSignal_BreakpointTrap;
+
+                                    const uintptr_t address = d.info.exception.address;
+                                    const bool is_instr     = d.info.exception.specific.break_point.type == svc::BreakPointType_HardwareInstruction;
+                                    AMS_DMNT2_GDB_LOG_DEBUG("BreakPoint %lx, addr=%lx, type=%s\n", thread_id, address, is_instr ? "Instr" : "Data");
+
+                                    if (is_instr) {
+                                        SetReply(send_buffer, "T%02Xthread:p%lx.%lx;hwbreak:;", static_cast<u32>(signal), m_process_id.value, thread_id);
+                                    } else {
+                                        bool read = false, write = false;
+                                        const char *type = "watch";
+                                        if (R_SUCCEEDED(m_debug_process.GetWatchPointInfo(address, read, write))) {
+                                            if (read && write) {
+                                                type = "awatch";
+                                            } else if (read) {
+                                                type = "rwatch";
+                                            }
+                                        } else {
+                                            AMS_DMNT2_GDB_LOG_DEBUG("GetWatchPointInfo FAIL %lx, addr=%lx, type=%s\n", thread_id, address, is_instr ? "Instr" : "Data");
+                                        }
+
+                                        SetReply(send_buffer, "T%02Xthread:p%lx.%lx;%s:%lx;", static_cast<u32>(signal), m_process_id.value, thread_id, type, address);
+                                    }
+
+                                    reply = true;
+                                }
+                                break;
+                            case svc::DebugException_DebuggerBreak:
+                                {
+                                    AMS_DMNT2_GDB_LOG_DEBUG("DebuggerBreak %lx, last=%lx\n", thread_id, m_debug_process.GetLastThreadId());
+                                    signal = GdbSignal_Interrupt;
+                                    thread_id = m_debug_process.GetLastThreadId();
+                                    m_debug_process.SetLastThreadId(thread_id);
+                                }
+                                break;
+                            case svc::DebugException_UndefinedInstruction:
+                                {
+                                    signal            = GdbSignal_IllegalInstruction;
+
+                                    uintptr_t address = d.info.exception.address;
+                                    const u32 insn    = d.info.exception.specific.undefined_instruction.insn;
+                                    u32 new_insn      = 0;
+
+                                    svc::ThreadContext ctx;
+                                    if (R_SUCCEEDED(m_debug_process.GetThreadContext(std::addressof(ctx), thread_id, svc::ThreadContextFlag_Control))) {
+                                        bool insn_changed = false;
+                                        if (ctx.pstate & 0x20) {
+                                            /* Thumb mode. */
+                                            address &= ~1;
+
+                                            if (R_SUCCEEDED(m_debug_process.ReadMemory(std::addressof(new_insn), address, 2))) {
+                                                switch ((new_insn >> 11) & 0x1F) {
+                                                    case 0x1D:
+                                                    case 0x1E:
+                                                    case 0x1F:
+                                                        {
+                                                            if (R_SUCCEEDED(m_debug_process.ReadMemory(reinterpret_cast<u8 *>(std::addressof(new_insn)) + 2, address + 2, 2))) {
+                                                                insn_changed = (new_insn != insn);
+                                                            }
+                                                        }
+                                                        break;
+                                                    default:
+                                                        insn_changed = (new_insn != insn);
+                                                        break;
+                                                }
+
+                                                if ((insn & T16BreakPointMask) == T16BreakPoint) {
+                                                    signal = GdbSignal_BreakpointTrap;
+                                                }
+                                            }
+                                        } else {
+                                            /* Non-thumb. */
+                                            if (R_SUCCEEDED(m_debug_process.ReadMemory(std::addressof(new_insn), address, sizeof(new_insn)))) {
+                                                insn_changed = (new_insn != insn);
+                                            }
+
+                                            if (((insn & SdkBreakPointMask) == SdkBreakPoint) ||
+                                                ((insn & ArmBreakPointMask) == ArmBreakPoint) ||
+                                                ((insn & A64BreakPointMask) == A64BreakPoint) ||
+                                                ((insn & A32BreakPointMask) == A32BreakPoint) ||
+                                                ((insn & A64HaltMask)       == A64Halt))
+                                            {
+                                                signal = GdbSignal_BreakpointTrap;
+                                            }
+                                        }
+
+                                        if (insn_changed) {
+                                            AMS_DMNT2_GDB_LOG_DEBUG("Instruction Changed %lx, address=%p, insn=%08x, new_insn=%08x\n", thread_id, reinterpret_cast<void *>(address), insn, new_insn);
+                                        }
+                                    }
+
+                                    if (signal == GdbSignal_IllegalInstruction) {
+                                        AMS_DMNT2_GDB_LOG_DEBUG("Undefined Instruction %lx, address=%p, insn=%08x\n", thread_id, reinterpret_cast<void *>(address), insn);
+                                    } else if (signal == GdbSignal_BreakpointTrap && ((insn & SdkBreakPointMask) != SdkBreakPoint)) {
+                                        AMS_DMNT2_GDB_LOG_DEBUG("Non-SDK BreakPoint %lx, address=%p, insn=%08x\n", thread_id, reinterpret_cast<void *>(address), insn);
+                                    }
+
+                                    if (signal == GdbSignal_BreakpointTrap) {
+                                        SetReply(send_buffer, "T%02Xthread:p%lx.%lx;swbreak:;", static_cast<u32>(signal), m_process_id.value, thread_id);
+                                        reply = true;
+                                    }
+
+                                    m_debug_process.ClearStep();
+                                }
+                                break;
+                            default:
+                                AMS_DMNT2_GDB_LOG_DEBUG("Unhandled Exception %u %lx\n", static_cast<u32>(d.info.exception.type), thread_id);
+                                signal = GdbSignal_SegmentationFault;
+                                break;
+                        }
+
+                        if (!reply) {
+                            SetReply(send_buffer, "T%02Xthread:p%lx.%lx;", static_cast<u32>(signal), m_process_id.value, thread_id);
+                            reply = true;
+                        }
+
+                        m_debug_process.SetLastThreadId(thread_id);
+                        m_debug_process.SetLastSignal(signal);
+                    }
                     break;
+                case svc::DebugEvent_CreateThread:
+                    {
+                        AMS_DMNT2_GDB_LOG_DEBUG("CreateThread %lx\n", thread_id);
+
+                        if (m_debug_process.IsValid()) {
+                            m_debug_process.Continue();
+                        } else {
+                            SetReply(send_buffer, "W00");
+                            reply = true;
+                        }
+                    }
+                case svc::DebugEvent_ExitThread:
+                    {
+                        AMS_DMNT2_GDB_LOG_DEBUG("ExitThread %lx\n", thread_id);
+
+                        if (m_debug_process.IsValid()) {
+                            m_debug_process.Continue();
+                        } else {
+                            SetReply(send_buffer, "W00");
+                            reply = true;
+                        }
+                    }
+                    break;
+                case svc::DebugEvent_ExitProcess:
+                    {
+                        m_killed = true;
+                        AMS_DMNT2_GDB_LOG_DEBUG("ExitProcess\n");
+
+                        if (d.info.exit_process.reason == svc::ProcessExitReason_ExitProcess) {
+                            SetReply(send_buffer, "W00");
+                        } else {
+                            SetReply(send_buffer, "X%02X", GdbSignal_Killed);
+                        }
+
+                        m_debug_process.Detach();
+                        reply = true;
+                    }
+                    break;
+                default:
+                    AMS_DMNT2_GDB_LOG_DEBUG("Unhandled ProcessEvent %u %lx\n", static_cast<u32>(d.type), thread_id);
+                    m_debug_process.Continue();
+                    break;
+            }
+
+            if (reply) {
+                bool do_break;
+                this->SendPacket(std::addressof(do_break), send_buffer);
+                if (do_break) {
+                    m_debug_process.Break();
+                }
             }
         }
     }
@@ -658,6 +865,12 @@ namespace ams::dmnt {
             case 'H':
                 this->H();
                 break;
+            case 'T':
+                this->T();
+                break;
+            case 'Z':
+                this->Z();
+                break;
             case 'g':
                 if (!this->g()) {
                     m_killed = true;
@@ -672,6 +885,9 @@ namespace ams::dmnt {
             case 'q':
                 this->q();
                 break;
+            case 'z':
+                this->z();
+                break;
             case '!':
                 SetReplyOk(m_reply_packet);
                 break;
@@ -730,6 +946,106 @@ namespace ams::dmnt {
         }
     }
 
+    void GdbServerImpl::T() {
+        if (const char *dot = std::strchr(m_receive_packet, '.'); dot != nullptr) {
+            const u64 thread_id = DecodeHex(dot + 1);
+
+            svc::ThreadContext ctx;
+            if (R_SUCCEEDED(m_debug_process.GetThreadContext(std::addressof(ctx), thread_id, svc::ThreadContextFlag_Control))) {
+                SetReplyOk(m_reply_packet);
+            } else {
+                SetReply(m_reply_packet, "E01");
+            }
+        } else {
+            SetReplyError(m_reply_packet, "E01");
+        }
+    }
+
+    void GdbServerImpl::Z() {
+        /* Increment past the 'Z'. */
+        ++m_receive_packet;
+
+        /* Decode the type. */
+        if (!('0' <= m_receive_packet[0] && m_receive_packet[0] <= '4') || m_receive_packet[1] != ',') {
+            SetReplyError(m_reply_packet, "E01");
+            return;
+        }
+
+        const auto type = m_receive_packet[0] - '0';
+        m_receive_packet += 2;
+
+        /* Decode the address/length. */
+        const char *comma = std::strchr(m_receive_packet, ',');
+        if (comma == nullptr) {
+            SetReplyError(m_reply_packet, "E01");
+            return;
+        }
+
+        /* Parse address/length. */
+        const u64 address = DecodeHex(m_receive_packet);
+        const u64 length  = DecodeHex(comma + 1);
+
+        switch (type) {
+            case 0: /* SW */
+                {
+                    if (length == 2 || length == 4) {
+                        if (R_SUCCEEDED(m_debug_process.SetBreakPoint(address, length, false))) {
+                            SetReplyOk(m_reply_packet);
+                        } else {
+                            SetReplyError(m_reply_packet, "E01");
+                        }
+                    }
+                }
+                break;
+            case 1: /* HW */
+                {
+                    if (length == 2 || length == 4) {
+                        if (R_SUCCEEDED(m_debug_process.SetHardwareBreakPoint(address, length, false))) {
+                            SetReplyOk(m_reply_packet);
+                        } else {
+                            SetReplyError(m_reply_packet, "E01");
+                        }
+                    }
+                }
+                break;
+            case 2: /* Watch-W */
+                {
+                    if (m_debug_process.IsValidWatchPoint(address, length)) {
+                        if (R_SUCCEEDED(m_debug_process.SetWatchPoint(address, length, false, true))) {
+                            SetReplyOk(m_reply_packet);
+                        } else {
+                            SetReplyError(m_reply_packet, "E01");
+                        }
+                    }
+                }
+                break;
+            case 3: /* Watch-R */
+                {
+                    if (m_debug_process.IsValidWatchPoint(address, length)) {
+                        if (R_SUCCEEDED(m_debug_process.SetWatchPoint(address, length, true, false))) {
+                            SetReplyOk(m_reply_packet);
+                        } else {
+                            SetReplyError(m_reply_packet, "E01");
+                        }
+                    }
+                }
+                break;
+            case 4: /* Watch-A */
+                {
+                    if (m_debug_process.IsValidWatchPoint(address, length)) {
+                        if (R_SUCCEEDED(m_debug_process.SetWatchPoint(address, length, true, true))) {
+                            SetReplyOk(m_reply_packet);
+                        } else {
+                            SetReplyError(m_reply_packet, "E01");
+                        }
+                    }
+                }
+                break;
+            default:
+                break;
+        }
+    }
+
     bool GdbServerImpl::g() {
         /* Get thread id. */
         u64 thread_id = m_debug_process.GetThreadIdOverride();
@@ -781,6 +1097,8 @@ namespace ams::dmnt {
     void GdbServerImpl::v() {
         if (ParsePrefix(m_receive_packet, "vAttach;")) {
             this->vAttach();
+        } else if (ParsePrefix(m_receive_packet, "vCont")) {
+            this->vCont();
         } else {
             AMS_DMNT2_GDB_LOG_DEBUG("Not Implemented v: %s\n", m_receive_packet);
         }
@@ -817,6 +1135,140 @@ namespace ams::dmnt {
         }
     }
 
+    void GdbServerImpl::vCont() {
+        /* Check if this is a query about what we support. */
+        if (ParsePrefix(m_receive_packet, "?")) {
+            SetReply(m_reply_packet, "vCont;c;C;s;S;");
+            return;
+        }
+
+        /* We want to parse semicolon separated fields repeatedly. */
+        char *saved;
+        char *token = strtok_r(m_receive_packet, ";", std::addressof(saved));
+
+        /* Validate the initial token. */
+        if (token == nullptr) {
+            return;
+        }
+
+        /* Prepare to parse threads. */
+        u64 thread_ids[DebugProcess::ThreadCountMax] = {};
+        u8 continue_modes[DebugProcess::ThreadCountMax] = {};
+
+        s32 num_threads;
+        if (R_FAILED(m_debug_process.GetThreadList(std::addressof(num_threads), thread_ids, util::size(thread_ids)))) {
+            AMS_DMNT2_GDB_LOG_ERROR("vCont: Failed to get thread list\n");
+            SetReplyError(m_reply_packet, "E01");
+            return;
+        }
+
+        /* Handle each token. */
+        Result result = ResultSuccess();
+        DebugProcess::ContinueMode default_continue_mode = DebugProcess::ContinueMode_Stopped;
+        while (token != nullptr && R_SUCCEEDED(result)) {
+            result = this->ParseVCont(token, thread_ids, continue_modes, num_threads, default_continue_mode);
+            token = strtok_r(nullptr, ";", std::addressof(saved));
+        }
+
+        AMS_DMNT2_GDB_LOG_DEBUG("vCont: NumThreads=%d, Default Continue Mode=%d\n", num_threads, static_cast<int>(default_continue_mode));
+
+        /* Act on all threads. */
+        s64 thread_id = -1;
+        for (auto i = 0; i < num_threads; ++i) {
+            if (continue_modes[i] == DebugProcess::ContinueMode_Step || (continue_modes[i] == DebugProcess::ContinueMode_Stopped && default_continue_mode == DebugProcess::ContinueMode_Step)) {
+                thread_id = thread_ids[i];
+                result = m_debug_process.Step(thread_ids[i]);
+            }
+        }
+
+        /* Continue the last thread. */
+        if (static_cast<u64>(thread_id) == m_debug_process.GetLastThreadId() && default_continue_mode != DebugProcess::ContinueMode_Continue) {
+            result = m_debug_process.Continue(thread_id);
+        } else {
+            result = m_debug_process.Continue();
+        }
+
+        /* Set reply. */
+        if (R_SUCCEEDED(result)) {
+            SetReplyOk(m_reply_packet);
+        } else {
+            AMS_DMNT2_GDB_LOG_ERROR("vCont: Failed %08x\n", result.GetValue());
+            SetReplyError(m_reply_packet, "E01");
+        }
+    }
+
+    Result GdbServerImpl::ParseVCont(char * const token, u64 *thread_ids, u8 *continue_modes, s32 num_threads, DebugProcess::ContinueMode &default_continue_mode) {
+        /* Parse the thread id. */
+        s64 thread_id = -1;
+        s32 signal = -1;
+        s32 thread_ix = -1;
+
+        if (token[0] && token[1]) {
+            if (char *colon = std::strchr(token, ':'); colon != nullptr) {
+                *colon = 0;
+                if (char *dot = std::strchr(colon + 1, '.'); dot != nullptr) {
+                    *dot = 0;
+                    thread_id = std::strcmp(dot + 1, "-1") == 0 ? -1 : static_cast<s64>(DecodeHex(dot + 1));
+                    thread_ix = FindThreadIdIndex(thread_ids, num_threads, static_cast<u64>(thread_id));
+                }
+            }
+        }
+
+        /* Check that we don't already have a default mode. */
+        if (thread_id == -1 && default_continue_mode != DebugProcess::ContinueMode_Stopped) {
+            AMS_DMNT2_GDB_LOG_ERROR("vCont: Too many defaults specified\n");
+        }
+
+        /* Handle the action. */
+        switch (token[0]) {
+            case 'c':
+                if (thread_id > 0) {
+                    AMS_DMNT2_GDB_LOG_DEBUG("vCont: Continue %lx\n", static_cast<u64>(thread_id));
+                    continue_modes[thread_ix] = DebugProcess::ContinueMode_Continue;
+                } else {
+                    default_continue_mode = DebugProcess::ContinueMode_Continue;
+                }
+                break;
+            case 'C':
+                if (token[1]) {
+                    signal = std::strcmp(token + 1, "-1") == 0 ? -1 : static_cast<s32>(DecodeHex(token + 1));
+                }
+                AMS_DMNT2_GDB_LOG_WARN("vCont: Ignoring C, signal=%d\n", signal);
+                if (thread_id > 0) {
+                    AMS_DMNT2_GDB_LOG_DEBUG("vCont: Continue %lx, signal=%d\n", static_cast<u64>(thread_id), signal);
+                    continue_modes[thread_ix] = DebugProcess::ContinueMode_Continue;
+                } else {
+                    default_continue_mode = DebugProcess::ContinueMode_Continue;
+                }
+                break;
+            case 's':
+                if (thread_id > 0) {
+                    AMS_DMNT2_GDB_LOG_DEBUG("vCont: Step %lx\n", static_cast<u64>(thread_id));
+                    continue_modes[thread_ix] = DebugProcess::ContinueMode_Step;
+                } else {
+                    default_continue_mode = DebugProcess::ContinueMode_Step;
+                }
+                break;
+            case 'S':
+                if (token[1]) {
+                    signal = std::strcmp(token + 1, "-1") == 0 ? -1 : static_cast<s32>(DecodeHex(token + 1));
+                }
+                AMS_DMNT2_GDB_LOG_WARN("vCont: Ignoring S, signal=%d\n", signal);
+                if (thread_id > 0) {
+                    AMS_DMNT2_GDB_LOG_DEBUG("vCont: Step %lx, signal=%d\n", static_cast<u64>(thread_id), signal);
+                    continue_modes[thread_ix] = DebugProcess::ContinueMode_Step;
+                } else {
+                    default_continue_mode = DebugProcess::ContinueMode_Step;
+                }
+                break;
+            default:
+                AMS_DMNT2_GDB_LOG_WARN("vCont: Ignoring %c\n", token[0]);
+                break;
+        }
+
+        return ResultSuccess();
+    }
+
     void GdbServerImpl::q() {
         if (ParsePrefix(m_receive_packet, "qAttached:")) {
             this->qAttached();
@@ -1081,6 +1533,65 @@ namespace ams::dmnt {
         return true;
     }
 
+    void GdbServerImpl::z() {
+        /* Increment past the 'z'. */
+        ++m_receive_packet;
+
+        /* Decode the type. */
+        if (!('0' <= m_receive_packet[0] && m_receive_packet[0] <= '4') || m_receive_packet[1] != ',') {
+            SetReplyError(m_reply_packet, "E01");
+            return;
+        }
+
+        const auto type = m_receive_packet[0] - '0';
+        m_receive_packet += 2;
+
+        /* Decode the address/length. */
+        const char *comma = std::strchr(m_receive_packet, ',');
+        if (comma == nullptr) {
+            SetReplyError(m_reply_packet, "E01");
+            return;
+        }
+
+        /* Parse address/length. */
+        const u64 address = DecodeHex(m_receive_packet);
+        const u64 length  = DecodeHex(comma + 1);
+
+        switch (type) {
+            case 0: /* SW */
+                {
+                    if (R_SUCCEEDED(m_debug_process.ClearBreakPoint(address, length))) {
+                        SetReplyOk(m_reply_packet);
+                    } else {
+                        SetReplyError(m_reply_packet, "E01");
+                    }
+                }
+                break;
+            case 1: /* HW */
+                {
+                    if (R_SUCCEEDED(m_debug_process.ClearHardwareBreakPoint(address, length))) {
+                        SetReplyOk(m_reply_packet);
+                    } else {
+                        SetReplyError(m_reply_packet, "E01");
+                    }
+                }
+                break;
+            case 2: /* Watch-W */
+            case 3: /* Watch-R */
+            case 4: /* Watch-A */
+                {
+                    if (R_SUCCEEDED(m_debug_process.ClearWatchPoint(address, length))) {
+                        SetReplyOk(m_reply_packet);
+                    } else {
+                        SetReplyError(m_reply_packet, "E01");
+                    }
+                }
+                break;
+            default:
+                break;
+        }
+    }
+
     void GdbServerImpl::QuestionMark() {
         if (m_debug_process.IsValid()) {
             if (m_debug_process.GetLastThreadId() == 0) {

stratosphere/dmnt.gen2/source/dmnt2_gdb_server_impl.hpp

@@ -65,6 +65,10 @@ namespace ams::dmnt {
             void H();
             void Hg();
 
+            void T();
+
+            void Z();
+
             bool g();
 
             void m();
@@ -72,6 +76,7 @@ namespace ams::dmnt {
             void v();
 
             void vAttach();
+            void vCont();
 
             void q();
 
@@ -84,7 +89,11 @@ namespace ams::dmnt {
             void qXferOsdataRead();
             bool qXferThreadsRead();
 
+            void z();
+
             void QuestionMark();
+        private:
+            Result ParseVCont(char * const token, u64 *thread_ids, u8 *continue_modes, s32 num_threads, DebugProcess::ContinueMode &default_continue_mode);
     };
 
 }

stratosphere/dmnt.gen2/source/dmnt2_hardware_breakpoint.cpp

@@ -0,0 +1,235 @@
+/*
+ * Copyright (c) 2018-2020 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 <stratosphere.hpp>
+#include "dmnt2_hardware_breakpoint.hpp"
+#include "dmnt2_debug_process.hpp"
+#include "dmnt2_debug_log.hpp"
+
+namespace ams::dmnt {
+
+    namespace {
+
+        constinit auto g_last_bp_register      = -1;
+        constinit auto g_first_bp_ctx_register = -1;
+        constinit auto g_last_bp_ctx_register  = -1;
+        constinit auto g_last_wp_register      = -1;
+
+        constinit os::SdkMutex g_multicore_lock;
+        constinit bool g_multicore_started = false;
+
+        alignas(os::ThreadStackAlignment) constinit u8 g_multicore_thread_stack[16_KB];
+        constinit os::ThreadType g_multicore_thread;
+        constinit os::MessageQueueType g_multicore_request_queue;
+        constinit os::MessageQueueType g_multicore_response_queue;
+        constinit uintptr_t g_multicore_request_storage[2];
+        constinit uintptr_t g_multicore_response_storage[2];
+
+        void MultiCoreThread(void *) {
+            /* Get thread core mask. */
+            s32 cur_core;
+            u64 core_mask;
+            R_ABORT_UNLESS(svc::GetThreadCoreMask(std::addressof(cur_core), std::addressof(core_mask), svc::PseudoHandle::CurrentThread));
+
+            /* Service requests. */
+            while (true) {
+                /* Wait for a request to come in. */
+                uintptr_t request_v;
+                os::ReceiveMessageQueue(std::addressof(request_v), std::addressof(g_multicore_request_queue));
+
+                /* Process the request. */
+                const uintptr_t *request = reinterpret_cast<const uintptr_t *>(request_v);
+                bool success = true;
+                if (request[0] == 1) {
+                    /* Set on each core. */
+                    for (s32 core = 0; core < 4; ++core) {
+                        /* Switch to the desired core. */
+                        R_ABORT_UNLESS(svc::SetThreadCoreMask(svc::PseudoHandle::CurrentThread, core, (1 << core)));
+
+                        /* Get the core mask. */
+                        R_ABORT_UNLESS(svc::GetThreadCoreMask(std::addressof(cur_core), std::addressof(core_mask), svc::PseudoHandle::CurrentThread));
+
+                        /* Set the breakpoint. */
+                        const Result result = svc::SetHardwareBreakPoint(static_cast<svc::HardwareBreakPointRegisterName>(request[1]), request[2], request[3]);
+                        if (R_FAILED(result)) {
+                            success = false;
+                            AMS_DMNT2_GDB_LOG_ERROR("SetHardwareBreakPoint FAIL 0x%08x, core=%d, reg=%lu, ctrl=%lx, val=%lx\n", result.GetValue(), core, request[1], request[2], request[3]);
+                            break;
+                        }
+                    }
+                }
+
+                os::SendMessageQueue(std::addressof(g_multicore_response_queue), static_cast<uintptr_t>(success));
+            }
+        }
+
+        void EnsureMultiCoreStarted() {
+            std::scoped_lock lk(g_multicore_lock);
+            if (!g_multicore_started) {
+                os::InitializeMessageQueue(std::addressof(g_multicore_request_queue), g_multicore_request_storage, util::size(g_multicore_request_storage));
+                os::InitializeMessageQueue(std::addressof(g_multicore_response_queue), g_multicore_response_storage, util::size(g_multicore_response_storage));
+
+                R_ABORT_UNLESS(os::CreateThread(std::addressof(g_multicore_thread), MultiCoreThread, nullptr, g_multicore_thread_stack, sizeof(g_multicore_thread_stack), os::HighestThreadPriority - 1));
+                os::StartThread(std::addressof(g_multicore_thread));
+
+                g_multicore_started = true;
+            }
+        }
+
+    }
+
+    Result HardwareBreakPoint::Clear(DebugProcess *debug_process) {
+        Result result = svc::ResultInvalidArgument();
+        if (m_in_use) {
+            AMS_DMNT2_GDB_LOG_DEBUG("HardwareBreakPoint::Clear %p 0x%lx\n", this, m_address);
+            result = HardwareBreakPointManager::SetExecutionBreakPoint(m_reg, m_ctx, 0);
+            this->Reset();
+        }
+        return result;
+    }
+
+    Result HardwareBreakPoint::Set(DebugProcess *debug_process, uintptr_t address, size_t size, bool is_step) {
+        /* Set fields. */
+        m_is_step = is_step;
+        m_address = address;
+        m_size    = size;
+
+        /* Set context breakpoint. */
+        R_TRY(HardwareBreakPointManager::SetContextBreakPoint(m_ctx, debug_process));
+
+        /* Set execution breakpoint. */
+        R_TRY(HardwareBreakPointManager::SetExecutionBreakPoint(m_reg, m_ctx, address));
+
+        /* Set as in-use. */
+        m_in_use = true;
+        return ResultSuccess();
+    }
+
+    HardwareBreakPointManager::HardwareBreakPointManager(DebugProcess *debug_process) : BreakPointManager(debug_process) {
+        /* Determine the number of breakpoint registers. */
+        CountBreakPointRegisters();
+
+        /* Initialize all breakpoints. */
+        for (size_t i = 0; i < util::size(m_breakpoints); ++i) {
+            m_breakpoints[i].Initialize(static_cast<svc::HardwareBreakPointRegisterName>(svc::HardwareBreakPointRegisterName_I0 + i), static_cast<svc::HardwareBreakPointRegisterName>(g_first_bp_ctx_register));
+        }
+    }
+
+    BreakPointBase *HardwareBreakPointManager::GetBreakPoint(size_t index) {
+        if (index < util::size(m_breakpoints)) {
+            return m_breakpoints + index;
+        } else {
+            return nullptr;
+        }
+    }
+
+    Result HardwareBreakPointManager::SetHardwareBreakPoint(u32 r, u64 dbgbcr, u64 value) {
+        /* Send request. */
+        const uintptr_t request[4] = {
+            1,
+            r,
+            dbgbcr,
+            value
+        };
+        R_UNLESS(SendMultiCoreRequest(request), dmnt::ResultUnknown());
+
+        return ResultSuccess();
+    }
+
+    Result HardwareBreakPointManager::SetContextBreakPoint(svc::HardwareBreakPointRegisterName ctx, DebugProcess *debug_process) {
+        /* Encode the register. */
+        const u64 dbgbcr = (0x3 << 20) | (0 << 16) | (0xF << 5) | 1;
+
+        const Result result = SetHardwareBreakPoint(ctx, dbgbcr, debug_process->GetHandle());
+        if (R_FAILED(result)) {
+            AMS_DMNT2_GDB_LOG_ERROR("SetContextBreakPoint FAIL 0x%08x ctx=%d\n", result.GetValue(), ctx);
+        }
+
+        return result;
+    }
+
+    svc::HardwareBreakPointRegisterName HardwareBreakPointManager::GetWatchPointContextRegister() {
+        CountBreakPointRegisters();
+        return static_cast<svc::HardwareBreakPointRegisterName>(g_first_bp_ctx_register + 1);
+    }
+
+    Result HardwareBreakPointManager::SetExecutionBreakPoint(svc::HardwareBreakPointRegisterName reg, svc::HardwareBreakPointRegisterName ctx, u64 address) {
+        /* Encode the register. */
+        const u64 dbgbcr = (0x1 << 20) | (ctx << 16) | (0xF << 5) | ((address != 0) ? 1 : 0);
+
+        const Result result = SetHardwareBreakPoint(reg, dbgbcr, address);
+        if (R_FAILED(result)) {
+            AMS_DMNT2_GDB_LOG_ERROR("SetContextBreakPoint FAIL 0x%08x reg=%d, ctx=%d, address=%lx\n", result.GetValue(), reg, ctx, address);
+        }
+
+        return result;
+    }
+
+    void HardwareBreakPointManager::CountBreakPointRegisters() {
+        /* Determine the valid breakpoint extents. */
+        if (g_last_bp_ctx_register == -1) {
+            /* Keep setting until we see a failure. */
+            for (int i = svc::HardwareBreakPointRegisterName_I0; i <= static_cast<int>(svc::HardwareBreakPointRegisterName_I15); ++i) {
+                if (R_FAILED(svc::SetHardwareBreakPoint(static_cast<svc::HardwareBreakPointRegisterName>(i), 0, 0))) {
+                    break;
+                }
+                g_last_bp_register = i;
+            }
+            AMS_DMNT2_GDB_LOG_DEBUG("Last valid breakpoint=%d\n", g_last_bp_register);
+
+            /* Determine the context register range. */
+            const u64 dbgbcr = (0x3 << 20) | (0x0 << 16) | (0xF << 5) | 1;
+
+            g_last_bp_ctx_register = g_last_bp_register;
+            for (int i = g_last_bp_ctx_register; i >= static_cast<int>(svc::HardwareBreakPointRegisterName_I0); --i) {
+                const Result result = svc::SetHardwareBreakPoint(static_cast<svc::HardwareBreakPointRegisterName>(i), dbgbcr, svc::PseudoHandle::CurrentProcess);
+                svc::SetHardwareBreakPoint(static_cast<svc::HardwareBreakPointRegisterName>(i), 0, 0);
+
+                if (R_FAILED(result)) {
+                    if (!svc::ResultInvalidHandle::Includes(result)) {
+                        break;
+                    }
+                }
+
+                g_first_bp_ctx_register = i;
+            }
+            AMS_DMNT2_GDB_LOG_DEBUG("Context BreakPoints = %d-%d\n", g_first_bp_ctx_register, g_last_bp_ctx_register);
+
+            /* Determine valid watchpoint registers. */
+            for (int i = svc::HardwareBreakPointRegisterName_D0; i <= static_cast<int>(svc::HardwareBreakPointRegisterName_D15); ++i) {
+                if (R_FAILED(svc::SetHardwareBreakPoint(static_cast<svc::HardwareBreakPointRegisterName>(i), 0, 0))) {
+                    break;
+                }
+                g_last_wp_register = i - svc::HardwareBreakPointRegisterName_D0;
+            }
+            AMS_DMNT2_GDB_LOG_DEBUG("Last valid watchpoint=%d\n", g_last_wp_register);
+        }
+    }
+
+    bool HardwareBreakPointManager::SendMultiCoreRequest(const void *request) {
+        /* Ensure the multi core thread is active. */
+        EnsureMultiCoreStarted();
+
+        /* Send the request. */
+        os::SendMessageQueue(std::addressof(g_multicore_request_queue), reinterpret_cast<uintptr_t>(request));
+
+        /* Get the response. */
+        uintptr_t response;
+        os::ReceiveMessageQueue(std::addressof(response), std::addressof(g_multicore_response_queue));
+
+        return static_cast<bool>(response);
+    }
+
+}

stratosphere/dmnt.gen2/source/dmnt2_hardware_breakpoint.hpp

@@ -0,0 +1,55 @@
+/*
+ * Copyright (c) 2018-2020 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 <stratosphere.hpp>
+#include "dmnt2_breakpoint_manager.hpp"
+
+namespace ams::dmnt {
+
+    struct HardwareBreakPoint : public BreakPoint {
+        svc::HardwareBreakPointRegisterName m_reg;
+        svc::HardwareBreakPointRegisterName m_ctx;
+
+        void Initialize(svc::HardwareBreakPointRegisterName r, svc::HardwareBreakPointRegisterName c) {
+            m_reg = r;
+            m_ctx = c;
+        }
+
+        virtual Result Clear(DebugProcess *debug_process) override;
+        virtual Result Set(DebugProcess *debug_process, uintptr_t address, size_t size, bool is_step) override;
+    };
+
+    class HardwareBreakPointManager : public BreakPointManager {
+        public:
+            static constexpr size_t BreakPointCountMax = 0x10;
+        private:
+            HardwareBreakPoint m_breakpoints[BreakPointCountMax];
+        public:
+            static Result SetHardwareBreakPoint(u32 r, u64 dbgbcr, u64 value);
+            static Result SetContextBreakPoint(svc::HardwareBreakPointRegisterName ctx, DebugProcess *debug_process);
+            static svc::HardwareBreakPointRegisterName GetWatchPointContextRegister();
+            static Result SetExecutionBreakPoint(svc::HardwareBreakPointRegisterName reg, svc::HardwareBreakPointRegisterName ctx, u64 address);
+        public:
+            explicit HardwareBreakPointManager(DebugProcess *debug_process);
+        private:
+            virtual BreakPointBase *GetBreakPoint(size_t index) override;
+        private:
+            static void CountBreakPointRegisters();
+
+            static bool SendMultiCoreRequest(const void *request);
+    };
+
+}

stratosphere/dmnt.gen2/source/dmnt2_hardware_watchpoint.cpp

@@ -0,0 +1,167 @@
+/*
+ * Copyright (c) 2018-2020 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 <stratosphere.hpp>
+#include "dmnt2_hardware_watchpoint.hpp"
+#include "dmnt2_hardware_breakpoint.hpp"
+#include "dmnt2_debug_process.hpp"
+#include "dmnt2_debug_log.hpp"
+
+namespace ams::dmnt {
+
+    namespace {
+
+        Result SetDataBreakPoint(svc::HardwareBreakPointRegisterName reg, svc::HardwareBreakPointRegisterName ctx, u64 address, u64 size, bool read, bool write) {
+            /* Determine lsc. */
+            const u8 lsc = (read ? 1 : 0) | (write ? 2 : 0);
+
+            /* Check that the watchpoint is valid. */
+            if (lsc != 0) {
+                R_UNLESS(HardwareWatchPointManager::IsValidWatchPoint(address, size), svc::ResultInvalidArgument());
+            }
+
+            /* Determine bas/mask. */
+            u8 bas = 0, mask = 0;
+            if (size <= 8) {
+                bas     = ((1 << size) - 1) << (address & 7);
+                address = util::AlignDown(address, 8);
+            } else {
+                bas  = 0xFF;
+                mask = util::PopCount(size - 1);
+            }
+
+            /* Build dbgbcr value. */
+            const u64 dbgbcr = (mask << 24) | (ctx << 16) | (bas << 5) | (lsc << 3) | ((lsc != 0) ? 1 : 0);
+
+            /* Set the breakpoint. */
+            const Result result = HardwareBreakPointManager::SetHardwareBreakPoint(reg, dbgbcr, address);
+            if (R_FAILED(result)) {
+                AMS_DMNT2_GDB_LOG_ERROR("SetDataBreakPoint FAIL 0x%08x, reg=%d, address=0x%lx\n", result.GetValue(), reg, address);
+            }
+            return result;
+        }
+
+    }
+
+    bool HardwareWatchPointManager::IsValidWatchPoint(u64 address, u64 size) {
+        /* Check size. */
+        if (size == 0) {
+            AMS_DMNT2_GDB_LOG_ERROR("HardwareWatchPointManager::IsValidWatchPoint(%lx, %lx) FAIL size == 0\n", address, size);
+            return false;
+        }
+
+        /* Validate. */
+        if (size <= 8) {
+            /* Check that address is aligned. */
+            if (util::AlignDown(address, 8) != util::AlignDown(address + size - 1, 8)) {
+                AMS_DMNT2_GDB_LOG_ERROR("HardwareWatchPointManager::IsValidWatchPoint(%lx, %lx) FAIL range crosses qword boundary\n", address, size);
+                return false;
+            }
+        } else {
+            /* Check size is small enough. */
+            if (size > 0x80000000) {
+                AMS_DMNT2_GDB_LOG_ERROR("HardwareWatchPointManager::IsValidWatchPoint(%lx, %lx) FAIL size too big\n", address, size);
+                return false;
+            }
+
+            /* Check size is power of two. */
+            if (!util::IsPowerOfTwo(size)) {
+                AMS_DMNT2_GDB_LOG_ERROR("HardwareWatchPointManager::IsValidWatchPoint(%lx, %lx) FAIL size not power of two\n", address, size);
+                return false;
+            }
+
+            /* Check alignment. */
+            if (!util::IsAligned(address, size)) {
+                AMS_DMNT2_GDB_LOG_ERROR("HardwareWatchPointManager::IsValidWatchPoint(%lx, %lx) FAIL address not size-aligned\n", address, size);
+                return false;
+            }
+        }
+
+        return true;
+    }
+
+    Result WatchPoint::Clear(DebugProcess *debug_process) {
+        Result result = svc::ResultInvalidArgument();
+        if (m_in_use) {
+            AMS_DMNT2_GDB_LOG_DEBUG("WatchPoint::Clear %p 0x%lx\n", this, m_address);
+            result = SetDataBreakPoint(m_reg, m_ctx, 0, 0, false, false);
+            this->Reset();
+        }
+        return result;
+    }
+
+    Result WatchPoint::Set(DebugProcess *debug_process, uintptr_t address, size_t size, bool read, bool write) {
+        /* Set fields. */
+        m_address = address;
+        m_size    = size;
+        m_read    = read;
+        m_write   = write;
+
+        /* Set context breakpoint. */
+        R_TRY(HardwareBreakPointManager::SetContextBreakPoint(m_ctx, debug_process));
+
+        /* Set watchpoint. */
+        R_TRY(SetDataBreakPoint(m_reg, m_ctx, address, size, read, write));
+
+        /* Set as in-use. */
+        m_in_use = true;
+        return ResultSuccess();
+    }
+
+    HardwareWatchPointManager::HardwareWatchPointManager(DebugProcess *debug_process) : BreakPointManagerBase(debug_process) {
+        const svc::HardwareBreakPointRegisterName ctx = HardwareBreakPointManager::GetWatchPointContextRegister();
+        for (size_t i = 0; i < util::size(m_breakpoints); ++i) {
+            m_breakpoints[i].Initialize(static_cast<svc::HardwareBreakPointRegisterName>(svc::HardwareBreakPointRegisterName_D0 + i), ctx);
+        }
+    }
+
+    BreakPointBase *HardwareWatchPointManager::GetBreakPoint(size_t index) {
+        if (index < util::size(m_breakpoints)) {
+            return m_breakpoints + index;
+        } else {
+            return nullptr;
+        }
+    }
+
+    Result HardwareWatchPointManager::SetWatchPoint(u64 address, u64 size, bool read, bool write) {
+        /* Get a free watchpoint. */
+        auto *bp = static_cast<WatchPoint *>(this->GetFreeBreakPoint());
+        R_UNLESS(bp != nullptr, svc::ResultOutOfHandles());
+
+        /* Set the watchpoint. */
+        return bp->Set(m_debug_process, address, size, read, write);
+    }
+
+    Result HardwareWatchPointManager::GetWatchPointInfo(u64 address, bool &read, bool &write) {
+        /* Find a matching watchpoint. */
+        for (const auto &bp : m_breakpoints) {
+            if (bp.m_in_use) {
+                if (bp.m_address <= address && address < bp.m_address + bp.m_size) {
+                    read  = bp.m_read;
+                    write = bp.m_write;
+                    return ResultSuccess();
+                }
+            }
+        }
+
+        /* Otherwise, we failed. */
+        AMS_DMNT2_GDB_LOG_ERROR("HardwareWatchPointManager::GetWatchPointInfo FAIL 0x%lx\n", address);
+        read  = false;
+        write = false;
+
+        return svc::ResultInvalidArgument();
+    }
+
+}

stratosphere/dmnt.gen2/source/dmnt2_hardware_watchpoint.hpp

@@ -0,0 +1,53 @@
+/*
+ * Copyright (c) 2018-2020 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 <stratosphere.hpp>
+#include "dmnt2_breakpoint_manager_base.hpp"
+
+namespace ams::dmnt {
+
+    struct WatchPoint : public BreakPointBase {
+        svc::HardwareBreakPointRegisterName m_reg;
+        svc::HardwareBreakPointRegisterName m_ctx;
+        bool m_read;
+        bool m_write;
+
+        void Initialize(svc::HardwareBreakPointRegisterName r, svc::HardwareBreakPointRegisterName c) {
+            m_reg = r;
+            m_ctx = c;
+        }
+
+        virtual Result Clear(DebugProcess *debug_process) override;
+        Result Set(DebugProcess *debug_process, uintptr_t address, size_t size, bool read, bool write);
+    };
+
+    class HardwareWatchPointManager : public BreakPointManagerBase {
+        public:
+            static constexpr size_t BreakPointCountMax = 0x10;
+        private:
+            WatchPoint m_breakpoints[BreakPointCountMax];
+        public:
+            static bool IsValidWatchPoint(u64 address, u64 size);
+        public:
+            explicit HardwareWatchPointManager(DebugProcess *debug_process);
+
+            Result SetWatchPoint(u64 address, u64 size, bool read, bool write);
+            Result GetWatchPointInfo(u64 address, bool &read, bool &write);
+        private:
+            virtual BreakPointBase *GetBreakPoint(size_t index) override;
+    };
+
+}

stratosphere/dmnt.gen2/source/dmnt2_software_breakpoint.cpp

@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2018-2020 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 <stratosphere.hpp>
+#include "dmnt2_software_breakpoint.hpp"
+#include "dmnt2_debug_process.hpp"
+#include "dmnt2_debug_log.hpp"
+
+namespace ams::dmnt {
+
+    namespace {
+
+        constexpr const u8 Aarch64BreakInstruction[]      = {0xFF, 0xFF, 0xFF, 0xE7};
+        constexpr const u8 Aarch32BreakInstruction[]      = {0xFE, 0xDE, 0xFF, 0xE7};
+        constexpr const u8 Aarch32ThumbBreakInstruction[] = {0x80, 0xB6};
+
+    }
+
+    Result SoftwareBreakPoint::Clear(DebugProcess *debug_process) {
+        Result result = svc::ResultInvalidArgument();
+        if (m_in_use) {
+            if (m_address) {
+                result = debug_process->WriteMemory(std::addressof(m_insn), m_address, m_size);
+                if (R_SUCCEEDED(result)) {
+                    AMS_DMNT2_GDB_LOG_DEBUG("SoftwareBreakPoint::Clear %p 0x%lx, insn=0x%x\n", this, m_address, m_insn);
+                } else {
+                    AMS_DMNT2_GDB_LOG_DEBUG("SoftwareBreakPoint::Clear %p 0x%lx, insn=0x%x, !!! Fail %08x !!!\n", this, m_address, m_insn, result.GetValue());
+                }
+                this->Reset();
+            } else {
+                AMS_DMNT2_GDB_LOG_ERROR("SoftwareBreakPoint::Clear %p 0x%lx, insn=0x%x, !!! Null Address !!!\n", this, m_address, m_insn);
+            }
+        }
+        return result;
+    }
+
+    Result SoftwareBreakPoint::Set(DebugProcess *debug_process, uintptr_t address, size_t size, bool is_step) {
+        /* Set fields. */
+        m_is_step = is_step;
+        m_address = address;
+        m_size    = size;
+
+        /* Read our instruction. */
+        Result result = debug_process->ReadMemory(std::addressof(m_insn), m_address, m_size);
+        AMS_DMNT2_GDB_LOG_DEBUG("SoftwareBreakPoint::Set %p 0x%lx, insn=0x%x\n", this, m_address, m_insn);
+
+        /* Set the breakpoint. */
+        if (debug_process->Is64Bit()) {
+            if (m_size == sizeof(Aarch64BreakInstruction)) {
+                result = debug_process->WriteMemory(Aarch64BreakInstruction, m_address, m_size);
+            } else {
+                result = svc::ResultInvalidArgument();
+            }
+        } else {
+            if (m_size == sizeof(Aarch32BreakInstruction) || m_size == sizeof(Aarch32ThumbBreakInstruction)) {
+                result = debug_process->WriteMemory(m_size == sizeof(Aarch32BreakInstruction) ? Aarch32BreakInstruction : Aarch32ThumbBreakInstruction, m_address, m_size);
+            } else {
+                result = svc::ResultInvalidArgument();
+            }
+        }
+
+        /* Check that we succeeded. */
+        if (R_SUCCEEDED(result)) {
+            m_in_use = true;
+        }
+
+        return result;
+    }
+
+    SoftwareBreakPointManager::SoftwareBreakPointManager(DebugProcess *debug_process) : BreakPointManager(debug_process) {
+        /* ... */
+    }
+
+    BreakPointBase *SoftwareBreakPointManager::GetBreakPoint(size_t index) {
+        if (index < util::size(m_breakpoints)) {
+            return m_breakpoints + index;
+        } else {
+            return nullptr;
+        }
+    }
+
+
+}

stratosphere/dmnt.gen2/source/dmnt2_software_breakpoint.hpp

@@ -0,0 +1,40 @@
+/*
+ * Copyright (c) 2018-2020 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 <stratosphere.hpp>
+#include "dmnt2_breakpoint_manager.hpp"
+
+namespace ams::dmnt {
+
+    struct SoftwareBreakPoint : public BreakPoint {
+        u32 m_insn;
+
+        virtual Result Clear(DebugProcess *debug_process) override;
+        virtual Result Set(DebugProcess *debug_process, uintptr_t address, size_t size, bool is_step) override;
+    };
+
+    class SoftwareBreakPointManager : public BreakPointManager {
+        public:
+            static constexpr size_t BreakPointCountMax = 0x80;
+        private:
+            SoftwareBreakPoint m_breakpoints[BreakPointCountMax];
+        public:
+            explicit SoftwareBreakPointManager(DebugProcess *debug_process);
+        private:
+            virtual BreakPointBase *GetBreakPoint(size_t index) override;
+    };
+
+}