From 583b01fa34e3507e49bb02495c24ff354734508b Mon Sep 17 00:00:00 2001
From: Nav <nav@navnav.me>
Date: Fri, 8 Apr 2022 22:18:52 +0100
Subject: [PATCH] New GDB Monitor command packet class, for "qRcmd" command
 packets

---
 src/DebugServer/CMakeLists.txt                |  1 +
 .../Gdb/CommandPackets/Monitor.cpp            | 27 +++++++++++++++++++
 .../Gdb/CommandPackets/Monitor.hpp            | 24 +++++++++++++++++
 src/DebugServer/Gdb/GdbRspDebugServer.cpp     |  7 +++++
 4 files changed, 59 insertions(+)
 create mode 100644 src/DebugServer/Gdb/CommandPackets/Monitor.cpp
 create mode 100644 src/DebugServer/Gdb/CommandPackets/Monitor.hpp

diff --git a/src/DebugServer/CMakeLists.txt b/src/DebugServer/CMakeLists.txt
index e6d550d9..d4ed26b6 100755
--- a/src/DebugServer/CMakeLists.txt
+++ b/src/DebugServer/CMakeLists.txt
@@ -15,6 +15,7 @@ target_sources(
         ${CMAKE_CURRENT_SOURCE_DIR}/Gdb/CommandPackets/InterruptExecution.cpp
         ${CMAKE_CURRENT_SOURCE_DIR}/Gdb/CommandPackets/SetBreakpoint.cpp
         ${CMAKE_CURRENT_SOURCE_DIR}/Gdb/CommandPackets/RemoveBreakpoint.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/Gdb/CommandPackets/Monitor.cpp
         ${CMAKE_CURRENT_SOURCE_DIR}/Gdb/ResponsePackets/SupportedFeaturesResponse.cpp
 
         # AVR GDB Server
diff --git a/src/DebugServer/Gdb/CommandPackets/Monitor.cpp b/src/DebugServer/Gdb/CommandPackets/Monitor.cpp
new file mode 100644
index 00000000..f38af34d
--- /dev/null
+++ b/src/DebugServer/Gdb/CommandPackets/Monitor.cpp
@@ -0,0 +1,27 @@
+#include "Monitor.hpp"
+
+#include "src/DebugServer/Gdb/ResponsePackets/EmptyResponsePacket.hpp"
+
+#include "src/Logger/Logger.hpp"
+
+namespace Bloom::DebugServer::Gdb::CommandPackets
+{
+    using ResponsePackets::EmptyResponsePacket;
+
+    Monitor::Monitor(const std::vector<unsigned char>& rawPacket)
+        : CommandPacket(rawPacket)
+    {
+        if (this->data.size() > 6) {
+            const auto decodedCommand = Packet::hexToData(
+                std::string(this->data.begin() + 6, this->data.end())
+            );
+
+            this->command = std::string(decodedCommand.begin(), decodedCommand.end());
+        }
+    }
+
+    void Monitor::handle(DebugSession& debugSession, TargetControllerConsole& targetControllerConsole) {
+        Logger::error("Unknown custom GDB command (" + this->command + ") received.");
+        debugSession.connection.writePacket(EmptyResponsePacket());
+    }
+}
diff --git a/src/DebugServer/Gdb/CommandPackets/Monitor.hpp b/src/DebugServer/Gdb/CommandPackets/Monitor.hpp
new file mode 100644
index 00000000..537f76d8
--- /dev/null
+++ b/src/DebugServer/Gdb/CommandPackets/Monitor.hpp
@@ -0,0 +1,24 @@
+#pragma once
+
+#include <cstdint>
+
+#include "CommandPacket.hpp"
+
+namespace Bloom::DebugServer::Gdb::CommandPackets
+{
+    /**
+     * This is a base class for 'qRcmd' packets - invoked by the GDB 'monitor' command.
+     */
+    class Monitor: public CommandPacket
+    {
+    public:
+        /**
+         * The decoded command string which was input by the GDB user.
+         */
+        std::string command;
+
+        explicit Monitor(const std::vector<unsigned char>& rawPacket);
+
+        void handle(DebugSession& debugSession, TargetControllerConsole& targetControllerConsole) override;
+    };
+}
diff --git a/src/DebugServer/Gdb/GdbRspDebugServer.cpp b/src/DebugServer/Gdb/GdbRspDebugServer.cpp
index ede0595e..4375b4cc 100644
--- a/src/DebugServer/Gdb/GdbRspDebugServer.cpp
+++ b/src/DebugServer/Gdb/GdbRspDebugServer.cpp
@@ -24,6 +24,7 @@
 #include "CommandPackets/WriteRegister.hpp"
 #include "CommandPackets/SetBreakpoint.hpp"
 #include "CommandPackets/RemoveBreakpoint.hpp"
+#include "CommandPackets/Monitor.hpp"
 
 // Response packets
 #include "ResponsePackets/TargetStopped.hpp"
@@ -265,6 +266,12 @@ namespace Bloom::DebugServer::Gdb
             if (rawPacketString[1] == 'z') {
                 return std::make_unique<CommandPackets::RemoveBreakpoint>(rawPacket);
             }
+
+            if (rawPacketString.find("qRcmd") == 1) {
+                // This is a monitor packet
+                auto monitorCommand = std::make_unique<CommandPackets::Monitor>(rawPacket);
+                return monitorCommand;
+            }
         }
 
         return std::make_unique<CommandPacket>(rawPacket);