Moved GDB command packet handling to individual CommandPacket classes

src/DebugServers/GdbRsp/CommandPackets/WriteRegister.cpp

@@ -1,15 +1,24 @@
 #include "WriteRegister.hpp"
 
-#include "src/DebugServers/GdbRsp/GdbRspDebugServer.hpp"
+#include "src/DebugServers/GdbRsp/ResponsePackets/TargetStopped.hpp"
+#include "src/DebugServers/GdbRsp/ResponsePackets/OkResponsePacket.hpp"
+#include "src/DebugServers/GdbRsp/ResponsePackets/ErrorResponsePacket.hpp"
+
+#include "src/Targets/TargetRegister.hpp"
+
+#include "src/Logger/Logger.hpp"
 #include "src/Exceptions/Exception.hpp"
 
 namespace Bloom::DebugServers::Gdb::CommandPackets
 {
-    using namespace Bloom::Exceptions;
+    using Targets::TargetRegister;
+    using Targets::TargetRegisterDescriptors;
 
-    void WriteRegister::dispatchToHandler(Gdb::GdbRspDebugServer& gdbRspDebugServer) {
-        gdbRspDebugServer.handleGdbPacket(*this);
-    }
+    using ResponsePackets::ResponsePacket;
+    using ResponsePackets::OkResponsePacket;
+    using ResponsePackets::ErrorResponsePacket;
+
+    using Exceptions::Exception;
 
     void WriteRegister::init() {
         // The P packet updates a single register
@@ -28,4 +37,41 @@ namespace Bloom::DebugServers::Gdb::CommandPackets
         this->registerValue = Packet::hexToData(packetSegments.back().toStdString());
         std::reverse(this->registerValue.begin(), this->registerValue.end());
     }
+
+    void WriteRegister::handle(DebugSession& debugSession, TargetControllerConsole& targetControllerConsole) {
+        Logger::debug("Handling WriteRegister packet");
+
+        try {
+            auto targetRegisterDescriptor = debugSession.targetDescriptor.getTargetRegisterDescriptorFromNumber(this->registerNumber);
+
+            const auto valueSize = this->registerValue.size();
+            if (valueSize > 0 && valueSize > targetRegisterDescriptor.size) {
+                // Attempt to trim the higher zero-value bytes from the register value, until we reach the correct size.
+                for (auto i = this->registerValue.size() - 1; i >= targetRegisterDescriptor.size; i--) {
+                    if (this->registerValue.at(i) != 0x00) {
+                        // If we reach a non-zero byte, we cannot trim anymore without changing the data
+                        break;
+                    }
+
+                    this->registerValue.erase(this->registerValue.begin() + i);
+                }
+
+                if (this->registerValue.size() > targetRegisterDescriptor.size) {
+                    const auto& gdbRegisterDescriptor = debugSession.targetDescriptor.getRegisterDescriptorFromNumber(this->registerNumber);
+                    throw Exception("Cannot set value for " + gdbRegisterDescriptor.name
+                        + " - value size exceeds register size."
+                    );
+                }
+            }
+
+            targetControllerConsole.writeRegisters({
+                TargetRegister(targetRegisterDescriptor, this->registerValue)
+            });
+            debugSession.connection.writePacket(OkResponsePacket());
+
+        } catch (const Exception& exception) {
+            Logger::error("Failed to write registers - " + exception.getMessage());
+            debugSession.connection.writePacket(ErrorResponsePacket());
+        }
+    }
 }