diff --git a/Messaging/Inc/messaging_packet.hpp b/Messaging/Inc/messaging_packet.hpp index 46b7f80..3caa0e1 100644 --- a/Messaging/Inc/messaging_packet.hpp +++ b/Messaging/Inc/messaging_packet.hpp @@ -18,20 +18,31 @@ namespace Messaging template struct Packet { + using length_type = std::uint32_t; + static constexpr std::uint8_t preamble[] = { 'A', 'A' }; - std::uint8_t data[N] = { 0 }; + std::array 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 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; } diff --git a/Messaging/Inc/messaging_parser.hpp b/Messaging/Inc/messaging_parser.hpp new file mode 100644 index 0000000..d4393d8 --- /dev/null +++ b/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 +#include +#include + +namespace Messaging +{ + +template +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 _buffer; + _State _state = _State::Preamble; + + std::uint32_t _current_pos = 0; + std::uint32_t _current_offset = 0; + std::uint32_t _expected = 0; + + template + 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(_current_offset); + break; + case _State::Body: + _state = _State::Done; + break; + default: + break; + } + + _current_offset += _current_pos; + _current_pos = 0; + } +}; + +} + +#endif //SKULLC_MESSAGING_PARSER_HPP diff --git a/Tests/CMakeLists.txt b/Tests/CMakeLists.txt index b9a5fb2..39fb3a0 100644 --- a/Tests/CMakeLists.txt +++ b/Tests/CMakeLists.txt @@ -6,7 +6,7 @@ add_executable(tests main.cpp ringbuffer.cpp packet.cpp -) + parser.cpp) target_link_libraries(tests PUBLIC diff --git a/Tests/parser.cpp b/Tests/parser.cpp new file mode 100644 index 0000000..a7db3b7 --- /dev/null +++ b/Tests/parser.cpp @@ -0,0 +1,154 @@ +// +// Created by erki on 27.03.21. +// + +#include + +#include +#include + +using Packet = Messaging::Packet<2>; + +namespace +{ + +std::array test_data = { 'C', 'D' }; + +std::array getRawData(const std::array& data) +{ + std::array 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 getRawData() +{ + return getRawData({'C', 'D'}); +} + +} + +TEST_CASE("Parser parses raw message successfully.", "[messaging],[parser]") +{ + Messaging::Parser 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 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 parser; + const std::array 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 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'); + } + } +}