diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index e4811f5628..544348c919 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 a914d4a4a1..1360f8723b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -80,13 +80,80 @@ static pthread_mutex_t mavlink_module_mutex = PTHREAD_MUTEX_INITIALIZER; 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 + { + 0xce, 0x39, 0x7e, 0x07, 0x27, 0x6c, 0xc8, 0xa1, 0xd9, 0x88, 0x76, 0x92, 0x8a, 0x9a, 0xab, 0xbb, + 0x72, 0x7b, 0x9f, 0xbe, 0xee, 0xb7, 0x32, 0x71, 0xc6, 0x0c, 0x9c, 0xa1, 0x8a, 0x16, 0x14, 0xe3 + } // plain text hex key ce397e07276cc8a1d98876928a9aabbb727b9fbeeeb73271c60c9ca18a1614e3 +}; 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}; @@ -122,6 +189,17 @@ Mavlink::Mavlink() : } _vehicle_command_sub.subscribe(); + + // set the signing procedure + // TODO: implementation key fetch from parameters + 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; } Mavlink::~Mavlink() @@ -276,6 +354,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) @@ -793,6 +885,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 d16cf978e5..6a35e42832 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -141,6 +141,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; } @@ -207,6 +209,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) { @@ -329,6 +337,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 */ @@ -431,6 +449,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 */ @@ -498,6 +521,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 odometry_loopback_enabled() const { return _param_mav_odom_lp.get(); } @@ -548,6 +572,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 */ @@ -609,6 +634,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) @@ -664,6 +690,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, (ParamBool) _param_mav_odom_lp, (ParamInt) _param_mav_radio_timeout, diff --git a/src/modules/mavlink/mavlink_params.c b/src/modules/mavlink/mavlink_params.c index fb88876134..590b7a18e8 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