Impl setting registers via GDB

1 files changed, 83 insertions, 30 deletions

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();