Messaging: simple and fun packet parser implementation.

Messaging/Inc/messaging_packet.hpp

@@ -18,20 +18,31 @@ namespace Messaging
 template<std::size_t N>
 struct Packet
 {
+  using length_type = std::uint32_t;
+
   static constexpr std::uint8_t preamble[] = { 'A', 'A' };
-  std::uint8_t data[N] = { 0 };
+  std::array<std::uint8_t, N> data = { 0 };
   std::uint32_t data_length = 0;
 
   const std::uint32_t max_data_length = N;
   const std::uint32_t preamble_length = sizeof(preamble);
   const std::uint32_t data_length_length = sizeof(data_length);
 
+  static constexpr std::size_t totalLength()
+  {
+    return N + sizeof(preamble) + sizeof(data_length);
+  }
+
+  Packet() = default;
+  Packet(const Packet&) = default;
+  Packet(Packet&&) noexcept = default;
+
   template<std::size_t data_in_length>
   void copy_data_in(const std::uint8_t (&data_in)[data_in_length])
   {
     const std::uint32_t to_copy_length = std::min(std::uint32_t(data_in_length), max_data_length);
 
-    std::memcpy(data, data_in, to_copy_length);
+    std::memcpy(data.data(), data_in, to_copy_length);
     data_length = to_copy_length;
   }
 
@@ -40,7 +51,7 @@ struct Packet
   {
     const std::uint32_t to_copy_length = std::min(std::uint32_t(data_in_length), max_data_length);
 
-    std::memcpy(data, data_in.data(), to_copy_length);
+    std::memcpy(data.data(), data_in.data(), to_copy_length);
     data_length = to_copy_length;
   }
 
@@ -57,12 +68,12 @@ struct Packet
     std::memcpy(buffer, &data_length, data_length_length);
     buffer += data_length_length;
 
-    std::memcpy(buffer, data, data_length);
+    std::memcpy(buffer, data.data(), data_length);
 
     return true;
   }
 
-  bool deserialize(std::uint8_t* buffer, const std::uint32_t length)
+  bool deserialize(const std::uint8_t* buffer, const std::uint32_t length)
   {
     const std::uint32_t header_length = preamble_length + data_length_length;
 
@@ -79,7 +90,7 @@ struct Packet
     if (length != final_length)
       return false;
 
-    std::memcpy(data, buffer + header_length, data_length);
+    std::memcpy(data.data(), buffer + header_length, data_length);
 
     return true;
   }

Messaging/Inc/messaging_parser.hpp

@@ -0,0 +1,131 @@
+//
+// Created by erki on 27.03.21.
+//
+
+#ifndef SKULLC_MESSAGING_PARSER_HPP
+#define SKULLC_MESSAGING_PARSER_HPP
+
+#include <cstdint>
+#include <cstring>
+#include <array>
+
+namespace Messaging
+{
+
+template<typename P, typename std::size_t N>
+class Parser
+{
+public:
+  using Packet = P;
+  const std::size_t buffer_length = N;
+
+  Parser()
+  {
+    reset();
+  }
+  Parser(const Parser&) = delete;
+  Parser(Parser&&) = delete;
+
+  void reset()
+  {
+    _state = _State::Preamble;
+    _expected = sizeof(P::preamble);
+    _current_pos = 0;
+    _current_offset = 0;
+  }
+
+  void pushByte(const std::uint8_t byte)
+  {
+    if (packetReady())
+      return;
+
+    const std::uint32_t buffer_loc = _current_offset + _current_pos;
+
+    switch (_state)
+    {
+    case _State::Preamble:
+      if (byte != P::preamble[_current_pos])
+      {
+        reset();
+        return;
+      }
+      [[fallthrough]];
+    case _State::Length:
+    case _State::Body:
+      _buffer[buffer_loc] = byte;
+      _current_pos++;
+      break;
+    default:
+      break;
+    }
+
+    if (_current_pos == _expected)
+    {
+      _setupNextState();
+    }
+  }
+
+  bool packetReady() const
+  {
+    return _state == _State::Done;
+  }
+
+  bool getPacket(Packet& packet) const
+  {
+    return packet.deserialize(_buffer.data(), _current_offset);
+  }
+
+private:
+  enum class _State : std::uint32_t
+  {
+    Preamble,
+    Length,
+    Body,
+    Done
+  };
+
+  std::array<std::uint8_t, N> _buffer;
+  _State _state = _State::Preamble;
+
+  std::uint32_t _current_pos = 0;
+  std::uint32_t _current_offset = 0;
+  std::uint32_t _expected = 0;
+
+  template<typename T>
+  T _deserializeLength(const std::uint32_t offset)
+  {
+    std::uint8_t* begin = _buffer.data() + offset;
+    T len(0);
+
+    std::memcpy(&len, begin, sizeof(T));
+
+    return len;
+  }
+
+  void _setupNextState()
+  {
+    switch (_state)
+    {
+    case _State::Preamble:
+      _state = _State::Length;
+      _expected = sizeof(typename P::length_type);
+      break;
+    case _State::Length:
+      _state = _State::Body;
+      _expected = _deserializeLength<typename P::length_type>(_current_offset);
+      break;
+    case _State::Body:
+      _state = _State::Done;
+      break;
+    default:
+      break;
+    }
+
+    _current_offset += _current_pos;
+    _current_pos = 0;
+  }
+};
+
+}
+
+#endif //SKULLC_MESSAGING_PARSER_HPP

Tests/CMakeLists.txt

@@ -6,7 +6,7 @@ add_executable(tests
     main.cpp
     ringbuffer.cpp
     packet.cpp
-)
+    parser.cpp)
 
 target_link_libraries(tests
     PUBLIC

Tests/parser.cpp

@@ -0,0 +1,154 @@
+//
+// Created by erki on 27.03.21.
+//
+
+#include <catch2/catch.hpp>
+
+#include <messaging_parser.hpp>
+#include <messaging_packet.hpp>
+
+using Packet = Messaging::Packet<2>;
+
+namespace
+{
+
+std::array<std::uint8_t, 2> test_data = { 'C', 'D' };
+
+std::array<std::uint8_t, 8> getRawData(const std::array<std::uint8_t, 2>& data)
+{
+  std::array<std::uint8_t, 8> raw = {
+          'A', 'A', 0, 0, 0, 0, data[0], data[1]
+  };
+
+  const std::uint32_t len = 2;
+  std::memcpy(raw.data() + 2, &len, 4);
+
+  return raw;
+}
+
+std::array<std::uint8_t, 8> getRawData()
+{
+  return getRawData({'C', 'D'});
+}
+
+}
+
+TEST_CASE("Parser parses raw message successfully.", "[messaging],[parser]")
+{
+  Messaging::Parser<Packet, Packet::totalLength()> parser;
+
+  for (const std::uint8_t& byte : getRawData())
+  {
+    parser.pushByte(byte);
+  }
+
+  REQUIRE(parser.packetReady());
+
+  SECTION("Retrieved packet is correct.")
+  {
+    Packet packet;
+    REQUIRE(parser.getPacket(packet));
+
+    REQUIRE(packet.data_length == 2);
+    REQUIRE(packet.data[0] == 'C');
+    REQUIRE(packet.data[1] == 'D');
+  }
+}
+
+TEST_CASE("Parser ignores extra bytes when done.", "[messaging],[parser]")
+{
+  Messaging::Parser<Packet, Packet::totalLength()> parser;
+
+  for (const std::uint8_t& byte : getRawData())
+  {
+    parser.pushByte(byte);
+  }
+
+  REQUIRE(parser.packetReady());
+
+  for (const std::uint8_t& byte : getRawData({ 'E', 'F' }))
+  {
+    parser.pushByte(byte);
+  }
+
+  SECTION("Retrieved packet is correct.")
+  {
+    Packet packet;
+    REQUIRE(parser.getPacket(packet));
+
+    REQUIRE(packet.data_length == 2);
+    REQUIRE(packet.data[0] == 'C');
+    REQUIRE(packet.data[1] == 'D');
+  }
+}
+
+TEST_CASE("Parser ignores junk data until header is spotted.", "[messaging],[parser]")
+{
+  Messaging::Parser<Packet, Packet::totalLength()> parser;
+  const std::array<std::uint8_t, 8> junk_data = {
+          'E', 'F', 'A', 'H', 'I', 'J', 'K', 'L'
+  };
+
+  for (const std::uint8_t& byte : junk_data)
+  {
+    parser.pushByte(byte);
+  }
+
+  REQUIRE(parser.packetReady() == false);
+
+  SECTION("Valid packet after junk is parsed successfully.")
+  {
+    for (const std::uint8_t& byte : getRawData())
+    {
+      parser.pushByte(byte);
+    }
+
+    REQUIRE(parser.packetReady());
+
+    SECTION("Retrieved packet is correct.")
+    {
+      Packet packet;
+      REQUIRE(parser.getPacket(packet));
+
+      REQUIRE(packet.data_length == 2);
+      REQUIRE(packet.data[0] == 'C');
+      REQUIRE(packet.data[1] == 'D');
+    }
+  }
+}
+
+TEST_CASE("Parser resets successfully when required.", "[messaging],[parser]")
+{
+  Messaging::Parser<Packet, Packet::totalLength()> parser;
+
+  for (const std::uint8_t& byte : getRawData())
+  {
+    parser.pushByte(byte);
+  }
+
+  REQUIRE(parser.packetReady());
+
+  parser.reset();
+
+  REQUIRE(parser.packetReady() == false);
+
+  SECTION("Follow-up packet is parsed successfully.")
+  {
+    for (const std::uint8_t& byte : getRawData())
+    {
+      parser.pushByte(byte);
+    }
+
+    REQUIRE(parser.packetReady());
+
+    SECTION("Retrieved packet is correct.")
+    {
+      Packet packet;
+      REQUIRE(parser.getPacket(packet));
+
+      REQUIRE(packet.data_length == 2);
+      REQUIRE(packet.data[0] == 'C');
+      REQUIRE(packet.data[1] == 'D');
+    }
+  }
+}