2 files changed, 153 insertions, 26 deletions

diff --git a/src/core/gdbstub/gdbstub.cpp b/src/core/gdbstub/gdbstub.cpp
index 6c5a40ba8..2603192fe 100644
--- a/src/core/gdbstub/gdbstub.cpp
+++ b/src/core/gdbstub/gdbstub.cpp
@@ -32,9 +32,13 @@
 
 #include "common/logging/log.h"
 #include "common/string_util.h"
+#include "common/swap.h"
 #include "core/arm/arm_interface.h"
 #include "core/core.h"
+#include "core/core_cpu.h"
 #include "core/gdbstub/gdbstub.h"
+#include "core/hle/kernel/kernel.h"
+#include "core/hle/kernel/scheduler.h"
 #include "core/loader/loader.h"
 #include "core/memory.h"
 
@@ -137,15 +141,17 @@ static u8 command_buffer[GDB_BUFFER_SIZE];
 static u32 command_length;
 
 static u32 latest_signal = 0;
-static bool step_break = false;
 static bool memory_break = false;
 
+static Kernel::Thread* current_thread = nullptr;
+
 // Binding to a port within the reserved ports range (0-1023) requires root permissions,
 // so default to a port outside of that range.
 static u16 gdbstub_port = 24689;
 
 static bool halt_loop = true;
 static bool step_loop = false;
+static bool send_trap = false;
 
 // If set to false, the server will never be started and no
 // gdbstub-related functions will be executed.
@@ -165,6 +171,53 @@ static std::map<u64, Breakpoint> breakpoints_execute;
 static std::map<u64, Breakpoint> breakpoints_read;
 static std::map<u64, Breakpoint> breakpoints_write;
 
+static Kernel::Thread* FindThreadById(int id) {
+    for (int core = 0; core < Core::NUM_CPU_CORES; core++) {
+        auto threads = Core::System::GetInstance().Scheduler(core)->GetThreadList();
+        for (auto thread : threads) {
+            if (thread->GetThreadId() == id) {
+                current_thread = thread.get();
+                return current_thread;
+            }
+        }
+    }
+    return nullptr;
+}
+
+static u64 RegRead(int id, Kernel::Thread* thread = nullptr) {
+    if (!thread) {
+        return 0;
+    }
+
+    if (id < SP_REGISTER) {
+        return thread->context.cpu_registers[id];
+    } else if (id == SP_REGISTER) {
+        return thread->context.sp;
+    } else if (id == PC_REGISTER) {
+        return thread->context.pc;
+    } else if (id == CPSR_REGISTER) {
+        return thread->context.cpsr;
+    } else {
+        return 0;
+    }
+}
+
+static void RegWrite(int id, u64 val, Kernel::Thread* thread = nullptr) {
+    if (!thread) {
+        return;
+    }
+
+    if (id < SP_REGISTER) {
+        thread->context.cpu_registers[id] = val;
+    } else if (id == SP_REGISTER) {
+        thread->context.sp = val;
+    } else if (id == PC_REGISTER) {
+        thread->context.pc = val;
+    } else if (id == CPSR_REGISTER) {
+        thread->context.cpsr = val;
+    }
+}
+
 /**
  * Turns hex string character into the equivalent byte.
  *
@@ -193,7 +246,7 @@ static u8 NibbleToHex(u8 n) {
     if (n < 0xA) {
         return '0' + n;
     } else {
-        return 'A' + n - 0xA;
+        return 'a' + n - 0xA;
     }
 }
 
@@ -439,6 +492,8 @@ static void SendReply(const char* reply) {
         return;
     }
 
+    NGLOG_DEBUG(Debug_GDBStub, "Reply: {}", reply);
+
     memset(command_buffer, 0, sizeof(command_buffer));
 
     command_length = static_cast<u32>(strlen(reply));
@@ -483,6 +538,22 @@ static void HandleQuery() {
     } else if (strncmp(query, "Xfer:features:read:target.xml:",
                        strlen("Xfer:features:read:target.xml:")) == 0) {
         SendReply(target_xml);
+    } else if (strncmp(query, "Offsets", strlen("Offsets")) == 0) {
+        std::string buffer = fmt::format("TextSeg={:0x}", Memory::PROCESS_IMAGE_VADDR);
+        SendReply(buffer.c_str());
+    } else if (strncmp(query, "fThreadInfo", strlen("fThreadInfo")) == 0) {
+        std::string val = "m";
+        for (int core = 0; core < Core::NUM_CPU_CORES; core++) {
+            auto threads = Core::System::GetInstance().Scheduler(core)->GetThreadList();
+            for (auto thread : threads) {
+                val += fmt::format("{:x}", thread->GetThreadId());
+                val += ",";
+            }
+        }
+        val.pop_back();
+        SendReply(val.c_str());
+    } else if (strncmp(query, "sThreadInfo", strlen("sThreadInfo")) == 0) {
+        SendReply("l");
     } else {
         SendReply("");
     }
@@ -490,11 +561,40 @@ static void HandleQuery() {
 
 /// Handle set thread command from gdb client.
 static void HandleSetThread() {
-    if (memcmp(command_buffer, "Hg0", 3) == 0 || memcmp(command_buffer, "Hc-1", 4) == 0 ||
-        memcmp(command_buffer, "Hc0", 4) == 0 || memcmp(command_buffer, "Hc1", 4) == 0) {
-        return SendReply("OK");
+    if (memcmp(command_buffer, "Hc", 2) == 0 || memcmp(command_buffer, "Hg", 2) == 0) {
+        int thread_id = -1;
+        if (command_buffer[2] != '-') {
+            thread_id = static_cast<int>(HexToInt(
+                command_buffer + 2,
+                command_length - 2 /*strlen(reinterpret_cast<char*>(command_buffer) + 2)*/));
+        }
+        if (thread_id >= 1) {
+            current_thread = FindThreadById(thread_id);
+        }
+        if (!current_thread) {
+            thread_id = 1;
+            current_thread = FindThreadById(thread_id);
+        }
+        if (current_thread) {
+            SendReply("OK");
+            return;
+        }
     }
+    SendReply("E01");
+}
 
+/// Handle thread alive command from gdb client.
+static void HandleThreadAlive() {
+    int thread_id = static_cast<int>(
+        HexToInt(command_buffer + 1,
+                 command_length - 1 /*strlen(reinterpret_cast<char*>(command_buffer) + 1)*/));
+    if (thread_id == 0) {
+        thread_id = 1;
+    }
+    if (FindThreadById(thread_id)) {
+        SendReply("OK");
+        return;
+    }
     SendReply("E01");
 }
 
@@ -503,15 +603,24 @@ static void HandleSetThread() {
  *
  * @param signal Signal to be sent to client.
  */
-static void SendSignal(u32 signal) {
+static void SendSignal(Kernel::Thread* thread, u32 signal, bool full = true) {
     if (gdbserver_socket == -1) {
         return;
     }
 
     latest_signal = signal;
 
-    std::string buffer = fmt::format("T{:02x}", latest_signal);
-    NGLOG_DEBUG(Debug_GDBStub, "Response: {}", buffer);
+    std::string buffer;
+    if (full) {
+        buffer = fmt::format("T{:02x}{:02x}:{:016x};{:02x}:{:016x};", latest_signal, PC_REGISTER,
+                             Common::swap64(RegRead(PC_REGISTER, thread)), SP_REGISTER,
+                             Common::swap64(RegRead(SP_REGISTER, thread)));
+    } else {
+        buffer = fmt::format("T{:02x};", latest_signal);
+    }
+
+    buffer += fmt::format("thread:{:x};", thread->GetThreadId());
+
     SendReply(buffer.c_str());
 }
 
@@ -527,7 +636,7 @@ static void ReadCommand() {
     } else if (c == 0x03) {
         NGLOG_INFO(Debug_GDBStub, "gdb: found break command");
         halt_loop = true;
-        SendSignal(SIGTRAP);
+        SendSignal(current_thread, SIGTRAP);
         return;
     } else if (c != GDB_STUB_START) {
         NGLOG_DEBUG(Debug_GDBStub, "gdb: read invalid byte {:02X}", c);
@@ -598,11 +707,11 @@ static void ReadRegister() {
     }
 
     if (id <= SP_REGISTER) {
-        LongToGdbHex(reply, Core::CurrentArmInterface().GetReg(static_cast<int>(id)));
+        LongToGdbHex(reply, RegRead(id, current_thread));
     } else if (id == PC_REGISTER) {
-        LongToGdbHex(reply, Core::CurrentArmInterface().GetPC());
+        LongToGdbHex(reply, RegRead(id, current_thread));
     } else if (id == CPSR_REGISTER) {
-        IntToGdbHex(reply, Core::CurrentArmInterface().GetCPSR());
+        IntToGdbHex(reply, (u32)RegRead(id, current_thread));
     } else {
         return SendReply("E01");
     }
@@ -618,16 +727,16 @@ static void ReadRegisters() {
     u8* bufptr = buffer;
 
     for (int reg = 0; reg <= SP_REGISTER; reg++) {
-        LongToGdbHex(bufptr + reg * 16, Core::CurrentArmInterface().GetReg(reg));
+        LongToGdbHex(bufptr + reg * 16, RegRead(reg, current_thread));
     }
 
     bufptr += (32 * 16);
 
-    LongToGdbHex(bufptr, Core::CurrentArmInterface().GetPC());
+    LongToGdbHex(bufptr, RegRead(PC_REGISTER, current_thread));
 
     bufptr += 16;
 
-    IntToGdbHex(bufptr, Core::CurrentArmInterface().GetCPSR());
+    IntToGdbHex(bufptr, (u32)RegRead(CPSR_REGISTER, current_thread));
 
     bufptr += 8;
 
@@ -646,11 +755,11 @@ static void WriteRegister() {
     }
 
     if (id <= SP_REGISTER) {
-        Core::CurrentArmInterface().SetReg(id, GdbHexToLong(buffer_ptr));
+        RegWrite(id, GdbHexToLong(buffer_ptr), current_thread);
     } else if (id == PC_REGISTER) {
-        Core::CurrentArmInterface().SetPC(GdbHexToLong(buffer_ptr));
+        RegWrite(id, GdbHexToLong(buffer_ptr), current_thread);
     } else if (id == CPSR_REGISTER) {
-        Core::CurrentArmInterface().SetCPSR(GdbHexToInt(buffer_ptr));
+        RegWrite(id, GdbHexToInt(buffer_ptr), current_thread);
     } else {
         return SendReply("E01");
     }
@@ -667,11 +776,11 @@ static void WriteRegisters() {
 
     for (int i = 0, reg = 0; reg <= CPSR_REGISTER; i++, reg++) {
         if (reg <= SP_REGISTER) {
-            Core::CurrentArmInterface().SetReg(reg, GdbHexToLong(buffer_ptr + i * 16));
+            RegWrite(reg, GdbHexToLong(buffer_ptr + i * 16), current_thread);
         } else if (reg == PC_REGISTER) {
-            Core::CurrentArmInterface().SetPC(GdbHexToLong(buffer_ptr + i * 16));
+            RegWrite(PC_REGISTER, GdbHexToLong(buffer_ptr + i * 16), current_thread);
         } else if (reg == CPSR_REGISTER) {
-            Core::CurrentArmInterface().SetCPSR(GdbHexToInt(buffer_ptr + i * 16));
+            RegWrite(CPSR_REGISTER, GdbHexToInt(buffer_ptr + i * 16), current_thread);
         } else {
             UNIMPLEMENTED();
         }
@@ -734,7 +843,7 @@ static void WriteMemory() {
 void Break(bool is_memory_break) {
     if (!halt_loop) {
         halt_loop = true;
-        SendSignal(SIGTRAP);
+        send_trap = true;
     }
 
     memory_break = is_memory_break;
@@ -744,10 +853,10 @@ void Break(bool is_memory_break) {
 static void Step() {
     step_loop = true;
     halt_loop = true;
-    step_break = true;
-    SendSignal(SIGTRAP);
+    send_trap = true;
 }
 
+/// Tell the CPU if we hit a memory breakpoint.
 bool IsMemoryBreak() {
     if (IsConnected()) {
         return false;
@@ -759,7 +868,6 @@ bool IsMemoryBreak() {
 /// Tell the CPU to continue executing.
 static void Continue() {
     memory_break = false;
-    step_break = false;
     step_loop = false;
     halt_loop = false;
 }
@@ -898,7 +1006,7 @@ void HandlePacket() {
         HandleSetThread();
         break;
     case '?':
-        SendSignal(latest_signal);
+        SendSignal(current_thread, latest_signal);
         break;
     case 'k':
         Shutdown();
@@ -935,6 +1043,9 @@ void HandlePacket() {
     case 'Z':
         AddBreakpoint();
         break;
+    case 'T':
+        HandleThreadAlive();
+        break;
     default:
         SendReply("");
         break;
@@ -1079,4 +1190,11 @@ bool GetCpuStepFlag() {
 void SetCpuStepFlag(bool is_step) {
     step_loop = is_step;
 }
+
+void SendTrap(Kernel::Thread* thread, int trap) {
+    if (send_trap) {
+        send_trap = false;
+        SendSignal(thread, trap);
+    }
+}
 }; // namespace GDBStub
diff --git a/src/core/gdbstub/gdbstub.h b/src/core/gdbstub/gdbstub.h
index 201fca095..f2418c9e4 100644
--- a/src/core/gdbstub/gdbstub.h
+++ b/src/core/gdbstub/gdbstub.h
@@ -7,6 +7,7 @@
 #pragma once
 
 #include "common/common_types.h"
+#include "core/hle/kernel/thread.h"
 
 namespace GDBStub {
 
@@ -91,4 +92,12 @@ bool GetCpuStepFlag();
  * @param is_step
  */
 void SetCpuStepFlag(bool is_step);
+
+/**
+ * Send trap signal from thread back to the gdbstub server.
+ *
+ * @param thread Sending thread.
+ * @param trap Trap no.
+ */
+void SendTrap(Kernel::Thread* thread, int trap);
 } // namespace GDBStub