Impl setting registers via GDB

5 files changed, 154 insertions, 34 deletions

diff --git a/emulator.cpp b/emulator.cpp
index 7e6ad25..b5ad031 100644
--- a/emulator.cpp
+++ b/emulator.cpp
@@ -211,7 +211,7 @@ static std::string CreateResponse(
         {
             const M68KCPUState state = m68k_debug.GetCPUState();
             std::string result{};
-            for (size_t i = 0; i < state.registers_count ;i++)
+            for (size_t i = 0; i < state.registers_count; i++)
             {
                 constexpr size_t value_size = std::string_view("12345678").length();
                 char value[value_size + 1]{};
@@ -221,6 +221,46 @@ static std::string CreateResponse(
             }
             return result;
         }
+    case PacketType::kWriteGeneralRegisters:
+        {
+            const auto * const data =
+                static_cast<const PacketDataWriteGeneralRegisters*>(packet.data.get());
+            const auto& registers = data->registers;
+            for (size_t i = 0; i < registers.size(); i++) {
+                m68k_debug.SetRegister(static_cast<M68KRegister>(i), registers[i]);
+            }
+            return "OK";
+        }
+        break;
+    case PacketType::kReadRegister:
+        {
+            const auto * const data =
+                static_cast<const PacketDataReadRegister*>(packet.data.get());
+            const auto reg_id = static_cast<M68KRegister>(data->reg_id);
+            if (reg_id >= M68KRegister::kRegistersCount) {
+                return "E00";
+            }
+            const uint32_t value = m68k_debug.GetRegister(reg_id);
+            constexpr size_t value_size = std::string_view("12345678").length();
+            char value_str[value_size + 1]{};
+            const int ret = snprintf(value_str, value_size + 1, "%08x", value);
+            assert(ret == value_size);
+            return std::string(value_str, value_size);
+        }
+        break;
+    case PacketType::kWriteRegister:
+        {
+            const auto * const data =
+                static_cast<const PacketDataWriteRegister*>(packet.data.get());
+            const auto reg_id = static_cast<M68KRegister>(data->reg_id);
+            const auto value = data->value;
+            if (reg_id >= M68KRegister::kRegistersCount) {
+                return "E00";
+            }
+            m68k_debug.SetRegister(reg_id, value);
+            return "OK";
+        }
+        break;
     case PacketType::kReadMemory:
         {
             const auto * const data =
diff --git a/gdbremote_parser.cpp b/gdbremote_parser.cpp
index 2c13668..8d70370 100644
--- a/gdbremote_parser.cpp
+++ b/gdbremote_parser.cpp
@@ -15,19 +15,21 @@ enum class TokenType: uint8_t {
     kSeparatorComma,
     kSeparatorColon,
     kSeparatorSemicolon,
+    kSeparatorEquals,
     kHexNumeric,
     kArbitrary,
 };
 
 static inline bool isseparator(uint8_t byte)
 {
-    return ':' == byte || ';' == byte || ',' == byte;
+    return ':' == byte || ';' == byte || ',' == byte || '=' == byte;
 }
 
 static inline TokenType separatorTypeFromByte(uint8_t byte)
 {
     if (':' == byte) return TokenType::kSeparatorColon;
     if (';' == byte) return TokenType::kSeparatorSemicolon;
+    if ('=' == byte) return TokenType::kSeparatorEquals;
     return TokenType::kSeparatorComma;
 }
 
@@ -49,6 +51,7 @@ struct Token {
         for (; i < packet_data.length(); i++) {
             assert(type != TokenType::kSeparatorColon);
             assert(type != TokenType::kSeparatorSemicolon);
+            assert(type != TokenType::kSeparatorEquals);
             assert(type != TokenType::kSeparatorComma);
             const uint8_t byte = packet_data[i];
             if (isseparator(byte)) {
@@ -90,48 +93,48 @@ static std::string DecodeBinaryHex(const std::string& data_hex)
 
 struct Command {
     std::string name;
-    Packet (*parse)(const std::vector<Token>&&);
-    static Packet parseNone(const std::vector<Token>&&)
+    Packet (*parse)(std::vector<Token>&&);
+    static Packet parseNone(std::vector<Token>&&)
     {
         return Packet::None();
     }
-    static Packet parseQuerySupported(const std::vector<Token>&&)
+    static Packet parseQuerySupported(std::vector<Token>&&)
     {
         // TODO arguments
         return Packet{PacketType::kQuerySupported};
     }
-    static Packet parseQueryHaltReason(const std::vector<Token>&&)
+    static Packet parseQueryHaltReason(std::vector<Token>&&)
     {
         return Packet{PacketType::kQueryHaltReason};
     }
-    static Packet parseQueryC(const std::vector<Token>&&)
+    static Packet parseQueryC(std::vector<Token>&&)
     {
         // TODO arguments
         return Packet{PacketType::kQueryC};
     }
-    static Packet parseQueryFThreadInfo(const std::vector<Token>&&)
+    static Packet parseQueryFThreadInfo(std::vector<Token>&&)
     {
         return Packet{PacketType::kQueryFThreadInfo};
     }
-    static Packet parseQuerySThreadInfo(const std::vector<Token>&&)
+    static Packet parseQuerySThreadInfo(std::vector<Token>&&)
     {
         return Packet{PacketType::kQuerySThreadInfo};
     }
-    static Packet parseQueryRTOSThreadInfo(const std::vector<Token>&&)
+    static Packet parseQueryRTOSThreadInfo(std::vector<Token>&&)
     {
         // TODO arguments
         return Packet{PacketType::kQueryRTOSThreadInfo};
     }
-    static Packet parseQueryAttached(const std::vector<Token>&&)
+    static Packet parseQueryAttached(std::vector<Token>&&)
     {
         // TODO arguments
         return Packet{PacketType::kQueryAttached};
     }
-    static Packet parseQueryTraceStatus(const std::vector<Token>&&)
+    static Packet parseQueryTraceStatus(std::vector<Token>&&)
     {
         return Packet{PacketType::kQueryTraceStatus};
     }
-    static Packet parseQueryRcmd(const std::vector<Token>&& tokens)
+    static Packet parseQueryRcmd(std::vector<Token>&& tokens)
     {
         if (tokens.size() < 3)
             return Packet::None();
@@ -140,40 +143,38 @@ struct Command {
         if (tokens[2].type != TokenType::kHexNumeric)
             return Packet::None();
         const std::string& data_encoded = tokens[2].data;
-        const std::string data = DecodeBinaryHex(data_encoded);
-        printf("%s\n", data.c_str());
         return Packet{
             PacketType::kQueryRcmd,
-            std::make_unique<const PacketDataRcmd>(std::move(data)),
+            std::make_unique<const PacketDataRcmd>(std::move(DecodeBinaryHex(data_encoded))),
         };
     }
-    static Packet parseQStartNoAckMode(const std::vector<Token>&&)
+    static Packet parseQStartNoAckMode(std::vector<Token>&&)
     {
         return Packet{PacketType::kQStartNoAckMode};
     }
-    static Packet parseSetThreadForCont(const std::vector<Token>&&)
+    static Packet parseSetThreadForCont(std::vector<Token>&&)
     {
         // TODO arguments
         return Packet{PacketType::kSetThreadForCont};
     }
-    static Packet parseSetThreadForOps(const std::vector<Token>&&)
+    static Packet parseSetThreadForOps(std::vector<Token>&&)
     {
         // TODO arguments
         return Packet{PacketType::kSetThreadForOps};
     }
-    static Packet parseMustReplyEmpty(const std::vector<Token>&&)
+    static Packet parseMustReplyEmpty(std::vector<Token>&&)
     {
         return Packet{PacketType::kVMustReplyEmpty};
     }
-    static Packet parseEnableExtendedMode(const std::vector<Token>&&)
+    static Packet parseEnableExtendedMode(std::vector<Token>&&)
     {
         return Packet{PacketType::kEnableExtendedMode};
     }
-    static Packet parseInterrupt(const std::vector<Token>&&)
+    static Packet parseInterrupt(std::vector<Token>&&)
     {
         return Packet{PacketType::kInterrupt};
     }
-    static Packet parseContinueVCont(const std::vector<Token>&& tokens)
+    static Packet parseContinueVCont(std::vector<Token>&& tokens)
     {
         const std::string first_arg = tokens.size() > 1 ? tokens[1].data : "";
         // Check if it is "vCont?" command
@@ -183,11 +184,11 @@ struct Command {
         // TODO arguments
         return Packet{PacketType::kContinue};
     }
-    static Packet parseContinue(const std::vector<Token>&&)
+    static Packet parseContinue(std::vector<Token>&&)
     {
         return Packet{PacketType::kContinue};
     }
-    static Packet parseReadMemory(const std::vector<Token>&& tokens)
+    static Packet parseReadMemory(std::vector<Token>&& tokens)
     {
         if (tokens.size() < 4)
             return Packet::None();
@@ -207,7 +208,7 @@ struct Command {
             std::make_unique<const PacketDataReadMemory>(offset, lenght),
         };
     }
-    static Packet parseWriteMemory(const std::vector<Token>&& tokens)
+    static Packet parseWriteMemory(std::vector<Token>&& tokens)
     {
         if (tokens.size() < 6)
             return Packet::None();
@@ -239,15 +240,65 @@ struct Command {
             std::make_unique<const PacketDataWriteMemory>(offset, lenght, std::move(data)),
         };
     }
-    static Packet parseReadGeneralRegisters(const std::vector<Token>&&)
+    static Packet parseReadGeneralRegisters(std::vector<Token>&&)
     {
         return Packet{PacketType::kReadGeneralRegisters};
     }
-    static Packet parseStep(const std::vector<Token>&&)
+    static Packet parseWriteGeneralRegisters(std::vector<Token>&& tokens)
+    {
+        if (tokens.size() < 2)
+            return Packet::None();
+        if (tokens[1].type != TokenType::kHexNumeric)
+            return Packet::None();
+        const std::string& first = tokens[1].data;
+        constexpr size_t value_str_size = std::string_view("12345678").length();
+        std::vector<uint32_t> registers(first.length()/value_str_size);
+        for (size_t i = 0; i < first.length()/value_str_size; i++) {
+            const std::string value_str(first.substr(i * value_str_size, value_str_size));
+            registers[i] = strtol(value_str.c_str(), nullptr, 16);
+        }
+        return Packet{
+            PacketType::kWriteGeneralRegisters,
+            std::make_unique<const PacketDataWriteGeneralRegisters>(std::move(registers)),
+        };
+    }
+    static Packet parseReadRegister(std::vector<Token>&& tokens)
+    {
+        if (tokens.size() < 2)
+            return Packet::None();
+        if (tokens[1].type != TokenType::kHexNumeric)
+            return Packet::None();
+        const std::string& first = tokens[1].data;
+        const uint32_t reg_id = strtol(first.c_str(), nullptr, 16);
+        return Packet{
+            PacketType::kReadRegister,
+            std::make_unique<const PacketDataReadRegister>(reg_id),
+        };
+    }
+    static Packet parseWriteRegister(std::vector<Token>&& tokens)
+    {
+        if (tokens.size() < 4)
+            return Packet::None();
+        if (tokens[1].type != TokenType::kHexNumeric)
+            return Packet::None();
+        if (tokens[2].type != TokenType::kSeparatorEquals)
+            return Packet::None();
+        if (tokens[3].type != TokenType::kHexNumeric)
+            return Packet::None();
+        const std::string& first = tokens[1].data;
+        const std::string& second = tokens[3].data;
+        const uint32_t reg_id = strtol(first.c_str(), nullptr, 16);
+        const uint32_t value = strtol(second.c_str(), nullptr, 16);
+        return Packet{
+            PacketType::kWriteRegister,
+            std::make_unique<const PacketDataWriteRegister>(reg_id, value),
+        };
+    }
+    static Packet parseStep(std::vector<Token>&&)
     {
         return Packet{PacketType::kStep};
     }
-    static Packet parseSetOrDeleteBreakpoint(const std::vector<Token>&& tokens)
+    static Packet parseSetOrDeleteBreakpoint(std::vector<Token>&& tokens)
     {
         if (tokens.size() < 6)
             return Packet::None();
@@ -302,6 +353,9 @@ static const Command commands[] = {
     { "m",                  Command::parseReadMemory },
     { "M",                  Command::parseWriteMemory },
     { "g",                  Command::parseReadGeneralRegisters },
+    { "G",                  Command::parseWriteGeneralRegisters },
+    { "p",                  Command::parseReadRegister },
+    { "P",                  Command::parseWriteRegister },
     { "s",                  Command::parseStep },
     { "Z",                  Command::parseSetOrDeleteBreakpoint },
     { "z",                  Command::parseSetOrDeleteBreakpoint },
@@ -313,8 +367,7 @@ Packet Packet::Parse(std::string packet_data)
         const auto cmdlen = command.name.length();
         const auto packetlen = packet_data.length();
         if (cmdlen <= packetlen && 0 == memcmp(&command.name[0], &packet_data[0], cmdlen)) {
-            const auto tokens = Token::Tokenize(packet_data, command.name);
-            return command.parse(std::move(tokens));
+            return command.parse(std::move(Token::Tokenize(packet_data, command.name)));
         }
     }
     return Packet::None();
diff --git a/gdbremote_parser.hpp b/gdbremote_parser.hpp
index d35d78e..856206a 100644
--- a/gdbremote_parser.hpp
+++ b/gdbremote_parser.hpp
@@ -61,6 +61,9 @@ enum class PacketType: int {
     kContinue,
     kContinueAskSupported,
     kReadGeneralRegisters,
+    kWriteGeneralRegisters,
+    kReadRegister,
+    kWriteRegister,
     kReadMemory,
     kWriteMemory,
     kStep,
@@ -97,7 +100,7 @@ struct PacketDataWriteMemory: public PacketData {
 };
 
 struct PacketDataRcmd: public PacketData {
-    PacketDataRcmd(const std::string&& a_data): data(std::move(a_data)) {}
+    PacketDataRcmd(std::string&& a_data): data(std::move(a_data)) {}
     std::string data{};
     virtual ~PacketDataRcmd() {}
 };
@@ -120,6 +123,25 @@ struct PacketDataBreakpoint: public PacketData {
     virtual ~PacketDataBreakpoint() {}
 };
 
+struct PacketDataWriteGeneralRegisters: public PacketData {
+    PacketDataWriteGeneralRegisters(std::vector<uint32_t>&& regs): registers(std::move(regs)) {}
+    std::vector<uint32_t> registers{};
+    virtual ~PacketDataWriteGeneralRegisters() {}
+};
+
+struct PacketDataReadRegister: public PacketData {
+    PacketDataReadRegister(uint32_t a_reg_id): reg_id(a_reg_id) {}
+    uint32_t reg_id{};
+    virtual ~PacketDataReadRegister() {}
+};
+
+struct PacketDataWriteRegister: public PacketData {
+    PacketDataWriteRegister(uint32_t a_reg_id, uint32_t a_value)
+        : reg_id(a_reg_id), value(a_value) {}
+    uint32_t reg_id{}, value{};
+    virtual ~PacketDataWriteRegister() {}
+};
+
 struct Packet {
     const PacketType type{};
     const std::unique_ptr<const PacketData> data{nullptr};
@@ -174,6 +196,12 @@ struct Packet {
             return "vCont?";
         case PacketType::kReadGeneralRegisters:
             return "g";
+        case PacketType::kWriteGeneralRegisters:
+            return "G";
+        case PacketType::kReadRegister:
+            return "p";
+        case PacketType::kWriteRegister:
+            return "P";
         case PacketType::kReadMemory:
             return "m";
         case PacketType::kWriteMemory:
diff --git a/m68k_debugging.cpp b/m68k_debugging.cpp
index 2e64417..5bae0d0 100644
--- a/m68k_debugging.cpp
+++ b/m68k_debugging.cpp
@@ -56,7 +56,6 @@ static inline m68k_register_t ConvertRegisterId(const M68KRegister register_id)
     case M68KRegister::kFPC:
     case M68KRegister::kFPS:
     case M68KRegister::kFPI:
-    case M68KRegister::kRegistersCount:
         assert(!"Unsupported register_id");
         break;
     }
diff --git a/m68k_debugging.hpp b/m68k_debugging.hpp
index 1aa3bfc..7141809 100644
--- a/m68k_debugging.hpp
+++ b/m68k_debugging.hpp
@@ -29,11 +29,11 @@ enum class M68KRegister: size_t {
     kSP = kA7,
     kPS = 16,           /* Processor status (Condition code register)  */
     kPC = 17,           /* Program counter  */
+    kRegistersCount,
     kFP0 = 18,          /* Floating point register 0  */
     kFPC = 26,          /* 68881 control register  */
     kFPS = 27,          /* 68881 status register  */
     kFPI = 28,          /* Floating point register 1  */
-    kRegistersCount,
 };
 
 struct M68KCPUState {