Merge pull request #31 from Auterion/qgcgov-fixes-signed-packets

RAS-A Signed Packet Support

README.md

@@ -29,6 +29,22 @@ Since the library is header only, you only need the library on the build system.
 
 You can also include the library as a submodule in your project.
 
+### Running the tests
+
+Libmav uses [doctest](https://github.com/doctest/doctest/) and [gcovr](https://github.com/gcovr/gcovr/).
+
+To run the tests, build the library, then run the test executable. Test results will be output to console.
+
+```bash
+mkdir build && cd build && cmake .. && make tests
+./tests/tests
+```
+
+To test coverage, simple invoke the coverage tool from the root directory.
+```bash
+gcovr
+```
+
 ## Getting started
 
 ### Loading a message set

gcovr.cfg

@@ -1,3 +1,4 @@
 exclude-throw-branches = yes
 filter = include/mav
-exclude = include/mav/rapidxml/*
+exclude = include/mav/rapidxml/*
+exclude = include/mav/picosha2/*

include/mav/Message.h

@@ -41,6 +41,7 @@
 #include <variant>
 #include "MessageDefinition.h"
 #include "utils.h"
+#include "picosha2/picosha2.h"
 
 namespace mav {
 
@@ -153,6 +154,29 @@ namespace mav {
             throw std::runtime_error("Unknown base type"); // should never happen
         }
 
+        uint64_t _computeSignatureHash48(const std::array<uint8_t, MessageDefinition::KEY_SIZE>& key) const {
+            // signature = sha256_48(secret_key + header + payload + CRC + link-ID + timestamp)
+            picosha2::hash256_one_by_one hasher;
+            // secret_key
+            hasher.process(key.begin(), key.begin() + MessageDefinition::KEY_SIZE);
+            // header + payload + CRC
+            hasher.process(_backing_memory.begin(), _backing_memory.begin() + 
+                    MessageDefinition::HEADER_SIZE + header().len() + MessageDefinition::CHECKSUM_SIZE);
+            // link-ID
+            const uint8_t linkId = signature().linkId();
+            hasher.process(&linkId, &linkId + MessageDefinition::SIGNATURE_LINK_ID_SIZE);
+            // timestamp
+            const uint64_t timestamp = signature().timestamp();
+            std::array<uint8_t, sizeof(timestamp)> timestampSerialized;
+            serialize(timestamp, timestampSerialized.begin());
+            hasher.process(timestampSerialized.begin(), timestampSerialized.begin() + MessageDefinition::SIGNATURE_TIMESTAMP_SIZE);
+
+            hasher.finish();
+            std::vector<unsigned char> hash(picosha2::k_digest_size);
+            hasher.get_hash_bytes(hash.begin(), hash.end());
+            return deserialize<uint64_t>(hash.data(), MessageDefinition::SIGNATURE_SIGNATURE_SIZE);
+        }
+
     public:
 
         static inline Message _instantiateFromMemory(const MessageDefinition &definition, ConnectionPartner source_partner,
@@ -222,6 +246,20 @@ namespace mav {
             return Header<uint8_t*>(_backing_memory.data());
         }
 
+        [[nodiscard]] const Signature<const uint8_t*> signature() const {
+            if (!isFinalized()) {
+                throw std::runtime_error("Unable to parse unfinalized message.");
+            }
+            return Signature<const uint8_t*>(&_backing_memory[MessageDefinition::HEADER_SIZE + header().len() + MessageDefinition::CHECKSUM_SIZE]);
+        }
+
+        [[nodiscard]] Signature<uint8_t*> signature() {
+            if (!isFinalized()) {
+                throw std::runtime_error("Unable to parse unfinalized message.");
+            }
+            return Signature<uint8_t*>(&_backing_memory[MessageDefinition::HEADER_SIZE + header().len() + MessageDefinition::CHECKSUM_SIZE]);
+        }
+
         [[nodiscard]] const ConnectionPartner& source() const {
             return _source_partner;
         }
@@ -440,11 +478,23 @@ namespace mav {
             return ss.str();
         }
 
+        [[nodiscard]] bool validate(const std::array<uint8_t, MessageDefinition::KEY_SIZE>& key) const {
+            return signature().signature() == _computeSignatureHash48(key);
+        }
+
         [[nodiscard]] uint32_t finalize(uint8_t seq, const Identifier &sender) {
+            static const std::array<uint8_t, MessageDefinition::KEY_SIZE> null_key = {};
+            return finalize(seq, sender, null_key, 0, 0);
+        }
+
+        [[nodiscard]] uint32_t finalize(uint8_t seq, const Identifier &sender,
+                                        const std::array<uint8_t, MessageDefinition::KEY_SIZE>& key,
+                                        const uint64_t& timestamp, const uint8_t linkId = 0) {
             if (isFinalized()) {
                 _unFinalize();
             }
 
+            bool sign = (timestamp > 0);
             auto last_nonzero = std::find_if(_backing_memory.rend() -
                     MessageDefinition::HEADER_SIZE - _message_definition->maxPayloadSize(),
                     _backing_memory.rend(), [](const auto &v) {
@@ -457,7 +507,7 @@ namespace mav {
 
             header().magic() = 0xFD;
             header().len() = static_cast<uint8_t>(payload_size);
-            header().incompatFlags() = 0;
+            header().incompatFlags() = sign ? 0x01 : 0x00;
             header().compatFlags() = 0;
             header().seq() = seq;
             if (header().systemId() == 0) {
@@ -475,7 +525,15 @@ namespace mav {
             _crc_offset = MessageDefinition::HEADER_SIZE + payload_size;
             serialize(crc.crc16(), _backing_memory.data() + _crc_offset);
 
-            return MessageDefinition::HEADER_SIZE + payload_size + MessageDefinition::CHECKSUM_SIZE;
+            int signature_size = 0;
+            if (sign) {
+                signature().linkId() = linkId;
+                signature().timestamp() = timestamp;
+                signature().signature() = _computeSignatureHash48(key);
+                signature_size = MessageDefinition::SIGNATURE_SIZE;
+            }
+
+            return MessageDefinition::HEADER_SIZE + payload_size + MessageDefinition::CHECKSUM_SIZE + signature_size;
         }
 
         [[nodiscard]] const uint8_t* data() const {

include/mav/MessageDefinition.h

@@ -179,6 +179,10 @@ namespace mav {
             return _backing_memory[2];
         }
 
+        [[nodiscard]] inline bool isSigned() const {
+            return incompatFlags() & 0x01;
+        }
+
         inline uint8_t& compatFlags() {
             return _backing_memory[3];
         }
@@ -224,6 +228,61 @@ namespace mav {
         }
     };
 
+    template <typename BackingMemoryPointerType>
+    class Signature {
+      private:
+        BackingMemoryPointerType _backing_memory;
+
+        // Both timestamp and signature use 6-byte fields
+        class _SignatureField {
+          private:
+            BackingMemoryPointerType _ptr;
+          public:
+            explicit _SignatureField(BackingMemoryPointerType ptr) : _ptr(ptr) {}
+
+            operator uint64_t() const {
+                return static_cast<uint64_t>((*static_cast<const uint64_t*>(static_cast<const void*>(_ptr))) & 0xFFFFFFFFFFFF);
+            }
+
+            _SignatureField& operator=(uint64_t v) {
+                _ptr[0] = static_cast<uint8_t>(v & 0xFF);
+                _ptr[1] = static_cast<uint8_t>((v >> 8) & 0xFF);
+                _ptr[2] = static_cast<uint8_t>((v >> 16) & 0xFF);
+                _ptr[3] = static_cast<uint8_t>((v >> 24) & 0xFF);
+                _ptr[4] = static_cast<uint8_t>((v >> 32) & 0xFF);
+                _ptr[5] = static_cast<uint8_t>((v >> 40) & 0xFF);
+                return *this;
+            }
+        };
+
+      public:
+        explicit Signature(BackingMemoryPointerType backing_memory) : _backing_memory(backing_memory) {}
+
+        inline uint8_t& linkId(){
+            return _backing_memory[0];
+        }
+
+        [[nodiscard]] inline uint8_t linkId() const {
+            return _backing_memory[0];
+        }
+
+        inline _SignatureField timestamp() {
+            return _SignatureField(_backing_memory + 1);
+        }
+
+        [[nodiscard]] inline const _SignatureField timestamp() const {
+            return _SignatureField(_backing_memory + 1);
+        }
+
+        inline _SignatureField signature() {
+            return _SignatureField(_backing_memory + 7);
+        }
+
+        [[nodiscard]] inline const _SignatureField signature() const {
+            return _SignatureField(_backing_memory + 7);
+        }
+    };
+
     class FieldType {
     public:
         enum class BaseType {
@@ -308,8 +367,12 @@ namespace mav {
         static constexpr int MAX_PAYLOAD_SIZE = 255;
         static constexpr int HEADER_SIZE = 10;
         static constexpr int CHECKSUM_SIZE = 2;
-        static constexpr int SIGNATURE_SIZE = 13;
+        static constexpr int SIGNATURE_LINK_ID_SIZE = 1;
+        static constexpr int SIGNATURE_TIMESTAMP_SIZE = 6;
+        static constexpr int SIGNATURE_SIGNATURE_SIZE = 6;
+        static constexpr int SIGNATURE_SIZE = SIGNATURE_LINK_ID_SIZE + SIGNATURE_TIMESTAMP_SIZE + SIGNATURE_SIGNATURE_SIZE;
         static constexpr int MAX_MESSAGE_SIZE = MAX_PAYLOAD_SIZE + HEADER_SIZE + CHECKSUM_SIZE + SIGNATURE_SIZE;
+        static constexpr int KEY_SIZE = 32;
 
         [[nodiscard]] inline const std::string& name() const {
             return _name;

include/mav/Network.h

@@ -43,6 +43,7 @@
 #include <atomic>
 #include <iostream>
 #include <memory>
+#include <chrono>
 #include <utility>
 #include <future>
 #include "Connection.h"
@@ -117,7 +118,7 @@ namespace mav {
                 backing_memory[0] = 0xFD;
                 _interface.receive(backing_memory.data() + 1, MessageDefinition::HEADER_SIZE -1);
                 Header header{backing_memory.data()};
-                const bool message_is_signed = header.incompatFlags() & 0x01;
+                const bool message_is_signed = header.isSigned();
                 const int wire_length = MessageDefinition::HEADER_SIZE + header.len() + MessageDefinition::CHECKSUM_SIZE +
                                   (message_is_signed ? MessageDefinition::SIGNATURE_SIZE : 0);
                 auto partner = _interface.receive(backing_memory.data() + MessageDefinition::HEADER_SIZE,
@@ -145,6 +146,10 @@ namespace mav {
         }
     };
 
+    static uint64_t _get_timestamp_function_default() {
+        const auto now = std::chrono::system_clock::now();
+        return std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count();
+    }
 
     class NetworkRuntime {
     private:
@@ -158,6 +163,9 @@ namespace mav {
         std::mutex _heartbeat_message_mutex;
         StreamParser _parser;
         Identifier _own_id;
+        bool _sign;
+        std::array<uint8_t, MessageDefinition::KEY_SIZE> _key;
+        std::function<uint64_t(void)> _get_timestamp_function;
         std::mutex _connections_mutex;
         std::mutex _send_mutex;
         std::unordered_map<ConnectionPartner,
@@ -171,7 +179,12 @@ namespace mav {
         std::function<void(const std::shared_ptr<Connection>&)> _on_connection_lost;
 
         void _sendMessage(Message &message, const ConnectionPartner &partner) {
-            int wire_length = static_cast<int>(message.finalize(_seq++, _own_id));
+            int wire_length;
+            if (_sign) {
+                wire_length = static_cast<int>(message.finalize(_seq++, _own_id, _key, _get_timestamp_function()));
+            } else {
+                wire_length = static_cast<int>(message.finalize(_seq++, _own_id));
+            }
             std::unique_lock<std::mutex> lock(_send_mutex);
             _interface.send(message.data(), wire_length, partner);
         }
@@ -327,6 +340,7 @@ namespace mav {
                        std::function<void(const std::shared_ptr<Connection>&)> on_connection_lost = {}) :
                 _interface(interface), _message_set(message_set),
                 _parser(_message_set, _interface), _own_id(own_id),
+                _sign(false), _get_timestamp_function(_get_timestamp_function_default),
                 _on_connection(std::move(on_connection)), _on_connection_lost(std::move(on_connection_lost)) {
 
             _receive_thread = std::thread{
@@ -409,6 +423,17 @@ namespace mav {
             _sendMessage(message, {});
         }
 
+        void enableMessageSigning(std::array<uint8_t, MessageDefinition::KEY_SIZE> key,
+                                  std::function<uint64_t(void)> timestampFunction = _get_timestamp_function_default) {
+            _sign = true;
+            _key = key;
+            _get_timestamp_function = timestampFunction;
+        }
+
+        void disableMessageSigning() {
+            _sign = false;
+        }
+
         void stop() {
             _interface.close();
             _should_terminate.store(true);

include/mav/picosha2/license.txt

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2017 okdshin
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.