From 8a6f1e8659db2769c6fcb3c12fa809e27121d93b Mon Sep 17 00:00:00 2001
From: Nav <nav@navnav.me>
Date: Sun, 7 May 2023 19:44:19 +0100
Subject: [PATCH] Fixed bug with GDB interrupts not being serviced properly

---
 .../Gdb/CommandPackets/InterruptExecution.cpp |  5 ++
 src/DebugServer/Gdb/DebugSession.hpp          |  2 +
 src/DebugServer/Gdb/GdbRspDebugServer.cpp     | 48 +++++++++++++++++++
 src/DebugServer/Gdb/GdbRspDebugServer.hpp     |  6 +++
 4 files changed, 61 insertions(+)

diff --git a/src/DebugServer/Gdb/CommandPackets/InterruptExecution.cpp b/src/DebugServer/Gdb/CommandPackets/InterruptExecution.cpp
index ea15a6a4..0cac58fb 100644
--- a/src/DebugServer/Gdb/CommandPackets/InterruptExecution.cpp
+++ b/src/DebugServer/Gdb/CommandPackets/InterruptExecution.cpp
@@ -18,6 +18,11 @@ namespace Bloom::DebugServer::Gdb::CommandPackets
     void InterruptExecution::handle(DebugSession& debugSession, TargetControllerService& targetControllerService) {
         Logger::debug("Handling InterruptExecution packet");
 
+        if (targetControllerService.getTargetState() == Targets::TargetState::STOPPED) {
+            debugSession.pendingInterrupt = true;
+            return;
+        }
+
         try {
             targetControllerService.stopTargetExecution();
             debugSession.connection.writePacket(TargetStopped(Signal::INTERRUPTED));
diff --git a/src/DebugServer/Gdb/DebugSession.hpp b/src/DebugServer/Gdb/DebugSession.hpp
index 715efda1..53d46bd3 100644
--- a/src/DebugServer/Gdb/DebugSession.hpp
+++ b/src/DebugServer/Gdb/DebugSession.hpp
@@ -43,6 +43,8 @@ namespace Bloom::DebugServer::Gdb
          */
         bool waitingForBreak = false;
 
+        bool pendingInterrupt = false;
+
         /**
          * When the user attempts to program the target via GDB's 'load' command, GDB will send a number of
          * FlashWrite (vFlashWrite) packets to Bloom. We group the data in these packets and flush it all at once, upon
diff --git a/src/DebugServer/Gdb/GdbRspDebugServer.cpp b/src/DebugServer/Gdb/GdbRspDebugServer.cpp
index eb4ba5c1..f0f7e9b7 100644
--- a/src/DebugServer/Gdb/GdbRspDebugServer.cpp
+++ b/src/DebugServer/Gdb/GdbRspDebugServer.cpp
@@ -127,6 +127,10 @@ namespace Bloom::DebugServer::Gdb
             std::bind(&GdbRspDebugServer::onTargetExecutionStopped, this, std::placeholders::_1)
         );
 
+        this->eventListener.registerCallbackForEventType<Events::TargetExecutionResumed>(
+            std::bind(&GdbRspDebugServer::onTargetExecutionResumed, this, std::placeholders::_1)
+        );
+
         if (Services::ProcessService::isManagedByClion()) {
             Logger::warning(
                 "Bloom's process is being managed by CLion - Bloom will automatically shutdown upon detaching from GDB."
@@ -241,6 +245,13 @@ namespace Bloom::DebugServer::Gdb
         const auto rawPackets = this->activeDebugSession->connection.readRawPackets();
 
         if (rawPackets.size() > 1) {
+            const auto& firstRawPacket = rawPackets.front();
+
+            if (firstRawPacket.size() == 5 && firstRawPacket[1] == 0x03) {
+                // Interrupt packet that came in too quickly before another packet
+                this->activeDebugSession->pendingInterrupt = true;
+            }
+
             // We only process the last packet - any others will probably be duplicates from an impatient client.
             Logger::warning("Multiple packets received from GDB - only the most recent will be processed");
         }
@@ -368,4 +379,41 @@ namespace Bloom::DebugServer::Gdb
             return;
         }
     }
+
+    void GdbRspDebugServer::onTargetExecutionResumed(const Events::TargetExecutionResumed&) {
+        try {
+            if (
+                this->activeDebugSession.has_value()
+                && this->activeDebugSession->pendingInterrupt
+                && this->activeDebugSession->waitingForBreak
+            ) {
+                Logger::info("Servicing pending interrupt");
+                this->targetControllerService.stopTargetExecution();
+
+                this->activeDebugSession->connection.writePacket(
+                    ResponsePackets::TargetStopped(Signal::INTERRUPTED)
+                );
+
+                this->activeDebugSession->pendingInterrupt = false;
+                this->activeDebugSession->waitingForBreak = false;
+            }
+
+        } catch (const ClientDisconnected&) {
+            Logger::info("GDB RSP client disconnected");
+            this->activeDebugSession.reset();
+            return;
+
+        } catch (const ClientCommunicationError& exception) {
+            Logger::error(
+                "GDB RSP client communication error - " + exception.getMessage() + " - closing connection"
+            );
+            this->activeDebugSession.reset();
+            return;
+
+        } catch (const DebugServerInterrupted&) {
+            // Server was interrupted
+            Logger::debug("GDB RSP interrupted");
+            return;
+        }
+    }
 }
diff --git a/src/DebugServer/Gdb/GdbRspDebugServer.hpp b/src/DebugServer/Gdb/GdbRspDebugServer.hpp
index 983ad0b4..17ba2b8c 100644
--- a/src/DebugServer/Gdb/GdbRspDebugServer.hpp
+++ b/src/DebugServer/Gdb/GdbRspDebugServer.hpp
@@ -25,6 +25,7 @@
 
 #include "src/EventManager/Events/TargetControllerStateChanged.hpp"
 #include "src/EventManager/Events/TargetExecutionStopped.hpp"
+#include "src/EventManager/Events/TargetExecutionResumed.hpp"
 
 namespace Bloom::DebugServer::Gdb
 {
@@ -197,5 +198,10 @@ namespace Bloom::DebugServer::Gdb
          * a "stop reply" packet to the client once the target execution stops.
          */
         void onTargetExecutionStopped(const Events::TargetExecutionStopped&);
+
+        /**
+         * Services any pending interrupts.
+         */
+        void onTargetExecutionResumed(const Events::TargetExecutionResumed&);
     };
 }