Messaging: simple and fun packet parser implementation.

Messaging/Inc/messaging_packet.hpp

@@ -10,21 +10,51 @@
 
 #include <cstring>
 #include <cstdint>
+#include <array>
 
 namespace Messaging
 {
 
-template<size_t N>
+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];
+  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(), data_in, to_copy_length);
+    data_length = to_copy_length;
+  }
+
+  template<std::size_t data_in_length>
+  void copy_data_in(const std::array<std::uint8_t, data_in_length>& data_in)
+  {
+    const std::uint32_t to_copy_length = std::min(std::uint32_t(data_in_length), max_data_length);
+
+    std::memcpy(data.data(), data_in.data(), to_copy_length);
+    data_length = to_copy_length;
+  }
+
   bool serialize(std::uint8_t* buffer, const std::uint32_t max_length)
   {
     const std::uint32_t required_size = preamble_length + data_length_length + data_length;
@@ -38,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;
 
@@ -60,12 +90,15 @@ 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;
   }
 };
 
+template <std::size_t N>
+constexpr std::uint8_t Packet<N>::preamble[2];
+
 }
 
 

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

@@ -5,11 +5,13 @@ find_package(Catch2 REQUIRED)
 add_executable(tests
     main.cpp
     ringbuffer.cpp
-)
+    packet.cpp
+    parser.cpp)
 
 target_link_libraries(tests
     PUBLIC
         skullc::utility
+        skullc::messaging
         Catch2::Catch2
 )
 

Tests/packet.cpp

@@ -0,0 +1,149 @@
+//
+// Created by erki on 27.03.21.
+//
+
+#include <catch2/catch.hpp>
+
+#include <messaging_packet.hpp>
+
+TEST_CASE("Packet copy_data_in copies data in.", "[messaging],[packet]")
+{
+  Messaging::Packet<2> packet;
+
+  SECTION("Plain C-array data copies properly.")
+  {
+    const std::uint8_t data[2] = { 'C', 'D' };
+    const std::uint32_t data_size = sizeof(data);
+
+    packet.copy_data_in(data);
+
+    REQUIRE(packet.data_length == data_size);
+    REQUIRE(packet.data[0] == data[0]);
+    REQUIRE(packet.data[1] == data[1]);
+  }
+
+  SECTION("STL data copies properly.")
+  {
+    const std::array<std::uint8_t, 2> data = { 'C', 'D' };
+
+    packet.copy_data_in(data);
+
+    REQUIRE(packet.data_length == data.size());
+    REQUIRE(packet.data[0] == data[0]);
+    REQUIRE(packet.data[1] == data[1]);
+  }
+}
+
+TEST_CASE("Packet copying data in cuts off excess bytes.", "[messaging],[packet]")
+{
+  Messaging::Packet<2> packet;
+
+  const std::uint8_t data[4] = { 'C', 'D', 'E', 'F' };
+
+  packet.copy_data_in(data);
+
+  REQUIRE(packet.data_length == 2);
+  REQUIRE(packet.data[0] == 'C');
+  REQUIRE(packet.data[1] == 'D');
+}
+
+TEST_CASE("Packet gets serialized properly.", "[messaging],[packet]")
+{
+  Messaging::Packet<2> packet;
+
+  const std::uint8_t data[2] = { 'C', 'D' };
+  const std::uint32_t data_size = sizeof(data);
+
+  packet.copy_data_in(data);
+
+  std::uint8_t output[data_size + 2 + 4];
+  REQUIRE(packet.serialize(output, sizeof(output)));
+  REQUIRE(packet.data_length == 2);
+
+  SECTION("Preamble gets serialized properly.")
+  {
+    REQUIRE(output[0] == 'A');
+    REQUIRE(output[1] == 'A');
+  }
+
+  SECTION("Length gets serialized properly.")
+  {
+    std::uint32_t length;
+
+    std::memcpy(&length, output + 2, sizeof(length));
+    REQUIRE(length == 2);
+  }
+
+  SECTION("Data gets serialized properly.")
+  {
+    const std::uint32_t offset = sizeof(data_size) + 2;
+    REQUIRE(output[offset + 0] == 'C');
+    REQUIRE(output[offset + 1] == 'D');
+  }
+}
+
+TEST_CASE("Packet serialization fails if buffer too small.", "[messaging],[packet]")
+{
+  Messaging::Packet<2> packet;
+
+  const std::uint8_t data[2] = { 'C', 'D' };
+  packet.copy_data_in(data);
+
+  std::uint8_t output[4] = { 0 };
+  REQUIRE(packet.serialize(output, sizeof(output)) == false);
+
+  SECTION("Output buffer is left unmodified.")
+  {
+    for (const std::uint8_t& u : output)
+    {
+      REQUIRE(u == 0);
+    }
+  }
+}
+
+TEST_CASE("Packet deserialization works as expected.", "[messaging],[packet]")
+{
+  std::uint8_t data[] = {
+      'A', 'A', 0, 0, 0, 0, 'C', 'D'
+  };
+
+  const std::uint32_t data_length = 2;
+  std::memcpy(data + 2, &data_length, sizeof(data_length));
+
+  Messaging::Packet<2> packet;
+
+  REQUIRE(packet.deserialize(data, sizeof(data)));
+  REQUIRE(packet.data_length == data_length);
+  REQUIRE(packet.data[0] == 'C');
+  REQUIRE(packet.data[1] == 'D');
+}
+
+TEST_CASE("Packet deserialization fails with invalid conditions.", "[messaging],[packet]")
+{
+  std::uint8_t data[] = {
+      'A', 'A', 0, 0, 0, 0, 'C', 'D'
+  };
+
+  const std::uint32_t data_length = 2;
+  std::memcpy(data + 2, &data_length, sizeof(data_length));
+
+  Messaging::Packet<2> packet;
+
+  SECTION("Invalid preamble causes failure.")
+  {
+    data[0] = 'B';
+    REQUIRE(packet.deserialize(data, sizeof(data)) == false);
+
+    data[0] = 'A';
+    data[1] = 'B';
+    REQUIRE(packet.deserialize(data, sizeof(data)) == false);
+  }
+
+  SECTION("Data length exceeding expected buffer length causes failure.")
+  {
+    const std::uint32_t data_length_fake = 4;
+    std::memcpy(data + 2, &data_length_fake, sizeof(data_length_fake));
+
+    REQUIRE(packet.deserialize(data, sizeof(data)) == false);
+  }
+}

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');
+    }
+  }
+}

Tests/ringbuffer.cpp

@@ -9,7 +9,7 @@
 template<size_t N>
 using Ringbuffer = Utility::Ringbuffer<int, N>;
 
-TEST_CASE("Ringbuffer iterator", "[ringbuffer],[iterator]")
+TEST_CASE("Ringbuffer iterator", "[peripherals],[ringbuffer]")
 {
   using iterator = Ringbuffer<10>::iterator;
   const auto begin = iterator::pointer(10);
@@ -43,7 +43,7 @@ TEST_CASE("Ringbuffer iterator", "[ringbuffer],[iterator]")
   }
 }
 
-TEST_CASE("Ringbuffer iterator at the end", "[ringbuffer],[iterator]")
+TEST_CASE("Ringbuffer iterator at the end", "[peripherals],[ringbuffer]")
 {
   using iterator = Ringbuffer<10>::iterator;
   const auto begin = iterator::pointer(10);
@@ -71,7 +71,7 @@ TEST_CASE("Ringbuffer iterator at the end", "[ringbuffer],[iterator]")
   }
 }
 
-TEST_CASE("Ringbuffer iterator at the beginning", "[ringbuffer],[iterator]")
+TEST_CASE("Ringbuffer iterator at the beginning", "[peripherals],[ringbuffer]")
 {
   using iterator = Ringbuffer<10>::iterator;
   const auto begin = iterator::pointer(10);
@@ -87,7 +87,7 @@ TEST_CASE("Ringbuffer iterator at the beginning", "[ringbuffer],[iterator]")
   }
 }
 
-TEST_CASE("Constructed buffer is empty.", "[ringbuffer]")
+TEST_CASE("Constructed buffer is empty.", "[peripherals],[ringbuffer]")
 {
   Ringbuffer<10> buffer;
 
@@ -96,7 +96,7 @@ TEST_CASE("Constructed buffer is empty.", "[ringbuffer]")
   REQUIRE(buffer.empty());
 }
 
-TEST_CASE("Buffer reports size properly.", "[ringbuffer]")
+TEST_CASE("Buffer reports size properly.", "[peripherals],[ringbuffer]")
 {
   Ringbuffer<3> buffer;
 
@@ -140,7 +140,7 @@ TEST_CASE("Buffer reports size properly.", "[ringbuffer]")
   }
 }
 
-TEST_CASE("Adding single element.", "[ringbuffer]")
+TEST_CASE("Adding single element.", "[peripherals],[ringbuffer]")
 {
   Ringbuffer<10> buffer;
   const auto old_end = buffer.end();
@@ -191,7 +191,7 @@ TEST_CASE("Adding single element.", "[ringbuffer]")
   }
 }
 
-TEST_CASE("Adding multiple elements.", "[ringbuffer]")
+TEST_CASE("Adding multiple elements.", "[peripherals],[ringbuffer]")
 {
   Ringbuffer<10> buffer;
   const auto old_begin = buffer.begin();
@@ -223,7 +223,7 @@ TEST_CASE("Adding multiple elements.", "[ringbuffer]")
   }
 }
 
-TEST_CASE("Removing elements from the ringbuffer.", "[ringbuffer]")
+TEST_CASE("Removing elements from the ringbuffer.", "[peripherals],[ringbuffer]")
 {
   Ringbuffer<10> buffer;
   const auto old_begin = buffer.begin();
@@ -271,7 +271,7 @@ TEST_CASE("Removing elements from the ringbuffer.", "[ringbuffer]")
   }
 }
 
-TEST_CASE("Clearing a ringbuffer works.", "[ringbuffer]")
+TEST_CASE("Clearing a ringbuffer works.", "[peripherals],[ringbuffer]")
 {
   Ringbuffer<10> buffer;
 
@@ -293,7 +293,7 @@ TEST_CASE("Clearing a ringbuffer works.", "[ringbuffer]")
   }
 }
 
-TEST_CASE("Manually incrementing tail works.", "[ringbuffer]")
+TEST_CASE("Manually incrementing tail works.", "[peripherals],[ringbuffer]")
 {
   Ringbuffer<10> buffer;
 
@@ -312,7 +312,7 @@ TEST_CASE("Manually incrementing tail works.", "[ringbuffer]")
   }
 }
 
-TEST_CASE("Manually incrementing tail when full deletes data.", "[ringbuffer]")
+TEST_CASE("Manually incrementing tail when full deletes data.", "[peripherals],[ringbuffer]")
 {
   Ringbuffer<2> buffer;