1 files changed, 40 insertions, 21 deletions

diff --git a/gdbremote_parser.cpp b/gdbremote_parser.cpp
index eb2e29a..d8f4f50 100644
--- a/gdbremote_parser.cpp
+++ b/gdbremote_parser.cpp
@@ -40,8 +40,8 @@ struct Token {
         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++) {
+        size_t i{}, offset{};
+        for (; i < packet_data.length(); i++) {
             assert(type != TokenType::kSeparatorColon);
             assert(type != TokenType::kSeparatorSemicolon);
             assert(type != TokenType::kSeparatorComma);
@@ -96,6 +96,7 @@ struct Command {
     }
     static Packet parseQueryC(const std::vector<Token>&&)
     {
+        // TODO arguments
         return Packet{PacketType::kQueryC};
     }
     static Packet parseQueryFThreadInfo(const std::vector<Token>&&)
@@ -113,6 +114,7 @@ struct Command {
     }
     static Packet parseQueryAttached(const std::vector<Token>&&)
     {
+        // TODO arguments
         return Packet{PacketType::kQueryAttached};
     }
     static Packet parseQueryTraceStatus(const std::vector<Token>&&)
@@ -137,6 +139,18 @@ struct Command {
     {
         return Packet{PacketType::kEnableExtendedMode};
     }
+    static Packet parseInterrupt(const std::vector<Token>&&)
+    {
+        return Packet{PacketType::kInterrupt};
+    }
+    static Packet parseContinue(const std::vector<Token>&&)
+    {
+        return Packet{PacketType::kContinue};
+    }
+    static Packet parseReadGeneralRegisters(const std::vector<Token>&&)
+    {
+        return Packet{PacketType::kReadGeneralRegisters};
+    }
 };
 
 static const Command commands[] = {
@@ -152,6 +166,10 @@ static const Command commands[] = {
     { "Hg",                 Command::parseSetThreadForOps },
     { "vMustReplyEmpty",    Command::parseMustReplyEmpty },
     { "!",                  Command::parseEnableExtendedMode },
+    { "\x03",               Command::parseInterrupt },
+    { "vCtrlC",             Command::parseInterrupt },
+    { "vCont",              Command::parseContinue },
+    { "g",                  Command::parseReadGeneralRegisters },
 };
 
 Packet Packet::Parse(std::string packet_data)
@@ -190,6 +208,9 @@ std::unique_ptr<ExchangeResult> ExchangeContext::Consume(uint8_t byte)
     case ParsingState::kIdle:
         if (byte == '$') {
             transit(ParsingState::kPacketData);
+        } else if (byte == 0x03) {
+            _last_packet = std::string(1, 0x03);
+            return ExchangeResult::Ack(_last_packet);
         }
         break;
     case ParsingState::kPacketData:
@@ -236,26 +257,24 @@ std::unique_ptr<ExchangeResult> ExchangeContext::Consume(uint8_t byte)
     return nullptr;
 }
 
-std::string ExchangeContext::WrapDataToSend(Packet 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;
+static inline uint8_t CalcPacketDataChecksum(std::string packet) {
+    uint8_t checksum{};
+    for (const auto c: packet) {
+        checksum = (checksum + c) & 0xff;
     }
-    return std::string{};
+    return checksum;
+}
+
+std::string ExchangeContext::WrapDataToSend(std::string packet)
+{
+    const uint8_t checksum = CalcPacketDataChecksum(packet);
+    const uint8_t c1 = (checksum >> 4) & 0x0f;
+    const uint8_t c2 = checksum& 0x0f;
+    const char checksum_str[2] = {
+        static_cast<char>(c1 < 0xa ? c1 + '0' : c1 + ('a' - 0xa)),
+        static_cast<char>(c2 < 0xa ? c2 + '0' : c2 + ('a' - 0xa)),
+    };
+    return "$" + packet + "#" + std::string(checksum_str, 2);
 }
 
 void ExchangeContext::transit(ParsingState new_state)