From 1a541dcd17f5c4e3038c42c9b2f6a0bee698fddc Mon Sep 17 00:00:00 2001
From: Nav <nav@navnav.me>
Date: Sun, 29 May 2022 17:18:29 +0100
Subject: [PATCH] Support for vFlashWrite GDB command packet

---
 src/DebugServer/CMakeLists.txt                |  1 +
 src/DebugServer/Gdb/AvrGdb/AvrGdbRsp.cpp      |  6 ++
 .../Gdb/AvrGdb/CommandPackets/FlashWrite.cpp  | 68 +++++++++++++++++++
 .../Gdb/AvrGdb/CommandPackets/FlashWrite.hpp  | 27 ++++++++
 4 files changed, 102 insertions(+)
 create mode 100644 src/DebugServer/Gdb/AvrGdb/CommandPackets/FlashWrite.cpp
 create mode 100644 src/DebugServer/Gdb/AvrGdb/CommandPackets/FlashWrite.hpp

diff --git a/src/DebugServer/CMakeLists.txt b/src/DebugServer/CMakeLists.txt
index 523aa56b..4bfd2c21 100755
--- a/src/DebugServer/CMakeLists.txt
+++ b/src/DebugServer/CMakeLists.txt
@@ -32,4 +32,5 @@ target_sources(
         ${CMAKE_CURRENT_SOURCE_DIR}/Gdb/AvrGdb/CommandPackets/WriteMemory.cpp
         ${CMAKE_CURRENT_SOURCE_DIR}/Gdb/AvrGdb/CommandPackets/ReadMemoryMap.cpp
         ${CMAKE_CURRENT_SOURCE_DIR}/Gdb/AvrGdb/CommandPackets/FlashErase.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/Gdb/AvrGdb/CommandPackets/FlashWrite.cpp
 )
diff --git a/src/DebugServer/Gdb/AvrGdb/AvrGdbRsp.cpp b/src/DebugServer/Gdb/AvrGdb/AvrGdbRsp.cpp
index 2b0d605f..e3092236 100644
--- a/src/DebugServer/Gdb/AvrGdb/AvrGdbRsp.cpp
+++ b/src/DebugServer/Gdb/AvrGdb/AvrGdbRsp.cpp
@@ -5,6 +5,7 @@
 #include "CommandPackets/WriteMemory.hpp"
 #include "CommandPackets/ReadMemoryMap.hpp"
 #include "CommandPackets/FlashErase.hpp"
+#include "CommandPackets/FlashWrite.hpp"
 
 namespace Bloom::DebugServer::Gdb::AvrGdb
 {
@@ -36,6 +37,7 @@ namespace Bloom::DebugServer::Gdb::AvrGdb
         using AvrGdb::CommandPackets::WriteMemory;
         using AvrGdb::CommandPackets::ReadMemoryMap;
         using AvrGdb::CommandPackets::FlashErase;
+        using AvrGdb::CommandPackets::FlashWrite;
 
         if (rawPacket.size() >= 2) {
             if (rawPacket[1] == 'm') {
@@ -55,6 +57,10 @@ namespace Bloom::DebugServer::Gdb::AvrGdb
             if (rawPacketString.find("vFlashErase") == 0) {
                 return std::make_unique<FlashErase>(rawPacket);
             }
+
+            if (rawPacketString.find("vFlashWrite") == 0) {
+                return std::make_unique<FlashWrite>(rawPacket);
+            }
         }
 
         return GdbRspDebugServer::resolveCommandPacket(rawPacket);
diff --git a/src/DebugServer/Gdb/AvrGdb/CommandPackets/FlashWrite.cpp b/src/DebugServer/Gdb/AvrGdb/CommandPackets/FlashWrite.cpp
new file mode 100644
index 00000000..606537e9
--- /dev/null
+++ b/src/DebugServer/Gdb/AvrGdb/CommandPackets/FlashWrite.cpp
@@ -0,0 +1,68 @@
+#include "FlashWrite.hpp"
+
+#include <QByteArray>
+
+#include "src/DebugServer/Gdb/ResponsePackets/ErrorResponsePacket.hpp"
+#include "src/DebugServer/Gdb/ResponsePackets/OkResponsePacket.hpp"
+
+#include "src/Logger/Logger.hpp"
+#include "src/Exceptions/Exception.hpp"
+
+namespace Bloom::DebugServer::Gdb::AvrGdb::CommandPackets
+{
+    using TargetController::TargetControllerConsole;
+
+    using ResponsePackets::ErrorResponsePacket;
+    using ResponsePackets::OkResponsePacket;
+
+    using namespace Bloom::Exceptions;
+
+    FlashWrite::FlashWrite(const RawPacketType& rawPacket)
+        : MemoryAccessCommandPacket(rawPacket)
+    {
+        if (this->data.size() < 15) {
+            throw Exception("Invalid packet length");
+        }
+
+        /*
+         * The flash write ('vFlashWrite') packet consists of two segments, an address and a buffer.
+         *
+         * Seperated by a colon.
+         */
+        auto colonIt = std::find(this->data.begin() + 12, this->data.end(), ':');
+
+        if (colonIt == this->data.end()) {
+            throw Exception("Failed to find colon delimiter in write flash packet.");
+        }
+
+        bool conversionStatus = false;
+        this->startAddress = QByteArray(
+            reinterpret_cast<const char*>(this->data.data() + 12),
+            std::distance(this->data.begin(), colonIt) - 12
+        ).toUInt(&conversionStatus, 16);
+
+        if (!conversionStatus) {
+            throw Exception("Failed to parse start address from flash write packet data");
+        }
+
+        this->buffer = Targets::TargetMemoryBuffer(colonIt + 1, this->data.end());
+    }
+
+    void FlashWrite::handle(DebugSession& debugSession, TargetControllerConsole& targetControllerConsole) {
+        Logger::debug("Handling FlashWrite packet");
+
+        try {
+            targetControllerConsole.writeMemory(
+                Targets::TargetMemoryType::FLASH,
+                this->startAddress,
+                this->buffer
+            );
+
+            debugSession.connection.writePacket(OkResponsePacket());
+
+        } catch (const Exception& exception) {
+            Logger::error("Failed to write to flash memory - " + exception.getMessage());
+            debugSession.connection.writePacket(ErrorResponsePacket());
+        }
+    }
+}
diff --git a/src/DebugServer/Gdb/AvrGdb/CommandPackets/FlashWrite.hpp b/src/DebugServer/Gdb/AvrGdb/CommandPackets/FlashWrite.hpp
new file mode 100644
index 00000000..ea320fe5
--- /dev/null
+++ b/src/DebugServer/Gdb/AvrGdb/CommandPackets/FlashWrite.hpp
@@ -0,0 +1,27 @@
+#pragma once
+
+#include <cstdint>
+#include <optional>
+
+#include "MemoryAccessCommandPacket.hpp"
+
+namespace Bloom::DebugServer::Gdb::AvrGdb::CommandPackets
+{
+    /**
+     * The FlashWrite class implements the structure for the "vFlashWrite" packet. Upon receiving this packet, the
+     * server is expected to write to a particular region of the target's flash memory.
+     */
+    class FlashWrite: public MemoryAccessCommandPacket
+    {
+    public:
+        std::uint32_t startAddress = 0;
+        Targets::TargetMemoryBuffer buffer;
+
+        explicit FlashWrite(const RawPacketType& rawPacket);
+
+        void handle(
+            DebugSession& debugSession,
+            TargetController::TargetControllerConsole& targetControllerConsole
+        ) override;
+    };
+}