Enable hash check of used parameters to verify integrity of GCS local copy

This commit is contained in:
Nate Weibley 2015-10-12 13:36:37 -04:00
parent e685ef3c95
commit d7274ac5f0
3 changed files with 54 additions and 7 deletions

View File

@ -44,6 +44,8 @@
#include "mavlink_parameters.h"
#include "mavlink_main.h"
#define HASH_PARAM "_HASH_CHECK"
MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
_send_all_index(-1),
_rc_param_map_pub(nullptr),
@ -121,13 +123,27 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
/* when no index is given, loop through string ids and compare them */
if (req_read.param_index < 0) {
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
/* attempt to find parameter and send it */
send_param(param_find_no_notification(name));
if (strncmp(req_read.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN) == 0) {
/* return hash check for cached params */
uint32_t hash = param_hash_check();
/* build the one-off response message */
mavlink_param_value_t msg;
msg.param_count = param_count_used();
msg.param_index = -1;
strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
msg.param_type = MAV_PARAM_TYPE_UINT32;
memcpy(&msg.param_value, &hash, sizeof(hash));
_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg);
} else {
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
/* attempt to find parameter and send it */
send_param(param_find_no_notification(name));
}
} else {
/* when index is >= 0, send this parameter again */

View File

@ -65,6 +65,8 @@
#include "uORB/topics/parameter_update.h"
#include "px4_parameters.h"
#include <crc32.h>
#if 0
# define debug(fmt, args...) do { warnx(fmt, ##args); } while(0)
#else
@ -1035,3 +1037,25 @@ param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_chang
func(arg, param);
}
}
uint32_t param_hash_check(void)
{
uint32_t param_hash = 0;
param_lock();
/* compute the CRC32 over all string param names and 4 byte values */
for (param_t param = 0; handle_in_range(param); param++) {
if (!param_used(param)) {
continue;
}
const char *name = param_name(param);
const void *val = param_get_value_ptr(param);
param_hash = crc32part((const uint8_t*)name, strlen(name), param_hash);
param_hash = crc32part(val, sizeof(union param_value_u), param_hash);
}
param_unlock();
return param_hash;
}

View File

@ -333,6 +333,13 @@ __EXPORT int param_save_default(void);
*/
__EXPORT int param_load_default(void);
/**
* Generate the hash of all parameters and their values
*
* @return CRC32 hash of all param_ids and values
*/
__EXPORT uint32_t param_hash_check(void);
/*
* Macros creating static parameter definitions.
*