From c5d13e4f2d8e38b8e979b7abc610d3334354fb0b Mon Sep 17 00:00:00 2001 From: neo-nb3 Date: Mon, 10 Jun 2024 23:41:21 +0200 Subject: [PATCH] signature support wip --- .../init.d/airframes/0001_main_fts | 5 + src/modules/mavlink/mavlink_bridge_header.h | 6 ++ src/modules/mavlink/mavlink_main.cpp | 99 ++++++++++++++++++- src/modules/mavlink/mavlink_main.h | 27 +++++ src/modules/mavlink/mavlink_params.c | 9 ++ 5 files changed, 145 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/airframes/0001_main_fts b/ROMFS/px4fmu_common/init.d/airframes/0001_main_fts index ffbc93f..2623ec9 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/0001_main_fts +++ b/ROMFS/px4fmu_common/init.d/airframes/0001_main_fts @@ -21,6 +21,11 @@ param set-default FTS_TYPE 1 param set-default FTS_SIM_MODE 0 +# AFTS_ENABLED = 0 -> AFTS MODULE NOT PRESENT OR DISABLED +# AFTS_ENABLED = 1 -> AFTS MODULE PRESENT AND ENABLED + +param set-default AFTS_ENABLED 1 + param set-default MAV_TYPE 37 fts start diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 3e2866a..654ce98 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -52,6 +52,9 @@ #define MAVLINK_START_UART_SEND mavlink_start_uart_send #define MAVLINK_END_UART_SEND mavlink_end_uart_send +#define MAVLINK_START_SIGN_STREAM mavlink_start_sign_stream +#define MAVLINK_END_SIGN_STREAM mavlink_end_sign_stream + #define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer #define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status @@ -89,6 +92,9 @@ void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int leng void mavlink_start_uart_send(mavlink_channel_t chan, int length); void mavlink_end_uart_send(mavlink_channel_t chan, int length); +void mavlink_start_sign_stream(uint8_t chan); +void mavlink_end_sign_stream(uint8_t chan); + extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan); extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 89ce498..d399cf3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -84,13 +84,80 @@ static pthread_mutex_t mavlink_module_mutex = PTHREAD_MUTEX_INITIALIZER; events::EventBuffer *Mavlink::_event_buffer = nullptr; Mavlink *mavlink_module_instances[MAVLINK_COMM_NUM_BUFFERS] {}; +static mavlink_signing_streams_t global_mavlink_signig_streams = {}; + +// magic for versioning of the structure +#define SIGNING_KEY_MAGIC 0x3852fcd1 + +// structure stored in persistent memory +typedef struct { + uint32_t magic; + uint64_t timestamp; + uint8_t secret_key[32]; +} signing_key_t; + +static const signing_key_t mavlink_secret_key = { + SIGNING_KEY_MAGIC, + 1420070400,// 1st January 2015 + {'N', 'i', 'm', 'b', 'u', 's', '2', '0', '2', '0', + 'N', 'i', 'm', 'b', 'u', 's', '2', '0', '2', '0', + 'N', 'i', 'm', 'b', 'u', 's', '2', '0', '2', '0', '2', '0' + } +}; void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length) { mavlink_module_instances[chan]->send_bytes(ch, length); } void mavlink_start_uart_send(mavlink_channel_t chan, int length) { mavlink_module_instances[chan]->send_start(length); } void mavlink_end_uart_send(mavlink_channel_t chan, int length) { mavlink_module_instances[chan]->send_finish(); } +void mavlink_start_sign_stream(uint8_t chan) { mavlink_module_instances[chan]->begin_signing(); } +void mavlink_end_sign_stream(uint8_t chan) { mavlink_module_instances[chan]->end_signing(); } mavlink_status_t *mavlink_get_channel_status(uint8_t channel) { return mavlink_module_instances[channel]->get_status(); } mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel) { return mavlink_module_instances[channel]->get_buffer(); } +static const uint32_t unsigned_messages[] = { + MAVLINK_MSG_ID_RADIO_STATUS, + MAVLINK_MSG_ID_ADSB_VEHICLE, + MAVLINK_MSG_ID_COLLISION +}; + +static bool accept_unsigned_callback(const mavlink_status_t *status, uint32_t message_id) +{ + // Always accept a few select messages even if unsigned + for (unsigned i = 0; i < sizeof(unsigned_messages) / sizeof(unsigned_messages[0]); i++) { + if (unsigned_messages[i] == message_id) { + return true; + } + } + + Mavlink *m = Mavlink::get_instance_for_status(status); + + if (m != nullptr) { + + // Count the failure + m->count_sign_error(); + + unsigned sign_mode = m->sign_mode(); + + switch (sign_mode) { + // If signing is not required always return true + case Mavlink::PROTO_SIGN_OPTIONAL: + return true; + + // Accept USB links if enabled + case Mavlink::PROTO_SIGN_NON_USB: + return m->is_usb_uart(); + + case Mavlink::PROTO_SIGN_ALWAYS: + + // fallthrough + default: + return false; + + } + } + + return false; +} + static void usage(); hrt_abstime Mavlink::_first_start_time = {0}; @@ -127,7 +194,15 @@ Mavlink::Mavlink() : } _vehicle_command_sub.subscribe(); - + // set the signing procedure + memcpy(_mavlink_signing.secret_key, mavlink_secret_key.secret_key, 32); + _mavlink_signing.link_id = _instance_id; + _mavlink_signing.timestamp = mavlink_secret_key.timestamp; + _mavlink_signing.flags = MAVLINK_SIGNING_FLAG_SIGN_OUTGOING; + _mavlink_signing.accept_unsigned_callback = accept_unsigned_callback; + // copy pointer of the signing to status struct + _mavlink_status.signing = &_mavlink_signing; + _mavlink_status.signing_streams = &global_mavlink_signig_streams; if (orb_exists(ORB_ID(event), 0) == PX4_ERROR) { orb_advertise_queue(ORB_ID(event), nullptr, event_s::ORB_QUEUE_LENGTH); } @@ -320,6 +395,20 @@ Mavlink::get_instance_for_device(const char *device_name) return nullptr; } +Mavlink * +Mavlink::get_instance_for_status(const mavlink_status_t *status) +{ + LockGuard lg{mavlink_module_mutex}; + + for (Mavlink *inst : mavlink_module_instances) { + if (status == mavlink_get_channel_status(inst->get_instance_id())) { + return inst; + } + } + + return nullptr; +} + #ifdef MAVLINK_UDP Mavlink * Mavlink::get_instance_for_network_port(unsigned long port) @@ -816,6 +905,14 @@ void Mavlink::send_finish() pthread_mutex_unlock(&_send_mutex); } +void Mavlink::begin_signing() { + // TODO Might require a mutex on resources +} + +void Mavlink::end_signing() { + // TODO Might require a mutex on resources +} + void Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len) { if (!_tx_buffer_low) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 1ba71c6..c92028c 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -153,6 +153,8 @@ public: static Mavlink *get_instance_for_device(const char *device_name); + static Mavlink *get_instance_for_status(const mavlink_status_t *status); + mavlink_message_t *get_buffer() { return &_mavlink_buffer; } mavlink_status_t *get_status() { return &_mavlink_status; } @@ -227,6 +229,12 @@ public: FLOW_CONTROL_ON }; + enum PROTO_SIGN { + PROTO_SIGN_OPTIONAL = 0, + PROTO_SIGN_NON_USB, + PROTO_SIGN_ALWAYS + }; + static const char *mavlink_mode_str(enum MAVLINK_MODE mode) { switch (mode) { @@ -338,6 +346,16 @@ public: */ void send_bytes(const uint8_t *buf, unsigned packet_len); + /** + * Begin signing of a packet + */ + void begin_signing(); + + /** + * End signing of a packet + */ + void end_signing(); + /** * Flush the transmit buffer and send one MAVLink packet */ @@ -435,6 +453,11 @@ public: */ void count_rxbytes(unsigned n) { _bytes_rx += n; }; + /** + * Count sign errors + */ + void count_sign_error() { _sign_err++; }; + /** * Get the receive status of this MAVLink link */ @@ -497,6 +520,7 @@ public: bool ftp_enabled() const { return _ftp_on; } bool hash_check_enabled() const { return _param_mav_hash_chk_en.get(); } + int32_t sign_mode() const { return _param_mav_sign_mode.get(); } bool forward_heartbeats_enabled() const { return _param_mav_hb_forw_en.get(); } bool failure_injection_enabled() const { return _param_sys_failure_injection_enabled.get(); } @@ -553,6 +577,7 @@ private: mavlink_message_t _mavlink_buffer {}; mavlink_status_t _mavlink_status {}; + mavlink_signing_t _mavlink_signing {}; /* states */ bool _hil_enabled{false}; /**< Hardware In the Loop mode */ @@ -613,6 +638,7 @@ private: unsigned _bytes_tx{0}; unsigned _bytes_txerr{0}; unsigned _bytes_rx{0}; + unsigned _sign_err{0}; hrt_abstime _bytes_timestamp{0}; #if defined(MAVLINK_UDP) @@ -662,6 +688,7 @@ private: (ParamBool) _param_mav_usehilgps, (ParamBool) _param_mav_fwdextsp, (ParamBool) _param_mav_hash_chk_en, + (ParamInt) _param_mav_sign_mode, (ParamBool) _param_mav_hb_forw_en, (ParamInt) _param_mav_radio_timeout, (ParamInt) _param_sys_hitl, diff --git a/src/modules/mavlink/mavlink_params.c b/src/modules/mavlink/mavlink_params.c index 8a793f5..ff8b5c4 100644 --- a/src/modules/mavlink/mavlink_params.c +++ b/src/modules/mavlink/mavlink_params.c @@ -49,6 +49,15 @@ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); */ PARAM_DEFINE_INT32(MAV_COMP_ID, 1); +/** + * MAVLink protocol signing + * @group MAVLink + * @value 0 Do not require signing + * @value 1 Signing enabled on non-USB + * @value 2 Signing always enabled + */ +PARAM_DEFINE_INT32(MAV_SIGN_MODE, 0); + /** * MAVLink protocol version * @group MAVLink