From 39ac96a54b79d2e417e8451e1fac4ba1d410676c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 19 Aug 2019 20:05:33 +1000 Subject: [PATCH 1/2] AP_Param: support @READONLY marker in param files allows for read-only parameters embedded in firmware --- libraries/AP_Param/AP_Param.cpp | 65 +++++++++++++++++++++++++++++---- libraries/AP_Param/AP_Param.h | 11 ++++-- 2 files changed, 65 insertions(+), 11 deletions(-) diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index 6bd0b2872a8c8..82c026cc0bcfd 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -76,6 +76,7 @@ const AP_Param::Info *AP_Param::_var_info; struct AP_Param::param_override *AP_Param::param_overrides = nullptr; uint16_t AP_Param::num_param_overrides = 0; +uint16_t AP_Param::num_read_only = 0; ObjectBuffer AP_Param::save_queue{30}; bool AP_Param::registered_save_handler; @@ -1243,8 +1244,11 @@ bool AP_Param::configured_in_storage(void) const return scan(&phdr, &ofs) && (phdr.type == AP_PARAM_VECTOR3F || idx == 0); } -bool AP_Param::configured_in_defaults_file(void) const +bool AP_Param::configured_in_defaults_file(bool &read_only) const { + if (num_param_overrides == 0) { + return false; + } uint32_t group_element = 0; const struct GroupInfo *ginfo; struct GroupNesting group_nesting {}; @@ -1257,6 +1261,7 @@ bool AP_Param::configured_in_defaults_file(void) const for (uint16_t i=0; i AP_MAX_NAME_SIZE) { return false; } - const char *value_s = strtok_r(nullptr, ", =\t", &saveptr); + const char *value_s = strtok_r(nullptr, ", =\t\r\n", &saveptr); if (value_s == nullptr) { return false; } value = atof(value_s); *vname = pname; + + const char *flags_s = strtok_r(nullptr, ", =\t\r\n", &saveptr); + if (flags_s && strcmp(flags_s, "@READONLY") == 0) { + read_only = true; + } else { + read_only = false; + } + return true; } @@ -1890,7 +1925,8 @@ bool AP_Param::count_defaults_in_file(const char *filename, uint16_t &num_defaul while (fgets(line, sizeof(line)-1, f)) { char *pname; float value; - if (!parse_param_line(line, &pname, value)) { + bool read_only; + if (!parse_param_line(line, &pname, value, read_only)) { continue; } enum ap_var_type var_type; @@ -1918,7 +1954,8 @@ bool AP_Param::read_param_defaults_file(const char *filename, bool last_pass) while (fgets(line, sizeof(line)-1, f)) { char *pname; float value; - if (!parse_param_line(line, &pname, value)) { + bool read_only; + if (!parse_param_line(line, &pname, value, read_only)) { continue; } enum ap_var_type var_type; @@ -1935,6 +1972,10 @@ bool AP_Param::read_param_defaults_file(const char *filename, bool last_pass) } param_overrides[idx].object_ptr = vp; param_overrides[idx].value = value; + param_overrides[idx].read_only = read_only; + if (read_only) { + num_read_only++; + } idx++; if (!vp->configured_in_storage()) { vp->set_float(value, var_type); @@ -1972,6 +2013,7 @@ bool AP_Param::load_defaults_file(const char *filename, bool last_pass) delete[] param_overrides; num_param_overrides = 0; + num_read_only = 0; param_overrides = new param_override[num_defaults]; if (param_overrides == nullptr) { @@ -2019,6 +2061,7 @@ bool AP_Param::count_embedded_param_defaults(uint16_t &count) char line[100]; char *pname; float value; + bool read_only; uint16_t i; uint16_t n = MIN(length, sizeof(line)-1); for (i=0;iconfigured_in_storage()) { vp->set_float(value, var_type); diff --git a/libraries/AP_Param/AP_Param.h b/libraries/AP_Param/AP_Param.h index eeb1860b8c4a4..0aa8f4b027945 100644 --- a/libraries/AP_Param/AP_Param.h +++ b/libraries/AP_Param/AP_Param.h @@ -426,14 +426,17 @@ class AP_Param static bool check_var_info(void); // return true if the parameter is configured in the defaults file - bool configured_in_defaults_file(void) const; + bool configured_in_defaults_file(bool &read_only) const; // return true if the parameter is configured in EEPROM/FRAM bool configured_in_storage(void) const; // return true if the parameter is configured - bool configured(void) const { return configured_in_defaults_file() || configured_in_storage(); } + bool configured(void) const; + // return true if the parameter is read-only + bool is_read_only(void) const; + // count of parameters in tree static uint16_t count_parameters(void); @@ -601,7 +604,7 @@ class AP_Param // find a default value given a pointer to a default value in flash static float get_default_value(const AP_Param *object_ptr, const float *def_value_ptr); - static bool parse_param_line(char *line, char **vname, float &value); + static bool parse_param_line(char *line, char **vname, float &value, bool &read_only); #if HAL_OS_POSIX_IO == 1 /* @@ -632,9 +635,11 @@ class AP_Param struct param_override { const AP_Param *object_ptr; float value; + bool read_only; // param is marked @READONLY }; static struct param_override *param_overrides; static uint16_t num_param_overrides; + static uint16_t num_read_only; // values filled into the EEPROM header static const uint8_t k_EEPROM_magic0 = 0x50; From c908ed3a19d39dd03479e0c2d77968a8d044918c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 19 Aug 2019 20:05:55 +1000 Subject: [PATCH 2/2] GCS_MAVLink: refuse set of read-only parameters --- libraries/GCS_MAVLink/GCS_Param.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/GCS_MAVLink/GCS_Param.cpp b/libraries/GCS_MAVLink/GCS_Param.cpp index 7ae2fba9615a4..f81f389a2a0ac 100644 --- a/libraries/GCS_MAVLink/GCS_Param.cpp +++ b/libraries/GCS_MAVLink/GCS_Param.cpp @@ -276,7 +276,7 @@ void GCS_MAVLINK::handle_param_set(const mavlink_message_t &msg) float old_value = vp->cast_to_float(var_type); - if (parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) { + if ((parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) || vp->is_read_only()) { gcs().send_text(MAV_SEVERITY_WARNING, "Param write denied (%s)", key); // echo back the incorrect value so that we fulfull the // parameter state machine requirements: