From ea2f531450754daaa775cacd8e2d541275835a72 Mon Sep 17 00:00:00 2001 From: Dmytro Ochkas Date: Fri, 26 Jun 2026 11:54:39 +0200 Subject: [PATCH 01/24] Integrate Nano CBOR leaving the old cbor library as a backup --- .gitmodules | 3 + CMakeLists.txt | 31 +++- config.cmake | 1 + src/desert_classes/CBorStream.cpp | 286 +++++++++++++++++++++++++----- src/desert_classes/CBorStream.h | 112 +++++++----- thirdparty/NanoCBOR | 1 + 6 files changed, 337 insertions(+), 97 deletions(-) create mode 100644 .gitmodules create mode 100644 config.cmake create mode 160000 thirdparty/NanoCBOR diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..664718b --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "thirdparty/NanoCBOR"] + path = thirdparty/NanoCBOR + url = https://github.com/bergzand/NanoCBOR diff --git a/CMakeLists.txt b/CMakeLists.txt index 096b663..b1a1749 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,6 +2,8 @@ cmake_minimum_required(VERSION 3.5) project(rmw_desert) +include(config.cmake) + # Default to C++20 if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 20) @@ -52,15 +54,30 @@ add_library(rmw_desert src/desert_classes/Discovery.cpp src/desert_classes/TopicsConfig.cpp src/desert_classes/demangle.cpp - - ${CBOR_ROOT}/src/common.c - ${CBOR_ROOT}/src/parser.c - ${CBOR_ROOT}/src/decoder.c - ${CBOR_ROOT}/src/encoder.c - ${CBOR_ROOT}/src/helper.c - ${CBOR_ROOT}/src/ieee754.c ) +if (${CBOR_LIB} STREQUAL "libmcu_cbor") + set(CBOR_ROOT cbor) + target_sources(rmw_desert PRIVATE + ${CBOR_ROOT}/src/common.c + ${CBOR_ROOT}/src/parser.c + ${CBOR_ROOT}/src/decoder.c + ${CBOR_ROOT}/src/encoder.c + ${CBOR_ROOT}/src/helper.c + ${CBOR_ROOT}/src/ieee754.c + ) + target_compile_definitions(rmw_desert PRIVATE LIBMCU_CBOR_ENABLED) +elseif (${CBOR_LIB} STREQUAL "NanoCBOR") + set(CBOR_ROOT thirdparty/NanoCBOR) + target_sources(rmw_desert PRIVATE + ${CBOR_ROOT}/src/decoder.c + ${CBOR_ROOT}/src/encoder.c + ) + target_compile_definitions(rmw_desert PRIVATE NANOCBOR_ENABLED) +else () + message(FATAL_ERROR "At least one CBOR libraty is required") +endif () + INCLUDE (FindPkgConfig) # specific order: dependents before dependencies diff --git a/config.cmake b/config.cmake new file mode 100644 index 0000000..15333c9 --- /dev/null +++ b/config.cmake @@ -0,0 +1 @@ +set(CBOR_LIB "libmcu_cbor" CACHE STRING "Cbor library.") # libmcu_cbor, NanoCBOR \ No newline at end of file diff --git a/src/desert_classes/CBorStream.cpp b/src/desert_classes/CBorStream.cpp index 9f43d3c..3aef14d 100644 --- a/src/desert_classes/CBorStream.cpp +++ b/src/desert_classes/CBorStream.cpp @@ -1,5 +1,17 @@ #include "CBorStream.h" +#include + +#ifdef LIBMCU_CBOR_ENABLED +#include "half.hpp" +#include "cbor/encoder.h" +#include "cbor/decoder.h" +#endif + +#ifdef NANOCBOR_ENABLED +#include +#endif + namespace cbor { @@ -16,16 +28,22 @@ void TxStream::new_packet() { // Initialize packet and writer _packet = new uint8_t[MAX_PACKET_LENGTH]; +#ifdef LIBMCU_CBOR_ENABLED _writer = new cbor_writer_t; - cbor_writer_init(_writer, _packet, MAX_PACKET_LENGTH); +#endif + +#ifdef NANOCBOR_ENABLED + _encoder = new nanocbor_encoder_t; + nanocbor_encoder_init(_encoder, _packet, MAX_PACKET_LENGTH); +#endif } void TxStream::start_transmission(uint64_t sequence_id) { new_packet(); _overflow = false; - + // Stream type, service name/identifier and sequence id *this << _stream_type; *this << _stream_identifier; @@ -36,7 +54,7 @@ void TxStream::start_transmission() { new_packet(); _overflow = false; - + // Stream type and topic name/identifier *this << _stream_type; *this << _stream_identifier; @@ -44,25 +62,47 @@ void TxStream::start_transmission() void TxStream::end_transmission() { + // TODO: Memory leak !!! if (_overflow) return; - - std::vector daemon_packet; - - for(size_t i=0; i < cbor_writer_len(_writer); i++) + +#ifdef LIBMCU_CBOR_ENABLED + size_t encoded_len = cbor_writer_len(_writer); +#endif + +#ifdef NANOCBOR_ENABLED + size_t encoded_len = nanocbor_encoded_len(_encoder); +#endif + + std::vector daemon_packet(encoded_len); + + for(size_t i=0; i < encoded_len; i++) { daemon_packet.push_back(_packet[i]); } - + TcpDaemon::enqueue_packet(daemon_packet); - + delete _packet; +#ifdef LIBMCU_CBOR_ENABLED delete _writer; +#endif + +#ifdef NANOCBOR_ENABLED + delete _encoder; +#endif } TxStream & TxStream::operator<<(const uint64_t n) { +#ifdef LIBMCU_CBOR_ENABLED cbor_error_t result = cbor_encode_unsigned_integer(_writer, n); +#endif + +#ifdef NANOCBOR_ENABLED + nanocbor_error_t result = (nanocbor_error_t) nanocbor_fmt_uint(_encoder, n); +#endif + handle_overrun(result); return *this; } @@ -87,14 +127,28 @@ TxStream & TxStream::operator<<(const uint8_t n) TxStream & TxStream::operator<<(const int64_t n) { +#ifdef LIBMCU_CBOR_ENABLED cbor_error_t result; - + if (n >= 0) result = cbor_encode_unsigned_integer(_writer, n); else result = cbor_encode_negative_integer(_writer, n); - +#endif + +#ifdef NANOCBOR_ENABLED + nanocbor_error_t result; + + if (n >= 0) { + result = (nanocbor_error_t) nanocbor_fmt_uint(_encoder, n); + } else { + result = (nanocbor_error_t) nanocbor_fmt_int(_encoder, n); + } + +#endif + handle_overrun(result); + return *this; } @@ -125,7 +179,14 @@ TxStream & TxStream::operator<<(const char n) TxStream & TxStream::operator<<(const float f) { +#ifdef LIBMCU_CBOR_ENABLED cbor_error_t result = cbor_encode_float(_writer, f); +#endif + +#ifdef NANOCBOR_ENABLED + nanocbor_error_t result = (nanocbor_error_t) nanocbor_fmt_float(_encoder, f); +#endif + handle_overrun(result); return *this; } @@ -138,7 +199,14 @@ TxStream & TxStream::operator<<(const double d) TxStream & TxStream::operator<<(const std::string s) { +#ifdef LIBMCU_CBOR_ENABLED cbor_error_t result = cbor_encode_text_string(_writer, s.c_str(), s.size()); +#endif + +#ifdef NANOCBOR_ENABLED + nanocbor_error_t result = (nanocbor_error_t) nanocbor_put_tstrn(_encoder, s.c_str(), s.size()); +#endif + handle_overrun(result); return *this; } @@ -152,7 +220,14 @@ TxStream & TxStream::operator<<(const std::u16string s) TxStream & TxStream::operator<<(const bool b) { +#ifdef LIBMCU_CBOR_ENABLED cbor_error_t result = cbor_encode_bool(_writer, b); +#endif + +#ifdef NANOCBOR_ENABLED + nanocbor_error_t result = (nanocbor_error_t) nanocbor_fmt_bool(_encoder, b); +#endif + handle_overrun(result); return *this; } @@ -167,12 +242,21 @@ TxStream & TxStream::operator<<(const std::vector v) return *this; } -void TxStream::handle_overrun(cbor_error_t result) +void TxStream::handle_overrun(cbor_error_type result) { +#ifdef LIBMCU_CBOR_ENABLED if (result == CBOR_OVERRUN) { _overflow = true; } +#endif + +#ifdef NANOCBOR_ENABLED + if (result == NANOCBOR_ERR_END) + { + _overflow = true; + } +#endif } std::string TxStream::toUTF8(const std::u16string source) @@ -218,11 +302,11 @@ bool RxStream::data_available(int64_t sequence_id) // If a packet is already buffered, so do not overwrite it and wait for a clear if (_buffered_packet.size() > 0) return true; - + // No buffered packets, go on checking for other ones if (_received_packets.size() == 0) return false; - + // The received packets queue is not empty, so examine it if (sequence_id) { @@ -305,7 +389,7 @@ RxStream & RxStream::deserialize_integer(T & n) n = *static_cast(_buffered_packet[_buffered_iterator].first); _buffered_iterator++; - + return *this; } @@ -315,7 +399,7 @@ RxStream & RxStream::operator>>(char & n) n = (*static_cast(_buffered_packet[_buffered_iterator].first))[0]; _buffered_iterator++; - + return *this; } @@ -325,7 +409,7 @@ RxStream & RxStream::operator>>(float & f) f = *static_cast(_buffered_packet[_buffered_iterator].first); _buffered_iterator++; - + return *this; } @@ -343,7 +427,7 @@ RxStream & RxStream::operator>>(std::string & s) s = *static_cast(_buffered_packet[_buffered_iterator].first); _buffered_iterator++; - + return *this; } @@ -351,7 +435,7 @@ RxStream & RxStream::operator>>(std::u16string & s) { std::string str; *this >> str; - + s = toUTF16(str); return *this; } @@ -359,14 +443,14 @@ RxStream & RxStream::operator>>(std::u16string & s) RxStream & RxStream::operator>>(bool & b) { int8_t b_; - - if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == CBOR_ITEM_SIMPLE_VALUE) + + if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == CBOR_ITEM_SIMPLE_VALUE) { b_ = *static_cast(_buffered_packet[_buffered_iterator].first); + b = b_ ? true : false; + + _buffered_iterator++; + } - b = b_ ? true : false; - - _buffered_iterator++; - return *this; } @@ -406,41 +490,69 @@ void RxStream::push_packet(std::vector> packet) void RxStream::interpret_packets() { std::lock_guard lock(_rx_mutex); - + std::vector packet; for (packet = TcpDaemon::read_packet(); packet.size() != 0; packet = TcpDaemon::read_packet()) { // Initialize buffer and reader uint8_t * buffer = &packet[0]; +#ifdef LIBMCU_CBOR_ENABLED cbor_reader_t reader; cbor_item_t items[64]; size_t n; cbor_reader_init(&reader, items, sizeof(items) / sizeof(items[0])); cbor_parse(&reader, buffer, packet.size(), &n); - +#endif +#ifdef NANOCBOR_ENABLED + nanocbor_value_t decoder; + nanocbor_decoder_init(&decoder, buffer, packet.size()); +#endif + uint8_t stream_type; uint8_t stream_identifier; std::string stream_name; - + std::vector> interpreted_packet; - - for (size_t i = 0; i < n; i++) + +#ifdef LIBMCU_CBOR_ENABLED +#define WRITER_EXIT_COND (i < n) +#endif +#ifdef NANOCBOR_ENABLED +#define WRITER_EXIT_COND (!nanocbor_at_end(&decoder)) +#endif + + for (size_t i = 0; WRITER_EXIT_COND; i++) { +#ifdef LIBMCU_CBOR_ENABLED union _cbor_value val; - memset(&val, 0, sizeof(val)); cbor_decode(&reader, &items[i], &val, sizeof(val)); - +#endif + if (i == 0) { +#ifdef LIBMCU_CBOR_ENABLED stream_type = val.i8; +#endif +#ifdef NANOCBOR_ENABLED + if (nanocbor_get_uint8(&decoder, &stream_type) < 0) { + break; + } +#endif } else if (i == 1) { +#ifdef LIBMCU_CBOR_ENABLED stream_identifier = val.i8; +#endif +#ifdef NANOCBOR_ENABLED + if (nanocbor_get_uint8(&decoder, &stream_identifier) < 0) { + break; + } +#endif stream_name = TopicsConfig::get_identifier_topic(stream_identifier); - + if (stream_name.empty()) { break; @@ -448,30 +560,115 @@ void RxStream::interpret_packets() } else { +#ifdef LIBMCU_CBOR_ENABLED std::pair field = interpret_field(items, i, val); - +#endif +#ifdef NANOCBOR_ENABLED + std::pair field = interpret_field(&decoder); +#endif if (field.first) { interpreted_packet.push_back(field); } } } - + if (stream_name.empty()) { - continue; + goto clean_and_continue; } - + for (RxStream * stream : _listening_streams) { if (stream->get_type() == _stream_type_match_map.at(stream_type) && stream->get_identifier() == stream_identifier) { + // No deed to free ? stream->push_packet(interpreted_packet); + goto just_continue; + } + } + + clean_and_continue: + for (auto& field : interpreted_packet) { + if (field.first) { + free(field.first); + field.first = nullptr; + field.second = 0; } } + just_continue:; } } +#ifdef NANOCBOR_ENABLED +std::pair RxStream::interpret_field(nanocbor_value_t* cbor_value) { + int cbor_value_type = nanocbor_get_type(cbor_value); + switch (cbor_value_type) { + case NANOCBOR_TYPE_NINT: { + int64_t val; + if (nanocbor_get_int64(cbor_value, &val) < 0) { + goto error; + } + int64_t * number = new int64_t{val}; + return std::make_pair(number, NANOCBOR_TYPE_NINT); + } + case NANOCBOR_TYPE_UINT: { + uint64_t val; + if (nanocbor_get_uint64(cbor_value, &val) < 0) { + goto error; + } + uint64_t * number = new uint64_t{val}; + return std::make_pair(number, NANOCBOR_TYPE_UINT); + } + case NANOCBOR_TYPE_FLOAT: { + double val; + if (nanocbor_get_double(cbor_value, &val) < 0) { + goto error; + } + double* f = new double{val}; + return std::make_pair(static_cast(f), NANOCBOR_TYPE_FLOAT); + } + case NANOCBOR_TYPE_TSTR: { + const char* val; + size_t val_size; + if (nanocbor_get_tstr(cbor_value, (const uint8_t**) &val, &val_size) < 0) { + goto error; + } + std::string* s = new std::string(val, val_size); + return std::make_pair(s, NANOCBOR_TYPE_TSTR); + } + case NANOCBOR_TYPE_BSTR: { + const char* val; + size_t val_size; + if (nanocbor_get_bstr(cbor_value, (const uint8_t**) &val, &val_size) < 0) { + goto error; + } + std::string* s = new std::string(val, val_size); + return std::make_pair(s, NANOCBOR_TYPE_TSTR); + } + // Not sure + case NANOCBOR_SIMPLE_FALSE: + case NANOCBOR_SIMPLE_TRUE: + case NANOCBOR_SIMPLE_NULL: + case NANOCBOR_SIMPLE_UNDEF: { + uint8_t val; + if (nanocbor_get_simple(cbor_value, &val) < 0) { + goto error; + } + uint8_t * simple = new uint8_t{val}; + return std::make_pair(simple, NANOCBOR_SIMPLE_FALSE); + } + default: + nanocbor_skip(cbor_value); + goto error; + } + + error: + return std::make_pair(nullptr, 0); +} +#endif + +#ifdef LIBMCU_CBOR_ENABLED std::pair RxStream::interpret_field(cbor_item_t * items, size_t i, union _cbor_value & val) { switch (items[i].type) @@ -479,17 +676,17 @@ std::pair RxStream::interpret_field(cbor_item_t * items, size_t i, case CBOR_ITEM_INTEGER: { int64_t * number = new int64_t{val.i64}; - + return std::make_pair(static_cast(number), CBOR_ITEM_INTEGER); } case CBOR_ITEM_FLOAT: { float * number; - + if (abs(val.i32) < 65536) { int16_t * bin = new int16_t{val.i16}; - + half_float::half * f16 = reinterpret_cast(bin); number = new float{*f16}; } @@ -497,26 +694,27 @@ std::pair RxStream::interpret_field(cbor_item_t * items, size_t i, { number = new float{val.f32}; } - + return std::make_pair(static_cast(number), CBOR_ITEM_FLOAT); } - case CBOR_ITEM_STRING: + case CBOR_ITEM_STRING: { val.str_copy[items[i].size] = '\0'; std::string * str = new std::string{reinterpret_cast(val.str_copy)}; - + return std::make_pair(static_cast(str), CBOR_ITEM_STRING); } case CBOR_ITEM_SIMPLE_VALUE: { int8_t * value = new int8_t{val.i8}; - + return std::make_pair(static_cast(value), CBOR_ITEM_SIMPLE_VALUE); } default: return std::make_pair(nullptr, 0); } } +#endif std::u16string RxStream::toUTF16(const std::string source) { diff --git a/src/desert_classes/CBorStream.h b/src/desert_classes/CBorStream.h index 91a79cb..0c73070 100644 --- a/src/desert_classes/CBorStream.h +++ b/src/desert_classes/CBorStream.h @@ -52,13 +52,12 @@ /** @endcond */ -#include "cbor/encoder.h" -#include "cbor/ieee754.h" -#include "cbor/decoder.h" +#ifdef LIBMCU_CBOR_ENABLED #include "cbor/parser.h" -#include "cbor/helper.h" - -#include "half.hpp" +#endif +#ifdef NANOCBOR_ENABLED +#include +#endif #define PUBLISHER_TYPE 0 #define SUBSCRIBER_TYPE 1 @@ -67,6 +66,17 @@ #define MAX_BUFFER_CAPACITY 100 +#ifdef LIBMCU_CBOR_ENABLED +#define cbor_error_type cbor_error_t +#endif +#ifdef NANOCBOR_ENABLED +#define cbor_error_type nanocbor_error_t +#define CBOR_ITEM_INTEGER NANOCBOR_TYPE_NINT +#define CBOR_ITEM_STRING NANOCBOR_TYPE_TSTR +#define CBOR_ITEM_FLOAT NANOCBOR_TYPE_FLOAT +#define CBOR_ITEM_SIMPLE_VALUE NANOCBOR_SIMPLE_FALSE +#endif + template > class CircularQueue : public std::queue { public: @@ -94,12 +104,12 @@ class TxStream * @param stream_identifier Identifier of the topic or the service read from configuration */ TxStream(uint8_t stream_type, std::string stream_name, uint8_t stream_identifier); - + /** * @brief Tell the stream to create a new packet * - * Every time a transmission in started, a new empty packet must be - * generated and saved as a private member. Then type, service name + * Every time a transmission in started, a new empty packet must be + * generated and saved as a private member. Then type, service name * and sequence id are put in front of the data. * * @param sequence_id The id of the client service communication @@ -108,15 +118,15 @@ class TxStream /** * @brief Tell the stream to create a new packet * - * Every time a transmission in started, a new empty packet must be - * generated and saved as a private member. Then type and topic name + * Every time a transmission in started, a new empty packet must be + * generated and saved as a private member. Then type and topic name * are put in front of the data. */ void start_transmission(); /** * @brief Tell the stream to send down the packet * - * When the transmission is finished the packet is stored in the + * When the transmission is finished the packet is stored in the * static member of TcpDaemon in order to be sent to DESERT. */ void end_transmission(); @@ -191,7 +201,7 @@ class TxStream * @param b Field to encode */ TxStream & operator<<(const bool b); - + /** * @brief Encode vector * @param v Field to encode @@ -202,13 +212,13 @@ class TxStream *this << static_cast(v.size()); return serialize_sequence(v.data(), v.size()); } - + /** * @brief Encode bool vector * @param v Field to encode */ TxStream & operator<<(const std::vector v); - + /** * @brief Serialize a sequence of uniform elements * @param items Pointer to the first element @@ -228,16 +238,21 @@ class TxStream uint8_t _stream_type; std::string _stream_name; uint8_t _stream_identifier; - + bool _overflow; uint8_t * _packet; +#ifdef LIBMCU_CBOR_ENABLED cbor_writer_t * _writer; - +#endif +#ifdef NANOCBOR_ENABLED + nanocbor_encoder_t * _encoder; +#endif + void new_packet(); - void handle_overrun(cbor_error_t result); - + void handle_overrun(cbor_error_type result); + std::string toUTF8(const std::u16string source); - + }; class RxStream @@ -251,31 +266,31 @@ class RxStream * @param stream_identifier Identifier of the topic or the service read from configuration */ RxStream(uint8_t stream_type, std::string stream_name, uint8_t stream_identifier); - + /** * @brief Destroy the reception stream */ ~RxStream(); - + /** * @brief Check if there are data * - * A map contains the information received for all topics and services, - * so using the name saved in the current instance as key it is possible + * A map contains the information received for all topics and services, + * so using the name saved in the current instance as key it is possible * to know if a message is arrived for a specific entity. * * @param sequence_id The id of the client service communication */ bool data_available(int64_t sequence_id = 0); - + /** * @brief Clear the currently buffered packet * - * When the packet is read by the entity, this function must be called to clear the buffer + * When the packet is read by the entity, this function must be called to clear the buffer * and allow RxStream to add the next one in the queue. */ void clear_buffer(); - + /** * @brief Decode uint64 * @param n Field to decode @@ -316,14 +331,14 @@ class RxStream * @param n Field to decode */ RxStream & operator>>(int8_t & n); - + /** * @brief Decode a generic integer * @param n Field to decode */ template RxStream & deserialize_integer(T & n); - + /** * @brief Decode char * @param n Field to decode @@ -354,7 +369,7 @@ class RxStream * @param b Field to decode */ RxStream & operator>>(bool & b); - + /** * @brief Decode vector * @param v Field to decode @@ -365,16 +380,16 @@ class RxStream uint32_t size; *this >> size; v.resize(size); - + return deserialize_sequence(v.data(), size); } - + /** * @brief Decode bool vector * @param v Field to decode */ RxStream & operator>>(std::vector & v); - + /** * @brief Deserialize a sequence of uniform elements * @param items Pointer to the first element @@ -389,8 +404,8 @@ class RxStream } return *this; } - - + + /** * @brief Get the stream type of a specific instance * @return Type of the stream @@ -406,7 +421,7 @@ class RxStream * @return Topic identifier of the stream */ uint8_t get_identifier() const; - + /** * @brief Add a packet to _received_packets * @@ -417,30 +432,30 @@ class RxStream * @param packet The packet to add */ void push_packet(std::vector> packet); - + /** * @brief Interpret raw packets and splits them into different communication types * - * Raw packets from TcpDaemon are read and interpreted in order to put them in - * a map where the key allows to distinguish the topic name or the service name, + * Raw packets from TcpDaemon are read and interpreted in order to put them in + * a map where the key allows to distinguish the topic name or the service name, * and eventually the sequence identifier. */ static void interpret_packets(); - + private: uint8_t _stream_type; std::string _stream_name; uint8_t _stream_identifier; - + size_t _buffered_iterator; - + // packets: > std::vector> _buffered_packet; CircularQueue>, MAX_BUFFER_CAPACITY> _received_packets; - + static const std::map _stream_type_match_map; static std::vector _listening_streams; - + union _cbor_value { int8_t i8; int16_t i16; @@ -452,10 +467,15 @@ class RxStream char *str; uint8_t str_copy[128]; }; - + static std::mutex _rx_mutex; - +#ifdef LIBMCU_CBOR_ENABLED static std::pair interpret_field(cbor_item_t * items, size_t i, union _cbor_value & val); +#endif +#ifdef NANOCBOR_ENABLED + static std::pair interpret_field(nanocbor_value_t* cbor_value); +#endif + std::u16string toUTF16(const std::string source); }; diff --git a/thirdparty/NanoCBOR b/thirdparty/NanoCBOR new file mode 160000 index 0000000..0623c45 --- /dev/null +++ b/thirdparty/NanoCBOR @@ -0,0 +1 @@ +Subproject commit 0623c45686f6bc296c35416f3a41a71b148122d7 From be3986f45d444c6affab4e39a2fdf1f31014e65c Mon Sep 17 00:00:00 2001 From: matlin Date: Thu, 2 Jul 2026 14:13:34 +0200 Subject: [PATCH 02/24] Changed CBorStream abstraction paradigm --- CMakeLists.txt | 10 +- config.cmake | 2 +- src/desert_classes/CBorStream.h | 121 ++-- src/desert_classes/LibMcuCBorStream.cpp | 558 ++++++++++++++++++ .../{CBorStream.cpp => NanoCBorStream.cpp} | 300 +++------- src/get_service_endpoint_info.cpp | 1 - 6 files changed, 709 insertions(+), 283 deletions(-) create mode 100644 src/desert_classes/LibMcuCBorStream.cpp rename src/desert_classes/{CBorStream.cpp => NanoCBorStream.cpp} (67%) diff --git a/CMakeLists.txt b/CMakeLists.txt index b1a1749..5e8e86c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,9 +25,6 @@ find_package(rosidl_typesupport_introspection_cpp REQUIRED) find_package(rcutils REQUIRED) find_package(rcpputils REQUIRED) -set(CBOR_ROOT cbor) -include_directories(${CBOR_ROOT}/include) - add_library(rmw_desert src/get_node_info_and_types.cpp src/init.cpp @@ -48,7 +45,6 @@ add_library(rmw_desert src/desert_classes/DesertClient.cpp src/desert_classes/DesertService.cpp src/desert_classes/DesertGuardCondition.cpp - src/desert_classes/CBorStream.cpp src/desert_classes/CStringHelper.cpp src/desert_classes/TcpDaemon.cpp src/desert_classes/Discovery.cpp @@ -58,6 +54,7 @@ add_library(rmw_desert if (${CBOR_LIB} STREQUAL "libmcu_cbor") set(CBOR_ROOT cbor) + include_directories(${CBOR_ROOT}/include) target_sources(rmw_desert PRIVATE ${CBOR_ROOT}/src/common.c ${CBOR_ROOT}/src/parser.c @@ -65,19 +62,22 @@ if (${CBOR_LIB} STREQUAL "libmcu_cbor") ${CBOR_ROOT}/src/encoder.c ${CBOR_ROOT}/src/helper.c ${CBOR_ROOT}/src/ieee754.c + src/desert_classes/LibMcuCBorStream.cpp ) target_compile_definitions(rmw_desert PRIVATE LIBMCU_CBOR_ENABLED) elseif (${CBOR_LIB} STREQUAL "NanoCBOR") set(CBOR_ROOT thirdparty/NanoCBOR) + include_directories(${CBOR_ROOT}/include) target_sources(rmw_desert PRIVATE ${CBOR_ROOT}/src/decoder.c ${CBOR_ROOT}/src/encoder.c + src/desert_classes/NanoCBorStream.cpp ) target_compile_definitions(rmw_desert PRIVATE NANOCBOR_ENABLED) else () message(FATAL_ERROR "At least one CBOR libraty is required") endif () - + INCLUDE (FindPkgConfig) # specific order: dependents before dependencies diff --git a/config.cmake b/config.cmake index 15333c9..d2c5c18 100644 --- a/config.cmake +++ b/config.cmake @@ -1 +1 @@ -set(CBOR_LIB "libmcu_cbor" CACHE STRING "Cbor library.") # libmcu_cbor, NanoCBOR \ No newline at end of file +set(CBOR_LIB "libmcu_cbor" CACHE STRING "Cbor library.") # libmcu_cbor, NanoCBOR diff --git a/src/desert_classes/CBorStream.h b/src/desert_classes/CBorStream.h index 0c73070..92bd8bc 100644 --- a/src/desert_classes/CBorStream.h +++ b/src/desert_classes/CBorStream.h @@ -52,12 +52,7 @@ /** @endcond */ -#ifdef LIBMCU_CBOR_ENABLED -#include "cbor/parser.h" -#endif -#ifdef NANOCBOR_ENABLED -#include -#endif +#include "half.hpp" #define PUBLISHER_TYPE 0 #define SUBSCRIBER_TYPE 1 @@ -66,17 +61,6 @@ #define MAX_BUFFER_CAPACITY 100 -#ifdef LIBMCU_CBOR_ENABLED -#define cbor_error_type cbor_error_t -#endif -#ifdef NANOCBOR_ENABLED -#define cbor_error_type nanocbor_error_t -#define CBOR_ITEM_INTEGER NANOCBOR_TYPE_NINT -#define CBOR_ITEM_STRING NANOCBOR_TYPE_TSTR -#define CBOR_ITEM_FLOAT NANOCBOR_TYPE_FLOAT -#define CBOR_ITEM_SIMPLE_VALUE NANOCBOR_SIMPLE_FALSE -#endif - template > class CircularQueue : public std::queue { public: @@ -104,12 +88,12 @@ class TxStream * @param stream_identifier Identifier of the topic or the service read from configuration */ TxStream(uint8_t stream_type, std::string stream_name, uint8_t stream_identifier); - + /** * @brief Tell the stream to create a new packet * - * Every time a transmission in started, a new empty packet must be - * generated and saved as a private member. Then type, service name + * Every time a transmission in started, a new empty packet must be + * generated and saved as a private member. Then type, service name * and sequence id are put in front of the data. * * @param sequence_id The id of the client service communication @@ -118,15 +102,15 @@ class TxStream /** * @brief Tell the stream to create a new packet * - * Every time a transmission in started, a new empty packet must be - * generated and saved as a private member. Then type and topic name + * Every time a transmission in started, a new empty packet must be + * generated and saved as a private member. Then type and topic name * are put in front of the data. */ void start_transmission(); /** * @brief Tell the stream to send down the packet * - * When the transmission is finished the packet is stored in the + * When the transmission is finished the packet is stored in the * static member of TcpDaemon in order to be sent to DESERT. */ void end_transmission(); @@ -201,7 +185,7 @@ class TxStream * @param b Field to encode */ TxStream & operator<<(const bool b); - + /** * @brief Encode vector * @param v Field to encode @@ -212,13 +196,13 @@ class TxStream *this << static_cast(v.size()); return serialize_sequence(v.data(), v.size()); } - + /** * @brief Encode bool vector * @param v Field to encode */ TxStream & operator<<(const std::vector v); - + /** * @brief Serialize a sequence of uniform elements * @param items Pointer to the first element @@ -235,24 +219,23 @@ class TxStream } private: + // Forward declarations to abstract the implementation + struct WRITER; + struct ERROR; + uint8_t _stream_type; std::string _stream_name; uint8_t _stream_identifier; - + bool _overflow; - uint8_t * _packet; -#ifdef LIBMCU_CBOR_ENABLED - cbor_writer_t * _writer; -#endif -#ifdef NANOCBOR_ENABLED - nanocbor_encoder_t * _encoder; -#endif - + uint8_t * _packet; + WRITER * _writer; + void new_packet(); - void handle_overrun(cbor_error_type result); - + void handle_overrun(ERROR result); + std::string toUTF8(const std::u16string source); - + }; class RxStream @@ -266,31 +249,31 @@ class RxStream * @param stream_identifier Identifier of the topic or the service read from configuration */ RxStream(uint8_t stream_type, std::string stream_name, uint8_t stream_identifier); - + /** * @brief Destroy the reception stream */ ~RxStream(); - + /** * @brief Check if there are data * - * A map contains the information received for all topics and services, - * so using the name saved in the current instance as key it is possible + * A map contains the information received for all topics and services, + * so using the name saved in the current instance as key it is possible * to know if a message is arrived for a specific entity. * * @param sequence_id The id of the client service communication */ bool data_available(int64_t sequence_id = 0); - + /** * @brief Clear the currently buffered packet * - * When the packet is read by the entity, this function must be called to clear the buffer + * When the packet is read by the entity, this function must be called to clear the buffer * and allow RxStream to add the next one in the queue. */ void clear_buffer(); - + /** * @brief Decode uint64 * @param n Field to decode @@ -331,14 +314,14 @@ class RxStream * @param n Field to decode */ RxStream & operator>>(int8_t & n); - + /** * @brief Decode a generic integer * @param n Field to decode */ template RxStream & deserialize_integer(T & n); - + /** * @brief Decode char * @param n Field to decode @@ -369,7 +352,7 @@ class RxStream * @param b Field to decode */ RxStream & operator>>(bool & b); - + /** * @brief Decode vector * @param v Field to decode @@ -380,16 +363,16 @@ class RxStream uint32_t size; *this >> size; v.resize(size); - + return deserialize_sequence(v.data(), size); } - + /** * @brief Decode bool vector * @param v Field to decode */ RxStream & operator>>(std::vector & v); - + /** * @brief Deserialize a sequence of uniform elements * @param items Pointer to the first element @@ -404,8 +387,8 @@ class RxStream } return *this; } - - + + /** * @brief Get the stream type of a specific instance * @return Type of the stream @@ -421,7 +404,7 @@ class RxStream * @return Topic identifier of the stream */ uint8_t get_identifier() const; - + /** * @brief Add a packet to _received_packets * @@ -432,30 +415,33 @@ class RxStream * @param packet The packet to add */ void push_packet(std::vector> packet); - + /** * @brief Interpret raw packets and splits them into different communication types * - * Raw packets from TcpDaemon are read and interpreted in order to put them in - * a map where the key allows to distinguish the topic name or the service name, + * Raw packets from TcpDaemon are read and interpreted in order to put them in + * a map where the key allows to distinguish the topic name or the service name, * and eventually the sequence identifier. */ static void interpret_packets(); - + private: + // Forward declarations to abstract the implementation + struct ITEM; + uint8_t _stream_type; std::string _stream_name; uint8_t _stream_identifier; - + size_t _buffered_iterator; - + // packets: > std::vector> _buffered_packet; CircularQueue>, MAX_BUFFER_CAPACITY> _received_packets; - + static const std::map _stream_type_match_map; static std::vector _listening_streams; - + union _cbor_value { int8_t i8; int16_t i16; @@ -467,15 +453,10 @@ class RxStream char *str; uint8_t str_copy[128]; }; - + static std::mutex _rx_mutex; -#ifdef LIBMCU_CBOR_ENABLED - static std::pair interpret_field(cbor_item_t * items, size_t i, union _cbor_value & val); -#endif -#ifdef NANOCBOR_ENABLED - static std::pair interpret_field(nanocbor_value_t* cbor_value); -#endif - + + static std::pair interpret_field(ITEM * items, size_t i, union _cbor_value & val); std::u16string toUTF16(const std::string source); }; diff --git a/src/desert_classes/LibMcuCBorStream.cpp b/src/desert_classes/LibMcuCBorStream.cpp new file mode 100644 index 0000000..694a810 --- /dev/null +++ b/src/desert_classes/LibMcuCBorStream.cpp @@ -0,0 +1,558 @@ +#include "CBorStream.h" + +#include +#include +#include +#include +#include + +namespace cbor +{ + +// TX stream + +// Forward declarations to abstract the implementation +struct TxStream::WRITER +{ + cbor_writer_t libmcu_writer; +}; + +struct TxStream::ERROR +{ + cbor_error_t libmcu_error; +}; + +TxStream::TxStream(uint8_t stream_type, std::string stream_name, uint8_t stream_identifier) + : _stream_type(stream_type) + , _stream_name(stream_name) + , _stream_identifier(stream_identifier) +{ +} + +void TxStream::new_packet() +{ + // Initialize packet and writer + _packet = new uint8_t[MAX_PACKET_LENGTH]; + _writer = new WRITER; + + cbor_writer_init(&_writer->libmcu_writer, _packet, MAX_PACKET_LENGTH); +} + +void TxStream::start_transmission(uint64_t sequence_id) +{ + new_packet(); + _overflow = false; + + // Stream type, service name/identifier and sequence id + *this << _stream_type; + *this << _stream_identifier; + *this << sequence_id; +} + +void TxStream::start_transmission() +{ + new_packet(); + _overflow = false; + + // Stream type and topic name/identifier + *this << _stream_type; + *this << _stream_identifier; +} + +void TxStream::end_transmission() +{ + if (_overflow) + { + delete _packet; + delete _writer; + return; + } + + std::vector daemon_packet; + + for(size_t i=0; i < cbor_writer_len(&_writer->libmcu_writer); i++) + { + daemon_packet.push_back(_packet[i]); + } + + TcpDaemon::enqueue_packet(daemon_packet); + + delete _packet; + delete _writer; +} + +TxStream & TxStream::operator<<(const uint64_t n) +{ + cbor_error_t result = cbor_encode_unsigned_integer(&_writer->libmcu_writer, n); + handle_overrun(ERROR(result)); + return *this; +} + +TxStream & TxStream::operator<<(const uint32_t n) +{ + *this << static_cast(n); + return *this; +} + +TxStream & TxStream::operator<<(const uint16_t n) +{ + *this << static_cast(n); + return *this; +} + +TxStream & TxStream::operator<<(const uint8_t n) +{ + *this << static_cast(n); + return *this; +} + +TxStream & TxStream::operator<<(const int64_t n) +{ + cbor_error_t result; + + if (n >= 0) + result = cbor_encode_unsigned_integer(&_writer->libmcu_writer, n); + else + result = cbor_encode_negative_integer(&_writer->libmcu_writer, n); + + handle_overrun(ERROR(result)); + return *this; +} + +TxStream & TxStream::operator<<(const int32_t n) +{ + *this << static_cast(n); + return *this; +} + +TxStream & TxStream::operator<<(const int16_t n) +{ + *this << static_cast(n); + return *this; +} + +TxStream & TxStream::operator<<(const int8_t n) +{ + *this << static_cast(n); + return *this; +} + +TxStream & TxStream::operator<<(const char n) +{ + std::string single_char_string(1, n); + *this << single_char_string; + return *this; +} + +TxStream & TxStream::operator<<(const float f) +{ + cbor_error_t result = cbor_encode_float(&_writer->libmcu_writer, f); + handle_overrun(ERROR(result)); + return *this; +} + +TxStream & TxStream::operator<<(const double d) +{ + *this << static_cast(d); + return *this; +} + +TxStream & TxStream::operator<<(const std::string s) +{ + cbor_error_t result = cbor_encode_text_string(&_writer->libmcu_writer, s.c_str(), s.size()); + handle_overrun(ERROR(result)); + return *this; +} + +TxStream & TxStream::operator<<(const std::u16string s) +{ + // Cbor does not support UTF-16 so a conversion to UTF-8 is performed + *this << toUTF8(s); + return *this; +} + +TxStream & TxStream::operator<<(const bool b) +{ + cbor_error_t result = cbor_encode_bool(&_writer->libmcu_writer, b); + handle_overrun(ERROR(result)); + return *this; +} + +TxStream & TxStream::operator<<(const std::vector v) +{ + *this << static_cast(v.size()); + for (size_t i = 0; i < v.size(); ++i) + { + *this << v[i]; + } + return *this; +} + +void TxStream::handle_overrun(ERROR result) +{ + if (result.libmcu_error == CBOR_OVERRUN) + { + _overflow = true; + } +} + +std::string TxStream::toUTF8(const std::u16string source) +{ + std::string result; + + std::wstring_convert, char16_t> convertor; + result = convertor.to_bytes(source); + + return result; +} + + +// RX stream + +// Forward declarations to abstract the implementation +struct RxStream::ITEM +{ + cbor_item_t libmcu_item; +}; + +RxStream::RxStream(uint8_t stream_type, std::string stream_name, uint8_t stream_identifier) + : _stream_type(stream_type) + , _stream_name(stream_name) + , _stream_identifier(stream_identifier) +{ + _listening_streams.push_back(this); +} + +RxStream::~RxStream() +{ + std::vector::iterator position = std::find(_listening_streams.begin(), _listening_streams.end(), this); + if (position != _listening_streams.end()) + _listening_streams.erase(position); +} + +const std::map RxStream::_stream_type_match_map = { + { PUBLISHER_TYPE, SUBSCRIBER_TYPE }, + { CLIENT_TYPE, SERVICE_TYPE }, + { SERVICE_TYPE, CLIENT_TYPE } +}; + +std::vector RxStream::_listening_streams; + +std::mutex RxStream::_rx_mutex; + +bool RxStream::data_available(int64_t sequence_id) +{ + // If a packet is already buffered, so do not overwrite it and wait for a clear + if (_buffered_packet.size() > 0) + return true; + + // No buffered packets, go on checking for other ones + if (_received_packets.size() == 0) + return false; + + // The received packets queue is not empty, so examine it + if (sequence_id) + { + for (size_t i = 0; i < _received_packets.size(); i++) + { + // Check for the first element in the packet, which is the sequence id + if (*static_cast(_received_packets.front().at(0).first) == sequence_id) + { + _buffered_packet = _received_packets.front(); + _received_packets.pop(); + _buffered_iterator = 1; + return true; + } + else + { + _received_packets.push(_received_packets.front()); + _received_packets.pop(); + } + } + return false; + } + else + { + _buffered_packet = _received_packets.front(); + _received_packets.pop(); + _buffered_iterator = 0; + return true; + } +} + +void RxStream::clear_buffer() +{ + _buffered_packet.clear(); +} + +RxStream & RxStream::operator>>(uint64_t & n) +{ + return deserialize_integer(n); +} + +RxStream & RxStream::operator>>(uint32_t & n) +{ + return deserialize_integer(n); +} + +RxStream & RxStream::operator>>(uint16_t & n) +{ + return deserialize_integer(n); +} + +RxStream & RxStream::operator>>(uint8_t & n) +{ + return deserialize_integer(n); +} + +RxStream & RxStream::operator>>(int64_t & n) +{ + return deserialize_integer(n); +} + +RxStream & RxStream::operator>>(int32_t & n) +{ + return deserialize_integer(n); +} + +RxStream & RxStream::operator>>(int16_t & n) +{ + return deserialize_integer(n); +} + +RxStream & RxStream::operator>>(int8_t & n) +{ + return deserialize_integer(n); +} + +template +RxStream & RxStream::deserialize_integer(T & n) +{ + if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == CBOR_ITEM_INTEGER) + n = *static_cast(_buffered_packet[_buffered_iterator].first); + + _buffered_iterator++; + + return *this; +} + +RxStream & RxStream::operator>>(char & n) +{ + if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == CBOR_ITEM_STRING) + n = (*static_cast(_buffered_packet[_buffered_iterator].first))[0]; + + _buffered_iterator++; + + return *this; +} + +RxStream & RxStream::operator>>(float & f) +{ + if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == CBOR_ITEM_FLOAT) + f = *static_cast(_buffered_packet[_buffered_iterator].first); + + _buffered_iterator++; + + return *this; +} + +RxStream & RxStream::operator>>(double & d) +{ + float value; + *this >> value; + d = static_cast(value); + return *this; +} + +RxStream & RxStream::operator>>(std::string & s) +{ + if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == CBOR_ITEM_STRING) + s = *static_cast(_buffered_packet[_buffered_iterator].first); + + _buffered_iterator++; + + return *this; +} + +RxStream & RxStream::operator>>(std::u16string & s) +{ + std::string str; + *this >> str; + + s = toUTF16(str); + return *this; +} + +RxStream & RxStream::operator>>(bool & b) +{ + int8_t b_; + + if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == CBOR_ITEM_SIMPLE_VALUE) + b_ = *static_cast(_buffered_packet[_buffered_iterator].first); + + b = b_ ? true : false; + + _buffered_iterator++; + + return *this; +} + +RxStream & RxStream::operator>>(std::vector & v) +{ + uint32_t size; + *this >> size; + for (size_t i = 0; i < size; ++i) + { + int b; + *this >> b; + v[i] = b ? true : false; + } + return *this; +} + +uint8_t RxStream::get_type() const +{ + return _stream_type; +} + +std::string RxStream::get_name() const +{ + return _stream_name; +} + +uint8_t RxStream::get_identifier() const +{ + return _stream_identifier; +} + +void RxStream::push_packet(std::vector> packet) +{ + _received_packets.push(packet); +} + +void RxStream::interpret_packets() +{ + std::lock_guard lock(_rx_mutex); + + std::vector packet; + for (packet = TcpDaemon::read_packet(); packet.size() != 0; packet = TcpDaemon::read_packet()) + { + // Initialize buffer and reader + uint8_t * buffer = &packet[0]; + cbor_reader_t reader; + ITEM items[64]; + size_t n; + + cbor_reader_init(&reader, &items->libmcu_item, sizeof(items) / sizeof(items[0])); + cbor_parse(&reader, buffer, packet.size(), &n); + + uint8_t stream_type; + uint8_t stream_identifier; + std::string stream_name; + + std::vector> interpreted_packet; + + for (size_t i = 0; i < n; i++) + { + union _cbor_value val; + + memset(&val, 0, sizeof(val)); + cbor_decode(&reader, &items[i].libmcu_item, &val, sizeof(val)); + + if (i == 0) + { + stream_type = val.i8; + } + else if (i == 1) + { + stream_identifier = val.i8; + stream_name = TopicsConfig::get_identifier_topic(stream_identifier); + + if (stream_name.empty()) + { + break; + } + } + else + { + std::pair field = interpret_field(items, i, val); + + if (field.first) + { + interpreted_packet.push_back(field); + } + } + } + + if (stream_name.empty()) + { + continue; + } + + for (RxStream * stream : _listening_streams) + { + if (stream->get_type() == _stream_type_match_map.at(stream_type) && stream->get_identifier() == stream_identifier) + { + stream->push_packet(interpreted_packet); + } + } + } +} + +std::pair RxStream::interpret_field(ITEM * items, size_t i, union _cbor_value & val) +{ + switch (items[i].libmcu_item.type) + { + case CBOR_ITEM_INTEGER: + { + int64_t * number = new int64_t{val.i64}; + + return std::make_pair(static_cast(number), CBOR_ITEM_INTEGER); + } + case CBOR_ITEM_FLOAT: + { + float * number; + + if (abs(val.i32) < 65536) + { + int16_t * bin = new int16_t{val.i16}; + + half_float::half * f16 = reinterpret_cast(bin); + number = new float{*f16}; + } + else + { + number = new float{val.f32}; + } + + return std::make_pair(static_cast(number), CBOR_ITEM_FLOAT); + } + case CBOR_ITEM_STRING: + { + val.str_copy[items[i].libmcu_item.size] = '\0'; + std::string * str = new std::string{reinterpret_cast(val.str_copy)}; + + return std::make_pair(static_cast(str), CBOR_ITEM_STRING); + } + case CBOR_ITEM_SIMPLE_VALUE: + { + int8_t * value = new int8_t{val.i8}; + + return std::make_pair(static_cast(value), CBOR_ITEM_SIMPLE_VALUE); + } + default: + return std::make_pair(nullptr, 0); + } +} + +std::u16string RxStream::toUTF16(const std::string source) +{ + std::u16string result; + + std::wstring_convert, char16_t> convertor; + result = convertor.from_bytes(source); + + return result; +} + +} diff --git a/src/desert_classes/CBorStream.cpp b/src/desert_classes/NanoCBorStream.cpp similarity index 67% rename from src/desert_classes/CBorStream.cpp rename to src/desert_classes/NanoCBorStream.cpp index 3aef14d..567731a 100644 --- a/src/desert_classes/CBorStream.cpp +++ b/src/desert_classes/NanoCBorStream.cpp @@ -1,22 +1,28 @@ #include "CBorStream.h" -#include - -#ifdef LIBMCU_CBOR_ENABLED -#include "half.hpp" -#include "cbor/encoder.h" -#include "cbor/decoder.h" -#endif - -#ifdef NANOCBOR_ENABLED #include -#endif + +#define CBOR_ITEM_INTEGER NANOCBOR_TYPE_NINT +#define CBOR_ITEM_STRING NANOCBOR_TYPE_TSTR +#define CBOR_ITEM_FLOAT NANOCBOR_TYPE_FLOAT +#define CBOR_ITEM_SIMPLE_VALUE NANOCBOR_SIMPLE_FALSE namespace cbor { // TX stream +// Forward declarations to abstract the implementation +struct TxStream::WRITER +{ + nanocbor_encoder_t nanocbor_encoder; +}; + +struct TxStream::ERROR +{ + nanocbor_error_t nanocbor_error; +}; + TxStream::TxStream(uint8_t stream_type, std::string stream_name, uint8_t stream_identifier) : _stream_type(stream_type) , _stream_name(stream_name) @@ -28,15 +34,9 @@ void TxStream::new_packet() { // Initialize packet and writer _packet = new uint8_t[MAX_PACKET_LENGTH]; -#ifdef LIBMCU_CBOR_ENABLED - _writer = new cbor_writer_t; - cbor_writer_init(_writer, _packet, MAX_PACKET_LENGTH); -#endif + _writer = new WRITER; -#ifdef NANOCBOR_ENABLED - _encoder = new nanocbor_encoder_t; - nanocbor_encoder_init(_encoder, _packet, MAX_PACKET_LENGTH); -#endif + nanocbor_encoder_init(&_writer->nanocbor_encoder, _packet, MAX_PACKET_LENGTH); } void TxStream::start_transmission(uint64_t sequence_id) @@ -62,17 +62,14 @@ void TxStream::start_transmission() void TxStream::end_transmission() { - // TODO: Memory leak !!! if (_overflow) + { + delete _packet; + delete _writer; return; + } -#ifdef LIBMCU_CBOR_ENABLED - size_t encoded_len = cbor_writer_len(_writer); -#endif - -#ifdef NANOCBOR_ENABLED - size_t encoded_len = nanocbor_encoded_len(_encoder); -#endif + size_t encoded_len = nanocbor_encoded_len(&_writer->nanocbor_encoder); std::vector daemon_packet(encoded_len); @@ -84,26 +81,13 @@ void TxStream::end_transmission() TcpDaemon::enqueue_packet(daemon_packet); delete _packet; -#ifdef LIBMCU_CBOR_ENABLED delete _writer; -#endif - -#ifdef NANOCBOR_ENABLED - delete _encoder; -#endif } TxStream & TxStream::operator<<(const uint64_t n) { -#ifdef LIBMCU_CBOR_ENABLED - cbor_error_t result = cbor_encode_unsigned_integer(_writer, n); -#endif - -#ifdef NANOCBOR_ENABLED - nanocbor_error_t result = (nanocbor_error_t) nanocbor_fmt_uint(_encoder, n); -#endif - - handle_overrun(result); + nanocbor_error_t result = (nanocbor_error_t) nanocbor_fmt_uint(&_writer->nanocbor_encoder, n); + handle_overrun(ERROR(result)); return *this; } @@ -127,27 +111,18 @@ TxStream & TxStream::operator<<(const uint8_t n) TxStream & TxStream::operator<<(const int64_t n) { -#ifdef LIBMCU_CBOR_ENABLED - cbor_error_t result; + nanocbor_error_t result; if (n >= 0) - result = cbor_encode_unsigned_integer(_writer, n); + { + result = (nanocbor_error_t) nanocbor_fmt_uint(&_writer->nanocbor_encoder, n); + } else - result = cbor_encode_negative_integer(_writer, n); -#endif - -#ifdef NANOCBOR_ENABLED - nanocbor_error_t result; - - if (n >= 0) { - result = (nanocbor_error_t) nanocbor_fmt_uint(_encoder, n); - } else { - result = (nanocbor_error_t) nanocbor_fmt_int(_encoder, n); + { + result = (nanocbor_error_t) nanocbor_fmt_int(&_writer->nanocbor_encoder, n); } -#endif - - handle_overrun(result); + handle_overrun(ERROR(result)); return *this; } @@ -179,15 +154,8 @@ TxStream & TxStream::operator<<(const char n) TxStream & TxStream::operator<<(const float f) { -#ifdef LIBMCU_CBOR_ENABLED - cbor_error_t result = cbor_encode_float(_writer, f); -#endif - -#ifdef NANOCBOR_ENABLED - nanocbor_error_t result = (nanocbor_error_t) nanocbor_fmt_float(_encoder, f); -#endif - - handle_overrun(result); + nanocbor_error_t result = (nanocbor_error_t) nanocbor_fmt_float(&_writer->nanocbor_encoder, f); + handle_overrun(ERROR(result)); return *this; } @@ -199,15 +167,8 @@ TxStream & TxStream::operator<<(const double d) TxStream & TxStream::operator<<(const std::string s) { -#ifdef LIBMCU_CBOR_ENABLED - cbor_error_t result = cbor_encode_text_string(_writer, s.c_str(), s.size()); -#endif - -#ifdef NANOCBOR_ENABLED - nanocbor_error_t result = (nanocbor_error_t) nanocbor_put_tstrn(_encoder, s.c_str(), s.size()); -#endif - - handle_overrun(result); + nanocbor_error_t result = (nanocbor_error_t) nanocbor_put_tstrn(&_writer->nanocbor_encoder, s.c_str(), s.size()); + handle_overrun(ERROR(result)); return *this; } @@ -220,15 +181,8 @@ TxStream & TxStream::operator<<(const std::u16string s) TxStream & TxStream::operator<<(const bool b) { -#ifdef LIBMCU_CBOR_ENABLED - cbor_error_t result = cbor_encode_bool(_writer, b); -#endif - -#ifdef NANOCBOR_ENABLED - nanocbor_error_t result = (nanocbor_error_t) nanocbor_fmt_bool(_encoder, b); -#endif - - handle_overrun(result); + nanocbor_error_t result = (nanocbor_error_t) nanocbor_fmt_bool(&_writer->nanocbor_encoder, b); + handle_overrun(ERROR(result)); return *this; } @@ -242,21 +196,12 @@ TxStream & TxStream::operator<<(const std::vector v) return *this; } -void TxStream::handle_overrun(cbor_error_type result) +void TxStream::handle_overrun(ERROR result) { -#ifdef LIBMCU_CBOR_ENABLED - if (result == CBOR_OVERRUN) + if (result.nanocbor_error == NANOCBOR_ERR_END) { _overflow = true; } -#endif - -#ifdef NANOCBOR_ENABLED - if (result == NANOCBOR_ERR_END) - { - _overflow = true; - } -#endif } std::string TxStream::toUTF8(const std::u16string source) @@ -272,6 +217,12 @@ std::string TxStream::toUTF8(const std::u16string source) // RX stream +// Forward declarations to abstract the implementation +struct RxStream::ITEM +{ + nanocbor_value_t nanocbor_value; +}; + RxStream::RxStream(uint8_t stream_type, std::string stream_name, uint8_t stream_identifier) : _stream_type(stream_type) , _stream_name(stream_name) @@ -444,7 +395,8 @@ RxStream & RxStream::operator>>(bool & b) { int8_t b_; - if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == CBOR_ITEM_SIMPLE_VALUE) { + if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == CBOR_ITEM_SIMPLE_VALUE) + { b_ = *static_cast(_buffered_packet[_buffered_iterator].first); b = b_ ? true : false; @@ -496,18 +448,8 @@ void RxStream::interpret_packets() { // Initialize buffer and reader uint8_t * buffer = &packet[0]; -#ifdef LIBMCU_CBOR_ENABLED - cbor_reader_t reader; - cbor_item_t items[64]; - size_t n; - - cbor_reader_init(&reader, items, sizeof(items) / sizeof(items[0])); - cbor_parse(&reader, buffer, packet.size(), &n); -#endif -#ifdef NANOCBOR_ENABLED - nanocbor_value_t decoder; - nanocbor_decoder_init(&decoder, buffer, packet.size()); -#endif + ITEM decoder; + nanocbor_decoder_init(&decoder.nanocbor_value, buffer, packet.size()); uint8_t stream_type; uint8_t stream_identifier; @@ -515,42 +457,24 @@ void RxStream::interpret_packets() std::vector> interpreted_packet; -#ifdef LIBMCU_CBOR_ENABLED -#define WRITER_EXIT_COND (i < n) -#endif -#ifdef NANOCBOR_ENABLED -#define WRITER_EXIT_COND (!nanocbor_at_end(&decoder)) -#endif - - for (size_t i = 0; WRITER_EXIT_COND; i++) + for (size_t i = 0; !nanocbor_at_end(&decoder.nanocbor_value); i++) { -#ifdef LIBMCU_CBOR_ENABLED union _cbor_value val; - memset(&val, 0, sizeof(val)); - cbor_decode(&reader, &items[i], &val, sizeof(val)); -#endif if (i == 0) { -#ifdef LIBMCU_CBOR_ENABLED - stream_type = val.i8; -#endif -#ifdef NANOCBOR_ENABLED - if (nanocbor_get_uint8(&decoder, &stream_type) < 0) { + if (nanocbor_get_uint8(&decoder.nanocbor_value, &stream_type) < 0) + { break; } -#endif } else if (i == 1) { -#ifdef LIBMCU_CBOR_ENABLED - stream_identifier = val.i8; -#endif -#ifdef NANOCBOR_ENABLED - if (nanocbor_get_uint8(&decoder, &stream_identifier) < 0) { + if (nanocbor_get_uint8(&decoder.nanocbor_value, &stream_identifier) < 0) + { break; } -#endif + stream_name = TopicsConfig::get_identifier_topic(stream_identifier); if (stream_name.empty()) @@ -560,12 +484,8 @@ void RxStream::interpret_packets() } else { -#ifdef LIBMCU_CBOR_ENABLED - std::pair field = interpret_field(items, i, val); -#endif -#ifdef NANOCBOR_ENABLED - std::pair field = interpret_field(&decoder); -#endif + std::pair field = interpret_field(&decoder, 0, val); + if (field.first) { interpreted_packet.push_back(field); @@ -589,8 +509,10 @@ void RxStream::interpret_packets() } clean_and_continue: - for (auto& field : interpreted_packet) { - if (field.first) { + for (auto& field : interpreted_packet) + { + if (field.first) + { free(field.first); field.first = nullptr; field.second = 0; @@ -600,47 +522,60 @@ void RxStream::interpret_packets() } } -#ifdef NANOCBOR_ENABLED -std::pair RxStream::interpret_field(nanocbor_value_t* cbor_value) { - int cbor_value_type = nanocbor_get_type(cbor_value); - switch (cbor_value_type) { - case NANOCBOR_TYPE_NINT: { +std::pair RxStream::interpret_field(ITEM * cbor_value, size_t i, union _cbor_value & val) +{ + (void) i; + (void) val; + int cbor_value_type = nanocbor_get_type(&cbor_value->nanocbor_value); + switch (cbor_value_type) + { + case NANOCBOR_TYPE_NINT: + { int64_t val; - if (nanocbor_get_int64(cbor_value, &val) < 0) { + if (nanocbor_get_int64(&cbor_value->nanocbor_value, &val) < 0) + { goto error; } int64_t * number = new int64_t{val}; return std::make_pair(number, NANOCBOR_TYPE_NINT); } - case NANOCBOR_TYPE_UINT: { + case NANOCBOR_TYPE_UINT: + { uint64_t val; - if (nanocbor_get_uint64(cbor_value, &val) < 0) { + if (nanocbor_get_uint64(&cbor_value->nanocbor_value, &val) < 0) + { goto error; } uint64_t * number = new uint64_t{val}; return std::make_pair(number, NANOCBOR_TYPE_UINT); } - case NANOCBOR_TYPE_FLOAT: { + case NANOCBOR_TYPE_FLOAT: + { double val; - if (nanocbor_get_double(cbor_value, &val) < 0) { + if (nanocbor_get_double(&cbor_value->nanocbor_value, &val) < 0) + { goto error; } double* f = new double{val}; return std::make_pair(static_cast(f), NANOCBOR_TYPE_FLOAT); } - case NANOCBOR_TYPE_TSTR: { + case NANOCBOR_TYPE_TSTR: + { const char* val; size_t val_size; - if (nanocbor_get_tstr(cbor_value, (const uint8_t**) &val, &val_size) < 0) { + if (nanocbor_get_tstr(&cbor_value->nanocbor_value, (const uint8_t**) &val, &val_size) < 0) + { goto error; } std::string* s = new std::string(val, val_size); return std::make_pair(s, NANOCBOR_TYPE_TSTR); } - case NANOCBOR_TYPE_BSTR: { + case NANOCBOR_TYPE_BSTR: + { const char* val; size_t val_size; - if (nanocbor_get_bstr(cbor_value, (const uint8_t**) &val, &val_size) < 0) { + if (nanocbor_get_bstr(&cbor_value->nanocbor_value, (const uint8_t**) &val, &val_size) < 0) + { goto error; } std::string* s = new std::string(val, val_size); @@ -650,71 +585,24 @@ std::pair RxStream::interpret_field(nanocbor_value_t* cbor_value) { case NANOCBOR_SIMPLE_FALSE: case NANOCBOR_SIMPLE_TRUE: case NANOCBOR_SIMPLE_NULL: - case NANOCBOR_SIMPLE_UNDEF: { + case NANOCBOR_SIMPLE_UNDEF: + { uint8_t val; - if (nanocbor_get_simple(cbor_value, &val) < 0) { + if (nanocbor_get_simple(&cbor_value->nanocbor_value, &val) < 0) + { goto error; } uint8_t * simple = new uint8_t{val}; return std::make_pair(simple, NANOCBOR_SIMPLE_FALSE); } default: - nanocbor_skip(cbor_value); + nanocbor_skip(&cbor_value->nanocbor_value); goto error; } error: return std::make_pair(nullptr, 0); } -#endif - -#ifdef LIBMCU_CBOR_ENABLED -std::pair RxStream::interpret_field(cbor_item_t * items, size_t i, union _cbor_value & val) -{ - switch (items[i].type) - { - case CBOR_ITEM_INTEGER: - { - int64_t * number = new int64_t{val.i64}; - - return std::make_pair(static_cast(number), CBOR_ITEM_INTEGER); - } - case CBOR_ITEM_FLOAT: - { - float * number; - - if (abs(val.i32) < 65536) - { - int16_t * bin = new int16_t{val.i16}; - - half_float::half * f16 = reinterpret_cast(bin); - number = new float{*f16}; - } - else - { - number = new float{val.f32}; - } - - return std::make_pair(static_cast(number), CBOR_ITEM_FLOAT); - } - case CBOR_ITEM_STRING: - { - val.str_copy[items[i].size] = '\0'; - std::string * str = new std::string{reinterpret_cast(val.str_copy)}; - - return std::make_pair(static_cast(str), CBOR_ITEM_STRING); - } - case CBOR_ITEM_SIMPLE_VALUE: - { - int8_t * value = new int8_t{val.i8}; - - return std::make_pair(static_cast(value), CBOR_ITEM_SIMPLE_VALUE); - } - default: - return std::make_pair(nullptr, 0); - } -} -#endif std::u16string RxStream::toUTF16(const std::string source) { diff --git a/src/get_service_endpoint_info.cpp b/src/get_service_endpoint_info.cpp index 29d605a..b5f7a68 100644 --- a/src/get_service_endpoint_info.cpp +++ b/src/get_service_endpoint_info.cpp @@ -25,4 +25,3 @@ rmw_ret_t rmw_get_servers_info_by_service(const rmw_node_t * node, rcutils_alloc RMW_SET_ERROR_MSG("rmw_get_servers_info_by_service not implemented"); return RMW_RET_UNSUPPORTED; } - From 45a998c092789c369db99e0291a57a2dc3859444 Mon Sep 17 00:00:00 2001 From: matlin Date: Fri, 3 Jul 2026 13:15:22 +0200 Subject: [PATCH 03/24] Fixed integer static casts and NanoCBOR infinite loops --- src/desert_classes/CBorStream.h | 7 -- src/desert_classes/LibMcuCBorStream.cpp | 59 +++++++--- src/desert_classes/NanoCBorStream.cpp | 143 ++++++++++++------------ 3 files changed, 111 insertions(+), 98 deletions(-) diff --git a/src/desert_classes/CBorStream.h b/src/desert_classes/CBorStream.h index 92bd8bc..a95a47e 100644 --- a/src/desert_classes/CBorStream.h +++ b/src/desert_classes/CBorStream.h @@ -315,13 +315,6 @@ class RxStream */ RxStream & operator>>(int8_t & n); - /** - * @brief Decode a generic integer - * @param n Field to decode - */ - template - RxStream & deserialize_integer(T & n); - /** * @brief Decode char * @param n Field to decode diff --git a/src/desert_classes/LibMcuCBorStream.cpp b/src/desert_classes/LibMcuCBorStream.cpp index 694a810..8275de6 100644 --- a/src/desert_classes/LibMcuCBorStream.cpp +++ b/src/desert_classes/LibMcuCBorStream.cpp @@ -287,52 +287,67 @@ void RxStream::clear_buffer() RxStream & RxStream::operator>>(uint64_t & n) { - return deserialize_integer(n); + int64_t value; + *this >> value; + n = static_cast(value); + return *this; } RxStream & RxStream::operator>>(uint32_t & n) { - return deserialize_integer(n); + int64_t value; + *this >> value; + n = static_cast(value); + return *this; } RxStream & RxStream::operator>>(uint16_t & n) { - return deserialize_integer(n); + int64_t value; + *this >> value; + n = static_cast(value); + return *this; } RxStream & RxStream::operator>>(uint8_t & n) { - return deserialize_integer(n); + int64_t value; + *this >> value; + n = static_cast(value); + return *this; } RxStream & RxStream::operator>>(int64_t & n) { - return deserialize_integer(n); + if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == CBOR_ITEM_INTEGER) + n = *static_cast(_buffered_packet[_buffered_iterator].first); + + _buffered_iterator++; + + return *this; } RxStream & RxStream::operator>>(int32_t & n) { - return deserialize_integer(n); + int64_t value; + *this >> value; + n = static_cast(value); + return *this; } RxStream & RxStream::operator>>(int16_t & n) { - return deserialize_integer(n); + int64_t value; + *this >> value; + n = static_cast(value); + return *this; } RxStream & RxStream::operator>>(int8_t & n) { - return deserialize_integer(n); -} - -template -RxStream & RxStream::deserialize_integer(T & n) -{ - if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == CBOR_ITEM_INTEGER) - n = *static_cast(_buffered_packet[_buffered_iterator].first); - - _buffered_iterator++; - + int64_t value; + *this >> value; + n = static_cast(value); return *this; } @@ -486,6 +501,14 @@ void RxStream::interpret_packets() if (stream_name.empty()) { + for (auto& field : interpreted_packet) + { + if (field.first) + { + free(field.first); + field.first = nullptr; + } + } continue; } diff --git a/src/desert_classes/NanoCBorStream.cpp b/src/desert_classes/NanoCBorStream.cpp index 567731a..b6694cc 100644 --- a/src/desert_classes/NanoCBorStream.cpp +++ b/src/desert_classes/NanoCBorStream.cpp @@ -2,11 +2,6 @@ #include -#define CBOR_ITEM_INTEGER NANOCBOR_TYPE_NINT -#define CBOR_ITEM_STRING NANOCBOR_TYPE_TSTR -#define CBOR_ITEM_FLOAT NANOCBOR_TYPE_FLOAT -#define CBOR_ITEM_SIMPLE_VALUE NANOCBOR_SIMPLE_FALSE - namespace cbor { @@ -71,7 +66,7 @@ void TxStream::end_transmission() size_t encoded_len = nanocbor_encoded_len(&_writer->nanocbor_encoder); - std::vector daemon_packet(encoded_len); + std::vector daemon_packet; for(size_t i=0; i < encoded_len; i++) { @@ -181,8 +176,7 @@ TxStream & TxStream::operator<<(const std::u16string s) TxStream & TxStream::operator<<(const bool b) { - nanocbor_error_t result = (nanocbor_error_t) nanocbor_fmt_bool(&_writer->nanocbor_encoder, b); - handle_overrun(ERROR(result)); + *this << static_cast(b); return *this; } @@ -295,58 +289,76 @@ void RxStream::clear_buffer() RxStream & RxStream::operator>>(uint64_t & n) { - return deserialize_integer(n); + if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == NANOCBOR_TYPE_UINT) + n = *static_cast(_buffered_packet[_buffered_iterator].first); + + _buffered_iterator++; + + return *this; } RxStream & RxStream::operator>>(uint32_t & n) { - return deserialize_integer(n); + uint64_t value; + *this >> value; + n = static_cast(value); + return *this; } RxStream & RxStream::operator>>(uint16_t & n) { - return deserialize_integer(n); + uint64_t value; + *this >> value; + n = static_cast(value); + return *this; } RxStream & RxStream::operator>>(uint8_t & n) { - return deserialize_integer(n); + uint64_t value; + *this >> value; + n = static_cast(value); + return *this; } RxStream & RxStream::operator>>(int64_t & n) { - return deserialize_integer(n); + int type = _buffered_packet[_buffered_iterator].second; + if (_buffered_packet.size() > _buffered_iterator && (type == NANOCBOR_TYPE_UINT || type == NANOCBOR_TYPE_NINT)) + n = *static_cast(_buffered_packet[_buffered_iterator].first); + + _buffered_iterator++; + + return *this; } RxStream & RxStream::operator>>(int32_t & n) { - return deserialize_integer(n); + int64_t value; + *this >> value; + n = static_cast(value); + return *this; } RxStream & RxStream::operator>>(int16_t & n) { - return deserialize_integer(n); + int64_t value; + *this >> value; + n = static_cast(value); + return *this; } RxStream & RxStream::operator>>(int8_t & n) { - return deserialize_integer(n); -} - -template -RxStream & RxStream::deserialize_integer(T & n) -{ - if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == CBOR_ITEM_INTEGER) - n = *static_cast(_buffered_packet[_buffered_iterator].first); - - _buffered_iterator++; - + int64_t value; + *this >> value; + n = static_cast(value); return *this; } RxStream & RxStream::operator>>(char & n) { - if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == CBOR_ITEM_STRING) + if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == NANOCBOR_TYPE_TSTR) n = (*static_cast(_buffered_packet[_buffered_iterator].first))[0]; _buffered_iterator++; @@ -356,7 +368,7 @@ RxStream & RxStream::operator>>(char & n) RxStream & RxStream::operator>>(float & f) { - if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == CBOR_ITEM_FLOAT) + if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == NANOCBOR_TYPE_FLOAT) f = *static_cast(_buffered_packet[_buffered_iterator].first); _buffered_iterator++; @@ -374,7 +386,7 @@ RxStream & RxStream::operator>>(double & d) RxStream & RxStream::operator>>(std::string & s) { - if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == CBOR_ITEM_STRING) + if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == NANOCBOR_TYPE_TSTR) s = *static_cast(_buffered_packet[_buffered_iterator].first); _buffered_iterator++; @@ -395,7 +407,7 @@ RxStream & RxStream::operator>>(bool & b) { int8_t b_; - if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == CBOR_ITEM_SIMPLE_VALUE) + if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == NANOCBOR_TYPE_UINT) { b_ = *static_cast(_buffered_packet[_buffered_iterator].first); b = b_ ? true : false; @@ -495,30 +507,24 @@ void RxStream::interpret_packets() if (stream_name.empty()) { - goto clean_and_continue; + for (auto& field : interpreted_packet) + { + if (field.first) + { + free(field.first); + field.first = nullptr; + } + } + continue; } for (RxStream * stream : _listening_streams) { if (stream->get_type() == _stream_type_match_map.at(stream_type) && stream->get_identifier() == stream_identifier) { - // No deed to free ? stream->push_packet(interpreted_packet); - goto just_continue; - } - } - - clean_and_continue: - for (auto& field : interpreted_packet) - { - if (field.first) - { - free(field.first); - field.first = nullptr; - field.second = 0; } } - just_continue:; } } @@ -534,8 +540,9 @@ std::pair RxStream::interpret_field(ITEM * cbor_value, size_t i, un int64_t val; if (nanocbor_get_int64(&cbor_value->nanocbor_value, &val) < 0) { - goto error; + break; } + int64_t * number = new int64_t{val}; return std::make_pair(number, NANOCBOR_TYPE_NINT); } @@ -544,30 +551,33 @@ std::pair RxStream::interpret_field(ITEM * cbor_value, size_t i, un uint64_t val; if (nanocbor_get_uint64(&cbor_value->nanocbor_value, &val) < 0) { - goto error; + break; } + uint64_t * number = new uint64_t{val}; return std::make_pair(number, NANOCBOR_TYPE_UINT); } case NANOCBOR_TYPE_FLOAT: { - double val; - if (nanocbor_get_double(&cbor_value->nanocbor_value, &val) < 0) + float val; + if (nanocbor_get_float(&cbor_value->nanocbor_value, &val) < 0) { - goto error; + break; } - double* f = new double{val}; + + float * f = new float{val}; return std::make_pair(static_cast(f), NANOCBOR_TYPE_FLOAT); } case NANOCBOR_TYPE_TSTR: { - const char* val; + const char * val; size_t val_size; if (nanocbor_get_tstr(&cbor_value->nanocbor_value, (const uint8_t**) &val, &val_size) < 0) { - goto error; + break; } - std::string* s = new std::string(val, val_size); + + std::string * s = new std::string(val, val_size); return std::make_pair(s, NANOCBOR_TYPE_TSTR); } case NANOCBOR_TYPE_BSTR: @@ -576,31 +586,18 @@ std::pair RxStream::interpret_field(ITEM * cbor_value, size_t i, un size_t val_size; if (nanocbor_get_bstr(&cbor_value->nanocbor_value, (const uint8_t**) &val, &val_size) < 0) { - goto error; + break; } - std::string* s = new std::string(val, val_size); + + std::string * s = new std::string(val, val_size); return std::make_pair(s, NANOCBOR_TYPE_TSTR); } - // Not sure - case NANOCBOR_SIMPLE_FALSE: - case NANOCBOR_SIMPLE_TRUE: - case NANOCBOR_SIMPLE_NULL: - case NANOCBOR_SIMPLE_UNDEF: - { - uint8_t val; - if (nanocbor_get_simple(&cbor_value->nanocbor_value, &val) < 0) - { - goto error; - } - uint8_t * simple = new uint8_t{val}; - return std::make_pair(simple, NANOCBOR_SIMPLE_FALSE); - } default: nanocbor_skip(&cbor_value->nanocbor_value); - goto error; + return std::make_pair(nullptr, 0); } - - error: + + nanocbor_skip(&cbor_value->nanocbor_value); return std::make_pair(nullptr, 0); } From fc93a4d343a9ecc7243b25a9e946011244e851e1 Mon Sep 17 00:00:00 2001 From: matlin Date: Thu, 2 Jul 2026 14:13:34 +0200 Subject: [PATCH 04/24] Changed CBorStream abstraction paradigm and general fixes --- CMakeLists.txt | 10 +- config.cmake | 2 +- src/desert_classes/CBorStream.h | 126 ++-- .../{CBorStream.cpp => LibMcuCBorStream.cpp} | 406 ++++-------- src/desert_classes/NanoCBorStream.cpp | 614 ++++++++++++++++++ 5 files changed, 799 insertions(+), 359 deletions(-) rename src/desert_classes/{CBorStream.cpp => LibMcuCBorStream.cpp} (60%) create mode 100644 src/desert_classes/NanoCBorStream.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b1a1749..5e8e86c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,9 +25,6 @@ find_package(rosidl_typesupport_introspection_cpp REQUIRED) find_package(rcutils REQUIRED) find_package(rcpputils REQUIRED) -set(CBOR_ROOT cbor) -include_directories(${CBOR_ROOT}/include) - add_library(rmw_desert src/get_node_info_and_types.cpp src/init.cpp @@ -48,7 +45,6 @@ add_library(rmw_desert src/desert_classes/DesertClient.cpp src/desert_classes/DesertService.cpp src/desert_classes/DesertGuardCondition.cpp - src/desert_classes/CBorStream.cpp src/desert_classes/CStringHelper.cpp src/desert_classes/TcpDaemon.cpp src/desert_classes/Discovery.cpp @@ -58,6 +54,7 @@ add_library(rmw_desert if (${CBOR_LIB} STREQUAL "libmcu_cbor") set(CBOR_ROOT cbor) + include_directories(${CBOR_ROOT}/include) target_sources(rmw_desert PRIVATE ${CBOR_ROOT}/src/common.c ${CBOR_ROOT}/src/parser.c @@ -65,19 +62,22 @@ if (${CBOR_LIB} STREQUAL "libmcu_cbor") ${CBOR_ROOT}/src/encoder.c ${CBOR_ROOT}/src/helper.c ${CBOR_ROOT}/src/ieee754.c + src/desert_classes/LibMcuCBorStream.cpp ) target_compile_definitions(rmw_desert PRIVATE LIBMCU_CBOR_ENABLED) elseif (${CBOR_LIB} STREQUAL "NanoCBOR") set(CBOR_ROOT thirdparty/NanoCBOR) + include_directories(${CBOR_ROOT}/include) target_sources(rmw_desert PRIVATE ${CBOR_ROOT}/src/decoder.c ${CBOR_ROOT}/src/encoder.c + src/desert_classes/NanoCBorStream.cpp ) target_compile_definitions(rmw_desert PRIVATE NANOCBOR_ENABLED) else () message(FATAL_ERROR "At least one CBOR libraty is required") endif () - + INCLUDE (FindPkgConfig) # specific order: dependents before dependencies diff --git a/config.cmake b/config.cmake index 15333c9..d2c5c18 100644 --- a/config.cmake +++ b/config.cmake @@ -1 +1 @@ -set(CBOR_LIB "libmcu_cbor" CACHE STRING "Cbor library.") # libmcu_cbor, NanoCBOR \ No newline at end of file +set(CBOR_LIB "libmcu_cbor" CACHE STRING "Cbor library.") # libmcu_cbor, NanoCBOR diff --git a/src/desert_classes/CBorStream.h b/src/desert_classes/CBorStream.h index 0c73070..a95a47e 100644 --- a/src/desert_classes/CBorStream.h +++ b/src/desert_classes/CBorStream.h @@ -52,12 +52,7 @@ /** @endcond */ -#ifdef LIBMCU_CBOR_ENABLED -#include "cbor/parser.h" -#endif -#ifdef NANOCBOR_ENABLED -#include -#endif +#include "half.hpp" #define PUBLISHER_TYPE 0 #define SUBSCRIBER_TYPE 1 @@ -66,17 +61,6 @@ #define MAX_BUFFER_CAPACITY 100 -#ifdef LIBMCU_CBOR_ENABLED -#define cbor_error_type cbor_error_t -#endif -#ifdef NANOCBOR_ENABLED -#define cbor_error_type nanocbor_error_t -#define CBOR_ITEM_INTEGER NANOCBOR_TYPE_NINT -#define CBOR_ITEM_STRING NANOCBOR_TYPE_TSTR -#define CBOR_ITEM_FLOAT NANOCBOR_TYPE_FLOAT -#define CBOR_ITEM_SIMPLE_VALUE NANOCBOR_SIMPLE_FALSE -#endif - template > class CircularQueue : public std::queue { public: @@ -104,12 +88,12 @@ class TxStream * @param stream_identifier Identifier of the topic or the service read from configuration */ TxStream(uint8_t stream_type, std::string stream_name, uint8_t stream_identifier); - + /** * @brief Tell the stream to create a new packet * - * Every time a transmission in started, a new empty packet must be - * generated and saved as a private member. Then type, service name + * Every time a transmission in started, a new empty packet must be + * generated and saved as a private member. Then type, service name * and sequence id are put in front of the data. * * @param sequence_id The id of the client service communication @@ -118,15 +102,15 @@ class TxStream /** * @brief Tell the stream to create a new packet * - * Every time a transmission in started, a new empty packet must be - * generated and saved as a private member. Then type and topic name + * Every time a transmission in started, a new empty packet must be + * generated and saved as a private member. Then type and topic name * are put in front of the data. */ void start_transmission(); /** * @brief Tell the stream to send down the packet * - * When the transmission is finished the packet is stored in the + * When the transmission is finished the packet is stored in the * static member of TcpDaemon in order to be sent to DESERT. */ void end_transmission(); @@ -201,7 +185,7 @@ class TxStream * @param b Field to encode */ TxStream & operator<<(const bool b); - + /** * @brief Encode vector * @param v Field to encode @@ -212,13 +196,13 @@ class TxStream *this << static_cast(v.size()); return serialize_sequence(v.data(), v.size()); } - + /** * @brief Encode bool vector * @param v Field to encode */ TxStream & operator<<(const std::vector v); - + /** * @brief Serialize a sequence of uniform elements * @param items Pointer to the first element @@ -235,24 +219,23 @@ class TxStream } private: + // Forward declarations to abstract the implementation + struct WRITER; + struct ERROR; + uint8_t _stream_type; std::string _stream_name; uint8_t _stream_identifier; - + bool _overflow; - uint8_t * _packet; -#ifdef LIBMCU_CBOR_ENABLED - cbor_writer_t * _writer; -#endif -#ifdef NANOCBOR_ENABLED - nanocbor_encoder_t * _encoder; -#endif - + uint8_t * _packet; + WRITER * _writer; + void new_packet(); - void handle_overrun(cbor_error_type result); - + void handle_overrun(ERROR result); + std::string toUTF8(const std::u16string source); - + }; class RxStream @@ -266,31 +249,31 @@ class RxStream * @param stream_identifier Identifier of the topic or the service read from configuration */ RxStream(uint8_t stream_type, std::string stream_name, uint8_t stream_identifier); - + /** * @brief Destroy the reception stream */ ~RxStream(); - + /** * @brief Check if there are data * - * A map contains the information received for all topics and services, - * so using the name saved in the current instance as key it is possible + * A map contains the information received for all topics and services, + * so using the name saved in the current instance as key it is possible * to know if a message is arrived for a specific entity. * * @param sequence_id The id of the client service communication */ bool data_available(int64_t sequence_id = 0); - + /** * @brief Clear the currently buffered packet * - * When the packet is read by the entity, this function must be called to clear the buffer + * When the packet is read by the entity, this function must be called to clear the buffer * and allow RxStream to add the next one in the queue. */ void clear_buffer(); - + /** * @brief Decode uint64 * @param n Field to decode @@ -331,14 +314,7 @@ class RxStream * @param n Field to decode */ RxStream & operator>>(int8_t & n); - - /** - * @brief Decode a generic integer - * @param n Field to decode - */ - template - RxStream & deserialize_integer(T & n); - + /** * @brief Decode char * @param n Field to decode @@ -369,7 +345,7 @@ class RxStream * @param b Field to decode */ RxStream & operator>>(bool & b); - + /** * @brief Decode vector * @param v Field to decode @@ -380,16 +356,16 @@ class RxStream uint32_t size; *this >> size; v.resize(size); - + return deserialize_sequence(v.data(), size); } - + /** * @brief Decode bool vector * @param v Field to decode */ RxStream & operator>>(std::vector & v); - + /** * @brief Deserialize a sequence of uniform elements * @param items Pointer to the first element @@ -404,8 +380,8 @@ class RxStream } return *this; } - - + + /** * @brief Get the stream type of a specific instance * @return Type of the stream @@ -421,7 +397,7 @@ class RxStream * @return Topic identifier of the stream */ uint8_t get_identifier() const; - + /** * @brief Add a packet to _received_packets * @@ -432,30 +408,33 @@ class RxStream * @param packet The packet to add */ void push_packet(std::vector> packet); - + /** * @brief Interpret raw packets and splits them into different communication types * - * Raw packets from TcpDaemon are read and interpreted in order to put them in - * a map where the key allows to distinguish the topic name or the service name, + * Raw packets from TcpDaemon are read and interpreted in order to put them in + * a map where the key allows to distinguish the topic name or the service name, * and eventually the sequence identifier. */ static void interpret_packets(); - + private: + // Forward declarations to abstract the implementation + struct ITEM; + uint8_t _stream_type; std::string _stream_name; uint8_t _stream_identifier; - + size_t _buffered_iterator; - + // packets: > std::vector> _buffered_packet; CircularQueue>, MAX_BUFFER_CAPACITY> _received_packets; - + static const std::map _stream_type_match_map; static std::vector _listening_streams; - + union _cbor_value { int8_t i8; int16_t i16; @@ -467,15 +446,10 @@ class RxStream char *str; uint8_t str_copy[128]; }; - + static std::mutex _rx_mutex; -#ifdef LIBMCU_CBOR_ENABLED - static std::pair interpret_field(cbor_item_t * items, size_t i, union _cbor_value & val); -#endif -#ifdef NANOCBOR_ENABLED - static std::pair interpret_field(nanocbor_value_t* cbor_value); -#endif - + + static std::pair interpret_field(ITEM * items, size_t i, union _cbor_value & val); std::u16string toUTF16(const std::string source); }; diff --git a/src/desert_classes/CBorStream.cpp b/src/desert_classes/LibMcuCBorStream.cpp similarity index 60% rename from src/desert_classes/CBorStream.cpp rename to src/desert_classes/LibMcuCBorStream.cpp index 3aef14d..8275de6 100644 --- a/src/desert_classes/CBorStream.cpp +++ b/src/desert_classes/LibMcuCBorStream.cpp @@ -1,22 +1,27 @@ #include "CBorStream.h" -#include - -#ifdef LIBMCU_CBOR_ENABLED -#include "half.hpp" -#include "cbor/encoder.h" -#include "cbor/decoder.h" -#endif - -#ifdef NANOCBOR_ENABLED -#include -#endif +#include +#include +#include +#include +#include namespace cbor { // TX stream +// Forward declarations to abstract the implementation +struct TxStream::WRITER +{ + cbor_writer_t libmcu_writer; +}; + +struct TxStream::ERROR +{ + cbor_error_t libmcu_error; +}; + TxStream::TxStream(uint8_t stream_type, std::string stream_name, uint8_t stream_identifier) : _stream_type(stream_type) , _stream_name(stream_name) @@ -28,22 +33,16 @@ void TxStream::new_packet() { // Initialize packet and writer _packet = new uint8_t[MAX_PACKET_LENGTH]; -#ifdef LIBMCU_CBOR_ENABLED - _writer = new cbor_writer_t; - cbor_writer_init(_writer, _packet, MAX_PACKET_LENGTH); -#endif + _writer = new WRITER; -#ifdef NANOCBOR_ENABLED - _encoder = new nanocbor_encoder_t; - nanocbor_encoder_init(_encoder, _packet, MAX_PACKET_LENGTH); -#endif + cbor_writer_init(&_writer->libmcu_writer, _packet, MAX_PACKET_LENGTH); } void TxStream::start_transmission(uint64_t sequence_id) { new_packet(); _overflow = false; - + // Stream type, service name/identifier and sequence id *this << _stream_type; *this << _stream_identifier; @@ -54,7 +53,7 @@ void TxStream::start_transmission() { new_packet(); _overflow = false; - + // Stream type and topic name/identifier *this << _stream_type; *this << _stream_identifier; @@ -62,48 +61,30 @@ void TxStream::start_transmission() void TxStream::end_transmission() { - // TODO: Memory leak !!! if (_overflow) + { + delete _packet; + delete _writer; return; - -#ifdef LIBMCU_CBOR_ENABLED - size_t encoded_len = cbor_writer_len(_writer); -#endif - -#ifdef NANOCBOR_ENABLED - size_t encoded_len = nanocbor_encoded_len(_encoder); -#endif - - std::vector daemon_packet(encoded_len); - - for(size_t i=0; i < encoded_len; i++) + } + + std::vector daemon_packet; + + for(size_t i=0; i < cbor_writer_len(&_writer->libmcu_writer); i++) { daemon_packet.push_back(_packet[i]); } - + TcpDaemon::enqueue_packet(daemon_packet); - + delete _packet; -#ifdef LIBMCU_CBOR_ENABLED delete _writer; -#endif - -#ifdef NANOCBOR_ENABLED - delete _encoder; -#endif } TxStream & TxStream::operator<<(const uint64_t n) { -#ifdef LIBMCU_CBOR_ENABLED - cbor_error_t result = cbor_encode_unsigned_integer(_writer, n); -#endif - -#ifdef NANOCBOR_ENABLED - nanocbor_error_t result = (nanocbor_error_t) nanocbor_fmt_uint(_encoder, n); -#endif - - handle_overrun(result); + cbor_error_t result = cbor_encode_unsigned_integer(&_writer->libmcu_writer, n); + handle_overrun(ERROR(result)); return *this; } @@ -127,28 +108,14 @@ TxStream & TxStream::operator<<(const uint8_t n) TxStream & TxStream::operator<<(const int64_t n) { -#ifdef LIBMCU_CBOR_ENABLED cbor_error_t result; - + if (n >= 0) - result = cbor_encode_unsigned_integer(_writer, n); + result = cbor_encode_unsigned_integer(&_writer->libmcu_writer, n); else - result = cbor_encode_negative_integer(_writer, n); -#endif - -#ifdef NANOCBOR_ENABLED - nanocbor_error_t result; - - if (n >= 0) { - result = (nanocbor_error_t) nanocbor_fmt_uint(_encoder, n); - } else { - result = (nanocbor_error_t) nanocbor_fmt_int(_encoder, n); - } - -#endif - - handle_overrun(result); - + result = cbor_encode_negative_integer(&_writer->libmcu_writer, n); + + handle_overrun(ERROR(result)); return *this; } @@ -179,15 +146,8 @@ TxStream & TxStream::operator<<(const char n) TxStream & TxStream::operator<<(const float f) { -#ifdef LIBMCU_CBOR_ENABLED - cbor_error_t result = cbor_encode_float(_writer, f); -#endif - -#ifdef NANOCBOR_ENABLED - nanocbor_error_t result = (nanocbor_error_t) nanocbor_fmt_float(_encoder, f); -#endif - - handle_overrun(result); + cbor_error_t result = cbor_encode_float(&_writer->libmcu_writer, f); + handle_overrun(ERROR(result)); return *this; } @@ -199,15 +159,8 @@ TxStream & TxStream::operator<<(const double d) TxStream & TxStream::operator<<(const std::string s) { -#ifdef LIBMCU_CBOR_ENABLED - cbor_error_t result = cbor_encode_text_string(_writer, s.c_str(), s.size()); -#endif - -#ifdef NANOCBOR_ENABLED - nanocbor_error_t result = (nanocbor_error_t) nanocbor_put_tstrn(_encoder, s.c_str(), s.size()); -#endif - - handle_overrun(result); + cbor_error_t result = cbor_encode_text_string(&_writer->libmcu_writer, s.c_str(), s.size()); + handle_overrun(ERROR(result)); return *this; } @@ -220,15 +173,8 @@ TxStream & TxStream::operator<<(const std::u16string s) TxStream & TxStream::operator<<(const bool b) { -#ifdef LIBMCU_CBOR_ENABLED - cbor_error_t result = cbor_encode_bool(_writer, b); -#endif - -#ifdef NANOCBOR_ENABLED - nanocbor_error_t result = (nanocbor_error_t) nanocbor_fmt_bool(_encoder, b); -#endif - - handle_overrun(result); + cbor_error_t result = cbor_encode_bool(&_writer->libmcu_writer, b); + handle_overrun(ERROR(result)); return *this; } @@ -242,21 +188,12 @@ TxStream & TxStream::operator<<(const std::vector v) return *this; } -void TxStream::handle_overrun(cbor_error_type result) +void TxStream::handle_overrun(ERROR result) { -#ifdef LIBMCU_CBOR_ENABLED - if (result == CBOR_OVERRUN) - { - _overflow = true; - } -#endif - -#ifdef NANOCBOR_ENABLED - if (result == NANOCBOR_ERR_END) + if (result.libmcu_error == CBOR_OVERRUN) { _overflow = true; } -#endif } std::string TxStream::toUTF8(const std::u16string source) @@ -272,6 +209,12 @@ std::string TxStream::toUTF8(const std::u16string source) // RX stream +// Forward declarations to abstract the implementation +struct RxStream::ITEM +{ + cbor_item_t libmcu_item; +}; + RxStream::RxStream(uint8_t stream_type, std::string stream_name, uint8_t stream_identifier) : _stream_type(stream_type) , _stream_name(stream_name) @@ -302,11 +245,11 @@ bool RxStream::data_available(int64_t sequence_id) // If a packet is already buffered, so do not overwrite it and wait for a clear if (_buffered_packet.size() > 0) return true; - + // No buffered packets, go on checking for other ones if (_received_packets.size() == 0) return false; - + // The received packets queue is not empty, so examine it if (sequence_id) { @@ -344,52 +287,67 @@ void RxStream::clear_buffer() RxStream & RxStream::operator>>(uint64_t & n) { - return deserialize_integer(n); + int64_t value; + *this >> value; + n = static_cast(value); + return *this; } RxStream & RxStream::operator>>(uint32_t & n) { - return deserialize_integer(n); + int64_t value; + *this >> value; + n = static_cast(value); + return *this; } RxStream & RxStream::operator>>(uint16_t & n) { - return deserialize_integer(n); + int64_t value; + *this >> value; + n = static_cast(value); + return *this; } RxStream & RxStream::operator>>(uint8_t & n) { - return deserialize_integer(n); + int64_t value; + *this >> value; + n = static_cast(value); + return *this; } RxStream & RxStream::operator>>(int64_t & n) { - return deserialize_integer(n); + if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == CBOR_ITEM_INTEGER) + n = *static_cast(_buffered_packet[_buffered_iterator].first); + + _buffered_iterator++; + + return *this; } RxStream & RxStream::operator>>(int32_t & n) { - return deserialize_integer(n); + int64_t value; + *this >> value; + n = static_cast(value); + return *this; } RxStream & RxStream::operator>>(int16_t & n) { - return deserialize_integer(n); + int64_t value; + *this >> value; + n = static_cast(value); + return *this; } RxStream & RxStream::operator>>(int8_t & n) { - return deserialize_integer(n); -} - -template -RxStream & RxStream::deserialize_integer(T & n) -{ - if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == CBOR_ITEM_INTEGER) - n = *static_cast(_buffered_packet[_buffered_iterator].first); - - _buffered_iterator++; - + int64_t value; + *this >> value; + n = static_cast(value); return *this; } @@ -399,7 +357,7 @@ RxStream & RxStream::operator>>(char & n) n = (*static_cast(_buffered_packet[_buffered_iterator].first))[0]; _buffered_iterator++; - + return *this; } @@ -409,7 +367,7 @@ RxStream & RxStream::operator>>(float & f) f = *static_cast(_buffered_packet[_buffered_iterator].first); _buffered_iterator++; - + return *this; } @@ -427,7 +385,7 @@ RxStream & RxStream::operator>>(std::string & s) s = *static_cast(_buffered_packet[_buffered_iterator].first); _buffered_iterator++; - + return *this; } @@ -435,7 +393,7 @@ RxStream & RxStream::operator>>(std::u16string & s) { std::string str; *this >> str; - + s = toUTF16(str); return *this; } @@ -443,14 +401,14 @@ RxStream & RxStream::operator>>(std::u16string & s) RxStream & RxStream::operator>>(bool & b) { int8_t b_; - - if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == CBOR_ITEM_SIMPLE_VALUE) { + + if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == CBOR_ITEM_SIMPLE_VALUE) b_ = *static_cast(_buffered_packet[_buffered_iterator].first); - b = b_ ? true : false; - - _buffered_iterator++; - } + b = b_ ? true : false; + + _buffered_iterator++; + return *this; } @@ -490,69 +448,41 @@ void RxStream::push_packet(std::vector> packet) void RxStream::interpret_packets() { std::lock_guard lock(_rx_mutex); - + std::vector packet; for (packet = TcpDaemon::read_packet(); packet.size() != 0; packet = TcpDaemon::read_packet()) { // Initialize buffer and reader uint8_t * buffer = &packet[0]; -#ifdef LIBMCU_CBOR_ENABLED cbor_reader_t reader; - cbor_item_t items[64]; + ITEM items[64]; size_t n; - cbor_reader_init(&reader, items, sizeof(items) / sizeof(items[0])); + cbor_reader_init(&reader, &items->libmcu_item, sizeof(items) / sizeof(items[0])); cbor_parse(&reader, buffer, packet.size(), &n); -#endif -#ifdef NANOCBOR_ENABLED - nanocbor_value_t decoder; - nanocbor_decoder_init(&decoder, buffer, packet.size()); -#endif - + uint8_t stream_type; uint8_t stream_identifier; std::string stream_name; - + std::vector> interpreted_packet; - -#ifdef LIBMCU_CBOR_ENABLED -#define WRITER_EXIT_COND (i < n) -#endif -#ifdef NANOCBOR_ENABLED -#define WRITER_EXIT_COND (!nanocbor_at_end(&decoder)) -#endif - - for (size_t i = 0; WRITER_EXIT_COND; i++) + + for (size_t i = 0; i < n; i++) { -#ifdef LIBMCU_CBOR_ENABLED union _cbor_value val; - memset(&val, 0, sizeof(val)); - cbor_decode(&reader, &items[i], &val, sizeof(val)); -#endif + memset(&val, 0, sizeof(val)); + cbor_decode(&reader, &items[i].libmcu_item, &val, sizeof(val)); + if (i == 0) { -#ifdef LIBMCU_CBOR_ENABLED stream_type = val.i8; -#endif -#ifdef NANOCBOR_ENABLED - if (nanocbor_get_uint8(&decoder, &stream_type) < 0) { - break; - } -#endif } else if (i == 1) { -#ifdef LIBMCU_CBOR_ENABLED stream_identifier = val.i8; -#endif -#ifdef NANOCBOR_ENABLED - if (nanocbor_get_uint8(&decoder, &stream_identifier) < 0) { - break; - } -#endif stream_name = TopicsConfig::get_identifier_topic(stream_identifier); - + if (stream_name.empty()) { break; @@ -560,133 +490,56 @@ void RxStream::interpret_packets() } else { -#ifdef LIBMCU_CBOR_ENABLED std::pair field = interpret_field(items, i, val); -#endif -#ifdef NANOCBOR_ENABLED - std::pair field = interpret_field(&decoder); -#endif + if (field.first) { interpreted_packet.push_back(field); } } } - + if (stream_name.empty()) { - goto clean_and_continue; + for (auto& field : interpreted_packet) + { + if (field.first) + { + free(field.first); + field.first = nullptr; + } + } + continue; } - + for (RxStream * stream : _listening_streams) { if (stream->get_type() == _stream_type_match_map.at(stream_type) && stream->get_identifier() == stream_identifier) { - // No deed to free ? stream->push_packet(interpreted_packet); - goto just_continue; - } - } - - clean_and_continue: - for (auto& field : interpreted_packet) { - if (field.first) { - free(field.first); - field.first = nullptr; - field.second = 0; - } - } - just_continue:; - } -} - -#ifdef NANOCBOR_ENABLED -std::pair RxStream::interpret_field(nanocbor_value_t* cbor_value) { - int cbor_value_type = nanocbor_get_type(cbor_value); - switch (cbor_value_type) { - case NANOCBOR_TYPE_NINT: { - int64_t val; - if (nanocbor_get_int64(cbor_value, &val) < 0) { - goto error; - } - int64_t * number = new int64_t{val}; - return std::make_pair(number, NANOCBOR_TYPE_NINT); - } - case NANOCBOR_TYPE_UINT: { - uint64_t val; - if (nanocbor_get_uint64(cbor_value, &val) < 0) { - goto error; - } - uint64_t * number = new uint64_t{val}; - return std::make_pair(number, NANOCBOR_TYPE_UINT); - } - case NANOCBOR_TYPE_FLOAT: { - double val; - if (nanocbor_get_double(cbor_value, &val) < 0) { - goto error; - } - double* f = new double{val}; - return std::make_pair(static_cast(f), NANOCBOR_TYPE_FLOAT); - } - case NANOCBOR_TYPE_TSTR: { - const char* val; - size_t val_size; - if (nanocbor_get_tstr(cbor_value, (const uint8_t**) &val, &val_size) < 0) { - goto error; - } - std::string* s = new std::string(val, val_size); - return std::make_pair(s, NANOCBOR_TYPE_TSTR); - } - case NANOCBOR_TYPE_BSTR: { - const char* val; - size_t val_size; - if (nanocbor_get_bstr(cbor_value, (const uint8_t**) &val, &val_size) < 0) { - goto error; - } - std::string* s = new std::string(val, val_size); - return std::make_pair(s, NANOCBOR_TYPE_TSTR); - } - // Not sure - case NANOCBOR_SIMPLE_FALSE: - case NANOCBOR_SIMPLE_TRUE: - case NANOCBOR_SIMPLE_NULL: - case NANOCBOR_SIMPLE_UNDEF: { - uint8_t val; - if (nanocbor_get_simple(cbor_value, &val) < 0) { - goto error; } - uint8_t * simple = new uint8_t{val}; - return std::make_pair(simple, NANOCBOR_SIMPLE_FALSE); } - default: - nanocbor_skip(cbor_value); - goto error; } - - error: - return std::make_pair(nullptr, 0); } -#endif -#ifdef LIBMCU_CBOR_ENABLED -std::pair RxStream::interpret_field(cbor_item_t * items, size_t i, union _cbor_value & val) +std::pair RxStream::interpret_field(ITEM * items, size_t i, union _cbor_value & val) { - switch (items[i].type) + switch (items[i].libmcu_item.type) { case CBOR_ITEM_INTEGER: { int64_t * number = new int64_t{val.i64}; - + return std::make_pair(static_cast(number), CBOR_ITEM_INTEGER); } case CBOR_ITEM_FLOAT: { float * number; - + if (abs(val.i32) < 65536) { int16_t * bin = new int16_t{val.i16}; - + half_float::half * f16 = reinterpret_cast(bin); number = new float{*f16}; } @@ -694,27 +547,26 @@ std::pair RxStream::interpret_field(cbor_item_t * items, size_t i, { number = new float{val.f32}; } - + return std::make_pair(static_cast(number), CBOR_ITEM_FLOAT); } - case CBOR_ITEM_STRING: + case CBOR_ITEM_STRING: { - val.str_copy[items[i].size] = '\0'; + val.str_copy[items[i].libmcu_item.size] = '\0'; std::string * str = new std::string{reinterpret_cast(val.str_copy)}; - + return std::make_pair(static_cast(str), CBOR_ITEM_STRING); } case CBOR_ITEM_SIMPLE_VALUE: { int8_t * value = new int8_t{val.i8}; - + return std::make_pair(static_cast(value), CBOR_ITEM_SIMPLE_VALUE); } default: return std::make_pair(nullptr, 0); } } -#endif std::u16string RxStream::toUTF16(const std::string source) { diff --git a/src/desert_classes/NanoCBorStream.cpp b/src/desert_classes/NanoCBorStream.cpp new file mode 100644 index 0000000..b6694cc --- /dev/null +++ b/src/desert_classes/NanoCBorStream.cpp @@ -0,0 +1,614 @@ +#include "CBorStream.h" + +#include + +namespace cbor +{ + +// TX stream + +// Forward declarations to abstract the implementation +struct TxStream::WRITER +{ + nanocbor_encoder_t nanocbor_encoder; +}; + +struct TxStream::ERROR +{ + nanocbor_error_t nanocbor_error; +}; + +TxStream::TxStream(uint8_t stream_type, std::string stream_name, uint8_t stream_identifier) + : _stream_type(stream_type) + , _stream_name(stream_name) + , _stream_identifier(stream_identifier) +{ +} + +void TxStream::new_packet() +{ + // Initialize packet and writer + _packet = new uint8_t[MAX_PACKET_LENGTH]; + _writer = new WRITER; + + nanocbor_encoder_init(&_writer->nanocbor_encoder, _packet, MAX_PACKET_LENGTH); +} + +void TxStream::start_transmission(uint64_t sequence_id) +{ + new_packet(); + _overflow = false; + + // Stream type, service name/identifier and sequence id + *this << _stream_type; + *this << _stream_identifier; + *this << sequence_id; +} + +void TxStream::start_transmission() +{ + new_packet(); + _overflow = false; + + // Stream type and topic name/identifier + *this << _stream_type; + *this << _stream_identifier; +} + +void TxStream::end_transmission() +{ + if (_overflow) + { + delete _packet; + delete _writer; + return; + } + + size_t encoded_len = nanocbor_encoded_len(&_writer->nanocbor_encoder); + + std::vector daemon_packet; + + for(size_t i=0; i < encoded_len; i++) + { + daemon_packet.push_back(_packet[i]); + } + + TcpDaemon::enqueue_packet(daemon_packet); + + delete _packet; + delete _writer; +} + +TxStream & TxStream::operator<<(const uint64_t n) +{ + nanocbor_error_t result = (nanocbor_error_t) nanocbor_fmt_uint(&_writer->nanocbor_encoder, n); + handle_overrun(ERROR(result)); + return *this; +} + +TxStream & TxStream::operator<<(const uint32_t n) +{ + *this << static_cast(n); + return *this; +} + +TxStream & TxStream::operator<<(const uint16_t n) +{ + *this << static_cast(n); + return *this; +} + +TxStream & TxStream::operator<<(const uint8_t n) +{ + *this << static_cast(n); + return *this; +} + +TxStream & TxStream::operator<<(const int64_t n) +{ + nanocbor_error_t result; + + if (n >= 0) + { + result = (nanocbor_error_t) nanocbor_fmt_uint(&_writer->nanocbor_encoder, n); + } + else + { + result = (nanocbor_error_t) nanocbor_fmt_int(&_writer->nanocbor_encoder, n); + } + + handle_overrun(ERROR(result)); + + return *this; +} + +TxStream & TxStream::operator<<(const int32_t n) +{ + *this << static_cast(n); + return *this; +} + +TxStream & TxStream::operator<<(const int16_t n) +{ + *this << static_cast(n); + return *this; +} + +TxStream & TxStream::operator<<(const int8_t n) +{ + *this << static_cast(n); + return *this; +} + +TxStream & TxStream::operator<<(const char n) +{ + std::string single_char_string(1, n); + *this << single_char_string; + return *this; +} + +TxStream & TxStream::operator<<(const float f) +{ + nanocbor_error_t result = (nanocbor_error_t) nanocbor_fmt_float(&_writer->nanocbor_encoder, f); + handle_overrun(ERROR(result)); + return *this; +} + +TxStream & TxStream::operator<<(const double d) +{ + *this << static_cast(d); + return *this; +} + +TxStream & TxStream::operator<<(const std::string s) +{ + nanocbor_error_t result = (nanocbor_error_t) nanocbor_put_tstrn(&_writer->nanocbor_encoder, s.c_str(), s.size()); + handle_overrun(ERROR(result)); + return *this; +} + +TxStream & TxStream::operator<<(const std::u16string s) +{ + // Cbor does not support UTF-16 so a conversion to UTF-8 is performed + *this << toUTF8(s); + return *this; +} + +TxStream & TxStream::operator<<(const bool b) +{ + *this << static_cast(b); + return *this; +} + +TxStream & TxStream::operator<<(const std::vector v) +{ + *this << static_cast(v.size()); + for (size_t i = 0; i < v.size(); ++i) + { + *this << v[i]; + } + return *this; +} + +void TxStream::handle_overrun(ERROR result) +{ + if (result.nanocbor_error == NANOCBOR_ERR_END) + { + _overflow = true; + } +} + +std::string TxStream::toUTF8(const std::u16string source) +{ + std::string result; + + std::wstring_convert, char16_t> convertor; + result = convertor.to_bytes(source); + + return result; +} + + +// RX stream + +// Forward declarations to abstract the implementation +struct RxStream::ITEM +{ + nanocbor_value_t nanocbor_value; +}; + +RxStream::RxStream(uint8_t stream_type, std::string stream_name, uint8_t stream_identifier) + : _stream_type(stream_type) + , _stream_name(stream_name) + , _stream_identifier(stream_identifier) +{ + _listening_streams.push_back(this); +} + +RxStream::~RxStream() +{ + std::vector::iterator position = std::find(_listening_streams.begin(), _listening_streams.end(), this); + if (position != _listening_streams.end()) + _listening_streams.erase(position); +} + +const std::map RxStream::_stream_type_match_map = { + { PUBLISHER_TYPE, SUBSCRIBER_TYPE }, + { CLIENT_TYPE, SERVICE_TYPE }, + { SERVICE_TYPE, CLIENT_TYPE } +}; + +std::vector RxStream::_listening_streams; + +std::mutex RxStream::_rx_mutex; + +bool RxStream::data_available(int64_t sequence_id) +{ + // If a packet is already buffered, so do not overwrite it and wait for a clear + if (_buffered_packet.size() > 0) + return true; + + // No buffered packets, go on checking for other ones + if (_received_packets.size() == 0) + return false; + + // The received packets queue is not empty, so examine it + if (sequence_id) + { + for (size_t i = 0; i < _received_packets.size(); i++) + { + // Check for the first element in the packet, which is the sequence id + if (*static_cast(_received_packets.front().at(0).first) == sequence_id) + { + _buffered_packet = _received_packets.front(); + _received_packets.pop(); + _buffered_iterator = 1; + return true; + } + else + { + _received_packets.push(_received_packets.front()); + _received_packets.pop(); + } + } + return false; + } + else + { + _buffered_packet = _received_packets.front(); + _received_packets.pop(); + _buffered_iterator = 0; + return true; + } +} + +void RxStream::clear_buffer() +{ + _buffered_packet.clear(); +} + +RxStream & RxStream::operator>>(uint64_t & n) +{ + if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == NANOCBOR_TYPE_UINT) + n = *static_cast(_buffered_packet[_buffered_iterator].first); + + _buffered_iterator++; + + return *this; +} + +RxStream & RxStream::operator>>(uint32_t & n) +{ + uint64_t value; + *this >> value; + n = static_cast(value); + return *this; +} + +RxStream & RxStream::operator>>(uint16_t & n) +{ + uint64_t value; + *this >> value; + n = static_cast(value); + return *this; +} + +RxStream & RxStream::operator>>(uint8_t & n) +{ + uint64_t value; + *this >> value; + n = static_cast(value); + return *this; +} + +RxStream & RxStream::operator>>(int64_t & n) +{ + int type = _buffered_packet[_buffered_iterator].second; + if (_buffered_packet.size() > _buffered_iterator && (type == NANOCBOR_TYPE_UINT || type == NANOCBOR_TYPE_NINT)) + n = *static_cast(_buffered_packet[_buffered_iterator].first); + + _buffered_iterator++; + + return *this; +} + +RxStream & RxStream::operator>>(int32_t & n) +{ + int64_t value; + *this >> value; + n = static_cast(value); + return *this; +} + +RxStream & RxStream::operator>>(int16_t & n) +{ + int64_t value; + *this >> value; + n = static_cast(value); + return *this; +} + +RxStream & RxStream::operator>>(int8_t & n) +{ + int64_t value; + *this >> value; + n = static_cast(value); + return *this; +} + +RxStream & RxStream::operator>>(char & n) +{ + if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == NANOCBOR_TYPE_TSTR) + n = (*static_cast(_buffered_packet[_buffered_iterator].first))[0]; + + _buffered_iterator++; + + return *this; +} + +RxStream & RxStream::operator>>(float & f) +{ + if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == NANOCBOR_TYPE_FLOAT) + f = *static_cast(_buffered_packet[_buffered_iterator].first); + + _buffered_iterator++; + + return *this; +} + +RxStream & RxStream::operator>>(double & d) +{ + float value; + *this >> value; + d = static_cast(value); + return *this; +} + +RxStream & RxStream::operator>>(std::string & s) +{ + if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == NANOCBOR_TYPE_TSTR) + s = *static_cast(_buffered_packet[_buffered_iterator].first); + + _buffered_iterator++; + + return *this; +} + +RxStream & RxStream::operator>>(std::u16string & s) +{ + std::string str; + *this >> str; + + s = toUTF16(str); + return *this; +} + +RxStream & RxStream::operator>>(bool & b) +{ + int8_t b_; + + if (_buffered_packet.size() > _buffered_iterator && _buffered_packet[_buffered_iterator].second == NANOCBOR_TYPE_UINT) + { + b_ = *static_cast(_buffered_packet[_buffered_iterator].first); + b = b_ ? true : false; + + _buffered_iterator++; + } + + return *this; +} + +RxStream & RxStream::operator>>(std::vector & v) +{ + uint32_t size; + *this >> size; + for (size_t i = 0; i < size; ++i) + { + int b; + *this >> b; + v[i] = b ? true : false; + } + return *this; +} + +uint8_t RxStream::get_type() const +{ + return _stream_type; +} + +std::string RxStream::get_name() const +{ + return _stream_name; +} + +uint8_t RxStream::get_identifier() const +{ + return _stream_identifier; +} + +void RxStream::push_packet(std::vector> packet) +{ + _received_packets.push(packet); +} + +void RxStream::interpret_packets() +{ + std::lock_guard lock(_rx_mutex); + + std::vector packet; + for (packet = TcpDaemon::read_packet(); packet.size() != 0; packet = TcpDaemon::read_packet()) + { + // Initialize buffer and reader + uint8_t * buffer = &packet[0]; + ITEM decoder; + nanocbor_decoder_init(&decoder.nanocbor_value, buffer, packet.size()); + + uint8_t stream_type; + uint8_t stream_identifier; + std::string stream_name; + + std::vector> interpreted_packet; + + for (size_t i = 0; !nanocbor_at_end(&decoder.nanocbor_value); i++) + { + union _cbor_value val; + + if (i == 0) + { + if (nanocbor_get_uint8(&decoder.nanocbor_value, &stream_type) < 0) + { + break; + } + } + else if (i == 1) + { + if (nanocbor_get_uint8(&decoder.nanocbor_value, &stream_identifier) < 0) + { + break; + } + + stream_name = TopicsConfig::get_identifier_topic(stream_identifier); + + if (stream_name.empty()) + { + break; + } + } + else + { + std::pair field = interpret_field(&decoder, 0, val); + + if (field.first) + { + interpreted_packet.push_back(field); + } + } + } + + if (stream_name.empty()) + { + for (auto& field : interpreted_packet) + { + if (field.first) + { + free(field.first); + field.first = nullptr; + } + } + continue; + } + + for (RxStream * stream : _listening_streams) + { + if (stream->get_type() == _stream_type_match_map.at(stream_type) && stream->get_identifier() == stream_identifier) + { + stream->push_packet(interpreted_packet); + } + } + } +} + +std::pair RxStream::interpret_field(ITEM * cbor_value, size_t i, union _cbor_value & val) +{ + (void) i; + (void) val; + int cbor_value_type = nanocbor_get_type(&cbor_value->nanocbor_value); + switch (cbor_value_type) + { + case NANOCBOR_TYPE_NINT: + { + int64_t val; + if (nanocbor_get_int64(&cbor_value->nanocbor_value, &val) < 0) + { + break; + } + + int64_t * number = new int64_t{val}; + return std::make_pair(number, NANOCBOR_TYPE_NINT); + } + case NANOCBOR_TYPE_UINT: + { + uint64_t val; + if (nanocbor_get_uint64(&cbor_value->nanocbor_value, &val) < 0) + { + break; + } + + uint64_t * number = new uint64_t{val}; + return std::make_pair(number, NANOCBOR_TYPE_UINT); + } + case NANOCBOR_TYPE_FLOAT: + { + float val; + if (nanocbor_get_float(&cbor_value->nanocbor_value, &val) < 0) + { + break; + } + + float * f = new float{val}; + return std::make_pair(static_cast(f), NANOCBOR_TYPE_FLOAT); + } + case NANOCBOR_TYPE_TSTR: + { + const char * val; + size_t val_size; + if (nanocbor_get_tstr(&cbor_value->nanocbor_value, (const uint8_t**) &val, &val_size) < 0) + { + break; + } + + std::string * s = new std::string(val, val_size); + return std::make_pair(s, NANOCBOR_TYPE_TSTR); + } + case NANOCBOR_TYPE_BSTR: + { + const char* val; + size_t val_size; + if (nanocbor_get_bstr(&cbor_value->nanocbor_value, (const uint8_t**) &val, &val_size) < 0) + { + break; + } + + std::string * s = new std::string(val, val_size); + return std::make_pair(s, NANOCBOR_TYPE_TSTR); + } + default: + nanocbor_skip(&cbor_value->nanocbor_value); + return std::make_pair(nullptr, 0); + } + + nanocbor_skip(&cbor_value->nanocbor_value); + return std::make_pair(nullptr, 0); +} + +std::u16string RxStream::toUTF16(const std::string source) +{ + std::u16string result; + + std::wstring_convert, char16_t> convertor; + result = convertor.from_bytes(source); + + return result; +} + +} From 78b5a40e6fd1d3634c280696cbc40f75096b85e3 Mon Sep 17 00:00:00 2001 From: Dmytro Ochkas Date: Wed, 8 Jul 2026 22:41:42 +0200 Subject: [PATCH 05/24] Implement security layer with cose - Add a new security layer that encrypts message before each Tx cbor stream end of transmission and decrypts after each Rx cbor stream Add mbedtls as an intermediary crypto library Secure mode is adjustable --- .gitmodules | 6 + CMakeLists.txt | 66 +++++++- config.cmake | 4 +- src/desert_classes/NanoCBorStream.cpp | 33 +++- .../security/SecurityConfig.cpp | 21 +++ src/desert_classes/security/SecurityConfig.h | 32 ++++ src/desert_classes/security/SecurityLayer.cpp | 141 ++++++++++++++++++ src/desert_classes/security/SecurityLayer.h | 92 ++++++++++++ src/desert_classes/security/sec_utils.cpp | 30 ++++ src/desert_classes/security/sec_utils.h | 10 ++ thirdparty/libcose | 1 + thirdparty/mbedtls | 1 + 12 files changed, 423 insertions(+), 14 deletions(-) create mode 100644 src/desert_classes/security/SecurityConfig.cpp create mode 100644 src/desert_classes/security/SecurityConfig.h create mode 100644 src/desert_classes/security/SecurityLayer.cpp create mode 100644 src/desert_classes/security/SecurityLayer.h create mode 100644 src/desert_classes/security/sec_utils.cpp create mode 100644 src/desert_classes/security/sec_utils.h create mode 160000 thirdparty/libcose create mode 160000 thirdparty/mbedtls diff --git a/.gitmodules b/.gitmodules index 664718b..81bfd5b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,9 @@ [submodule "thirdparty/NanoCBOR"] path = thirdparty/NanoCBOR url = https://github.com/bergzand/NanoCBOR +[submodule "thirdparty/libcose"] + path = thirdparty/libcose + url = https://github.com/bergzand/libcose.git +[submodule "thirdparty/mbedtls"] + path = thirdparty/mbedtls + url = https://github.com/Mbed-TLS/mbedtls.git diff --git a/CMakeLists.txt b/CMakeLists.txt index 5e8e86c..cca460b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,6 +13,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -pthread) endif() +set(CMAKE_POLICY_VERSION_MINIMUM 3.5) + find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros_core REQUIRED) @@ -54,7 +56,7 @@ add_library(rmw_desert if (${CBOR_LIB} STREQUAL "libmcu_cbor") set(CBOR_ROOT cbor) - include_directories(${CBOR_ROOT}/include) + set(CBOR_INC_DIR "${CBOR_ROOT}/include") target_sources(rmw_desert PRIVATE ${CBOR_ROOT}/src/common.c ${CBOR_ROOT}/src/parser.c @@ -66,18 +68,70 @@ if (${CBOR_LIB} STREQUAL "libmcu_cbor") ) target_compile_definitions(rmw_desert PRIVATE LIBMCU_CBOR_ENABLED) elseif (${CBOR_LIB} STREQUAL "NanoCBOR") - set(CBOR_ROOT thirdparty/NanoCBOR) - include_directories(${CBOR_ROOT}/include) - target_sources(rmw_desert PRIVATE + set(CBOR_ROOT "${PROJECT_SOURCE_DIR}/thirdparty/NanoCBOR") + set(NANOCBOR_LIB "NanoCBOR") + set(CBOR_INC_DIR "${CBOR_ROOT}/include") + add_library(${NANOCBOR_LIB} OBJECT ${CBOR_ROOT}/src/decoder.c ${CBOR_ROOT}/src/encoder.c - src/desert_classes/NanoCBorStream.cpp + ) + target_include_directories(${NANOCBOR_LIB} PRIVATE ${CBOR_ROOT}) + + target_sources(rmw_desert PRIVATE + $ + ${PROJECT_SOURCE_DIR}/src/desert_classes/NanoCBorStream.cpp ) target_compile_definitions(rmw_desert PRIVATE NANOCBOR_ENABLED) else () message(FATAL_ERROR "At least one CBOR libraty is required") endif () - + +target_include_directories(rmw_desert PRIVATE ${CBOR_INC_DIR}) + +if (SECURE_MODE_ENABLED) + if (NOT ${CBOR_LIB} STREQUAL "NanoCBOR") + message(FATAL_ERROR "Secure mode requires NanoCBOR lib.") + endif () + + set(COSE_ROOT "${PROJECT_SOURCE_DIR}/thirdparty/libcose") + set(COSE_LIB "libcose") + set(COSE_INC_DIR "${COSE_ROOT}/include") + add_library(${COSE_LIB} OBJECT + ${COSE_ROOT}/src/cose_common.c + ${COSE_ROOT}/src/cose_crypto.c + ${COSE_ROOT}/src/cose_encrypt.c + ${COSE_ROOT}/src/cose_hdr.c + ${COSE_ROOT}/src/cose_hkdf.c + ${COSE_ROOT}/src/cose_key.c + ${COSE_ROOT}/src/cose_recipient.c + ${COSE_ROOT}/src/cose_sign.c + ${COSE_ROOT}/src/cose_signature.c + ) + target_include_directories(${COSE_LIB} PRIVATE ${COSE_INC_DIR} ${CBOR_INC_DIR}) + + if (${CRYPTO_LIB} STREQUAL "mbedtls") +# find_package(MbedTLS) + add_subdirectory(thirdparty/mbedtls) + + target_sources(${COSE_LIB} PRIVATE ${COSE_ROOT}/src/crypt/mbedtls.c) + target_sources(${COSE_LIB} PRIVATE ${COSE_ROOT}/src/crypt/keygen_symm.c) + target_link_libraries(${COSE_LIB} PRIVATE mbedtls) + target_link_libraries(rmw_desert PRIVATE mbedtls) + add_compile_definitions(CRYPTO_MBEDTLS) + else () + message(FATAL_ERROR "Cryptolib ${CRYPTO_LIB} is not supported") + endif () + + target_sources(rmw_desert PRIVATE + $ + ${PROJECT_SOURCE_DIR}/src/desert_classes/security/sec_utils.cpp + ${PROJECT_SOURCE_DIR}/src/desert_classes/security/SecurityConfig.cpp + ${PROJECT_SOURCE_DIR}/src/desert_classes/security/SecurityLayer.cpp + ) + target_include_directories(rmw_desert PRIVATE ${COSE_INC_DIR}) + target_compile_definitions(rmw_desert PRIVATE SECURE_MODE_ENABLED) +endif () + INCLUDE (FindPkgConfig) # specific order: dependents before dependencies diff --git a/config.cmake b/config.cmake index d2c5c18..d7e5172 100644 --- a/config.cmake +++ b/config.cmake @@ -1 +1,3 @@ -set(CBOR_LIB "libmcu_cbor" CACHE STRING "Cbor library.") # libmcu_cbor, NanoCBOR +set(CBOR_LIB "NanoCBOR" CACHE STRING "Cbor library.") # libmcu_cbor, NanoCBOR +option(SECURE_MODE_ENABLED "" ON) +set(CRYPTO_LIB "mbedtls" CACHE STRING "Crypto library.") diff --git a/src/desert_classes/NanoCBorStream.cpp b/src/desert_classes/NanoCBorStream.cpp index b6694cc..0975b4f 100644 --- a/src/desert_classes/NanoCBorStream.cpp +++ b/src/desert_classes/NanoCBorStream.cpp @@ -2,6 +2,10 @@ #include +#ifdef SECURE_MODE_ENABLED +#include "security/SecurityLayer.h" +#endif + namespace cbor { @@ -66,13 +70,19 @@ void TxStream::end_transmission() size_t encoded_len = nanocbor_encoded_len(&_writer->nanocbor_encoder); - std::vector daemon_packet; + size_t wrapped_size = encoded_len; + uint8_t* wrapped_ptr = _packet; - for(size_t i=0; i < encoded_len; i++) - { - daemon_packet.push_back(_packet[i]); +#ifdef SECURE_MODE_ENABLED + auto st = rmw_desert::security::g_sec_layer->wrap(_packet, encoded_len, MAX_PACKET_LENGTH, &wrapped_ptr, &wrapped_size); + if (st != rmw_desert::security::OK) { + delete _packet; + delete _writer; + return; } +#endif + std::vector daemon_packet(wrapped_ptr, wrapped_ptr + wrapped_size); TcpDaemon::enqueue_packet(daemon_packet); delete _packet; @@ -293,7 +303,7 @@ RxStream & RxStream::operator>>(uint64_t & n) n = *static_cast(_buffered_packet[_buffered_iterator].first); _buffered_iterator++; - + return *this; } @@ -328,7 +338,7 @@ RxStream & RxStream::operator>>(int64_t & n) n = *static_cast(_buffered_packet[_buffered_iterator].first); _buffered_iterator++; - + return *this; } @@ -460,8 +470,17 @@ void RxStream::interpret_packets() { // Initialize buffer and reader uint8_t * buffer = &packet[0]; + size_t unwrapped_size = packet.size(); + +#ifdef SECURE_MODE_ENABLED + auto unwrap_st = rmw_desert::security::g_sec_layer->unwrap(buffer, packet.size(), &unwrapped_size); + if (unwrap_st != rmw_desert::security::OK) { + continue; + } +#endif + ITEM decoder; - nanocbor_decoder_init(&decoder.nanocbor_value, buffer, packet.size()); + nanocbor_decoder_init(&decoder.nanocbor_value, buffer, unwrapped_size); uint8_t stream_type; uint8_t stream_identifier; diff --git a/src/desert_classes/security/SecurityConfig.cpp b/src/desert_classes/security/SecurityConfig.cpp new file mode 100644 index 0000000..4457ab5 --- /dev/null +++ b/src/desert_classes/security/SecurityConfig.cpp @@ -0,0 +1,21 @@ +#include + +#include "SecurityConfig.h" +#include "SecurityLayer.h" + +namespace rmw_desert::security { + + std::shared_ptr ascon_aead128 = std::make_shared(ASCON_AEAD128, 16, 16, 16); + std::shared_ptr ascon_aead128_64 = std::make_shared(ASCON_AEAD128_64, 16, 16, 8); + std::shared_ptr ascon_aead128_32 = std::make_shared(ASCON_AEAD128_32, 16, 16, 4); + std::shared_ptr aes_gcm_128 = std::make_shared(A128GCM, 16, 12, 16); + + std::shared_ptr key_provider = std::make_shared("MASTER_SECRET_KEY"); + std::shared_ptr nonce_gen = std::make_shared(); + + const std::unique_ptr g_sec_layer = std::make_unique( + aes_gcm_128, key_provider, nonce_gen + ); + +} + diff --git a/src/desert_classes/security/SecurityConfig.h b/src/desert_classes/security/SecurityConfig.h new file mode 100644 index 0000000..82d6f6d --- /dev/null +++ b/src/desert_classes/security/SecurityConfig.h @@ -0,0 +1,32 @@ +#ifndef SECURITY_CONFIG_H +#define SECURITY_CONFIG_H + +#include + +namespace rmw_desert::security { + enum AeadAlgorithms { + ASCON_AEAD128, + ASCON_AEAD128_64, + ASCON_AEAD128_32, + A128GCM + }; + + class AeadParams { + public: + AeadParams(AeadAlgorithms alg, uint16_t key_size, uint16_t nonce_size, uint16_t tag_size) + : alg_(alg), key_size_(key_size), nonce_size_(nonce_size), tag_size_(tag_size) {} + + [[nodiscard]] AeadAlgorithms get_alg() const { return alg_; } + [[nodiscard]] uint16_t get_key_size() const { return key_size_; } + [[nodiscard]] uint16_t get_nonce_size() const { return nonce_size_; } + [[nodiscard]] uint16_t get_tag_size() const { return tag_size_; } + + private: + AeadAlgorithms alg_; + uint16_t key_size_; + uint16_t nonce_size_; + uint16_t tag_size_; + }; +} + +#endif diff --git a/src/desert_classes/security/SecurityLayer.cpp b/src/desert_classes/security/SecurityLayer.cpp new file mode 100644 index 0000000..f0805f9 --- /dev/null +++ b/src/desert_classes/security/SecurityLayer.cpp @@ -0,0 +1,141 @@ + +#include + +#include "SecurityConfig.h" +#include "SecurityLayer.h" +#include "sec_utils.h" + +namespace rmw_desert::security { + + namespace { + cose_algo_t aead_algo_to_cose_algo(AeadAlgorithms algo) { + switch (algo) { + case ASCON_AEAD128: + case ASCON_AEAD128_64: + case ASCON_AEAD128_32: + case A128GCM: + return COSE_ALGO_A128GCM; + } + + return COSE_ALGO_A128GCM; + } + } + + void KeyProvider::set_key_size(size_t k_size) { + k_size_ = k_size; + } + + size_t KeyProvider::get_key_size() { + return k_size_; + } + + EnvKeyProvider::EnvKeyProvider(std::string env_name) + : env_name_(std::move(env_name)) { + } + + SecurityResult EnvKeyProvider::provide(const uint8_t **key_buf) { + if (key_bytes_.empty()) { + const char* env_value = std::getenv(env_name_.c_str()); + if (env_value == nullptr || !decode_hex(env_value, key_bytes_)) { + *key_buf = nullptr; + return KEY_ENV_ERROR; + } + if (key_bytes_.size() < k_size_) { + return KEY_SIZE_ERROR; + } + } + + *key_buf = key_bytes_.data(); + return OK; + } + + void NonceGenerator::set_nonce_size(size_t n_size) { + n_size_ = n_size; + } + + size_t NonceGenerator::get_nonce_size() { + return n_size_; + } + + SecurityResult DefaultNVMNonceGenerator::generate(uint8_t* nonce_buf) { + // TODO: Implement + memset(nonce_buf, 0, n_size_); + return OK; + } + + CoseSecurityLayer::CoseSecurityLayer(std::shared_ptr params, + std::shared_ptr key_provider, + std::shared_ptr nonce_gen_v) + : aead_params_(std::move(params)), + key_provider_(std::move(key_provider)), + nonce_gen_(std::move(nonce_gen_v)) { + key_provider_->set_key_size(aead_params_->get_key_size()); + nonce_gen_->set_nonce_size(aead_params_->get_nonce_size()); + } + + SecurityResult CoseSecurityLayer::wrap(uint8_t* data, size_t data_len, size_t data_max_len, uint8_t** cose_ptr, size_t* cose_len) { + const uint8_t* key_bytes; + auto key_st = key_provider_->provide(&key_bytes); + if (key_st != OK) { + return key_st; + } + + cose_algo_t algo = aead_algo_to_cose_algo(aead_params_->get_alg()); + std::vector nonce_bytes(nonce_gen_->get_nonce_size()); + auto nonce_st = nonce_gen_->generate(nonce_bytes.data()); + if (nonce_st != OK) { + return nonce_st; + } + static uint8_t kid[] = "rmw_desert_key"; + // uint8_t payload[data_len]; + std::vector payload(data, data + data_len); + // memcpy(payload, data, data_len); + + cose_key_t key; + cose_key_init(&key); + cose_key_set_kid(&key, kid, sizeof(kid) - 1); + cose_key_set_keys(&key, COSE_EC_NONE, algo, nullptr, nullptr, const_cast(key_bytes)); + + cose_encrypt_t crypt; + cose_encrypt_init(&crypt, COSE_FLAGS_ENCRYPT0 | COSE_FLAGS_UNTAGGED); + cose_encrypt_add_recipient(&crypt, &key); + cose_encrypt_set_payload(&crypt, payload.data(), payload.size()); + cose_encrypt_set_algo(&crypt, COSE_ALGO_DIRECT); + COSE_ssize_t len = cose_encrypt_encode(&crypt, data, data_max_len, nonce_bytes.data(), cose_ptr); + if (len <= 0) { + return WRAP_ERROR; + } + *cose_len = len; + + return OK; + } + + SecurityResult CoseSecurityLayer::unwrap(uint8_t* data, size_t data_len, size_t* new_data_len) { + const uint8_t* key_bytes; + auto key_st = key_provider_->provide(&key_bytes); + if (key_st != OK) { + return key_st; + } + + cose_encrypt_dec_t decrypt; + if (cose_encrypt_decode(&decrypt, data, data_len) != COSE_OK) { + return UNWRAP_ERROR; + } + + cose_algo_t algo = aead_algo_to_cose_algo(aead_params_->get_alg()); + static uint8_t kid[] = "rmw_desert_key"; + + cose_key_t key; + cose_key_init(&key); + cose_key_set_kid(&key, kid, sizeof(kid) - 1); + cose_key_set_keys(&key, COSE_EC_NONE, algo, nullptr, nullptr, const_cast(key_bytes)); + + std::vector plaintext(data_len); + if (cose_encrypt_decrypt(&decrypt, nullptr, &key, internal_buf_, sizeof(internal_buf_), plaintext.data(), new_data_len)) { + return UNWRAP_ERROR; + } + memcpy(data, plaintext.data(), *new_data_len); + + return OK; + } +} diff --git a/src/desert_classes/security/SecurityLayer.h b/src/desert_classes/security/SecurityLayer.h new file mode 100644 index 0000000..fd4642d --- /dev/null +++ b/src/desert_classes/security/SecurityLayer.h @@ -0,0 +1,92 @@ +#ifndef SECURITY_LAYER_H +#define SECURITY_LAYER_H + +// #include +// #include +#include +#include +#include + +#include "SecurityConfig.h" + +#define COSE_INTERNAL_BUF_SIZE 512 + +namespace rmw_desert::security { + + class SecurityLayer; + + extern const std::unique_ptr g_sec_layer; + + enum SecurityResult { + OK, KEY_SIZE_ERROR, KEY_ENV_ERROR, WRAP_ERROR, UNWRAP_ERROR + }; + + class NonceGenerator { + public: + virtual ~NonceGenerator() = default; + + virtual void set_nonce_size(size_t n_size); + virtual size_t get_nonce_size(); + virtual SecurityResult generate(uint8_t* nonce_buf) = 0; + protected: + size_t n_size_{16}; + }; + + class DefaultNVMNonceGenerator : public NonceGenerator { + public: + SecurityResult generate(uint8_t* nonce_buf) override; + + }; + + class KeyProvider { + public: + // explicit KeyProvider(size_t k_size); + virtual ~KeyProvider() = default; + + virtual void set_key_size(size_t k_size); + virtual size_t get_key_size(); + virtual SecurityResult provide(const uint8_t **key_buf) = 0; + + protected: + size_t k_size_{16}; + }; + + class EnvKeyProvider : public KeyProvider { + public: + EnvKeyProvider(std::string env_name); + // EnvKeyProvider(size_t k_size, std::string env_name); + + SecurityResult provide(const uint8_t **key_buf) override; + + private: + std::string env_name_; + std::vector key_bytes_{}; + }; + + class SecurityLayer { + public: + virtual ~SecurityLayer() = default; + + virtual SecurityResult wrap(uint8_t* data, size_t data_len, size_t data_max_len, uint8_t** cose_ptr, size_t* cose_len) = 0; + virtual SecurityResult unwrap(uint8_t* data, size_t data_len, size_t* new_data_len) = 0; + }; + + class CoseSecurityLayer : public SecurityLayer { + public: + explicit CoseSecurityLayer(std::shared_ptr params, + std::shared_ptr key_provider, + std::shared_ptr nonce_gen); + + SecurityResult wrap(uint8_t* data, size_t data_len, size_t data_max_len, uint8_t** cose_ptr, size_t* cose_len) override; + SecurityResult unwrap(uint8_t* data, size_t data_len, size_t* new_data_len) override; + + private: + std::shared_ptr aead_params_; + std::shared_ptr key_provider_; + std::shared_ptr nonce_gen_; + uint8_t internal_buf_[COSE_INTERNAL_BUF_SIZE]{}; + }; + +} + +#endif diff --git a/src/desert_classes/security/sec_utils.cpp b/src/desert_classes/security/sec_utils.cpp new file mode 100644 index 0000000..d9dcf86 --- /dev/null +++ b/src/desert_classes/security/sec_utils.cpp @@ -0,0 +1,30 @@ +#include "sec_utils.h" + +static int hex_value(char c) { + if (c >= '0' && c <= '9') return c - '0'; + if (c >= 'a' && c <= 'f') return 10 + (c - 'a'); + if (c >= 'A' && c <= 'F') return 10 + (c - 'A'); + return -1; +} + +bool decode_hex(const std::string& hex, std::vector& out) { + if (hex.size() % 2 != 0) { + return false; + } + + out.clear(); + out.reserve(hex.size() / 2); + + for (size_t i = 0; i < hex.size(); i += 2) { + const int hi = hex_value(hex[i]); + const int lo = hex_value(hex[i + 1]); + if (hi < 0 || lo < 0) { + out.clear(); + return false; + } + + out.push_back(static_cast((hi << 4) | lo)); + } + + return true; +} \ No newline at end of file diff --git a/src/desert_classes/security/sec_utils.h b/src/desert_classes/security/sec_utils.h new file mode 100644 index 0000000..4dd228a --- /dev/null +++ b/src/desert_classes/security/sec_utils.h @@ -0,0 +1,10 @@ +#ifndef SEC_UTILS_H +#define SEC_UTILS_H + +#include +#include +#include + +bool decode_hex(const std::string& hex, std::vector& out); + +#endif diff --git a/thirdparty/libcose b/thirdparty/libcose new file mode 160000 index 0000000..ea1fed8 --- /dev/null +++ b/thirdparty/libcose @@ -0,0 +1 @@ +Subproject commit ea1fed87d6ca9b478f8bed323af97e6b192c0a6d diff --git a/thirdparty/mbedtls b/thirdparty/mbedtls new file mode 160000 index 0000000..3a91dad --- /dev/null +++ b/thirdparty/mbedtls @@ -0,0 +1 @@ +Subproject commit 3a91dad9dceb484eea8b41f8941facafc4520021 From 75bc70fe546e1f0c38a3683146310cd091a04a5c Mon Sep 17 00:00:00 2001 From: Dmytro Ochkas Date: Wed, 8 Jul 2026 22:46:40 +0200 Subject: [PATCH 06/24] Downgrade NanoCBOR to compatible version --- thirdparty/NanoCBOR | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/thirdparty/NanoCBOR b/thirdparty/NanoCBOR index 0623c45..3f0f042 160000 --- a/thirdparty/NanoCBOR +++ b/thirdparty/NanoCBOR @@ -1 +1 @@ -Subproject commit 0623c45686f6bc296c35416f3a41a71b148122d7 +Subproject commit 3f0f0422b86aed01c98c1f14bbcb58061012d805 From efffb9fa4052eeee00072135b3b132d064a39c43 Mon Sep 17 00:00:00 2001 From: matlin Date: Thu, 9 Jul 2026 11:18:43 +0200 Subject: [PATCH 07/24] Fixed a new nosense bug in CMakeLists --- CMakeLists.txt | 16 +++++----------- src/get_service_endpoint_info.cpp | 1 + 2 files changed, 6 insertions(+), 11 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index cca460b..c2e483b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -56,7 +56,7 @@ add_library(rmw_desert if (${CBOR_LIB} STREQUAL "libmcu_cbor") set(CBOR_ROOT cbor) - set(CBOR_INC_DIR "${CBOR_ROOT}/include") + include_directories(${CBOR_ROOT}/include) target_sources(rmw_desert PRIVATE ${CBOR_ROOT}/src/common.c ${CBOR_ROOT}/src/parser.c @@ -68,18 +68,12 @@ if (${CBOR_LIB} STREQUAL "libmcu_cbor") ) target_compile_definitions(rmw_desert PRIVATE LIBMCU_CBOR_ENABLED) elseif (${CBOR_LIB} STREQUAL "NanoCBOR") - set(CBOR_ROOT "${PROJECT_SOURCE_DIR}/thirdparty/NanoCBOR") - set(NANOCBOR_LIB "NanoCBOR") - set(CBOR_INC_DIR "${CBOR_ROOT}/include") - add_library(${NANOCBOR_LIB} OBJECT + set(CBOR_ROOT thirdparty/NanoCBOR) + include_directories(${CBOR_ROOT}/include) + target_sources(rmw_desert PRIVATE ${CBOR_ROOT}/src/decoder.c ${CBOR_ROOT}/src/encoder.c - ) - target_include_directories(${NANOCBOR_LIB} PRIVATE ${CBOR_ROOT}) - - target_sources(rmw_desert PRIVATE - $ - ${PROJECT_SOURCE_DIR}/src/desert_classes/NanoCBorStream.cpp + src/desert_classes/NanoCBorStream.cpp ) target_compile_definitions(rmw_desert PRIVATE NANOCBOR_ENABLED) else () diff --git a/src/get_service_endpoint_info.cpp b/src/get_service_endpoint_info.cpp index b5f7a68..29d605a 100644 --- a/src/get_service_endpoint_info.cpp +++ b/src/get_service_endpoint_info.cpp @@ -25,3 +25,4 @@ rmw_ret_t rmw_get_servers_info_by_service(const rmw_node_t * node, rcutils_alloc RMW_SET_ERROR_MSG("rmw_get_servers_info_by_service not implemented"); return RMW_RET_UNSUPPORTED; } + From 48e0a46ddeb418152d802c81736b1d0f0d8febb3 Mon Sep 17 00:00:00 2001 From: matlin Date: Thu, 9 Jul 2026 11:49:45 +0200 Subject: [PATCH 08/24] Fixed another compile error --- CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c2e483b..4d0e625 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -87,6 +87,8 @@ if (SECURE_MODE_ENABLED) message(FATAL_ERROR "Secure mode requires NanoCBOR lib.") endif () + add_compile_options(-fPIC) + set(COSE_ROOT "${PROJECT_SOURCE_DIR}/thirdparty/libcose") set(COSE_LIB "libcose") set(COSE_INC_DIR "${COSE_ROOT}/include") @@ -104,7 +106,6 @@ if (SECURE_MODE_ENABLED) target_include_directories(${COSE_LIB} PRIVATE ${COSE_INC_DIR} ${CBOR_INC_DIR}) if (${CRYPTO_LIB} STREQUAL "mbedtls") -# find_package(MbedTLS) add_subdirectory(thirdparty/mbedtls) target_sources(${COSE_LIB} PRIVATE ${COSE_ROOT}/src/crypt/mbedtls.c) From 37ea31dc6e41d1f4baf33c5a4cbeb185e0c96d44 Mon Sep 17 00:00:00 2001 From: matlin Date: Thu, 9 Jul 2026 11:53:50 +0200 Subject: [PATCH 09/24] Changed to coherent notations in CMakeList --- CMakeLists.txt | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4d0e625..8047d74 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -89,7 +89,7 @@ if (SECURE_MODE_ENABLED) add_compile_options(-fPIC) - set(COSE_ROOT "${PROJECT_SOURCE_DIR}/thirdparty/libcose") + set(COSE_ROOT "thirdparty/libcose") set(COSE_LIB "libcose") set(COSE_INC_DIR "${COSE_ROOT}/include") add_library(${COSE_LIB} OBJECT @@ -118,10 +118,9 @@ if (SECURE_MODE_ENABLED) endif () target_sources(rmw_desert PRIVATE - $ - ${PROJECT_SOURCE_DIR}/src/desert_classes/security/sec_utils.cpp - ${PROJECT_SOURCE_DIR}/src/desert_classes/security/SecurityConfig.cpp - ${PROJECT_SOURCE_DIR}/src/desert_classes/security/SecurityLayer.cpp + src/desert_classes/security/sec_utils.cpp + src/desert_classes/security/SecurityConfig.cpp + src/desert_classes/security/SecurityLayer.cpp ) target_include_directories(rmw_desert PRIVATE ${COSE_INC_DIR}) target_compile_definitions(rmw_desert PRIVATE SECURE_MODE_ENABLED) From ef458889fccf3a807750a71bd704ec24057f7394 Mon Sep 17 00:00:00 2001 From: matlin Date: Fri, 10 Jul 2026 12:04:53 +0200 Subject: [PATCH 10/24] Cleanup --- src/desert_classes/NanoCBorStream.cpp | 12 +- .../security/SecurityConfig.cpp | 45 +++- src/desert_classes/security/SecurityConfig.h | 54 +++-- src/desert_classes/security/SecurityLayer.cpp | 229 ++++++++---------- src/desert_classes/security/SecurityLayer.h | 91 ++----- 5 files changed, 192 insertions(+), 239 deletions(-) diff --git a/src/desert_classes/NanoCBorStream.cpp b/src/desert_classes/NanoCBorStream.cpp index 0975b4f..3122bc9 100644 --- a/src/desert_classes/NanoCBorStream.cpp +++ b/src/desert_classes/NanoCBorStream.cpp @@ -74,8 +74,10 @@ void TxStream::end_transmission() uint8_t* wrapped_ptr = _packet; #ifdef SECURE_MODE_ENABLED - auto st = rmw_desert::security::g_sec_layer->wrap(_packet, encoded_len, MAX_PACKET_LENGTH, &wrapped_ptr, &wrapped_size); - if (st != rmw_desert::security::OK) { + security::SecurityLayer g_sec_layer; + auto st = g_sec_layer.wrap(_packet, encoded_len, MAX_PACKET_LENGTH, &wrapped_ptr, &wrapped_size); + if (st != security::OK) + { delete _packet; delete _writer; return; @@ -473,8 +475,10 @@ void RxStream::interpret_packets() size_t unwrapped_size = packet.size(); #ifdef SECURE_MODE_ENABLED - auto unwrap_st = rmw_desert::security::g_sec_layer->unwrap(buffer, packet.size(), &unwrapped_size); - if (unwrap_st != rmw_desert::security::OK) { + security::SecurityLayer g_sec_layer; + auto unwrap_st = g_sec_layer.unwrap(buffer, packet.size(), &unwrapped_size); + if (unwrap_st != security::OK) + { continue; } #endif diff --git a/src/desert_classes/security/SecurityConfig.cpp b/src/desert_classes/security/SecurityConfig.cpp index 4457ab5..9d1ec65 100644 --- a/src/desert_classes/security/SecurityConfig.cpp +++ b/src/desert_classes/security/SecurityConfig.cpp @@ -3,19 +3,38 @@ #include "SecurityConfig.h" #include "SecurityLayer.h" -namespace rmw_desert::security { - - std::shared_ptr ascon_aead128 = std::make_shared(ASCON_AEAD128, 16, 16, 16); - std::shared_ptr ascon_aead128_64 = std::make_shared(ASCON_AEAD128_64, 16, 16, 8); - std::shared_ptr ascon_aead128_32 = std::make_shared(ASCON_AEAD128_32, 16, 16, 4); - std::shared_ptr aes_gcm_128 = std::make_shared(A128GCM, 16, 12, 16); - - std::shared_ptr key_provider = std::make_shared("MASTER_SECRET_KEY"); - std::shared_ptr nonce_gen = std::make_shared(); - - const std::unique_ptr g_sec_layer = std::make_unique( - aes_gcm_128, key_provider, nonce_gen - ); +namespace security +{ +AeadParams::AeadParams(AeadAlgorithms alg) + : alg_(alg) +{ + switch (alg_) + { + case ASCON_AEAD128: + key_size_ = 16; + nonce_size_ = 16; + tag_size_ = 16; + break; + case ASCON_AEAD128_64: + key_size_ = 16; + nonce_size_ = 16; + tag_size_ = 8; + break; + case ASCON_AEAD128_32: + key_size_ = 16; + nonce_size_ = 16; + tag_size_ = 4; + break; + case A128GCM: + key_size_ = 16; + nonce_size_ = 12; + tag_size_ = 16; + break; + default: + throw std::runtime_error("Invalid security profile"); + } } +} // namespace security + diff --git a/src/desert_classes/security/SecurityConfig.h b/src/desert_classes/security/SecurityConfig.h index 82d6f6d..7b8b299 100644 --- a/src/desert_classes/security/SecurityConfig.h +++ b/src/desert_classes/security/SecurityConfig.h @@ -3,30 +3,34 @@ #include -namespace rmw_desert::security { - enum AeadAlgorithms { - ASCON_AEAD128, - ASCON_AEAD128_64, - ASCON_AEAD128_32, - A128GCM - }; - - class AeadParams { - public: - AeadParams(AeadAlgorithms alg, uint16_t key_size, uint16_t nonce_size, uint16_t tag_size) - : alg_(alg), key_size_(key_size), nonce_size_(nonce_size), tag_size_(tag_size) {} - - [[nodiscard]] AeadAlgorithms get_alg() const { return alg_; } - [[nodiscard]] uint16_t get_key_size() const { return key_size_; } - [[nodiscard]] uint16_t get_nonce_size() const { return nonce_size_; } - [[nodiscard]] uint16_t get_tag_size() const { return tag_size_; } - - private: - AeadAlgorithms alg_; - uint16_t key_size_; - uint16_t nonce_size_; - uint16_t tag_size_; - }; -} +namespace security +{ + +enum AeadAlgorithms +{ + ASCON_AEAD128, + ASCON_AEAD128_64, + ASCON_AEAD128_32, + A128GCM +}; + +class AeadParams +{ + public: + AeadParams(AeadAlgorithms alg); + + AeadAlgorithms get_alg() const { return alg_; } + uint16_t get_key_size() const { return key_size_; } + uint16_t get_nonce_size() const { return nonce_size_; } + uint16_t get_tag_size() const { return tag_size_; } + + private: + AeadAlgorithms alg_; + uint16_t key_size_; + uint16_t nonce_size_; + uint16_t tag_size_; +}; + +} // namespace security #endif diff --git a/src/desert_classes/security/SecurityLayer.cpp b/src/desert_classes/security/SecurityLayer.cpp index f0805f9..5b1d025 100644 --- a/src/desert_classes/security/SecurityLayer.cpp +++ b/src/desert_classes/security/SecurityLayer.cpp @@ -1,141 +1,118 @@ - #include #include "SecurityConfig.h" #include "SecurityLayer.h" #include "sec_utils.h" -namespace rmw_desert::security { - - namespace { - cose_algo_t aead_algo_to_cose_algo(AeadAlgorithms algo) { - switch (algo) { - case ASCON_AEAD128: - case ASCON_AEAD128_64: - case ASCON_AEAD128_32: - case A128GCM: - return COSE_ALGO_A128GCM; - } - - return COSE_ALGO_A128GCM; - } - } - - void KeyProvider::set_key_size(size_t k_size) { - k_size_ = k_size; - } - - size_t KeyProvider::get_key_size() { - return k_size_; - } - - EnvKeyProvider::EnvKeyProvider(std::string env_name) - : env_name_(std::move(env_name)) { - } +namespace security +{ + +cose_algo_t aead_algo_to_cose_algo(AeadAlgorithms algo) +{ + switch (algo) + { + case ASCON_AEAD128: + case ASCON_AEAD128_64: + case ASCON_AEAD128_32: + case A128GCM: + return COSE_ALGO_A128GCM; + } + + return COSE_ALGO_A128GCM; +} - SecurityResult EnvKeyProvider::provide(const uint8_t **key_buf) { - if (key_bytes_.empty()) { - const char* env_value = std::getenv(env_name_.c_str()); - if (env_value == nullptr || !decode_hex(env_value, key_bytes_)) { - *key_buf = nullptr; - return KEY_ENV_ERROR; - } - if (key_bytes_.size() < k_size_) { - return KEY_SIZE_ERROR; - } - } - - *key_buf = key_bytes_.data(); - return OK; - } +SecurityLayer::SecurityLayer() + : aead_params_(AeadParams(A128GCM)) +{ + if (get_key() != OK) + { + throw std::runtime_error("Failed to get the symmetric key"); + } +} - void NonceGenerator::set_nonce_size(size_t n_size) { - n_size_ = n_size; - } +SecurityResult SecurityLayer::generate_nonce(uint8_t* nonce_buf) +{ + // TODO: Implement + memset(nonce_buf, 0, aead_params_.get_nonce_size()); + return OK; +} - size_t NonceGenerator::get_nonce_size() { - return n_size_; +SecurityResult SecurityLayer::get_key() +{ + if (key_bytes_.empty()) + { + const char* env_value = std::getenv("MASTER_SECRET_KEY"); + if (env_value == nullptr || !decode_hex(env_value, key_bytes_)) + { + return KEY_ENV_ERROR; } - - SecurityResult DefaultNVMNonceGenerator::generate(uint8_t* nonce_buf) { - // TODO: Implement - memset(nonce_buf, 0, n_size_); - return OK; + if (key_bytes_.size() < aead_params_.get_key_size()) + { + return KEY_SIZE_ERROR; } + } - CoseSecurityLayer::CoseSecurityLayer(std::shared_ptr params, - std::shared_ptr key_provider, - std::shared_ptr nonce_gen_v) - : aead_params_(std::move(params)), - key_provider_(std::move(key_provider)), - nonce_gen_(std::move(nonce_gen_v)) { - key_provider_->set_key_size(aead_params_->get_key_size()); - nonce_gen_->set_nonce_size(aead_params_->get_nonce_size()); - } + return OK; +} - SecurityResult CoseSecurityLayer::wrap(uint8_t* data, size_t data_len, size_t data_max_len, uint8_t** cose_ptr, size_t* cose_len) { - const uint8_t* key_bytes; - auto key_st = key_provider_->provide(&key_bytes); - if (key_st != OK) { - return key_st; - } - - cose_algo_t algo = aead_algo_to_cose_algo(aead_params_->get_alg()); - std::vector nonce_bytes(nonce_gen_->get_nonce_size()); - auto nonce_st = nonce_gen_->generate(nonce_bytes.data()); - if (nonce_st != OK) { - return nonce_st; - } - static uint8_t kid[] = "rmw_desert_key"; - // uint8_t payload[data_len]; - std::vector payload(data, data + data_len); - // memcpy(payload, data, data_len); - - cose_key_t key; - cose_key_init(&key); - cose_key_set_kid(&key, kid, sizeof(kid) - 1); - cose_key_set_keys(&key, COSE_EC_NONE, algo, nullptr, nullptr, const_cast(key_bytes)); - - cose_encrypt_t crypt; - cose_encrypt_init(&crypt, COSE_FLAGS_ENCRYPT0 | COSE_FLAGS_UNTAGGED); - cose_encrypt_add_recipient(&crypt, &key); - cose_encrypt_set_payload(&crypt, payload.data(), payload.size()); - cose_encrypt_set_algo(&crypt, COSE_ALGO_DIRECT); - COSE_ssize_t len = cose_encrypt_encode(&crypt, data, data_max_len, nonce_bytes.data(), cose_ptr); - if (len <= 0) { - return WRAP_ERROR; - } - *cose_len = len; - - return OK; - } +SecurityResult SecurityLayer::wrap(uint8_t* data, size_t data_len, size_t data_max_len, uint8_t** cose_ptr, size_t* cose_len) +{ + cose_algo_t algo = aead_algo_to_cose_algo(aead_params_.get_alg()); + std::vector nonce_bytes(aead_params_.get_nonce_size()); + auto nonce_st = generate_nonce(nonce_bytes.data()); + if (nonce_st != OK) + { + return nonce_st; + } + static uint8_t kid[] = "rmw_desert_key"; + // uint8_t payload[data_len]; + std::vector payload(data, data + data_len); + // memcpy(payload, data, data_len); + + cose_key_t key; + cose_key_init(&key); + cose_key_set_kid(&key, kid, sizeof(kid) - 1); + cose_key_set_keys(&key, COSE_EC_NONE, algo, nullptr, nullptr, const_cast(key_bytes_.data())); + + cose_encrypt_t crypt; + cose_encrypt_init(&crypt, COSE_FLAGS_ENCRYPT0 | COSE_FLAGS_UNTAGGED); + cose_encrypt_add_recipient(&crypt, &key); + cose_encrypt_set_payload(&crypt, payload.data(), payload.size()); + cose_encrypt_set_algo(&crypt, COSE_ALGO_DIRECT); + COSE_ssize_t len = cose_encrypt_encode(&crypt, data, data_max_len, nonce_bytes.data(), cose_ptr); + if (len <= 0) + { + return WRAP_ERROR; + } + *cose_len = len; + + return OK; +} - SecurityResult CoseSecurityLayer::unwrap(uint8_t* data, size_t data_len, size_t* new_data_len) { - const uint8_t* key_bytes; - auto key_st = key_provider_->provide(&key_bytes); - if (key_st != OK) { - return key_st; - } - - cose_encrypt_dec_t decrypt; - if (cose_encrypt_decode(&decrypt, data, data_len) != COSE_OK) { - return UNWRAP_ERROR; - } - - cose_algo_t algo = aead_algo_to_cose_algo(aead_params_->get_alg()); - static uint8_t kid[] = "rmw_desert_key"; - - cose_key_t key; - cose_key_init(&key); - cose_key_set_kid(&key, kid, sizeof(kid) - 1); - cose_key_set_keys(&key, COSE_EC_NONE, algo, nullptr, nullptr, const_cast(key_bytes)); - - std::vector plaintext(data_len); - if (cose_encrypt_decrypt(&decrypt, nullptr, &key, internal_buf_, sizeof(internal_buf_), plaintext.data(), new_data_len)) { - return UNWRAP_ERROR; - } - memcpy(data, plaintext.data(), *new_data_len); - - return OK; - } +SecurityResult SecurityLayer::unwrap(uint8_t* data, size_t data_len, size_t* new_data_len) +{ + cose_encrypt_dec_t decrypt; + if (cose_encrypt_decode(&decrypt, data, data_len) != COSE_OK) + { + return UNWRAP_ERROR; + } + + cose_algo_t algo = aead_algo_to_cose_algo(aead_params_.get_alg()); + static uint8_t kid[] = "rmw_desert_key"; + + cose_key_t key; + cose_key_init(&key); + cose_key_set_kid(&key, kid, sizeof(kid) - 1); + cose_key_set_keys(&key, COSE_EC_NONE, algo, nullptr, nullptr, const_cast(key_bytes_.data())); + + std::vector plaintext(data_len); + if (cose_encrypt_decrypt(&decrypt, nullptr, &key, internal_buf_, sizeof(internal_buf_), plaintext.data(), new_data_len)) + { + return UNWRAP_ERROR; + } + memcpy(data, plaintext.data(), *new_data_len); + + return OK; } + +} // namespace security diff --git a/src/desert_classes/security/SecurityLayer.h b/src/desert_classes/security/SecurityLayer.h index fd4642d..640fdba 100644 --- a/src/desert_classes/security/SecurityLayer.h +++ b/src/desert_classes/security/SecurityLayer.h @@ -11,82 +11,31 @@ #define COSE_INTERNAL_BUF_SIZE 512 -namespace rmw_desert::security { +namespace security +{ - class SecurityLayer; +enum SecurityResult +{ + OK, KEY_SIZE_ERROR, KEY_ENV_ERROR, WRAP_ERROR, UNWRAP_ERROR +}; - extern const std::unique_ptr g_sec_layer; +class SecurityLayer +{ + public: + SecurityLayer(); - enum SecurityResult { - OK, KEY_SIZE_ERROR, KEY_ENV_ERROR, WRAP_ERROR, UNWRAP_ERROR - }; + SecurityResult wrap(uint8_t* data, size_t data_len, size_t data_max_len, uint8_t** cose_ptr, size_t* cose_len); + SecurityResult unwrap(uint8_t* data, size_t data_len, size_t* new_data_len); - class NonceGenerator { - public: - virtual ~NonceGenerator() = default; + private: + SecurityResult get_key(); + SecurityResult generate_nonce(uint8_t* nonce_buf); - virtual void set_nonce_size(size_t n_size); - virtual size_t get_nonce_size(); - virtual SecurityResult generate(uint8_t* nonce_buf) = 0; - protected: - size_t n_size_{16}; - }; + AeadParams aead_params_; + std::vector key_bytes_{}; + uint8_t internal_buf_[COSE_INTERNAL_BUF_SIZE]{}; +}; - class DefaultNVMNonceGenerator : public NonceGenerator { - public: - SecurityResult generate(uint8_t* nonce_buf) override; - - }; - - class KeyProvider { - public: - // explicit KeyProvider(size_t k_size); - virtual ~KeyProvider() = default; - - virtual void set_key_size(size_t k_size); - virtual size_t get_key_size(); - virtual SecurityResult provide(const uint8_t **key_buf) = 0; - - protected: - size_t k_size_{16}; - }; - - class EnvKeyProvider : public KeyProvider { - public: - EnvKeyProvider(std::string env_name); - // EnvKeyProvider(size_t k_size, std::string env_name); - - SecurityResult provide(const uint8_t **key_buf) override; - - private: - std::string env_name_; - std::vector key_bytes_{}; - }; - - class SecurityLayer { - public: - virtual ~SecurityLayer() = default; - - virtual SecurityResult wrap(uint8_t* data, size_t data_len, size_t data_max_len, uint8_t** cose_ptr, size_t* cose_len) = 0; - virtual SecurityResult unwrap(uint8_t* data, size_t data_len, size_t* new_data_len) = 0; - }; - - class CoseSecurityLayer : public SecurityLayer { - public: - explicit CoseSecurityLayer(std::shared_ptr params, - std::shared_ptr key_provider, - std::shared_ptr nonce_gen); - - SecurityResult wrap(uint8_t* data, size_t data_len, size_t data_max_len, uint8_t** cose_ptr, size_t* cose_len) override; - SecurityResult unwrap(uint8_t* data, size_t data_len, size_t* new_data_len) override; - - private: - std::shared_ptr aead_params_; - std::shared_ptr key_provider_; - std::shared_ptr nonce_gen_; - uint8_t internal_buf_[COSE_INTERNAL_BUF_SIZE]{}; - }; - -} +} // namespace security #endif From da8db2670f2af82a953fa22b1804ca653fc7c378 Mon Sep 17 00:00:00 2001 From: matlin Date: Fri, 10 Jul 2026 13:57:26 +0200 Subject: [PATCH 11/24] Implemented runtime key checks --- CMakeLists.txt | 3 +- .../{SecurityConfig.cpp => AeadParams.cpp} | 4 +- .../{SecurityConfig.h => AeadParams.h} | 0 src/desert_classes/security/SecurityLayer.cpp | 7 ++- src/desert_classes/security/SecurityLayer.h | 4 +- src/desert_classes/security/sec_utils.cpp | 45 ++++++++++--------- 6 files changed, 34 insertions(+), 29 deletions(-) rename src/desert_classes/security/{SecurityConfig.cpp => AeadParams.cpp} (92%) rename src/desert_classes/security/{SecurityConfig.h => AeadParams.h} (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8047d74..0651bba 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -118,8 +118,9 @@ if (SECURE_MODE_ENABLED) endif () target_sources(rmw_desert PRIVATE + $ src/desert_classes/security/sec_utils.cpp - src/desert_classes/security/SecurityConfig.cpp + src/desert_classes/security/AeadParams.cpp src/desert_classes/security/SecurityLayer.cpp ) target_include_directories(rmw_desert PRIVATE ${COSE_INC_DIR}) diff --git a/src/desert_classes/security/SecurityConfig.cpp b/src/desert_classes/security/AeadParams.cpp similarity index 92% rename from src/desert_classes/security/SecurityConfig.cpp rename to src/desert_classes/security/AeadParams.cpp index 9d1ec65..1fe14d5 100644 --- a/src/desert_classes/security/SecurityConfig.cpp +++ b/src/desert_classes/security/AeadParams.cpp @@ -1,7 +1,7 @@ #include -#include "SecurityConfig.h" -#include "SecurityLayer.h" +#include "AeadParams.h" +#include "AeadParams.h" namespace security { diff --git a/src/desert_classes/security/SecurityConfig.h b/src/desert_classes/security/AeadParams.h similarity index 100% rename from src/desert_classes/security/SecurityConfig.h rename to src/desert_classes/security/AeadParams.h diff --git a/src/desert_classes/security/SecurityLayer.cpp b/src/desert_classes/security/SecurityLayer.cpp index 5b1d025..692f3a0 100644 --- a/src/desert_classes/security/SecurityLayer.cpp +++ b/src/desert_classes/security/SecurityLayer.cpp @@ -1,6 +1,5 @@ #include -#include "SecurityConfig.h" #include "SecurityLayer.h" #include "sec_utils.h" @@ -26,7 +25,7 @@ SecurityLayer::SecurityLayer() { if (get_key() != OK) { - throw std::runtime_error("Failed to get the symmetric key"); + throw std::runtime_error("Failed to parse the encryption key"); } } @@ -44,10 +43,14 @@ SecurityResult SecurityLayer::get_key() const char* env_value = std::getenv("MASTER_SECRET_KEY"); if (env_value == nullptr || !decode_hex(env_value, key_bytes_)) { + printf("CRITICAL: the master secret key is invalid\n"); + printf("Please fill the MASTER_SECRET_KEY environment variable with stream of hexadecimal bytes\n"); return KEY_ENV_ERROR; } if (key_bytes_.size() < aead_params_.get_key_size()) { + printf("CRITICAL: the master secret key size does not match the required suite\n"); + printf("Please set a MASTER_SECRET_KEY of %d bytes\n", aead_params_.get_key_size()); return KEY_SIZE_ERROR; } } diff --git a/src/desert_classes/security/SecurityLayer.h b/src/desert_classes/security/SecurityLayer.h index 640fdba..fc5d82c 100644 --- a/src/desert_classes/security/SecurityLayer.h +++ b/src/desert_classes/security/SecurityLayer.h @@ -1,13 +1,11 @@ #ifndef SECURITY_LAYER_H #define SECURITY_LAYER_H -// #include -// #include #include #include #include -#include "SecurityConfig.h" +#include "AeadParams.h" #define COSE_INTERNAL_BUF_SIZE 512 diff --git a/src/desert_classes/security/sec_utils.cpp b/src/desert_classes/security/sec_utils.cpp index d9dcf86..8e98ac1 100644 --- a/src/desert_classes/security/sec_utils.cpp +++ b/src/desert_classes/security/sec_utils.cpp @@ -1,30 +1,33 @@ #include "sec_utils.h" -static int hex_value(char c) { - if (c >= '0' && c <= '9') return c - '0'; - if (c >= 'a' && c <= 'f') return 10 + (c - 'a'); - if (c >= 'A' && c <= 'F') return 10 + (c - 'A'); - return -1; +static int hex_value(char c) +{ + if (c >= '0' && c <= '9') return c - '0'; + if (c >= 'a' && c <= 'f') return 10 + (c - 'a'); + if (c >= 'A' && c <= 'F') return 10 + (c - 'A'); + return -1; } bool decode_hex(const std::string& hex, std::vector& out) { - if (hex.size() % 2 != 0) { - return false; - } - - out.clear(); - out.reserve(hex.size() / 2); + if (hex.size() % 2 != 0) + { + return false; + } - for (size_t i = 0; i < hex.size(); i += 2) { - const int hi = hex_value(hex[i]); - const int lo = hex_value(hex[i + 1]); - if (hi < 0 || lo < 0) { - out.clear(); - return false; - } + out.clear(); + out.reserve(hex.size() / 2); - out.push_back(static_cast((hi << 4) | lo)); + for (size_t i = 0; i < hex.size(); i += 2) + { + const int hi = hex_value(hex[i]); + const int lo = hex_value(hex[i + 1]); + if (hi < 0 || lo < 0) { + out.clear(); + return false; } - return true; -} \ No newline at end of file + out.push_back(static_cast((hi << 4) | lo)); + } + + return true; +} From c73b685b4617e963f2bd1f6b16826de730f9058a Mon Sep 17 00:00:00 2001 From: matlin Date: Fri, 10 Jul 2026 14:17:17 +0200 Subject: [PATCH 12/24] Brace style fix --- src/desert_classes/security/sec_utils.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/desert_classes/security/sec_utils.cpp b/src/desert_classes/security/sec_utils.cpp index 8e98ac1..b174413 100644 --- a/src/desert_classes/security/sec_utils.cpp +++ b/src/desert_classes/security/sec_utils.cpp @@ -21,7 +21,8 @@ bool decode_hex(const std::string& hex, std::vector& out) { { const int hi = hex_value(hex[i]); const int lo = hex_value(hex[i + 1]); - if (hi < 0 || lo < 0) { + if (hi < 0 || lo < 0) + { out.clear(); return false; } From 2ce10aab1896a98c0c948c3225db82c614ab345f Mon Sep 17 00:00:00 2001 From: matlin Date: Fri, 10 Jul 2026 16:17:05 +0200 Subject: [PATCH 13/24] Brace style fix bis --- src/desert_classes/security/sec_utils.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/desert_classes/security/sec_utils.cpp b/src/desert_classes/security/sec_utils.cpp index b174413..104a232 100644 --- a/src/desert_classes/security/sec_utils.cpp +++ b/src/desert_classes/security/sec_utils.cpp @@ -8,7 +8,8 @@ static int hex_value(char c) return -1; } -bool decode_hex(const std::string& hex, std::vector& out) { +bool decode_hex(const std::string& hex, std::vector& out) +{ if (hex.size() % 2 != 0) { return false; From a8c0c147b0814af1ed81781dcc144126bf426c0f Mon Sep 17 00:00:00 2001 From: matlin Date: Mon, 13 Jul 2026 10:52:05 +0200 Subject: [PATCH 14/24] Prevent cherry-pick and patch error for backport --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0651bba..b692230 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,12 +9,12 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 20) endif() +set(CMAKE_POLICY_VERSION_MINIMUM 3.5) + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -pthread) endif() -set(CMAKE_POLICY_VERSION_MINIMUM 3.5) - find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros_core REQUIRED) From 0d7ffc74974baa674c65adc453fefd03c20da00c Mon Sep 17 00:00:00 2001 From: Dmytro Ochkas Date: Mon, 13 Jul 2026 17:22:43 +0200 Subject: [PATCH 15/24] Adding context initialization with key and iv derivations - Adapting encryption & decryption accordingly --- src/desert_classes/security/AeadParams.cpp | 5 +- src/desert_classes/security/SecurityLayer.cpp | 268 +++++++++++++++--- src/desert_classes/security/SecurityLayer.h | 47 ++- .../{AeadParams.h => SecurityParams.h} | 4 + src/desert_classes/security/sec_utils.h | 2 + 5 files changed, 272 insertions(+), 54 deletions(-) rename src/desert_classes/security/{AeadParams.h => SecurityParams.h} (94%) diff --git a/src/desert_classes/security/AeadParams.cpp b/src/desert_classes/security/AeadParams.cpp index 1fe14d5..d9e1158 100644 --- a/src/desert_classes/security/AeadParams.cpp +++ b/src/desert_classes/security/AeadParams.cpp @@ -1,7 +1,6 @@ -#include +#include "SecurityParams.h" -#include "AeadParams.h" -#include "AeadParams.h" +#include namespace security { diff --git a/src/desert_classes/security/SecurityLayer.cpp b/src/desert_classes/security/SecurityLayer.cpp index 692f3a0..7744398 100644 --- a/src/desert_classes/security/SecurityLayer.cpp +++ b/src/desert_classes/security/SecurityLayer.cpp @@ -1,12 +1,19 @@ #include +#include #include "SecurityLayer.h" + +#include + #include "sec_utils.h" +#define KDF_INFO_TYPE_KEY "Key" +#define KDF_INFO_TYPE_IV "IV" + namespace security { -cose_algo_t aead_algo_to_cose_algo(AeadAlgorithms algo) +static cose_algo_t aead_algo_to_cose_algo(AeadAlgorithms algo) { switch (algo) { @@ -16,79 +23,256 @@ cose_algo_t aead_algo_to_cose_algo(AeadAlgorithms algo) case A128GCM: return COSE_ALGO_A128GCM; } - - return COSE_ALGO_A128GCM; + + return COSE_ALGO_NONE; +} + +static cose_algo_t kdf_algo_to_cose_algo(KdfAlgorithms algo) { + switch (algo) { + // TODO: Change + case HKDF_ASCON: + return COSE_ALGO_HMAC256; + } + + return COSE_ALGO_NONE; +} + +static SecurityResult cbor_err_to_sec_err(int err) { + switch (err) { + case 0: + return OK; + case NANOCBOR_ERR_END: + return BUFFER_ERROR; + } + + return INTERNAL_ERROR; } SecurityLayer::SecurityLayer() - : aead_params_(AeadParams(A128GCM)) + : aead_params_(AeadParams(A128GCM)), + kdf_(HKDF_ASCON), + piv_size_(2), + piv_bytes_(piv_size_), + sender_seq_number_(0), + sender_key_bytes_(aead_params_.get_key_size()), + receiver_key_bytes_(aead_params_.get_key_size()) { - if (get_key() != OK) + std::vector master_key{}; + if (get_master_key_env(master_key) != OK) { throw std::runtime_error("Failed to parse the encryption key"); } + + std::vector master_salt{}; + if (get_master_salt_env(master_salt) != OK) + { + master_salt = std::vector(DEFAULT_MASTER_SALT_LEN, 0); + } + + auto res = derive_context(master_key, master_salt); + if (res != OK) { + throw std::runtime_error("Security context initialization error"); + } +} + +SecurityResult SecurityLayer::build_kdf_info(const std::vector& id, cose_algo_t alg, const std::string& type, size_t len, uint8_t** out, size_t* out_len) +{ + nanocbor_encoder_t info; + int st = COSE_OK; + nanocbor_encoder_init(&info, internal_buf_, sizeof(internal_buf_)); + st = nanocbor_fmt_array(&info, 4); + if (st < 0) { + return cbor_err_to_sec_err(st); + } + + st = nanocbor_put_bstr(&info, id.data(), id.size()); + if (st < 0) { + return cbor_err_to_sec_err(st); + } + + st = nanocbor_fmt_int(&info, alg); + if (st < 0) { + return cbor_err_to_sec_err(st); + } + + st = nanocbor_put_tstrn(&info, type.data(), type.size()); + if (st < 0) { + return cbor_err_to_sec_err(st); + } + + st = nanocbor_fmt_uint(&info, len); + if (st < 0) { + return cbor_err_to_sec_err(st); + } + + *out = internal_buf_; + *out_len = nanocbor_encoded_len(&info); + + return OK; } -SecurityResult SecurityLayer::generate_nonce(uint8_t* nonce_buf) +SecurityResult SecurityLayer::derive_key(const std::vector& ikm, const std::vector& master_salt, const std::vector& id, cose_algo_t alg, std::vector& key_out, cose_key_t* cose_k_out) { - // TODO: Implement - memset(nonce_buf, 0, aead_params_.get_nonce_size()); + uint8_t* info_ptr; + size_t info_len; + auto info_res = build_kdf_info(id, + alg, + KDF_INFO_TYPE_KEY, + aead_params_.get_key_size(), + &info_ptr, + &info_len); + if (info_res != OK) { + return info_res; + } + + auto kdf_res = cose_crypto_hkdf_derive(master_salt.data(), + master_salt.size(), + ikm.data(), + aead_params_.get_key_size(), + info_ptr, + info_len, + key_out.data(), + aead_params_.get_key_size(), + kdf_algo_to_cose_algo(kdf_)); + if (kdf_res != COSE_OK) { + return CONTEXT_DERIVE_ERROR; + } + + // static uint8_t kid[] = "rmw_desert_sender_key"; + cose_key_init(cose_k_out); + // cose_key_set_kid(&cose_key_, kid, sizeof(kid) - 1); + cose_key_set_keys(cose_k_out, COSE_EC_NONE, alg, nullptr, nullptr, key_out.data()); + return OK; } -SecurityResult SecurityLayer::get_key() +SecurityResult SecurityLayer::derive_iv(const std::vector& ikm, const std::vector& master_salt, cose_algo_t alg, std::vector& iv_out) { - if (key_bytes_.empty()) + uint8_t* info_ptr; + size_t info_len; + auto info_res = build_kdf_info({}, + alg, + KDF_INFO_TYPE_IV, + aead_params_.get_nonce_size(), + &info_ptr, + &info_len); + if (info_res != OK) { + return info_res; + } + + auto kdf_res = cose_crypto_hkdf_derive(master_salt.data(), + master_salt.size(), + ikm.data(), + aead_params_.get_key_size(), + info_ptr, + info_len, + iv_out.data(), + aead_params_.get_nonce_size(), + kdf_algo_to_cose_algo(kdf_)); + if (kdf_res != COSE_OK) { + return CONTEXT_DERIVE_ERROR; + } + + return OK; +} + +SecurityResult SecurityLayer::get_master_key_env(std::vector& master_key) const +{ + const char* env_value = std::getenv("MASTER_SECRET_KEY"); + if (env_value == nullptr || !decode_hex(env_value, master_key)) { - const char* env_value = std::getenv("MASTER_SECRET_KEY"); - if (env_value == nullptr || !decode_hex(env_value, key_bytes_)) - { - printf("CRITICAL: the master secret key is invalid\n"); - printf("Please fill the MASTER_SECRET_KEY environment variable with stream of hexadecimal bytes\n"); - return KEY_ENV_ERROR; - } - if (key_bytes_.size() < aead_params_.get_key_size()) - { - printf("CRITICAL: the master secret key size does not match the required suite\n"); - printf("Please set a MASTER_SECRET_KEY of %d bytes\n", aead_params_.get_key_size()); - return KEY_SIZE_ERROR; - } + printf("CRITICAL: Master Secret Key is invalid\n"); + printf("Please fill the MASTER_SECRET_KEY environment variable with stream of hexadecimal bytes\n"); + return KEY_ENV_ERROR; + } + if (master_key.size() < aead_params_.get_key_size()) + { + printf("CRITICAL: the master secret key size does not match the required suite\n"); + printf("Please set a MASTER_SECRET_KEY of %d bytes\n", aead_params_.get_key_size()); + return KEY_SIZE_ERROR; } return OK; } -SecurityResult SecurityLayer::wrap(uint8_t* data, size_t data_len, size_t data_max_len, uint8_t** cose_ptr, size_t* cose_len) +SecurityResult SecurityLayer::get_master_salt_env(std::vector& master_salt) const { + const char* env_value = std::getenv("MASTER_SALT"); + if (env_value == nullptr || !decode_hex(env_value, master_salt)) + { + printf("CRITICAL: Master Salt is invalid\n"); + printf("Please fill the MASTER_SALT environment variable with stream of hexadecimal bytes\n"); + return NONCE_ENV_ERROR; + } + + return OK; +} + +/* Generating Partial Initialization Vector instead of full nonce */ +SecurityResult SecurityLayer::generate_nonce() +{ + uint32_t seq = htonl(sender_seq_number_); + memcpy(piv_bytes_.data(), &seq, MIN(sizeof(seq), piv_size_)); + return OK; +} + +SecurityResult SecurityLayer::derive_context(const std::vector& master_key, const std::vector& master_salt) +{ + // Master Secret and Master Salt have to be set already cose_algo_t algo = aead_algo_to_cose_algo(aead_params_.get_alg()); - std::vector nonce_bytes(aead_params_.get_nonce_size()); - auto nonce_st = generate_nonce(nonce_bytes.data()); + if (algo == COSE_ALGO_NONE) { + return PARAM_ERROR; + } + + // Derive Sender Key + auto res = derive_key(master_key, master_salt, sender_id_, algo, sender_key_bytes_, &sender_cose_key_); + if (res != OK) { + return res; + } + // Derive Receiver Key + res = derive_key(master_key, master_salt, receiver_id_, algo, receiver_key_bytes_, &receiver_cose_key_); + if (res != OK) { + return res; + } + + // Derive Context IV + res = derive_iv(master_key, master_salt, algo, context_iv_); + return res; +} + +SecurityResult SecurityLayer::wrap(uint8_t* data, size_t data_len, size_t data_max_len, uint8_t** cose_ptr, size_t* cose_len) +{ + auto nonce_st = generate_nonce(); if (nonce_st != OK) { return nonce_st; } - static uint8_t kid[] = "rmw_desert_key"; - // uint8_t payload[data_len]; std::vector payload(data, data + data_len); - // memcpy(payload, data, data_len); - - cose_key_t key; - cose_key_init(&key); - cose_key_set_kid(&key, kid, sizeof(kid) - 1); - cose_key_set_keys(&key, COSE_EC_NONE, algo, nullptr, nullptr, const_cast(key_bytes_.data())); cose_encrypt_t crypt; cose_encrypt_init(&crypt, COSE_FLAGS_ENCRYPT0 | COSE_FLAGS_UNTAGGED); - cose_encrypt_add_recipient(&crypt, &key); + + cose_hdr_t piv = { + .next = nullptr, + .key = COSE_HDR_PARTIALIV, + .len = piv_size_, + .v = { + .data = piv_bytes_.data() + }, + .type = COSE_HDR_TYPE_BSTR + }; + cose_hdr_insert(&crypt.hdrs.unprot, &piv); + + cose_encrypt_add_recipient(&crypt, &sender_cose_key_); cose_encrypt_set_payload(&crypt, payload.data(), payload.size()); cose_encrypt_set_algo(&crypt, COSE_ALGO_DIRECT); - COSE_ssize_t len = cose_encrypt_encode(&crypt, data, data_max_len, nonce_bytes.data(), cose_ptr); + COSE_ssize_t len = cose_encrypt_encode(&crypt, data, data_max_len, context_iv_.data(), cose_ptr); if (len <= 0) { return WRAP_ERROR; } *cose_len = len; - + sender_seq_number_++; return OK; } @@ -100,16 +284,8 @@ SecurityResult SecurityLayer::unwrap(uint8_t* data, size_t data_len, size_t* new return UNWRAP_ERROR; } - cose_algo_t algo = aead_algo_to_cose_algo(aead_params_.get_alg()); - static uint8_t kid[] = "rmw_desert_key"; - - cose_key_t key; - cose_key_init(&key); - cose_key_set_kid(&key, kid, sizeof(kid) - 1); - cose_key_set_keys(&key, COSE_EC_NONE, algo, nullptr, nullptr, const_cast(key_bytes_.data())); - std::vector plaintext(data_len); - if (cose_encrypt_decrypt(&decrypt, nullptr, &key, internal_buf_, sizeof(internal_buf_), plaintext.data(), new_data_len)) + if (cose_encrypt_decrypt(&decrypt, nullptr, &receiver_cose_key_, internal_buf_, sizeof(internal_buf_), plaintext.data(), new_data_len)) { return UNWRAP_ERROR; } diff --git a/src/desert_classes/security/SecurityLayer.h b/src/desert_classes/security/SecurityLayer.h index fc5d82c..837619c 100644 --- a/src/desert_classes/security/SecurityLayer.h +++ b/src/desert_classes/security/SecurityLayer.h @@ -5,8 +5,11 @@ #include #include -#include "AeadParams.h" +#include +#include "SecurityParams.h" + +#define DEFAULT_MASTER_SALT_LEN 16 #define COSE_INTERNAL_BUF_SIZE 512 namespace security @@ -14,7 +17,17 @@ namespace security enum SecurityResult { - OK, KEY_SIZE_ERROR, KEY_ENV_ERROR, WRAP_ERROR, UNWRAP_ERROR + OK = 0, + PARAM_ERROR = -1, + KEY_SIZE_ERROR = -2, + KEY_ENV_ERROR = -3, + NONCE_ENV_ERROR = -4, + NONCE_SIZE_ERROR = -5, + BUFFER_ERROR = -6, + CONTEXT_DERIVE_ERROR = -7, + WRAP_ERROR = -100, + UNWRAP_ERROR = -200, + INTERNAL_ERROR = -999 }; class SecurityLayer @@ -26,11 +39,35 @@ class SecurityLayer SecurityResult unwrap(uint8_t* data, size_t data_len, size_t* new_data_len); private: - SecurityResult get_key(); - SecurityResult generate_nonce(uint8_t* nonce_buf); + SecurityResult get_master_key_env(std::vector& master_key) const; + SecurityResult get_master_salt_env(std::vector& master_salt) const; + SecurityResult generate_nonce(); + SecurityResult build_kdf_info(const std::vector& id, cose_algo_t alg, const std::string& type, size_t len, uint8_t** out, size_t* out_len); + SecurityResult derive_key(const std::vector& ikm, const std::vector& salt, const std::vector& id, cose_algo_t algo, std::vector& key_out, cose_key_t* cose_k_out); + SecurityResult derive_iv(const std::vector& ikm, const std::vector& salt, cose_algo_t alg, std::vector& iv_out); + SecurityResult SecurityLayer::derive_context(const std::vector& master_key, const std::vector& master_salt); + /* Algorithms to use */ AeadParams aead_params_; - std::vector key_bytes_{}; + KdfAlgorithms kdf_; + /* ***************** */ + + /* Nonce-related fields */ + std::vector context_iv_{}; + size_t piv_size_; + std::vector piv_bytes_{}; + size_t sender_seq_number_; + /* ******************** */ + + /* Key-related fields */ + std::vector sender_id_{}; + std::vector sender_key_bytes_{}; + cose_key_t sender_cose_key_{}; + std::vector receiver_id_{}; + std::vector receiver_key_bytes_{}; + cose_key_t receiver_cose_key_{}; + /* ****************** */ + uint8_t internal_buf_[COSE_INTERNAL_BUF_SIZE]{}; }; diff --git a/src/desert_classes/security/AeadParams.h b/src/desert_classes/security/SecurityParams.h similarity index 94% rename from src/desert_classes/security/AeadParams.h rename to src/desert_classes/security/SecurityParams.h index 7b8b299..48ba424 100644 --- a/src/desert_classes/security/AeadParams.h +++ b/src/desert_classes/security/SecurityParams.h @@ -14,6 +14,10 @@ enum AeadAlgorithms A128GCM }; +enum KdfAlgorithms { + HKDF_ASCON +}; + class AeadParams { public: diff --git a/src/desert_classes/security/sec_utils.h b/src/desert_classes/security/sec_utils.h index 4dd228a..f52cf2a 100644 --- a/src/desert_classes/security/sec_utils.h +++ b/src/desert_classes/security/sec_utils.h @@ -5,6 +5,8 @@ #include #include +#define MIN(a, b) ((a) > (b) ? (b) : (a)) + bool decode_hex(const std::string& hex, std::vector& out); #endif From d7ee409d95f3af3b3c36272d917a21d226dd3045 Mon Sep 17 00:00:00 2001 From: Dmytro Ochkas Date: Mon, 13 Jul 2026 19:51:38 +0200 Subject: [PATCH 16/24] Register configurations from config.cmake --- CMakeLists.txt | 117 +++++++++--------- config.cmake | 4 + src/desert_classes/security/SecurityLayer.cpp | 40 +++++- src/desert_classes/security/SecurityParams.h | 12 +- src/desert_classes/security/sec_utils.cpp | 13 +- 5 files changed, 120 insertions(+), 66 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b692230..5968c11 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,17 +15,17 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -pthread) endif() -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_ros_core REQUIRED) - -find_package(rmw REQUIRED) -find_package(rmw_dds_common REQUIRED) -find_package(rosidl_runtime_c REQUIRED) -find_package(rosidl_typesupport_introspection_c REQUIRED) -find_package(rosidl_typesupport_introspection_cpp REQUIRED) - -find_package(rcutils REQUIRED) -find_package(rcpputils REQUIRED) +#find_package(ament_cmake REQUIRED) +#find_package(ament_cmake_ros_core REQUIRED) +# +#find_package(rmw REQUIRED) +#find_package(rmw_dds_common REQUIRED) +#find_package(rosidl_runtime_c REQUIRED) +#find_package(rosidl_typesupport_introspection_c REQUIRED) +#find_package(rosidl_typesupport_introspection_cpp REQUIRED) +# +#find_package(rcutils REQUIRED) +#find_package(rcpputils REQUIRED) add_library(rmw_desert src/get_node_info_and_types.cpp @@ -125,50 +125,55 @@ if (SECURE_MODE_ENABLED) ) target_include_directories(rmw_desert PRIVATE ${COSE_INC_DIR}) target_compile_definitions(rmw_desert PRIVATE SECURE_MODE_ENABLED) + target_compile_definitions(rmw_desert PRIVATE + AEAD_ALGO="${AEAD_ALGO}" + KDF_ALGO="${KDF_ALGO}" + SENDER_ID="${SENDER_ID}" + RECEIVER_ID="${RECEIVER_ID}") endif () -INCLUDE (FindPkgConfig) - -# specific order: dependents before dependencies -target_link_libraries(rmw_desert PUBLIC - rmw::rmw - rcutils::rcutils - rcpputils::rcpputils - rmw_dds_common::rmw_dds_common_library - rosidl_typesupport_introspection_c::rosidl_typesupport_introspection_c - rosidl_typesupport_introspection_cpp::rosidl_typesupport_introspection_cpp - rosidl_runtime_c::rosidl_runtime_c -) - -configure_rmw_library(rmw_desert) - -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} -PRIVATE "rmw_desert_BUILDING_LIBRARY") - -# specific order: dependents before dependencies -ament_export_libraries(rmw_desert) - -ament_export_dependencies( - rcutils - rcpputils - rmw - rosidl_runtime_c - rmw_dds_common - rosidl_typesupport_introspection_c - rosidl_typesupport_introspection_cpp -) - -register_rmw_implementation( - "c:rosidl_typesupport_c:rosidl_typesupport_introspection_c" - "cpp:rosidl_typesupport_cpp:rosidl_typesupport_introspection_cpp") - -ament_package() - -install( - TARGETS rmw_desert - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) +#INCLUDE (FindPkgConfig) +# +## specific order: dependents before dependencies +#target_link_libraries(rmw_desert PUBLIC +# rmw::rmw +# rcutils::rcutils +# rcpputils::rcpputils +# rmw_dds_common::rmw_dds_common_library +# rosidl_typesupport_introspection_c::rosidl_typesupport_introspection_c +# rosidl_typesupport_introspection_cpp::rosidl_typesupport_introspection_cpp +# rosidl_runtime_c::rosidl_runtime_c +#) +# +#configure_rmw_library(rmw_desert) +# +## Causes the visibility macros to use dllexport rather than dllimport, +## which is appropriate when building the dll but not consuming it. +#target_compile_definitions(${PROJECT_NAME} +#PRIVATE "rmw_desert_BUILDING_LIBRARY") +# +## specific order: dependents before dependencies +#ament_export_libraries(rmw_desert) +# +#ament_export_dependencies( +# rcutils +# rcpputils +# rmw +# rosidl_runtime_c +# rmw_dds_common +# rosidl_typesupport_introspection_c +# rosidl_typesupport_introspection_cpp +#) +# +#register_rmw_implementation( +# "c:rosidl_typesupport_c:rosidl_typesupport_introspection_c" +# "cpp:rosidl_typesupport_cpp:rosidl_typesupport_introspection_cpp") +# +#ament_package() +# +#install( +# TARGETS rmw_desert +# ARCHIVE DESTINATION lib +# LIBRARY DESTINATION lib +# RUNTIME DESTINATION bin +#) diff --git a/config.cmake b/config.cmake index d7e5172..6f2465e 100644 --- a/config.cmake +++ b/config.cmake @@ -1,3 +1,7 @@ set(CBOR_LIB "NanoCBOR" CACHE STRING "Cbor library.") # libmcu_cbor, NanoCBOR option(SECURE_MODE_ENABLED "" ON) set(CRYPTO_LIB "mbedtls" CACHE STRING "Crypto library.") +set(AEAD_ALGO "Ascon-AEAD128" CACHE STRING "Algorithm for authenticated encryption with additional data.") +set(KDF_ALGO "HKDF-Ascon256" CACHE STRING "Algorithm for key derivation.") +set(SENDER_ID "0x01" CACHE STRING "Sender ID in hex.") +set(RECEIVER_ID "0x02" CACHE STRING "Receiver ID in hex.") diff --git a/src/desert_classes/security/SecurityLayer.cpp b/src/desert_classes/security/SecurityLayer.cpp index 7744398..c5d3829 100644 --- a/src/desert_classes/security/SecurityLayer.cpp +++ b/src/desert_classes/security/SecurityLayer.cpp @@ -13,6 +13,28 @@ namespace security { +static AeadAlgorithms str_to_aead_algo(const std::string& str) { + if (str == AEAD_ALGO_ASCON_128) { + return ASCON_AEAD128; + } + if (str == AEAD_ALGO_ASCON_128_64) { + return ASCON_AEAD128_64; + } + if (str == AEAD_ALGO_ASCON_128_32) { + return ASCON_AEAD128_32; + } + + return AEAD_UNKNOWN; +} + +static KdfAlgorithms str_to_kdf_algo(const std::string& str) { + if (str == KDF_ALGO_ASCON_256) { + return HKDF_ASCON; + } + + return KDF_UNKNOWN; +} + static cose_algo_t aead_algo_to_cose_algo(AeadAlgorithms algo) { switch (algo) @@ -49,14 +71,20 @@ static SecurityResult cbor_err_to_sec_err(int err) { } SecurityLayer::SecurityLayer() - : aead_params_(AeadParams(A128GCM)), - kdf_(HKDF_ASCON), + : aead_params_(AeadParams(str_to_aead_algo(AEAD_ALGO))), + kdf_(str_to_kdf_algo(KDF_ALGO)), piv_size_(2), piv_bytes_(piv_size_), - sender_seq_number_(0), - sender_key_bytes_(aead_params_.get_key_size()), - receiver_key_bytes_(aead_params_.get_key_size()) + sender_seq_number_(0) { + if (!decode_hex(SENDER_ID, sender_id_)) { + throw std::runtime_error("Failed to parse the sender id"); + } + + if (!decode_hex(RECEIVER_ID, receiver_id_)) { + throw std::runtime_error("Failed to parse the receiver id"); + } + std::vector master_key{}; if (get_master_key_env(master_key) != OK) { @@ -125,6 +153,7 @@ SecurityResult SecurityLayer::derive_key(const std::vector& ikm, const return info_res; } + key_out.resize(aead_params_.get_key_size()); auto kdf_res = cose_crypto_hkdf_derive(master_salt.data(), master_salt.size(), ikm.data(), @@ -160,6 +189,7 @@ SecurityResult SecurityLayer::derive_iv(const std::vector& ikm, const s return info_res; } + iv_out.resize(aead_params_.get_nonce_size()); auto kdf_res = cose_crypto_hkdf_derive(master_salt.data(), master_salt.size(), ikm.data(), diff --git a/src/desert_classes/security/SecurityParams.h b/src/desert_classes/security/SecurityParams.h index 48ba424..37240c9 100644 --- a/src/desert_classes/security/SecurityParams.h +++ b/src/desert_classes/security/SecurityParams.h @@ -3,6 +3,12 @@ #include +#define AEAD_ALGO_ASCON_128 "Ascon-AEAD128" +#define AEAD_ALGO_ASCON_128_64 "Ascon-AEAD128-64" +#define AEAD_ALGO_ASCON_128_32 "Ascon-AEAD128-32" + +#define KDF_ALGO_ASCON_256 "HKDF-Ascon256" + namespace security { @@ -11,11 +17,13 @@ enum AeadAlgorithms ASCON_AEAD128, ASCON_AEAD128_64, ASCON_AEAD128_32, - A128GCM + A128GCM, + AEAD_UNKNOWN }; enum KdfAlgorithms { - HKDF_ASCON + HKDF_ASCON, + KDF_UNKNOWN }; class AeadParams diff --git a/src/desert_classes/security/sec_utils.cpp b/src/desert_classes/security/sec_utils.cpp index 104a232..2ed8c87 100644 --- a/src/desert_classes/security/sec_utils.cpp +++ b/src/desert_classes/security/sec_utils.cpp @@ -10,15 +10,22 @@ static int hex_value(char c) bool decode_hex(const std::string& hex, std::vector& out) { - if (hex.size() % 2 != 0) + size_t start = 0; + if (hex.size() >= 2 && hex[0] == '0' && (hex[1] == 'x' || hex[1] == 'X')) + { + start = 2; + } + + size_t hex_length = hex.size() - start; + if (hex_length % 2 != 0) { return false; } out.clear(); - out.reserve(hex.size() / 2); + out.reserve(hex_length / 2); - for (size_t i = 0; i < hex.size(); i += 2) + for (size_t i = start; i < hex.size(); i += 2) { const int hi = hex_value(hex[i]); const int lo = hex_value(hex[i + 1]); From 630553bdf2d760e0d16ebe8dbc4f098d00fa720a Mon Sep 17 00:00:00 2001 From: Dmytro Ochkas Date: Tue, 14 Jul 2026 11:20:04 +0200 Subject: [PATCH 17/24] Integrate Ascon HKDF and nonce management --- CMakeLists.txt | 112 +++++++++--------- src/desert_classes/security/AeadParams.cpp | 2 +- src/desert_classes/security/SecurityLayer.cpp | 36 ++++-- src/desert_classes/security/SecurityLayer.h | 6 +- src/desert_classes/security/SecurityParams.h | 1 + 5 files changed, 92 insertions(+), 65 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5968c11..7dbda72 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,17 +15,17 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -pthread) endif() -#find_package(ament_cmake REQUIRED) -#find_package(ament_cmake_ros_core REQUIRED) -# -#find_package(rmw REQUIRED) -#find_package(rmw_dds_common REQUIRED) -#find_package(rosidl_runtime_c REQUIRED) -#find_package(rosidl_typesupport_introspection_c REQUIRED) -#find_package(rosidl_typesupport_introspection_cpp REQUIRED) -# -#find_package(rcutils REQUIRED) -#find_package(rcpputils REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros_core REQUIRED) + +find_package(rmw REQUIRED) +find_package(rmw_dds_common REQUIRED) +find_package(rosidl_runtime_c REQUIRED) +find_package(rosidl_typesupport_introspection_c REQUIRED) +find_package(rosidl_typesupport_introspection_cpp REQUIRED) + +find_package(rcutils REQUIRED) +find_package(rcpputils REQUIRED) add_library(rmw_desert src/get_node_info_and_types.cpp @@ -132,48 +132,48 @@ if (SECURE_MODE_ENABLED) RECEIVER_ID="${RECEIVER_ID}") endif () -#INCLUDE (FindPkgConfig) -# -## specific order: dependents before dependencies -#target_link_libraries(rmw_desert PUBLIC -# rmw::rmw -# rcutils::rcutils -# rcpputils::rcpputils -# rmw_dds_common::rmw_dds_common_library -# rosidl_typesupport_introspection_c::rosidl_typesupport_introspection_c -# rosidl_typesupport_introspection_cpp::rosidl_typesupport_introspection_cpp -# rosidl_runtime_c::rosidl_runtime_c -#) -# -#configure_rmw_library(rmw_desert) -# -## Causes the visibility macros to use dllexport rather than dllimport, -## which is appropriate when building the dll but not consuming it. -#target_compile_definitions(${PROJECT_NAME} -#PRIVATE "rmw_desert_BUILDING_LIBRARY") -# -## specific order: dependents before dependencies -#ament_export_libraries(rmw_desert) -# -#ament_export_dependencies( -# rcutils -# rcpputils -# rmw -# rosidl_runtime_c -# rmw_dds_common -# rosidl_typesupport_introspection_c -# rosidl_typesupport_introspection_cpp -#) -# -#register_rmw_implementation( -# "c:rosidl_typesupport_c:rosidl_typesupport_introspection_c" -# "cpp:rosidl_typesupport_cpp:rosidl_typesupport_introspection_cpp") -# -#ament_package() -# -#install( -# TARGETS rmw_desert -# ARCHIVE DESTINATION lib -# LIBRARY DESTINATION lib -# RUNTIME DESTINATION bin -#) +INCLUDE (FindPkgConfig) + +# specific order: dependents before dependencies +target_link_libraries(rmw_desert PUBLIC + rmw::rmw + rcutils::rcutils + rcpputils::rcpputils + rmw_dds_common::rmw_dds_common_library + rosidl_typesupport_introspection_c::rosidl_typesupport_introspection_c + rosidl_typesupport_introspection_cpp::rosidl_typesupport_introspection_cpp + rosidl_runtime_c::rosidl_runtime_c +) + +configure_rmw_library(rmw_desert) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} +PRIVATE "rmw_desert_BUILDING_LIBRARY") + +# specific order: dependents before dependencies +ament_export_libraries(rmw_desert) + +ament_export_dependencies( + rcutils + rcpputils + rmw + rosidl_runtime_c + rmw_dds_common + rosidl_typesupport_introspection_c + rosidl_typesupport_introspection_cpp +) + +register_rmw_implementation( + "c:rosidl_typesupport_c:rosidl_typesupport_introspection_c" + "cpp:rosidl_typesupport_cpp:rosidl_typesupport_introspection_cpp") + +ament_package() + +install( + TARGETS rmw_desert + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) diff --git a/src/desert_classes/security/AeadParams.cpp b/src/desert_classes/security/AeadParams.cpp index d9e1158..c6bf223 100644 --- a/src/desert_classes/security/AeadParams.cpp +++ b/src/desert_classes/security/AeadParams.cpp @@ -1,6 +1,6 @@ #include "SecurityParams.h" -#include +#include namespace security { diff --git a/src/desert_classes/security/SecurityLayer.cpp b/src/desert_classes/security/SecurityLayer.cpp index c5d3829..54f1a18 100644 --- a/src/desert_classes/security/SecurityLayer.cpp +++ b/src/desert_classes/security/SecurityLayer.cpp @@ -40,23 +40,27 @@ static cose_algo_t aead_algo_to_cose_algo(AeadAlgorithms algo) switch (algo) { case ASCON_AEAD128: + return COSE_ALGO_ASCON_AEAD128; case ASCON_AEAD128_64: + return COSE_ALGO_ASCON_AEAD128_64; case ASCON_AEAD128_32: + return COSE_ALGO_ASCON_AEAD128_32; case A128GCM: return COSE_ALGO_A128GCM; + default: + return COSE_ALGO_NONE; } - - return COSE_ALGO_NONE; } static cose_algo_t kdf_algo_to_cose_algo(KdfAlgorithms algo) { switch (algo) { - // TODO: Change case HKDF_ASCON: + return COSE_ALGO_HMAC_ASCON_HASH256; + case HKDF_HMAC256: return COSE_ALGO_HMAC256; + default: + return COSE_ALGO_NONE; } - - return COSE_ALGO_NONE; } static SecurityResult cbor_err_to_sec_err(int err) { @@ -103,6 +107,23 @@ SecurityLayer::SecurityLayer() } } +SecurityLayer::SecurityLayer(const AeadParams &aead_params, KdfAlgorithms kdf, const std::vector &master_key, + const std::vector &master_salt, size_t piv_size, size_t sender_seq_number, const std::vector &sender_id, + const std::vector &receiver_id) + : aead_params_(aead_params), + kdf_(kdf), + piv_size_(piv_size), + piv_bytes_(piv_size), + sender_seq_number_(sender_seq_number), + sender_id_(sender_id), + receiver_id_(receiver_id) +{ + auto res = derive_context(master_key, master_salt); + if (res != OK) { + throw std::runtime_error("Security context initialization error"); + } +} + SecurityResult SecurityLayer::build_kdf_info(const std::vector& id, cose_algo_t alg, const std::string& type, size_t len, uint8_t** out, size_t* out_len) { nanocbor_encoder_t info; @@ -242,7 +263,8 @@ SecurityResult SecurityLayer::get_master_salt_env(std::vector& master_s SecurityResult SecurityLayer::generate_nonce() { uint32_t seq = htonl(sender_seq_number_); - memcpy(piv_bytes_.data(), &seq, MIN(sizeof(seq), piv_size_)); + uint32_t offset = MIN(sizeof(seq), piv_size_); + memcpy(piv_bytes_.data(), reinterpret_cast(&seq) + (sizeof(seq) - offset), offset); return OK; } @@ -315,7 +337,7 @@ SecurityResult SecurityLayer::unwrap(uint8_t* data, size_t data_len, size_t* new } std::vector plaintext(data_len); - if (cose_encrypt_decrypt(&decrypt, nullptr, &receiver_cose_key_, internal_buf_, sizeof(internal_buf_), plaintext.data(), new_data_len)) + if (cose_encrypt_decrypt_lw(&decrypt, nullptr, &receiver_cose_key_, internal_buf_, sizeof(internal_buf_), plaintext.data(), new_data_len, context_iv_.data())) { return UNWRAP_ERROR; } diff --git a/src/desert_classes/security/SecurityLayer.h b/src/desert_classes/security/SecurityLayer.h index 837619c..a649100 100644 --- a/src/desert_classes/security/SecurityLayer.h +++ b/src/desert_classes/security/SecurityLayer.h @@ -6,6 +6,7 @@ #include #include +#include #include "SecurityParams.h" @@ -34,6 +35,9 @@ class SecurityLayer { public: SecurityLayer(); + SecurityLayer(const AeadParams &aead_params, KdfAlgorithms kdf, const std::vector &master_key, + const std::vector &master_salt, size_t piv_size, size_t sender_seq_number, + const std::vector &sender_id, const std::vector &receiver_id); SecurityResult wrap(uint8_t* data, size_t data_len, size_t data_max_len, uint8_t** cose_ptr, size_t* cose_len); SecurityResult unwrap(uint8_t* data, size_t data_len, size_t* new_data_len); @@ -45,7 +49,7 @@ class SecurityLayer SecurityResult build_kdf_info(const std::vector& id, cose_algo_t alg, const std::string& type, size_t len, uint8_t** out, size_t* out_len); SecurityResult derive_key(const std::vector& ikm, const std::vector& salt, const std::vector& id, cose_algo_t algo, std::vector& key_out, cose_key_t* cose_k_out); SecurityResult derive_iv(const std::vector& ikm, const std::vector& salt, cose_algo_t alg, std::vector& iv_out); - SecurityResult SecurityLayer::derive_context(const std::vector& master_key, const std::vector& master_salt); + SecurityResult derive_context(const std::vector& master_key, const std::vector& master_salt); /* Algorithms to use */ AeadParams aead_params_; diff --git a/src/desert_classes/security/SecurityParams.h b/src/desert_classes/security/SecurityParams.h index 37240c9..9782b69 100644 --- a/src/desert_classes/security/SecurityParams.h +++ b/src/desert_classes/security/SecurityParams.h @@ -23,6 +23,7 @@ enum AeadAlgorithms enum KdfAlgorithms { HKDF_ASCON, + HKDF_HMAC256, KDF_UNKNOWN }; From 5c558293d8bc00a7936975dc53464d17567de182 Mon Sep 17 00:00:00 2001 From: Dmytro Ochkas Date: Tue, 14 Jul 2026 11:23:57 +0200 Subject: [PATCH 18/24] Remove original libcose --- .gitmodules | 3 --- thirdparty/libcose | 1 - 2 files changed, 4 deletions(-) delete mode 160000 thirdparty/libcose diff --git a/.gitmodules b/.gitmodules index 81bfd5b..162e833 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,9 +1,6 @@ [submodule "thirdparty/NanoCBOR"] path = thirdparty/NanoCBOR url = https://github.com/bergzand/NanoCBOR -[submodule "thirdparty/libcose"] - path = thirdparty/libcose - url = https://github.com/bergzand/libcose.git [submodule "thirdparty/mbedtls"] path = thirdparty/mbedtls url = https://github.com/Mbed-TLS/mbedtls.git diff --git a/thirdparty/libcose b/thirdparty/libcose deleted file mode 160000 index ea1fed8..0000000 --- a/thirdparty/libcose +++ /dev/null @@ -1 +0,0 @@ -Subproject commit ea1fed87d6ca9b478f8bed323af97e6b192c0a6d From becc7d0fefeb4500a7668286c06ee3cfe5e1febe Mon Sep 17 00:00:00 2001 From: Dmytro Ochkas Date: Tue, 14 Jul 2026 11:44:20 +0200 Subject: [PATCH 19/24] Integrate ascon in Security Layer and add submodules --- .gitmodules | 6 +++ CMakeLists.txt | 128 +++++++++++++++++++++++++-------------------- config.cmake | 2 +- thirdparty/ascon-c | 1 + thirdparty/libcose | 1 + 5 files changed, 80 insertions(+), 58 deletions(-) create mode 160000 thirdparty/ascon-c create mode 160000 thirdparty/libcose diff --git a/.gitmodules b/.gitmodules index 162e833..55e342c 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,9 @@ [submodule "thirdparty/mbedtls"] path = thirdparty/mbedtls url = https://github.com/Mbed-TLS/mbedtls.git +[submodule "thirdparty/libcose"] + path = thirdparty/libcose + url = https://github.com/dmochkas/libcose +[submodule "thirdparty/ascon-c"] + path = thirdparty/ascon-c + url = https://github.com/dmochkas/ascon-c diff --git a/CMakeLists.txt b/CMakeLists.txt index 7dbda72..3d1735b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,17 +15,17 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -pthread) endif() -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_ros_core REQUIRED) - -find_package(rmw REQUIRED) -find_package(rmw_dds_common REQUIRED) -find_package(rosidl_runtime_c REQUIRED) -find_package(rosidl_typesupport_introspection_c REQUIRED) -find_package(rosidl_typesupport_introspection_cpp REQUIRED) - -find_package(rcutils REQUIRED) -find_package(rcpputils REQUIRED) +#find_package(ament_cmake REQUIRED) +#find_package(ament_cmake_ros_core REQUIRED) +# +#find_package(rmw REQUIRED) +#find_package(rmw_dds_common REQUIRED) +#find_package(rosidl_runtime_c REQUIRED) +#find_package(rosidl_typesupport_introspection_c REQUIRED) +#find_package(rosidl_typesupport_introspection_cpp REQUIRED) +# +#find_package(rcutils REQUIRED) +#find_package(rcpputils REQUIRED) add_library(rmw_desert src/get_node_info_and_types.cpp @@ -102,6 +102,7 @@ if (SECURE_MODE_ENABLED) ${COSE_ROOT}/src/cose_recipient.c ${COSE_ROOT}/src/cose_sign.c ${COSE_ROOT}/src/cose_signature.c + ${COSE_ROOT}/src/crypt/keygen_symm.c ) target_include_directories(${COSE_LIB} PRIVATE ${COSE_INC_DIR} ${CBOR_INC_DIR}) @@ -109,10 +110,23 @@ if (SECURE_MODE_ENABLED) add_subdirectory(thirdparty/mbedtls) target_sources(${COSE_LIB} PRIVATE ${COSE_ROOT}/src/crypt/mbedtls.c) - target_sources(${COSE_LIB} PRIVATE ${COSE_ROOT}/src/crypt/keygen_symm.c) target_link_libraries(${COSE_LIB} PRIVATE mbedtls) target_link_libraries(rmw_desert PRIVATE mbedtls) add_compile_definitions(CRYPTO_MBEDTLS) + elseif (${CRYPTO_LIB} STREQUAL "ascon-c") + set(ASCON_LIB_NAME "ascon-c") + set(ASCON_AEAD_ROOT thirdparty/ascon-c/crypto_aead/asconaead128/opt64) + set(ASCON_HASH_ROOT thirdparty/ascon-c/crypto_hash/asconhash256/opt64) + add_library(${ASCON_LIB_NAME} OBJECT + ${ASCON_AEAD_ROOT}/aead.c + ${ASCON_AEAD_ROOT}/permutations.c + ${ASCON_AEAD_ROOT}/printstate.c + ${ASCON_HASH_ROOT}/hash.c + ) + target_compile_definitions(${ASCON_LIB_NAME} PRIVATE ASCON_INLINE_MODE=0) + + target_sources(${COSE_LIB} PRIVATE ${COSE_ROOT}/src/crypt/ascon_c.c) + add_compile_definitions(CRYPTO_ASCON) else () message(FATAL_ERROR "Cryptolib ${CRYPTO_LIB} is not supported") endif () @@ -132,48 +146,48 @@ if (SECURE_MODE_ENABLED) RECEIVER_ID="${RECEIVER_ID}") endif () -INCLUDE (FindPkgConfig) - -# specific order: dependents before dependencies -target_link_libraries(rmw_desert PUBLIC - rmw::rmw - rcutils::rcutils - rcpputils::rcpputils - rmw_dds_common::rmw_dds_common_library - rosidl_typesupport_introspection_c::rosidl_typesupport_introspection_c - rosidl_typesupport_introspection_cpp::rosidl_typesupport_introspection_cpp - rosidl_runtime_c::rosidl_runtime_c -) - -configure_rmw_library(rmw_desert) - -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(${PROJECT_NAME} -PRIVATE "rmw_desert_BUILDING_LIBRARY") - -# specific order: dependents before dependencies -ament_export_libraries(rmw_desert) - -ament_export_dependencies( - rcutils - rcpputils - rmw - rosidl_runtime_c - rmw_dds_common - rosidl_typesupport_introspection_c - rosidl_typesupport_introspection_cpp -) - -register_rmw_implementation( - "c:rosidl_typesupport_c:rosidl_typesupport_introspection_c" - "cpp:rosidl_typesupport_cpp:rosidl_typesupport_introspection_cpp") - -ament_package() - -install( - TARGETS rmw_desert - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) +#INCLUDE (FindPkgConfig) +# +## specific order: dependents before dependencies +#target_link_libraries(rmw_desert PUBLIC +# rmw::rmw +# rcutils::rcutils +# rcpputils::rcpputils +# rmw_dds_common::rmw_dds_common_library +# rosidl_typesupport_introspection_c::rosidl_typesupport_introspection_c +# rosidl_typesupport_introspection_cpp::rosidl_typesupport_introspection_cpp +# rosidl_runtime_c::rosidl_runtime_c +#) +# +#configure_rmw_library(rmw_desert) +# +## Causes the visibility macros to use dllexport rather than dllimport, +## which is appropriate when building the dll but not consuming it. +#target_compile_definitions(${PROJECT_NAME} +#PRIVATE "rmw_desert_BUILDING_LIBRARY") +# +## specific order: dependents before dependencies +#ament_export_libraries(rmw_desert) +# +#ament_export_dependencies( +# rcutils +# rcpputils +# rmw +# rosidl_runtime_c +# rmw_dds_common +# rosidl_typesupport_introspection_c +# rosidl_typesupport_introspection_cpp +#) +# +#register_rmw_implementation( +# "c:rosidl_typesupport_c:rosidl_typesupport_introspection_c" +# "cpp:rosidl_typesupport_cpp:rosidl_typesupport_introspection_cpp") +# +#ament_package() +# +#install( +# TARGETS rmw_desert +# ARCHIVE DESTINATION lib +# LIBRARY DESTINATION lib +# RUNTIME DESTINATION bin +#) diff --git a/config.cmake b/config.cmake index 6f2465e..8c9ca6b 100644 --- a/config.cmake +++ b/config.cmake @@ -1,6 +1,6 @@ set(CBOR_LIB "NanoCBOR" CACHE STRING "Cbor library.") # libmcu_cbor, NanoCBOR option(SECURE_MODE_ENABLED "" ON) -set(CRYPTO_LIB "mbedtls" CACHE STRING "Crypto library.") +set(CRYPTO_LIB "ascon-c" CACHE STRING "Crypto library.") set(AEAD_ALGO "Ascon-AEAD128" CACHE STRING "Algorithm for authenticated encryption with additional data.") set(KDF_ALGO "HKDF-Ascon256" CACHE STRING "Algorithm for key derivation.") set(SENDER_ID "0x01" CACHE STRING "Sender ID in hex.") diff --git a/thirdparty/ascon-c b/thirdparty/ascon-c new file mode 160000 index 0000000..677ef5e --- /dev/null +++ b/thirdparty/ascon-c @@ -0,0 +1 @@ +Subproject commit 677ef5e514495c3afbdd0baa277fc26e7d9f2e89 diff --git a/thirdparty/libcose b/thirdparty/libcose new file mode 160000 index 0000000..264139e --- /dev/null +++ b/thirdparty/libcose @@ -0,0 +1 @@ +Subproject commit 264139ea159832195cd9efed1161de8fc04f2209 From a54cd2cf56c1c1b1f82ae7265fef57d6c2052336 Mon Sep 17 00:00:00 2001 From: Dmytro Ochkas Date: Tue, 14 Jul 2026 11:46:00 +0200 Subject: [PATCH 20/24] Cleanup the build --- CMakeLists.txt | 112 ++++++++++++++++++++++++------------------------- 1 file changed, 56 insertions(+), 56 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3d1735b..b1a3954 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,17 +15,17 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -pthread) endif() -#find_package(ament_cmake REQUIRED) -#find_package(ament_cmake_ros_core REQUIRED) -# -#find_package(rmw REQUIRED) -#find_package(rmw_dds_common REQUIRED) -#find_package(rosidl_runtime_c REQUIRED) -#find_package(rosidl_typesupport_introspection_c REQUIRED) -#find_package(rosidl_typesupport_introspection_cpp REQUIRED) -# -#find_package(rcutils REQUIRED) -#find_package(rcpputils REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros_core REQUIRED) + +find_package(rmw REQUIRED) +find_package(rmw_dds_common REQUIRED) +find_package(rosidl_runtime_c REQUIRED) +find_package(rosidl_typesupport_introspection_c REQUIRED) +find_package(rosidl_typesupport_introspection_cpp REQUIRED) + +find_package(rcutils REQUIRED) +find_package(rcpputils REQUIRED) add_library(rmw_desert src/get_node_info_and_types.cpp @@ -146,48 +146,48 @@ if (SECURE_MODE_ENABLED) RECEIVER_ID="${RECEIVER_ID}") endif () -#INCLUDE (FindPkgConfig) -# -## specific order: dependents before dependencies -#target_link_libraries(rmw_desert PUBLIC -# rmw::rmw -# rcutils::rcutils -# rcpputils::rcpputils -# rmw_dds_common::rmw_dds_common_library -# rosidl_typesupport_introspection_c::rosidl_typesupport_introspection_c -# rosidl_typesupport_introspection_cpp::rosidl_typesupport_introspection_cpp -# rosidl_runtime_c::rosidl_runtime_c -#) -# -#configure_rmw_library(rmw_desert) -# -## Causes the visibility macros to use dllexport rather than dllimport, -## which is appropriate when building the dll but not consuming it. -#target_compile_definitions(${PROJECT_NAME} -#PRIVATE "rmw_desert_BUILDING_LIBRARY") -# -## specific order: dependents before dependencies -#ament_export_libraries(rmw_desert) -# -#ament_export_dependencies( -# rcutils -# rcpputils -# rmw -# rosidl_runtime_c -# rmw_dds_common -# rosidl_typesupport_introspection_c -# rosidl_typesupport_introspection_cpp -#) -# -#register_rmw_implementation( -# "c:rosidl_typesupport_c:rosidl_typesupport_introspection_c" -# "cpp:rosidl_typesupport_cpp:rosidl_typesupport_introspection_cpp") -# -#ament_package() -# -#install( -# TARGETS rmw_desert -# ARCHIVE DESTINATION lib -# LIBRARY DESTINATION lib -# RUNTIME DESTINATION bin -#) +INCLUDE (FindPkgConfig) + +# specific order: dependents before dependencies +target_link_libraries(rmw_desert PUBLIC + rmw::rmw + rcutils::rcutils + rcpputils::rcpputils + rmw_dds_common::rmw_dds_common_library + rosidl_typesupport_introspection_c::rosidl_typesupport_introspection_c + rosidl_typesupport_introspection_cpp::rosidl_typesupport_introspection_cpp + rosidl_runtime_c::rosidl_runtime_c +) + +configure_rmw_library(rmw_desert) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} +PRIVATE "rmw_desert_BUILDING_LIBRARY") + +# specific order: dependents before dependencies +ament_export_libraries(rmw_desert) + +ament_export_dependencies( + rcutils + rcpputils + rmw + rosidl_runtime_c + rmw_dds_common + rosidl_typesupport_introspection_c + rosidl_typesupport_introspection_cpp +) + +register_rmw_implementation( + "c:rosidl_typesupport_c:rosidl_typesupport_introspection_c" + "cpp:rosidl_typesupport_cpp:rosidl_typesupport_introspection_cpp") + +ament_package() + +install( + TARGETS rmw_desert + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) From e68d5b5e1f972ff753ca59a92266aee2b5c69d40 Mon Sep 17 00:00:00 2001 From: Dmytro Ochkas Date: Tue, 14 Jul 2026 11:58:10 +0200 Subject: [PATCH 21/24] Dynamic PIV len and config instructions --- CMakeLists.txt | 1 + config.cmake | 5 +++-- src/desert_classes/security/SecurityLayer.cpp | 2 +- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b1a3954..5a3fe9b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -142,6 +142,7 @@ if (SECURE_MODE_ENABLED) target_compile_definitions(rmw_desert PRIVATE AEAD_ALGO="${AEAD_ALGO}" KDF_ALGO="${KDF_ALGO}" + PIV_LEN=${PIV_LEN} SENDER_ID="${SENDER_ID}" RECEIVER_ID="${RECEIVER_ID}") endif () diff --git a/config.cmake b/config.cmake index 8c9ca6b..1d3799a 100644 --- a/config.cmake +++ b/config.cmake @@ -1,7 +1,8 @@ set(CBOR_LIB "NanoCBOR" CACHE STRING "Cbor library.") # libmcu_cbor, NanoCBOR option(SECURE_MODE_ENABLED "" ON) -set(CRYPTO_LIB "ascon-c" CACHE STRING "Crypto library.") -set(AEAD_ALGO "Ascon-AEAD128" CACHE STRING "Algorithm for authenticated encryption with additional data.") +set(CRYPTO_LIB "ascon-c" CACHE STRING "Crypto library.") # ascon-c, mbedtls +set(AEAD_ALGO "Ascon-AEAD128" CACHE STRING "Algorithm for authenticated encryption with additional data.") # Ascon-AEAD128, Ascon-AEAD128-64, Ascon-AEAD128-32 set(KDF_ALGO "HKDF-Ascon256" CACHE STRING "Algorithm for key derivation.") +set(PIV_LEN 2 CACHE STRING "Length of Partial IV.") set(SENDER_ID "0x01" CACHE STRING "Sender ID in hex.") set(RECEIVER_ID "0x02" CACHE STRING "Receiver ID in hex.") diff --git a/src/desert_classes/security/SecurityLayer.cpp b/src/desert_classes/security/SecurityLayer.cpp index 54f1a18..cd9083f 100644 --- a/src/desert_classes/security/SecurityLayer.cpp +++ b/src/desert_classes/security/SecurityLayer.cpp @@ -77,7 +77,7 @@ static SecurityResult cbor_err_to_sec_err(int err) { SecurityLayer::SecurityLayer() : aead_params_(AeadParams(str_to_aead_algo(AEAD_ALGO))), kdf_(str_to_kdf_algo(KDF_ALGO)), - piv_size_(2), + piv_size_(PIV_LEN), piv_bytes_(piv_size_), sender_seq_number_(0) { From ca3f4b94b0836c33a8fb791b47666eba02109789 Mon Sep 17 00:00:00 2001 From: matlin Date: Tue, 14 Jul 2026 14:31:58 +0200 Subject: [PATCH 22/24] Included ASCON in rmw_desert library --- CMakeLists.txt | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5a3fe9b..f66902f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -114,18 +114,19 @@ if (SECURE_MODE_ENABLED) target_link_libraries(rmw_desert PRIVATE mbedtls) add_compile_definitions(CRYPTO_MBEDTLS) elseif (${CRYPTO_LIB} STREQUAL "ascon-c") - set(ASCON_LIB_NAME "ascon-c") + set(ASCON_LIB "ascon-c") set(ASCON_AEAD_ROOT thirdparty/ascon-c/crypto_aead/asconaead128/opt64) set(ASCON_HASH_ROOT thirdparty/ascon-c/crypto_hash/asconhash256/opt64) - add_library(${ASCON_LIB_NAME} OBJECT + add_library(${ASCON_LIB} OBJECT ${ASCON_AEAD_ROOT}/aead.c ${ASCON_AEAD_ROOT}/permutations.c ${ASCON_AEAD_ROOT}/printstate.c ${ASCON_HASH_ROOT}/hash.c ) - target_compile_definitions(${ASCON_LIB_NAME} PRIVATE ASCON_INLINE_MODE=0) + target_compile_definitions(${ASCON_LIB} PRIVATE ASCON_INLINE_MODE=0) target_sources(${COSE_LIB} PRIVATE ${COSE_ROOT}/src/crypt/ascon_c.c) + target_sources(rmw_desert PRIVATE $) add_compile_definitions(CRYPTO_ASCON) else () message(FATAL_ERROR "Cryptolib ${CRYPTO_LIB} is not supported") From 9ac8deb457de765272e2b4cc9f5edc52fa5a8733 Mon Sep 17 00:00:00 2001 From: Dmytro Ochkas Date: Tue, 14 Jul 2026 15:13:06 +0200 Subject: [PATCH 23/24] Fix warnings and make Security Layer global --- CMakeLists.txt | 1 - src/desert_classes/NanoCBorStream.cpp | 6 ++++-- thirdparty/libcose | 2 +- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f66902f..a0b97ad 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -120,7 +120,6 @@ if (SECURE_MODE_ENABLED) add_library(${ASCON_LIB} OBJECT ${ASCON_AEAD_ROOT}/aead.c ${ASCON_AEAD_ROOT}/permutations.c - ${ASCON_AEAD_ROOT}/printstate.c ${ASCON_HASH_ROOT}/hash.c ) target_compile_definitions(${ASCON_LIB} PRIVATE ASCON_INLINE_MODE=0) diff --git a/src/desert_classes/NanoCBorStream.cpp b/src/desert_classes/NanoCBorStream.cpp index 3122bc9..881b10b 100644 --- a/src/desert_classes/NanoCBorStream.cpp +++ b/src/desert_classes/NanoCBorStream.cpp @@ -9,6 +9,10 @@ namespace cbor { +#ifdef SECURE_MODE_ENABLED +static security::SecurityLayer g_sec_layer{}; +#endif + // TX stream // Forward declarations to abstract the implementation @@ -74,7 +78,6 @@ void TxStream::end_transmission() uint8_t* wrapped_ptr = _packet; #ifdef SECURE_MODE_ENABLED - security::SecurityLayer g_sec_layer; auto st = g_sec_layer.wrap(_packet, encoded_len, MAX_PACKET_LENGTH, &wrapped_ptr, &wrapped_size); if (st != security::OK) { @@ -475,7 +478,6 @@ void RxStream::interpret_packets() size_t unwrapped_size = packet.size(); #ifdef SECURE_MODE_ENABLED - security::SecurityLayer g_sec_layer; auto unwrap_st = g_sec_layer.unwrap(buffer, packet.size(), &unwrapped_size); if (unwrap_st != security::OK) { diff --git a/thirdparty/libcose b/thirdparty/libcose index 264139e..2c22884 160000 --- a/thirdparty/libcose +++ b/thirdparty/libcose @@ -1 +1 @@ -Subproject commit 264139ea159832195cd9efed1161de8fc04f2209 +Subproject commit 2c22884da2bcb80e7bb921d62bd6d8912779473e From 1dbe0fae7faed710cbe08b88fa69263c44b9eb2e Mon Sep 17 00:00:00 2001 From: Dmytro Ochkas Date: Thu, 16 Jul 2026 15:00:15 +0200 Subject: [PATCH 24/24] Add COSE stateless comression that removes the serialization overhead --- CMakeLists.txt | 3 + config.cmake | 5 +- src/desert_classes/NanoCBorStream.cpp | 6 +- src/desert_classes/security/SecurityLayer.cpp | 106 ++++++++++++++++-- src/desert_classes/security/SecurityLayer.h | 8 +- 5 files changed, 116 insertions(+), 12 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a0b97ad..ef934ee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -145,6 +145,9 @@ if (SECURE_MODE_ENABLED) PIV_LEN=${PIV_LEN} SENDER_ID="${SENDER_ID}" RECEIVER_ID="${RECEIVER_ID}") + if (COSE_STATELESS_COMP_ENABLED) + target_compile_definitions(rmw_desert PRIVATE COSE_STATELESS_COMP_ENABLED) + endif () endif () INCLUDE (FindPkgConfig) diff --git a/config.cmake b/config.cmake index 1d3799a..262ef99 100644 --- a/config.cmake +++ b/config.cmake @@ -1,8 +1,9 @@ set(CBOR_LIB "NanoCBOR" CACHE STRING "Cbor library.") # libmcu_cbor, NanoCBOR -option(SECURE_MODE_ENABLED "" ON) +option(SECURE_MODE_ENABLED "Enable COSE Security Layer." ON) +option(COSE_STATELESS_COMP_ENABLED "Enable COSE stateless compression." ON) set(CRYPTO_LIB "ascon-c" CACHE STRING "Crypto library.") # ascon-c, mbedtls set(AEAD_ALGO "Ascon-AEAD128" CACHE STRING "Algorithm for authenticated encryption with additional data.") # Ascon-AEAD128, Ascon-AEAD128-64, Ascon-AEAD128-32 set(KDF_ALGO "HKDF-Ascon256" CACHE STRING "Algorithm for key derivation.") -set(PIV_LEN 2 CACHE STRING "Length of Partial IV.") +set(PIV_LEN 2 CACHE STRING "Length of Partial IV.") # Max 16 set(SENDER_ID "0x01" CACHE STRING "Sender ID in hex.") set(RECEIVER_ID "0x02" CACHE STRING "Receiver ID in hex.") diff --git a/src/desert_classes/NanoCBorStream.cpp b/src/desert_classes/NanoCBorStream.cpp index 881b10b..e2e317a 100644 --- a/src/desert_classes/NanoCBorStream.cpp +++ b/src/desert_classes/NanoCBorStream.cpp @@ -478,7 +478,11 @@ void RxStream::interpret_packets() size_t unwrapped_size = packet.size(); #ifdef SECURE_MODE_ENABLED - auto unwrap_st = g_sec_layer.unwrap(buffer, packet.size(), &unwrapped_size); +#ifdef COSE_STATELESS_COMP_ENABLED + // Need some extra space for decompression + packet.resize(packet.size() + COSE_SERIALIZATION_MAX_OVERHEAD); +#endif + auto unwrap_st = g_sec_layer.unwrap(buffer, unwrapped_size, packet.size(), &unwrapped_size); if (unwrap_st != security::OK) { continue; diff --git a/src/desert_classes/security/SecurityLayer.cpp b/src/desert_classes/security/SecurityLayer.cpp index cd9083f..ee80797 100644 --- a/src/desert_classes/security/SecurityLayer.cpp +++ b/src/desert_classes/security/SecurityLayer.cpp @@ -188,9 +188,7 @@ SecurityResult SecurityLayer::derive_key(const std::vector& ikm, const return CONTEXT_DERIVE_ERROR; } - // static uint8_t kid[] = "rmw_desert_sender_key"; cose_key_init(cose_k_out); - // cose_key_set_kid(&cose_key_, kid, sizeof(kid) - 1); cose_key_set_keys(cose_k_out, COSE_EC_NONE, alg, nullptr, nullptr, key_out.data()); return OK; @@ -292,12 +290,90 @@ SecurityResult SecurityLayer::derive_context(const std::vector& master_ return res; } +SecurityResult SecurityLayer::cose_stateless_compress(uint8_t* cose, size_t cose_len, uint8_t** out, size_t* out_len) const +{ + /* + 83 # array(3) + 40 # bytes(0) + # "" + A1 # map(1) + 06 # unsigned(6) + 42 # bytes(2) + 00FE # "\u0000\xFE" + 58 18 # bytes(24) + 78AEE7DB75167291B572751B304A662925F1C71A36EED8E6 + */ + uint8_t piv_offset = 5; + const uint8_t* piv_ptr = cose + piv_offset; + const uint8_t* ciphertext_bstr = piv_ptr + piv_size_; + size_t ciphertext_bstr_len = cose + cose_len - ciphertext_bstr; + nanocbor_value_t v; + nanocbor_decoder_init(&v, ciphertext_bstr, ciphertext_bstr_len); + + const uint8_t* ciphertext; + size_t ciphertext_len; + int res = nanocbor_get_bstr(&v, &ciphertext, &ciphertext_len); + if (res < 0) { + return COMP_ERROR; + } + uint8_t* pkt_start = const_cast(ciphertext) - piv_size_; + memmove(pkt_start, piv_ptr, piv_size_); + + *out = pkt_start; + *out_len = piv_size_ + ciphertext_len; + return OK; +} + +SecurityResult SecurityLayer::cose_stateless_decompress(uint8_t* data, size_t data_len, size_t data_max_len, size_t* out_len) const +{ + static constexpr uint8_t cose_prefix[] = {0x83, 0x40, 0xA1, 0x06}; + static constexpr size_t cose_prefix_len = sizeof(cose_prefix); + + uint8_t* payload_ptr = data + piv_size_; + size_t payload_len = data_len - piv_size_; + + nanocbor_encoder_t enc; + nanocbor_encoder_init(&enc, nullptr, 0); + nanocbor_fmt_bstr(&enc, piv_size_); + size_t piv_ind_len = nanocbor_encoded_len(&enc); + + nanocbor_encoder_init(&enc, nullptr, 0); + nanocbor_fmt_bstr(&enc, payload_len); + size_t pl_ind_len = nanocbor_encoded_len(&enc); + + size_t decompressed_len = cose_prefix_len + piv_ind_len + piv_size_ + pl_ind_len + payload_len; + if (decompressed_len > data_max_len) { + return BUFFER_ERROR; + } + + memmove(payload_ptr + cose_prefix_len + piv_ind_len + pl_ind_len, payload_ptr, payload_len); + memmove(data + cose_prefix_len + piv_ind_len, data, piv_size_); + memcpy(data, cose_prefix, cose_prefix_len); + + uint8_t* piv_bstr_ptr = data + cose_prefix_len; + nanocbor_encoder_init(&enc, piv_bstr_ptr, piv_ind_len); + int res = nanocbor_fmt_bstr(&enc, piv_size_); + if (res < 0) { + return INTERNAL_ERROR; + } + + uint8_t* pl_bstr_ptr = payload_ptr + cose_prefix_len + piv_ind_len; + nanocbor_encoder_init(&enc, pl_bstr_ptr, payload_len); + res = nanocbor_fmt_bstr(&enc, payload_len); + if (res < 0) { + return INTERNAL_ERROR; + } + + *out_len = decompressed_len; + return OK; +} + SecurityResult SecurityLayer::wrap(uint8_t* data, size_t data_len, size_t data_max_len, uint8_t** cose_ptr, size_t* cose_len) { - auto nonce_st = generate_nonce(); - if (nonce_st != OK) + auto st = generate_nonce(); + if (st != OK) { - return nonce_st; + return st; } std::vector payload(data, data + data_len); @@ -324,12 +400,27 @@ SecurityResult SecurityLayer::wrap(uint8_t* data, size_t data_len, size_t data_m return WRAP_ERROR; } *cose_len = len; +#ifdef COSE_STATELESS_COMP_ENABLED + st = cose_stateless_compress(*cose_ptr, len, cose_ptr, cose_len); + if (st != OK) { + return st; + } +#endif sender_seq_number_++; return OK; } -SecurityResult SecurityLayer::unwrap(uint8_t* data, size_t data_len, size_t* new_data_len) +SecurityResult SecurityLayer::unwrap(uint8_t* data, size_t data_len, size_t data_max_len, size_t* new_data_len) { +#ifdef COSE_STATELESS_COMP_ENABLED + auto st = cose_stateless_decompress(data, data_len, data_max_len, &data_len); + if (st != OK) { + return st; + } +#else + (void)data_max_len; +#endif + cose_encrypt_dec_t decrypt; if (cose_encrypt_decode(&decrypt, data, data_len) != COSE_OK) { @@ -337,7 +428,8 @@ SecurityResult SecurityLayer::unwrap(uint8_t* data, size_t data_len, size_t* new } std::vector plaintext(data_len); - if (cose_encrypt_decrypt_lw(&decrypt, nullptr, &receiver_cose_key_, internal_buf_, sizeof(internal_buf_), plaintext.data(), new_data_len, context_iv_.data())) + if (cose_encrypt_decrypt_lw(&decrypt, nullptr, &receiver_cose_key_, internal_buf_, sizeof(internal_buf_), + plaintext.data(), new_data_len, context_iv_.data()) != COSE_OK) { return UNWRAP_ERROR; } diff --git a/src/desert_classes/security/SecurityLayer.h b/src/desert_classes/security/SecurityLayer.h index a649100..58d6606 100644 --- a/src/desert_classes/security/SecurityLayer.h +++ b/src/desert_classes/security/SecurityLayer.h @@ -11,6 +11,7 @@ #include "SecurityParams.h" #define DEFAULT_MASTER_SALT_LEN 16 +#define COSE_SERIALIZATION_MAX_OVERHEAD 12 #define COSE_INTERNAL_BUF_SIZE 512 namespace security @@ -26,6 +27,7 @@ enum SecurityResult NONCE_SIZE_ERROR = -5, BUFFER_ERROR = -6, CONTEXT_DERIVE_ERROR = -7, + COMP_ERROR = -8, WRAP_ERROR = -100, UNWRAP_ERROR = -200, INTERNAL_ERROR = -999 @@ -40,7 +42,7 @@ class SecurityLayer const std::vector &sender_id, const std::vector &receiver_id); SecurityResult wrap(uint8_t* data, size_t data_len, size_t data_max_len, uint8_t** cose_ptr, size_t* cose_len); - SecurityResult unwrap(uint8_t* data, size_t data_len, size_t* new_data_len); + SecurityResult unwrap(uint8_t* data, size_t data_len, size_t data_max_len, size_t* new_data_len); private: SecurityResult get_master_key_env(std::vector& master_key) const; @@ -50,8 +52,10 @@ class SecurityLayer SecurityResult derive_key(const std::vector& ikm, const std::vector& salt, const std::vector& id, cose_algo_t algo, std::vector& key_out, cose_key_t* cose_k_out); SecurityResult derive_iv(const std::vector& ikm, const std::vector& salt, cose_algo_t alg, std::vector& iv_out); SecurityResult derive_context(const std::vector& master_key, const std::vector& master_salt); + SecurityResult cose_stateless_compress(uint8_t* cose, size_t cose_len, uint8_t** out, size_t* out_len) const; + SecurityResult cose_stateless_decompress(uint8_t* data, size_t data_len, size_t data_max_len, size_t* out_len) const; - /* Algorithms to use */ + /* Algorithms to use */ AeadParams aead_params_; KdfAlgorithms kdf_; /* ***************** */