First stab at Mavlink signing

posix-configs/SITL/init/ekf2/iris

@@ -44,6 +44,7 @@ param set SENS_BOARD_X_OFF 0.000001
 param set SYS_AUTOSTART 4010
 param set SYS_MC_EST_GROUP 2
 param set SYS_RESTART_TYPE 2
+param set MAV_PROTO_VER 2
 replay tryapplyparams
 simulator start -s
 tone_alarm start

src/modules/mavlink/mavlink_bridge_header.h

@@ -50,6 +50,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
 
@@ -81,6 +84,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

@@ -106,6 +106,26 @@
 //#define MAVLINK_PRINT_PACKETS
 
 static Mavlink *_mavlink_instances = nullptr;
+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
+};
 
 /**
  * mavlink app start / stop handling function
@@ -156,6 +176,24 @@ void mavlink_end_uart_send(mavlink_channel_t chan, int length)
 	}
 }
 
+void mavlink_start_sign_stream(uint8_t chan)
+{
+	Mavlink *m = Mavlink::get_instance((unsigned)chan);
+
+	if (m != nullptr) {
+		(void)m->begin_signing();
+	}
+}
+
+void mavlink_end_sign_stream(uint8_t chan)
+{
+	Mavlink *m = Mavlink::get_instance((unsigned)chan);
+
+	if (m != nullptr) {
+		(void)m->end_signing();
+	}
+}
+
 /*
  * Internal function to give access to the channel status for each channel
  */
@@ -186,6 +224,51 @@ mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
 	}
 }
 
+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->increase_proto_sign_err();
+
+		unsigned sign_mode = m->get_proto_sign();
+
+		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();
 
 bool Mavlink::_boot_complete = false;
@@ -200,6 +283,7 @@ Mavlink::Mavlink() :
 	_task_running(false),
 	_mavlink_buffer{},
 	_mavlink_status{},
+	_mavlink_signing{},
 	_hil_enabled(false),
 	_generate_rc(false),
 	_use_hil_gps(false),
@@ -265,6 +349,7 @@ Mavlink::Mavlink() :
 	_message_buffer {},
 	_message_buffer_mutex {},
 	_send_mutex {},
+	_signing_mutex {},
 	_param_initialized(false),
 	_broadcast_mode(Mavlink::BROADCAST_MODE_OFF),
 	_param_system_id(PARAM_INVALID),
@@ -275,6 +360,8 @@ Mavlink::Mavlink() :
 	_param_forward_externalsp(PARAM_INVALID),
 	_param_broadcast(PARAM_INVALID),
 	_system_type(0),
+	_proto_sign(0),
+	_proto_sign_err(0),
 
 	/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
@@ -325,6 +412,17 @@ Mavlink::Mavlink() :
 	}
 
 	_rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
+
+	// set the signing procedure
+	// TODO: implementation key fetch from memory
+	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()
@@ -420,6 +518,20 @@ Mavlink::get_instance_for_device(const char *device_name)
 	return nullptr;
 }
 
+Mavlink *
+Mavlink::get_instance_for_status(const mavlink_status_t *status)
+{
+	Mavlink *inst;
+
+	LL_FOREACH(::_mavlink_instances, inst) {
+		if (status == mavlink_get_channel_status(inst->get_instance_id())) {
+			return inst;
+		}
+	}
+
+	return nullptr;
+}
+
 Mavlink *
 Mavlink::get_instance_for_network_port(unsigned long port)
 {
@@ -602,6 +714,7 @@ void Mavlink::mavlink_update_system()
 	if (!_param_initialized) {
 		_param_system_id = param_find("MAV_SYS_ID");
 		_param_component_id = param_find("MAV_COMP_ID");
+		_param_proto_sign = param_find("MAV_PROTO_SIGN");
 		_param_proto_ver = param_find("MAV_PROTO_VER");
 		_param_radio_id = param_find("MAV_RADIO_ID");
 		_param_system_type = param_find("MAV_TYPE");
@@ -620,6 +733,8 @@ void Mavlink::mavlink_update_system()
 	int32_t component_id;
 	param_get(_param_component_id, &component_id);
 
+	param_get(_param_proto_sign, &_proto_sign);
+
 	int32_t proto = 0;
 	param_get(_param_proto_ver, &proto);
 
@@ -1023,6 +1138,16 @@ Mavlink::send_packet()
 	return ret;
 }
 
+void Mavlink::begin_signing()
+{
+	pthread_mutex_lock(&_signing_mutex);
+}
+
+void Mavlink::end_signing()
+{
+	pthread_mutex_unlock(&_signing_mutex);
+}
+
 void
 Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
 {
@@ -1970,6 +2095,9 @@ Mavlink::task_main(int argc, char *argv[])
 	/* initialize send mutex */
 	pthread_mutex_init(&_send_mutex, nullptr);
 
+	/* initialize signing mutex */
+	pthread_mutex_init(&_signing_mutex, nullptr);
+
 	/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
 	if (_forwarding_on || _ftp_on) {
 		/* initialize message buffer if multiplexing is on or its needed for FTP.
@@ -2605,7 +2733,9 @@ Mavlink::display_status()
 	}
 
 	printf("\taccepting commands: %s\n", (accepting_commands()) ? "YES" : "NO");
-	printf("\tMAVLink version: %i\n", _protocol_version);
+	printf("\tMAVLink version: %i signing:%s (err #: %i)\n", _protocol_version,
+	       (_mavlink_signing.flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING) ? " ON" : "",
+	       _proto_sign_err);
 
 	printf("\ttransport protocol: ");
 

src/modules/mavlink/mavlink_main.h

@@ -111,6 +111,8 @@ class Mavlink
 
 	static Mavlink		*get_instance(unsigned instance);
 
+	static Mavlink		*get_instance_for_status(const mavlink_status_t *status);
+
 	static Mavlink 		*get_instance_for_device(const char *device_name);
 
 	static Mavlink 		*get_instance_for_network_port(unsigned long port);
@@ -179,6 +181,12 @@ class Mavlink
 		BROADCAST_MODE_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) {
@@ -305,6 +313,9 @@ class Mavlink
 	 */
 	int             	send_packet();
 
+	void			begin_signing();
+	void			end_signing();
+
 	/**
 	 * Resend message as is, don't change sequence number and CRC.
 	 */
@@ -424,6 +435,12 @@ class Mavlink
 
 	Protocol 		get_protocol() { return _protocol; }
 
+	unsigned		get_proto_sign() { return _proto_sign; }
+
+	void			increase_proto_sign_err() { _proto_sign_err++; }
+
+	unsigned		get_proto_sign_err() { return _proto_sign_err; }
+
 	unsigned short		get_network_port() { return _network_port; }
 
 	unsigned short		get_remote_port() { return _remote_port; }
@@ -491,6 +508,7 @@ class Mavlink
 	static constexpr float MAVLINK_MIN_MULTIPLIER = 0.0005f;
 	mavlink_message_t _mavlink_buffer;
 	mavlink_status_t _mavlink_status;
+	mavlink_signing_t _mavlink_signing;
 
 	/* states */
 	bool			_hil_enabled;		/**< Hardware In the Loop mode */
@@ -589,12 +607,14 @@ class Mavlink
 
 	pthread_mutex_t		_message_buffer_mutex;
 	pthread_mutex_t		_send_mutex;
+	pthread_mutex_t		_signing_mutex;
 
 	bool			_param_initialized;
 	uint32_t		_broadcast_mode;
 
 	param_t			_param_system_id;
 	param_t			_param_component_id;
+	param_t			_param_proto_sign;
 	param_t			_param_proto_ver;
 	param_t			_param_radio_id;
 	param_t			_param_system_type;
@@ -603,6 +623,8 @@ class Mavlink
 	param_t			_param_broadcast;
 
 	unsigned		_system_type;
+	unsigned		_proto_sign;
+	unsigned		_proto_sign_err;
 	static bool		_config_link_on;
 
 	perf_counter_t		_loop_perf;			/**< loop performance counter */

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 UART
+ * @value 2 Signing always enabled
+ */
+PARAM_DEFINE_INT32(MAV_PROTO_SIGN, 0);
+
 /**
  * MAVLink protocol version
  * @group MAVLink