signature support wip

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

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

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

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<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,
 		(ParamInt<px4::params::MAV_RADIO_TOUT>)      _param_mav_radio_timeout,
 		(ParamInt<px4::params::SYS_HITL>) _param_sys_hitl,

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