Impl writing memory via GDB

4 files changed, 81 insertions, 18 deletions

diff --git a/emulator.cpp b/emulator.cpp
index 96588cd..1c8c6f9 100644
--- a/emulator.cpp
+++ b/emulator.cpp
@@ -4,6 +4,7 @@
 #include "bus.hpp"
 #include "m68k_debugging.hpp"
 #include "gdbremote_parser.hpp"
+#include "utils.hpp"
 #include "musashi-m68k/m68k.h"
 
 #include <cassert>
@@ -92,14 +93,6 @@ static int setup_socket(uint16_t port)
     return socket_fd;
 }
 
-static inline void ConvertByteToHex(uint8_t byte, char* out)
-{
-    const uint8_t c1 = (byte >> 4) & 0x0f;
-    const uint8_t c2 = byte& 0x0f;
-    out[0] = static_cast<char>(c1 < 0xa ? c1 + '0' : c1 + ('a' - 0xa));
-    out[1] = static_cast<char>(c2 < 0xa ? c2 + '0' : c2 + ('a' - 0xa));
-}
-
 static std::string CreateResponse(
         M68KDebuggingControl& m68k_debug,
         const GDBRemote::Packet& packet)
@@ -131,9 +124,19 @@ static std::string CreateResponse(
         auto ret_data = std::string(length * 2, '\0');
         for (uint32_t i = 0; i < length; i++) {
             const uint8_t byte = m68k_debug.Read8(i + offset);
-            ConvertByteToHex(byte, &ret_data[i*2]);
+            Utils::ConvertByteToHex(byte, &ret_data[i*2]);
         }
         return ret_data;
+    } else if (packet.type == PacketType::kWriteMemory) {
+        const auto * const packet_data =
+            reinterpret_cast<const PacketDataWriteMemory*>(packet.data.get());
+        const uint32_t offset = packet_data->offset;
+        const uint32_t length = packet_data->length;
+        const auto write_data = packet_data->data;
+        for (uint32_t i = 0; i < length; i++) {
+            m68k_debug.Write8(i + offset, write_data[i]);
+        }
+        return "OK";
     } else if (packet.type == PacketType::kReadGeneralRegisters) {
         const M68KCPUState state = m68k_debug.GetCPUState();
         std::string result{};
diff --git a/gdbremote_parser.cpp b/gdbremote_parser.cpp
index 31e6af9..6e61802 100644
--- a/gdbremote_parser.cpp
+++ b/gdbremote_parser.cpp
@@ -2,6 +2,7 @@
  */
 
 #include "gdbremote_parser.hpp"
+#include "utils.hpp"
 
 #include <cctype>
 #include <cassert>
@@ -169,6 +170,7 @@ struct Command {
             return Packet::None();
         const std::string first = tokens[1].data;
         const std::string second = tokens[3].data;
+        // TODO handle number parsing error (ERANGE)
         const uint32_t offset = strtol(first.c_str(), nullptr, 16);
         const uint32_t lenght = strtol(second.c_str(), nullptr, 16);
         return Packet{
@@ -176,6 +178,38 @@ struct Command {
             std::make_unique<const PacketDataReadMemory>(offset, lenght),
         };
     }
+    static Packet parseWriteMemory(const std::vector<Token>&& tokens)
+    {
+        if (tokens.size() < 6)
+            return Packet::None();
+        if (tokens[1].type != TokenType::kHexNumeric)
+            return Packet::None();
+        if (tokens[2].type != TokenType::kSeparatorComma)
+            return Packet::None();
+        if (tokens[3].type != TokenType::kHexNumeric)
+            return Packet::None();
+        // XXX Is data parameter always present?
+        if (tokens[4].type != TokenType::kSeparatorColon)
+            return Packet::None();
+        if (tokens[5].type != TokenType::kHexNumeric)
+            return Packet::None();
+        // TODO handle number parsing error (ERANGE)
+        const std::string first = tokens[1].data;
+        const std::string second = tokens[3].data;
+        const std::string data_raw = tokens[5].data;
+        const size_t data_len = data_raw.length()/2;
+        std::vector<uint8_t> data(data_len);
+        assert(data.size() == data_len);
+        const uint32_t offset = strtol(first.c_str(), nullptr, 16);
+        const uint32_t lenght = strtol(second.c_str(), nullptr, 16);
+        for (size_t i = 0; i < data_len; i++) {
+            data[i] = Utils::ParseByteFromHexChars(data_raw[i*2], data_raw[i*2+1]);
+        }
+        return Packet{
+            PacketType::kWriteMemory,
+            std::make_unique<const PacketDataWriteMemory>(offset, lenght, std::move(data)),
+        };
+    }
     static Packet parseReadGeneralRegisters(const std::vector<Token>&&)
     {
         return Packet{PacketType::kReadGeneralRegisters};
@@ -215,6 +249,7 @@ static const Command commands[] = {
     { "vCont",              Command::parseContinueVCont },
     { "c",                  Command::parseContinue },
     { "m",                  Command::parseReadMemory },
+    { "M",                  Command::parseWriteMemory },
     { "g",                  Command::parseReadGeneralRegisters },
     { "s",                  Command::parseStep },
     { "Z",                  Command::parseSetBreakpoint },
@@ -234,14 +269,6 @@ Packet Packet::Parse(std::string packet_data)
     return Packet::None();
 }
 
-static inline uint8_t parseChecksumFromHexChars(uint8_t first, uint8_t second)
-{
-    // Assume, that given bytes are valid hex digits in ASCII
-    first = ((first & 0x40) ? (first + 9) : first) & 0x0f;
-    second = ((second & 0x40) ? (second + 9) : second) & 0x0f;
-    return ((first << 4) | second) & 0xff;
-}
-
 std::unique_ptr<ExchangeResult> ExchangeContext::Consume(uint8_t byte)
 {
     _last_packet += byte;
@@ -280,7 +307,7 @@ std::unique_ptr<ExchangeResult> ExchangeContext::Consume(uint8_t byte)
         transit(ParsingState::kIdle);
         if (isxdigit(byte)) {
             const uint8_t given_checksum =
-                parseChecksumFromHexChars(_given_checksum_first_byte, byte);
+                Utils::ParseByteFromHexChars(_given_checksum_first_byte, byte);
             if (given_checksum == checksum) {
                 return ExchangeResult::Ack(packet_data);
             } else {
diff --git a/gdbremote_parser.hpp b/gdbremote_parser.hpp
index 75c9bb7..e060daa 100644
--- a/gdbremote_parser.hpp
+++ b/gdbremote_parser.hpp
@@ -60,6 +60,7 @@ enum class PacketType: int {
     kContinueAskSupported,
     kReadGeneralRegisters,
     kReadMemory,
+    kWriteMemory,
     kStep,
     kSetBreakpoint,
     kDeleteBreakpoint,
@@ -85,6 +86,14 @@ struct PacketDataReadMemory: public PacketData {
     virtual ~PacketDataReadMemory() {}
 };
 
+struct PacketDataWriteMemory: public PacketData {
+    PacketDataWriteMemory(uint32_t a_offset, uint32_t a_length, std::vector<uint8_t>&& a_data)
+        : offset(a_offset), length(a_length), data(std::move(a_data)) {}
+    uint32_t offset{}, length{};
+    std::vector<uint8_t> data{};
+    virtual ~PacketDataWriteMemory() {}
+};
+
 struct Packet {
     const PacketType type{};
     const std::unique_ptr<const PacketData> data{nullptr};
@@ -137,6 +146,8 @@ struct Packet {
             return "g";
         case PacketType::kReadMemory:
             return "m";
+        case PacketType::kWriteMemory:
+            return "M";
         case PacketType::kStep:
             return "s";
         case PacketType::kSetBreakpoint:
diff --git a/utils.hpp b/utils.hpp
index 4a5e84d..4a83a8d 100644
--- a/utils.hpp
+++ b/utils.hpp
@@ -10,3 +10,25 @@
 #else
 #   define UNREACHABLE
 #endif
+
+#include <stdint.h>
+
+namespace Utils {
+
+static inline uint8_t ParseByteFromHexChars(uint8_t first, uint8_t second)
+{
+    // Assume, that given bytes are valid hex digits in ASCII
+    first = ((first & 0x40) ? (first + 9) : first) & 0x0f;
+    second = ((second & 0x40) ? (second + 9) : second) & 0x0f;
+    return ((first << 4) | second) & 0xff;
+}
+
+static inline void ConvertByteToHex(uint8_t byte, char* out)
+{
+    const uint8_t c1 = (byte >> 4) & 0x0f;
+    const uint8_t c2 = byte& 0x0f;
+    out[0] = static_cast<char>(c1 < 0xa ? c1 + '0' : c1 + ('a' - 0xa));
+    out[1] = static_cast<char>(c2 < 0xa ? c2 + '0' : c2 + ('a' - 0xa));
+}
+
+}