libmavconn: add MAVLink v2 signing support

Author: vooon

libmavconn/include/mavconn/interface.hpp

@@ -24,12 +24,14 @@
 #define MAVCONN__INTERFACE_HPP_
 
 #include <atomic>
+#include <array>
 #include <cassert>
 #include <chrono>
 #include <deque>
 #include <functional>
 #include <memory>
 #include <mutex>
+#include <optional>
 #include <sstream>
 #include <stdexcept>
 #include <string>
@@ -245,6 +247,31 @@ class MAVConnInterface
   void set_protocol_version(Protocol pver);
   [[nodiscard]] Protocol get_protocol_version();
 
+  /**
+   * @brief Enable MAVLink v2 packet signing for this connection.
+   *
+   * @param[in] secret_key         32-byte signing key
+   * @param[in] sign_outgoing      enable signing for outgoing packets
+   * @param[in] link_id            link id embedded in outgoing signatures
+   * @param[in] initial_timestamp  optional initial timestamp in 10 usec units
+   *                               since 2015-01-01 00:00:00 UTC.
+   */
+  void setup_signing(
+    const std::array<uint8_t, 32> & secret_key,
+    bool sign_outgoing = true,
+    uint8_t link_id = 0,
+    std::optional<uint64_t> initial_timestamp = std::nullopt);
+
+  /**
+   * @brief Disable MAVLink v2 packet signing.
+   */
+  void disable_signing();
+
+  /**
+   * @brief Configure callback for accepting unsigned packets while signing is enabled.
+   */
+  void set_accept_unsigned_callback(mavlink::mavlink_accept_unsigned_t cb);
+
   /**
    * @brief Construct connection from URL
    *
@@ -303,7 +330,7 @@ class MAVConnInterface
 
   inline mavlink::mavlink_status_t * get_status_p()
   {
-    return &m_parse_status;
+    return &m_tx_status;
   }
 
   inline mavlink::mavlink_message_t * get_buffer_p()
@@ -326,9 +353,18 @@ class MAVConnInterface
 private:
   friend const mavlink::mavlink_msg_entry_t * mavlink::mavlink_get_msg_entry(uint32_t msgid);
 
-  mavlink::mavlink_status_t m_parse_status;
+  static uint64_t default_signing_timestamp();
+  void apply_signing_config(bool sign_outgoing, uint8_t link_id, uint64_t timestamp);
+
+  mavlink::mavlink_status_t m_rx_parse_status;
   mavlink::mavlink_message_t m_buffer;
-  mavlink::mavlink_status_t m_mavlink_status;
+  mavlink::mavlink_status_t m_rx_status;
+  mavlink::mavlink_status_t m_tx_status;
+
+  mavlink::mavlink_signing_t m_rx_signing;
+  mavlink::mavlink_signing_t m_tx_signing;
+  mavlink::mavlink_signing_streams_t m_rx_signing_streams;
+  bool signing_enabled;
 
   std::atomic<size_t> tx_total_bytes, rx_total_bytes;
   std::mutex iostat_mutex;

libmavconn/src/interface.cpp

@@ -16,6 +16,7 @@
  */
 
 #include <cassert>
+#include <cstring>
 #include <limits>
 #include <memory>
 #include <set>
@@ -46,9 +47,14 @@ std::atomic<size_t> MAVConnInterface::conn_id_counter {0};
 MAVConnInterface::MAVConnInterface(uint8_t system_id, uint8_t component_id)
 : sys_id(system_id),
   comp_id(component_id),
-  m_parse_status{},
+  m_rx_parse_status{},
   m_buffer{},
-  m_mavlink_status{},
+  m_rx_status{},
+  m_tx_status{},
+  m_rx_signing{},
+  m_tx_signing{},
+  m_rx_signing_streams{},
+  signing_enabled(false),
   tx_total_bytes(0),
   rx_total_bytes(0),
   last_tx_total_bytes(0),
@@ -61,7 +67,7 @@ MAVConnInterface::MAVConnInterface(uint8_t system_id, uint8_t component_id)
 
 mavlink_status_t MAVConnInterface::get_status()
 {
-  return m_mavlink_status;
+  return m_rx_status;
 }
 
 MAVConnInterface::IOStat MAVConnInterface::get_iostat()
@@ -116,8 +122,8 @@ void MAVConnInterface::parse_buffer(
     auto c = *buf++;
 
     auto msg_received = static_cast<Framing>(mavlink::mavlink_frame_char_buffer(
-        &m_buffer, &m_parse_status, c,
-        &message, &m_mavlink_status));
+        &m_buffer, &m_rx_parse_status, c,
+        &message, &m_rx_status));
 
     if (msg_received != Framing::incomplete) {
       log_recv(pfx, message, msg_received);
@@ -191,23 +197,97 @@ void MAVConnInterface::send_message_ignore_drop(const mavlink::Message & msg, ui
 void MAVConnInterface::set_protocol_version(Protocol pver)
 {
   if (pver == Protocol::V10) {
-    m_mavlink_status.flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
-    m_parse_status.flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
+    m_tx_status.flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
+    m_rx_status.flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
+    m_rx_parse_status.flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
   } else {
-    m_mavlink_status.flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1);
-    m_parse_status.flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1);
+    m_tx_status.flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1);
+    m_rx_status.flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1);
+    m_rx_parse_status.flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1);
   }
 }
 
 Protocol MAVConnInterface::get_protocol_version()
 {
-  if (m_mavlink_status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) {
+  if (m_tx_status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) {
     return Protocol::V10;
   } else {
     return Protocol::V20;
   }
 }
 
+uint64_t MAVConnInterface::default_signing_timestamp()
+{
+  constexpr auto mavlink_signing_epoch = std::chrono::sys_days {std::chrono::year(2015) /
+      std::chrono::January / 1};
+  auto now = std::chrono::system_clock::now();
+  if (now < mavlink_signing_epoch) {
+    now = mavlink_signing_epoch;
+  }
+
+  const auto delta_us = std::chrono::duration_cast<std::chrono::microseconds>(
+    now - mavlink_signing_epoch).count();
+  return static_cast<uint64_t>(delta_us / 10);
+}
+
+void MAVConnInterface::apply_signing_config(bool sign_outgoing, uint8_t link_id, uint64_t timestamp)
+{
+  m_tx_signing.flags = sign_outgoing ? MAVLINK_SIGNING_FLAG_SIGN_OUTGOING : 0;
+  m_tx_signing.link_id = link_id;
+  m_tx_signing.timestamp = timestamp;
+  m_tx_signing.last_status = mavlink::MAVLINK_SIGNING_STATUS_NONE;
+
+  m_rx_signing.flags = sign_outgoing ? MAVLINK_SIGNING_FLAG_SIGN_OUTGOING : 0;
+  m_rx_signing.link_id = link_id;
+  m_rx_signing.timestamp = timestamp;
+  m_rx_signing.last_status = mavlink::MAVLINK_SIGNING_STATUS_NONE;
+
+  m_rx_signing_streams = {};
+
+  m_tx_status.signing = &m_tx_signing;
+  m_rx_parse_status.signing = &m_rx_signing;
+  m_rx_parse_status.signing_streams = &m_rx_signing_streams;
+  m_rx_status.signing = &m_rx_signing;
+  m_rx_status.signing_streams = &m_rx_signing_streams;
+
+  signing_enabled = true;
+}
+
+void MAVConnInterface::setup_signing(
+  const std::array<uint8_t, 32> & secret_key,
+  bool sign_outgoing,
+  uint8_t link_id,
+  std::optional<uint64_t> initial_timestamp)
+{
+  std::memcpy(m_tx_signing.secret_key, secret_key.data(), secret_key.size());
+  std::memcpy(m_rx_signing.secret_key, secret_key.data(), secret_key.size());
+
+  const auto timestamp = initial_timestamp.value_or(default_signing_timestamp());
+  apply_signing_config(sign_outgoing, link_id, timestamp);
+}
+
+void MAVConnInterface::disable_signing()
+{
+  m_tx_signing = {};
+  m_rx_signing = {};
+  m_rx_signing_streams = {};
+
+  m_tx_status.signing = nullptr;
+  m_tx_status.signing_streams = nullptr;
+  m_rx_parse_status.signing = nullptr;
+  m_rx_parse_status.signing_streams = nullptr;
+  m_rx_status.signing = nullptr;
+  m_rx_status.signing_streams = nullptr;
+
+  signing_enabled = false;
+}
+
+void MAVConnInterface::set_accept_unsigned_callback(mavlink::mavlink_accept_unsigned_t cb)
+{
+  m_tx_signing.accept_unsigned_callback = cb;
+  m_rx_signing.accept_unsigned_callback = cb;
+}
+
 /**
  * Parse host:port pairs
  */

libmavconn/test/test_mavconn.cpp

@@ -20,6 +20,7 @@
 // #include <gmock/gmock.h>
 #include <gtest/gtest.h>
 
+#include <array>
 #include <chrono>
 #include <cmath>
 #include <condition_variable>
@@ -271,6 +272,13 @@ class TCP : public UDP
 {
 };
 
+static bool accept_unsigned_for_test(
+  const mavlink::mavlink_status_t * status [[maybe_unused]],
+  uint32_t msgid [[maybe_unused]])
+{
+  return true;
+}
+
 TEST_F(TCP, bind_error)
 {
   MAVConnInterface::Ptr conns[2];
@@ -357,6 +365,129 @@ TEST(SERIAL, open_error)
     DeviceError);
 }
 
+TEST(SIGNING, udp_signed_packet_is_accepted)
+{
+  std::mutex mutex;
+  std::condition_variable cond;
+  auto got = false;
+  auto framing = Framing::incomplete;
+  auto message_id = std::numeric_limits<msgid_t>::max();
+
+  const std::array<uint8_t, 32> key {0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42,
+    0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42,
+    0x42, 0x42, 0x42, 0x42, 0x42, 0x42};
+
+  auto receiver = std::make_shared<MAVConnUDP>(42, 200, "0.0.0.0", 45042);
+  receiver->setup_signing(key, true, 1, 1U);
+  receiver->connect(
+    [&](const mavlink_message_t * message, const Framing msg_framing) {
+      std::lock_guard<std::mutex> lock(mutex);
+      message_id = message->msgid;
+      framing = msg_framing;
+      got = true;
+      cond.notify_all();
+    });
+
+  auto sender = std::make_shared<MAVConnUDP>(44, 200, "0.0.0.0", 45043, "localhost", 45042);
+  sender->setup_signing(key, true, 2, 1U);
+  sender->connect(MAVConnInterface::ReceivedCb());
+
+  send_heartbeat(sender.get());
+  send_heartbeat(sender.get());
+
+  {
+    std::unique_lock<std::mutex> lock(mutex);
+    EXPECT_TRUE(cond.wait_for(lock, std::chrono::seconds(2), [&]() {return got;}));
+  }
+  EXPECT_EQ(framing, Framing::ok);
+  EXPECT_EQ(message_id, mavlink::common::msg::HEARTBEAT::MSG_ID);
+
+  sender->close();
+  receiver->close();
+}
+
+TEST(SIGNING, udp_signature_mismatch_is_reported)
+{
+  std::mutex mutex;
+  std::condition_variable cond;
+  auto got = false;
+  auto framing = Framing::incomplete;
+
+  const std::array<uint8_t, 32> sender_key {0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11,
+    0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11,
+    0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11};
+  const std::array<uint8_t, 32> receiver_key {0x22, 0x22, 0x22, 0x22, 0x22, 0x22, 0x22, 0x22, 0x22,
+    0x22, 0x22, 0x22, 0x22, 0x22, 0x22, 0x22, 0x22, 0x22, 0x22, 0x22, 0x22, 0x22, 0x22, 0x22, 0x22,
+    0x22, 0x22, 0x22, 0x22, 0x22, 0x22, 0x22};
+
+  auto receiver = std::make_shared<MAVConnUDP>(42, 200, "0.0.0.0", 45044);
+  receiver->setup_signing(receiver_key, true, 1, 1U);
+  receiver->connect(
+    [&](const mavlink_message_t * message [[maybe_unused]], const Framing msg_framing) {
+      std::lock_guard<std::mutex> lock(mutex);
+      framing = msg_framing;
+      got = true;
+      cond.notify_all();
+    });
+
+  auto sender = std::make_shared<MAVConnUDP>(44, 200, "0.0.0.0", 45045, "localhost", 45044);
+  sender->setup_signing(sender_key, true, 2, 1U);
+  sender->connect(MAVConnInterface::ReceivedCb());
+
+  send_heartbeat(sender.get());
+  send_heartbeat(sender.get());
+
+  {
+    std::unique_lock<std::mutex> lock(mutex);
+    EXPECT_TRUE(cond.wait_for(lock, std::chrono::seconds(2), [&]() {return got;}));
+  }
+  EXPECT_EQ(framing, Framing::bad_signature);
+
+  sender->close();
+  receiver->close();
+}
+
+TEST(SIGNING, udp_accept_unsigned_callback_allows_unsigned_packets)
+{
+  std::mutex mutex;
+  std::condition_variable cond;
+  auto got = false;
+  auto framing = Framing::incomplete;
+  auto message_id = std::numeric_limits<msgid_t>::max();
+
+  const std::array<uint8_t, 32> receiver_key {0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33,
+    0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33,
+    0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33};
+
+  auto receiver = std::make_shared<MAVConnUDP>(42, 200, "0.0.0.0", 45046);
+  receiver->setup_signing(receiver_key, true, 1, 1U);
+  receiver->set_accept_unsigned_callback(accept_unsigned_for_test);
+  receiver->connect(
+    [&](const mavlink_message_t * message, const Framing msg_framing) {
+      std::lock_guard<std::mutex> lock(mutex);
+      message_id = message->msgid;
+      framing = msg_framing;
+      got = true;
+      cond.notify_all();
+    });
+
+  auto sender = std::make_shared<MAVConnUDP>(44, 200, "0.0.0.0", 45047, "localhost", 45046);
+  sender->connect(MAVConnInterface::ReceivedCb());
+
+  send_heartbeat(sender.get());
+  send_heartbeat(sender.get());
+
+  {
+    std::unique_lock<std::mutex> lock(mutex);
+    EXPECT_TRUE(cond.wait_for(lock, std::chrono::seconds(2), [&]() {return got;}));
+  }
+  EXPECT_EQ(framing, Framing::ok);
+  EXPECT_EQ(message_id, mavlink::common::msg::HEARTBEAT::MSG_ID);
+
+  sender->close();
+  receiver->close();
+}
+
 #if 0
 TEST(URL, open_url_serial)
 {