Introduce sophisticated parsing in GDBRemote

3 files changed, 271 insertions, 26 deletions

diff --git a/gdbremote.cpp b/gdbremote.cpp
index 98a0e2a..a093192 100644
--- a/gdbremote.cpp
+++ b/gdbremote.cpp
@@ -78,15 +78,28 @@ int main(int argc, char *argv[])
     while ((read_size = recv(conn_fd, msg_buf, MESSAGE_BUFFER_SIZE, 0)) > 0) {
         for (size_t i = 0; i < static_cast<size_t>(read_size); i++) {
             const auto res = exchange_ctx.Consume(msg_buf[i]);
-            if (res != nullptr) {
-                if (res->packet.length() > 0) {
-                    printf("<- \"%s\"\n", res->packet.c_str());
-                }
-                if (res->response.length() > 0) {
-                    printf("-> \"%s\"\n", res->response.c_str());
-                    if (send(conn_fd, &res->response[0], res->response.length(), 0) == -1)
-                        perror("Send failed");
-                }
+            if (res == nullptr) continue;
+            if (res->packet.length() > 0) {
+                if (0) printf("<- \"%s\"\n", res->packet.c_str());
+                printf("<- \"%s\"\n", exchange_ctx.GetLastPacket().c_str());
+                const auto packet = GDBRemote::Packet::Parse(res->packet);
+                printf(
+                        "   Packet type: \"%s\"\n",
+                        GDBRemote::Packet::PacketTypeToString(packet.type));
+            }
+            if (res->ack.length() > 0) {
+                printf("-> \"%s\"\n", res->ack.c_str());
+                if (send(conn_fd, &res->ack[0], res->ack.length(), 0) == -1)
+                    perror("Send failed (ack/nak)");
+            }
+            if (res->packet.length() > 0) {
+                const auto packet = GDBRemote::Packet::Parse(res->packet);
+
+                (void) packet;
+                const auto response = exchange_ctx.WrapDataToSend();
+                printf("-> \"%s\"\n", response.c_str());
+                if (send(conn_fd, &response[0], response.length(), 0) == -1)
+                    perror("Send failed (response)");
             }
         }
         memset(msg_buf, '\0', MESSAGE_BUFFER_SIZE);
diff --git a/gdbremote_parser.cpp b/gdbremote_parser.cpp
index e3f4aae..eb2e29a 100644
--- a/gdbremote_parser.cpp
+++ b/gdbremote_parser.cpp
@@ -3,13 +3,176 @@
 
 #include "gdbremote_parser.hpp"
 
+#include <cctype>
+#include <cassert>
+#include <cstring>
+
 using namespace GDBRemote;
 
-static inline bool is_hex_digit(uint8_t byte)
+enum class TokenType: uint8_t {
+    kUnknown = 0,
+    kSeparatorComma,
+    kSeparatorColon,
+    kSeparatorSemicolon,
+    kNumeric,
+    kHexNumeric,
+    kArbitrary,
+};
+
+static inline bool isseparator(uint8_t byte)
+{
+    return ':' == byte || ';' == byte || ',' == byte;
+}
+
+static inline TokenType separatorTypeFromByte(uint8_t byte)
 {
-    return byte >= '0' && byte <= '9'
-        || byte >= 'A' && byte < 'F'
-        || byte >= 'a' && byte <= 'f';
+    if (':' == byte) return TokenType::kSeparatorColon;
+    if (';' == byte) return TokenType::kSeparatorSemicolon;
+    return TokenType::kSeparatorComma;
+}
+
+struct Token {
+    TokenType type{};
+    std::string data{};
+
+    static std::vector<Token> Tokenize(std::string packet_data)
+    {
+        if (packet_data.length() == 0) return std::vector<Token>{};
+        std::vector<Token> tokens{};
+        TokenType type{};
+        size_t i, offset;
+        for (i = 0, offset = 0; i < packet_data.length(); i++) {
+            assert(type != TokenType::kSeparatorColon);
+            assert(type != TokenType::kSeparatorSemicolon);
+            assert(type != TokenType::kSeparatorComma);
+            const uint8_t byte = packet_data[i];
+            if (isseparator(byte)) {
+                if (i > offset) {
+                    assert(type != TokenType::kUnknown);
+                    tokens.push_back(Token{type, packet_data.substr(offset, i - offset)});
+                    offset = i;
+                }
+                tokens.push_back(Token{
+                        separatorTypeFromByte(byte),
+                        packet_data.substr(offset, 1),
+                        });
+                offset++;
+                type = TokenType::kUnknown;
+            } else if (isdigit(byte)) {
+                if (type == TokenType::kUnknown) {
+                    type = TokenType::kNumeric;
+                }
+            } else if (isxdigit(byte)) {
+                if (type != TokenType::kArbitrary) {
+                    type = TokenType::kHexNumeric;
+                }
+            } else {
+                type = TokenType::kArbitrary;
+            }
+        }
+        if (i > offset) {
+            assert(type != TokenType::kUnknown);
+            tokens.push_back(Token{type, packet_data.substr(offset, i - offset)});
+        }
+        return tokens;
+    }
+};
+
+struct Command {
+    std::string name;
+    Packet (*parse)(const std::vector<Token>&&);
+    static Packet parseNone(const std::vector<Token>&&)
+    {
+        return Packet::None();
+    }
+    static Packet parseQuerySupported(const std::vector<Token>&&)
+    {
+        // TODO arguments
+        return Packet{PacketType::kQuerySupported};
+    }
+    static Packet parseQueryHaltReason(const std::vector<Token>&&)
+    {
+        return Packet{PacketType::kQueryHaltReason};
+    }
+    static Packet parseQueryC(const std::vector<Token>&&)
+    {
+        return Packet{PacketType::kQueryC};
+    }
+    static Packet parseQueryFThreadInfo(const std::vector<Token>&&)
+    {
+        return Packet{PacketType::kQueryFThreadInfo};
+    }
+    static Packet parseQuerySThreadInfo(const std::vector<Token>&&)
+    {
+        return Packet{PacketType::kQuerySThreadInfo};
+    }
+    static Packet parseQueryRTOSThreadInfo(const std::vector<Token>&&)
+    {
+        // TODO arguments
+        return Packet{PacketType::kQueryRTOSThreadInfo};
+    }
+    static Packet parseQueryAttached(const std::vector<Token>&&)
+    {
+        return Packet{PacketType::kQueryAttached};
+    }
+    static Packet parseQueryTraceStatus(const std::vector<Token>&&)
+    {
+        return Packet{PacketType::kQueryTraceStatus};
+    }
+    static Packet parseSetThreadForCont(const std::vector<Token>&&)
+    {
+        // TODO arguments
+        return Packet{PacketType::kSetThreadForCont};
+    }
+    static Packet parseSetThreadForOps(const std::vector<Token>&&)
+    {
+        // TODO arguments
+        return Packet{PacketType::kSetThreadForOps};
+    }
+    static Packet parseMustReplyEmpty(const std::vector<Token>&&)
+    {
+        return Packet{PacketType::kVMustReplyEmpty};
+    }
+    static Packet parseEnableExtendedMode(const std::vector<Token>&&)
+    {
+        return Packet{PacketType::kEnableExtendedMode};
+    }
+};
+
+static const Command commands[] = {
+    { "qSupported",         Command::parseQuerySupported },
+    { "?",                  Command::parseQueryHaltReason },
+    { "qC",                 Command::parseQueryC },
+    { "qfThreadInfo",       Command::parseQueryFThreadInfo },
+    { "qsThreadInfo",       Command::parseQuerySThreadInfo },
+    { "qL",                 Command::parseQueryRTOSThreadInfo },
+    { "qAttached",          Command::parseQueryAttached },
+    { "qTStatus",           Command::parseQueryTraceStatus },
+    { "Hc",                 Command::parseSetThreadForCont },
+    { "Hg",                 Command::parseSetThreadForOps },
+    { "vMustReplyEmpty",    Command::parseMustReplyEmpty },
+    { "!",                  Command::parseEnableExtendedMode },
+};
+
+Packet Packet::Parse(std::string packet_data)
+{
+    const auto tokens = Token::Tokenize(packet_data);
+    if (tokens.size() == 0) return Packet::None();
+    const auto first_token = tokens[0];
+    if (first_token.type != TokenType::kArbitrary) return Packet::None();
+    for (const auto& command: commands) {
+        const auto cmdlen = command.name.length();
+        const auto toklen = first_token.data.length();
+        const auto len = std::min(cmdlen, cmdlen);
+        // Make sure first token fully includes the command from the beginning
+        // and not necessary fully equals to it.
+        if (cmdlen <= toklen && 0 == memcmp(
+                    &command.name[0], &first_token.data[0], len))
+        {
+            return command.parse(std::move(tokens));
+        }
+    }
+    return Packet::None();
 }
 
 static inline uint8_t parseChecksumFromHexChars(uint8_t first, uint8_t second)
@@ -22,6 +185,7 @@ static inline uint8_t parseChecksumFromHexChars(uint8_t first, uint8_t second)
 
 std::unique_ptr<ExchangeResult> ExchangeContext::Consume(uint8_t byte)
 {
+    _last_packet += byte;
     switch (_parsing_state) {
     case ParsingState::kIdle:
         if (byte == '$') {
@@ -40,7 +204,7 @@ std::unique_ptr<ExchangeResult> ExchangeContext::Consume(uint8_t byte)
         // TODO escaped and binary data
         break;
     case ParsingState::kChecksum1:
-        if (is_hex_digit(byte)) {
+        if (isxdigit(byte)) {
             transit(ParsingState::kChecksum2);
             _given_checksum_first_byte = byte;
         } else {
@@ -52,7 +216,7 @@ std::unique_ptr<ExchangeResult> ExchangeContext::Consume(uint8_t byte)
         const uint8_t checksum = _checksum;
         std::string packet_data(std::move(_packet_data));
         transit(ParsingState::kIdle);
-        if (is_hex_digit(byte)) {
+        if (isxdigit(byte)) {
             const uint8_t given_checksum =
                 parseChecksumFromHexChars(_given_checksum_first_byte, byte);
             if (given_checksum == checksum) {
@@ -74,22 +238,41 @@ std::unique_ptr<ExchangeResult> ExchangeContext::Consume(uint8_t byte)
 
 std::string ExchangeContext::WrapDataToSend(Packet packet)
 {
-    (void) packet;
+    switch (packet.type) {
+    case PacketType::kNone:
+    case PacketType::kVMustReplyEmpty:
+    case PacketType::kQuerySupported:
+    case PacketType::kQueryHaltReason:
+    case PacketType::kQueryC:
+    case PacketType::kQueryFThreadInfo:
+    case PacketType::kQuerySThreadInfo:
+    case PacketType::kQueryRTOSThreadInfo:
+    case PacketType::kQueryAttached:
+    case PacketType::kQueryTraceStatus:
+    case PacketType::kSetThreadForCont:
+    case PacketType::kSetThreadForOps:
+    case PacketType::kEnableExtendedMode:
+        return "$#00";
+        break;
+    }
     return std::string{};
 }
 
 void ExchangeContext::transit(ParsingState new_state)
 {
-    printf(
-            "GDBRemote ParsingState: %s -> %s\n",
-            parsingStateToString(_parsing_state),
-            parsingStateToString(new_state));
+    if (0) {
+        printf(
+                "GDBRemote ParsingState: %s -> %s\n",
+                parsingStateToString(_parsing_state),
+                parsingStateToString(new_state));
+    }
     switch (new_state) {
     case ParsingState::kIdle:
         _checksum = 0;
         _packet_data.clear();
         break;
     case ParsingState::kPacketData:
+        _last_packet = "$";
         break;
     case ParsingState::kPacketDataBinSecond:
         break;
@@ -115,5 +298,5 @@ const char* ExchangeContext::parsingStateToString(ParsingState state)
     case ParsingState::kChecksum2:
         return "Checksum2";
     }
-    return "?";
+    return "<unknown>";
 }
diff --git a/gdbremote_parser.hpp b/gdbremote_parser.hpp
index 0e2f375..6bf37bd 100644
--- a/gdbremote_parser.hpp
+++ b/gdbremote_parser.hpp
@@ -44,11 +44,17 @@ struct Feature {
 enum class PacketType: int {
     kNone = 0,
     kQuerySupported,
+    kQueryHaltReason,
     kQueryC,
     kQueryFThreadInfo,
     kQuerySThreadInfo,
-    kH,
-    kMustReplyEmpty,
+    kQueryRTOSThreadInfo,
+    kQueryAttached,
+    kQueryTraceStatus,
+    kSetThreadForCont,
+    kSetThreadForOps,
+    kVMustReplyEmpty,
+    kEnableExtendedMode,
 };
 
 struct PacketData {
@@ -65,21 +71,57 @@ struct PacketDataSupportedFeatures: public PacketData {
 
 struct Packet {
     const PacketType type{};
-    const std::unique_ptr<PacketData> data;
+    const std::unique_ptr<PacketData> data{nullptr};
 
     /** Convert raw packet data into a Packet
      *
      * Packet data is the data extracted by ExchangeContext::Consume function.
      */
     static Packet Parse(std::string packet_data);
+
+    /** None packet creator */
+    static Packet None() { return Packet{}; }
+
+    /** Stringify PacketType for debugging purposes */
+    static const char* PacketTypeToString(PacketType type) {
+        switch (type) {
+        case PacketType::kNone:
+            return "None";
+        case PacketType::kQuerySupported:
+            return "qSupported";
+        case PacketType::kQueryHaltReason:
+            return "?";
+        case PacketType::kQueryC:
+            return "qC";
+        case PacketType::kQueryFThreadInfo:
+            return "qfThreadInfo";
+        case PacketType::kQuerySThreadInfo:
+            return "qsThreadInfo";
+        case PacketType::kQueryRTOSThreadInfo:
+            return "qL";
+        case PacketType::kQueryAttached:
+            return "qAttached";
+        case PacketType::kQueryTraceStatus:
+            return "qTStatus";
+        case PacketType::kSetThreadForCont:
+            return "Hc";
+        case PacketType::kSetThreadForOps:
+            return "Hg";
+        case PacketType::kVMustReplyEmpty:
+            return "vMustReplyEmpty";
+        case PacketType::kEnableExtendedMode:
+            return "!";
+        }
+        return "<unknown>";
+    }
 };
 
 struct ExchangeResult {
     const std::string packet{};
-    const std::string response{};
+    const std::string ack{};
 
     ExchangeResult(std::string a_packet, std::string a_response)
-        : packet(a_packet), response(a_response) {}
+        : packet(a_packet), ack(a_response) {}
 
     static std::unique_ptr<ExchangeResult> Nak(std::string data=std::string{})
     {
@@ -112,6 +154,12 @@ public:
      */
     std::string WrapDataToSend(Packet packet=Packet{});
 
+    /** Returns last recognized packet
+     *
+     * For debugging purposes.
+     */
+    const std::string GetLastPacket() { return _last_packet; }
+
 private:
     enum class ParsingState {
         kIdle,
@@ -126,6 +174,7 @@ private:
 
     ParsingState _parsing_state{ParsingState::kIdle};
     std::string _packet_data{};
+    std::string _last_packet{};
     uint8_t _checksum{};
     uint8_t _given_checksum_first_byte{};
 };