MAVLink app: Add signing support.

This adds support for MAVLink 2.0 signing. When enabled only signed messages (or the ones in the unsigned accept list) will be parsed by the system. This allows to harden the link and to ensure that only authorized access is possible.

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);
 

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) {

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<px4::params::MAV_USEHILGPS>) _param_mav_usehilgps,
 		(ParamBool<px4::params::MAV_FWDEXTSP>) _param_mav_fwdextsp,
 		(ParamBool<px4::params::MAV_HASH_CHK_EN>) _param_mav_hash_chk_en,
+		(ParamInt<px4::params::MAV_SIGN_MODE>) _param_mav_sign_mode,
 		(ParamBool<px4::params::MAV_HB_FORW_EN>) _param_mav_hb_forw_en,
 		(ParamBool<px4::params::MAV_ODOM_LP>) _param_mav_odom_lp,
 		(ParamInt<px4::params::MAV_RADIO_TOUT>)      _param_mav_radio_timeout,

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