gg/fts-can
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

signature support wip

Browse Source
This commit is contained in:
neo-nb3 2024-06-10 23:41:21 +02:00
parent 9e408e1d90
commit c5d13e4f2d
5 changed files with 145 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ROMFS/px4fmu_common/init.d/airframes/0001_main_fts
View File

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

6
src/modules/mavlink/mavlink_bridge_header.h
View File

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

99
src/modules/mavlink/mavlink_main.cpp
View File

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

27
src/modules/mavlink/mavlink_main.h
View File

@ -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,

9
src/modules/mavlink/mavlink_params.c
View File

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