4 files changed, 26 insertions, 2 deletions

diff --git a/src/core/arm/unicorn/arm_unicorn.cpp b/src/core/arm/unicorn/arm_unicorn.cpp
index 4c11f35a4..6bc349460 100644
--- a/src/core/arm/unicorn/arm_unicorn.cpp
+++ b/src/core/arm/unicorn/arm_unicorn.cpp
@@ -203,7 +203,7 @@ void ARM_Unicorn::ExecuteInstructions(int num_instructions) {
         }
         Kernel::Thread* thread = Kernel::GetCurrentThread();
         SaveContext(thread->context);
-        if (last_bkpt_hit || (num_instructions == 1)) {
+        if (last_bkpt_hit || GDBStub::GetCpuStepFlag()) {
             last_bkpt_hit = false;
             GDBStub::Break();
             GDBStub::SendTrap(thread, 5);
diff --git a/src/core/core.cpp b/src/core/core.cpp
index e01c45cdd..085ba68d0 100644
--- a/src/core/core.cpp
+++ b/src/core/core.cpp
@@ -62,7 +62,6 @@ System::ResultStatus System::RunLoop(bool tight_loop) {
         // execute. Otherwise, get out of the loop function.
         if (GDBStub::GetCpuHaltFlag()) {
             if (GDBStub::GetCpuStepFlag()) {
-                GDBStub::SetCpuStepFlag(false);
                 tight_loop = false;
             } else {
                 return ResultStatus::Success;
@@ -78,6 +77,10 @@ System::ResultStatus System::RunLoop(bool tight_loop) {
         }
     }
 
+    if (GDBStub::IsServerEnabled()) {
+        GDBStub::SetCpuStepFlag(false);
+    }
+
     return status;
 }
 
diff --git a/src/core/core.h b/src/core/core.h
index a3be88aa8..c8ca4b247 100644
--- a/src/core/core.h
+++ b/src/core/core.h
@@ -82,6 +82,17 @@ public:
      */
     ResultStatus SingleStep();
 
+    /**
+     * Invalidate the CPU instruction caches
+     * This function should only be used by GDB Stub to support breakpoints, memory updates and
+     * step/continue commands.
+     */
+    void InvalidateCpuInstructionCaches() {
+        for (auto& cpu : cpu_cores) {
+            cpu->ArmInterface().ClearInstructionCache();
+        }
+    }
+
     /// Shutdown the emulated system.
     void Shutdown();
 
diff --git a/src/core/gdbstub/gdbstub.cpp b/src/core/gdbstub/gdbstub.cpp
index 884e64e99..332e5c3d0 100644
--- a/src/core/gdbstub/gdbstub.cpp
+++ b/src/core/gdbstub/gdbstub.cpp
@@ -173,6 +173,7 @@ struct Breakpoint {
     bool active;
     VAddr addr;
     u64 len;
+    std::array<u8, 4> inst;
 };
 
 using BreakpointMap = std::map<VAddr, Breakpoint>;
@@ -453,6 +454,8 @@ static void RemoveBreakpoint(BreakpointType type, VAddr addr) {
 
     LOG_DEBUG(Debug_GDBStub, "gdb: removed a breakpoint: {:016X} bytes at {:016X} of type {}",
               bp->second.len, bp->second.addr, static_cast<int>(type));
+    Memory::WriteBlock(bp->second.addr, bp->second.inst.data(), bp->second.inst.size());
+    Core::System::GetInstance().InvalidateCpuInstructionCaches();
     p.erase(addr);
 }
 
@@ -937,6 +940,7 @@ static void WriteMemory() {
 
     GdbHexToMem(data.data(), len_pos + 1, len);
     Memory::WriteBlock(addr, data.data(), len);
+    Core::System::GetInstance().InvalidateCpuInstructionCaches();
     SendReply("OK");
 }
 
@@ -956,6 +960,7 @@ static void Step() {
     step_loop = true;
     halt_loop = true;
     send_trap = true;
+    Core::System::GetInstance().InvalidateCpuInstructionCaches();
 }
 
 /// Tell the CPU if we hit a memory breakpoint.
@@ -972,6 +977,7 @@ static void Continue() {
     memory_break = false;
     step_loop = false;
     halt_loop = false;
+    Core::System::GetInstance().InvalidateCpuInstructionCaches();
 }
 
 /**
@@ -988,6 +994,10 @@ static bool CommitBreakpoint(BreakpointType type, VAddr addr, u64 len) {
     breakpoint.active = true;
     breakpoint.addr = addr;
     breakpoint.len = len;
+    Memory::ReadBlock(addr, breakpoint.inst.data(), breakpoint.inst.size());
+    static constexpr std::array<u8, 4> btrap{{0xd4, 0x20, 0x7d, 0x0}};
+    Memory::WriteBlock(addr, btrap.data(), btrap.size());
+    Core::System::GetInstance().InvalidateCpuInstructionCaches();
     p.insert({addr, breakpoint});
 
     LOG_DEBUG(Debug_GDBStub, "gdb: added {} breakpoint: {:016X} bytes at {:016X}",