diff --git a/src/lib/parameters/flashparams/flashparams.cpp b/src/lib/parameters/flashparams/flashparams.cpp index 8d083a99f7..4eced1cb39 100644 --- a/src/lib/parameters/flashparams/flashparams.cpp +++ b/src/lib/parameters/flashparams/flashparams.cpp @@ -143,12 +143,6 @@ out: size_t buf_size = bson_encoder_buf_size(&encoder); - int shutdown_lock_ret = px4_shutdown_lock(); - - if (shutdown_lock_ret) { - PX4_ERR("px4_shutdown_lock() failed (%i)", shutdown_lock_ret); - } - /* Get a buffer from the flash driver with enough space */ uint8_t *buffer; @@ -177,11 +171,6 @@ out: free(enc_buff); parameter_flashfs_free(); } - - if (shutdown_lock_ret == 0) { - px4_shutdown_unlock(); - } - } return result; diff --git a/src/lib/parameters/param.h b/src/lib/parameters/param.h index d2188aa305..a7b669c1c0 100644 --- a/src/lib/parameters/param.h +++ b/src/lib/parameters/param.h @@ -317,12 +317,12 @@ __EXPORT void param_reset_specific(const char *resets[], int num_resets); * Export changed parameters to a file. * Note: this method requires a large amount of stack size! * - * @param fd File descriptor to export to (-1 selects the FLASH storage). + * @param filename Path to the default parameter file. * @param filter Filter parameters to be exported. The method should return true if * the parameter should be exported. No filtering if nullptr is passed. * @return Zero on success, nonzero on failure. */ -__EXPORT int param_export(int fd, param_filter_func filter); +__EXPORT int param_export(const char *filename, param_filter_func filter); /** * Import parameters from a file, discarding any unrecognized parameters. diff --git a/src/lib/parameters/parameters.cpp b/src/lib/parameters/parameters.cpp index e98280acea..0794ca1397 100644 --- a/src/lib/parameters/parameters.cpp +++ b/src/lib/parameters/parameters.cpp @@ -1176,61 +1176,89 @@ const char *param_get_backup_file() return param_backup_file; } -static int param_save_file_internal(const char *filename) -{ - int res = PX4_ERROR; - int attempts = 5; - - while (res != OK && attempts > 0) { - // write parameters to file - int fd = ::open(filename, O_RDWR | O_CREAT, PX4_O_MODE_666); - - if (fd > -1) { - res = param_export(fd, nullptr); - ::close(fd); - - if (res == PX4_OK) { - return PX4_OK; - - } else { - PX4_ERR("param_export failed, retrying %d", attempts); - px4_usleep(10000); // wait at least 10 milliseconds before trying again - } - - } else { - PX4_ERR("failed to open param file %s, retrying %d", filename, attempts); - } - - attempts--; - } - - if (res != OK) { - PX4_ERR("failed to write parameters to file: %s", filename); - } - - return PX4_ERROR; -} +static int param_export_internal(int fd, param_filter_func filter); +static int param_verify(int fd); int param_save_default() { + PX4_DEBUG("param_save_default"); + int shutdown_lock_ret = px4_shutdown_lock(); + + if (shutdown_lock_ret != 0) { + PX4_ERR("px4_shutdown_lock() failed (%i)", shutdown_lock_ret); + } + + // take the file lock + do {} while (px4_sem_wait(¶m_sem_save) != 0); + + param_lock_reader(); + int res = PX4_ERROR; const char *filename = param_get_default_file(); if (filename) { - res = param_save_file_internal(filename); + static constexpr int MAX_ATTEMPTS = 3; + + for (int attempt = 1; attempt <= MAX_ATTEMPTS; attempt++) { + // write parameters to file + int fd = ::open(filename, O_WRONLY | O_CREAT, PX4_O_MODE_666); + + if (fd > -1) { + perf_begin(param_export_perf); + res = param_export_internal(fd, nullptr); + perf_end(param_export_perf); + ::close(fd); + + if (res == PX4_OK) { + // reopen file to verify + int fd_verify = ::open(filename, O_RDONLY, PX4_O_MODE_666); + res = param_verify(fd_verify); + ::close(fd_verify); + } + } + + if (res == PX4_OK) { + break; + + } else { + PX4_ERR("parameter export to %s failed (%d) attempt %d", filename, res, attempt); + px4_usleep(10000); // wait at least 10 milliseconds before trying again + } + } } else { - param_lock_writer(); perf_begin(param_export_perf); res = flash_param_save(nullptr); - params_unsaved.reset(); perf_end(param_export_perf); - param_unlock_writer(); } - // backup file - if (param_backup_file) { - param_save_file_internal(param_backup_file); + if (res != PX4_OK) { + PX4_ERR("param export failed (%d)", res); + + } else { + params_unsaved.reset(); + + // backup file + if (param_backup_file) { + int fd_backup_file = ::open(param_backup_file, O_WRONLY | O_CREAT, PX4_O_MODE_666); + + if (fd_backup_file > -1) { + int backup_export_ret = param_export_internal(fd_backup_file, nullptr); + + if (backup_export_ret != 0) { + PX4_ERR("backup parameter export to %s failed (%d)", param_backup_file, backup_export_ret); + } + + ::close(fd_backup_file); + } + } + } + + param_unlock_reader(); + px4_sem_post(¶m_sem_save); + + if (shutdown_lock_ret == 0) { + px4_shutdown_unlock(); } return res; @@ -1335,6 +1363,8 @@ static int param_verify_callback(bson_decoder_t decoder, bson_node_t node) static int param_verify(int fd) { + PX4_DEBUG("param_verify"); + if (fd < 0) { return -1; } @@ -1375,27 +1405,13 @@ static int param_verify(int fd) } int -param_export(int fd, param_filter_func filter) +param_export(const char *filename, param_filter_func filter) { - int result = -1; - perf_begin(param_export_perf); - - if (fd < 0) { - param_lock_writer(); - // flash_param_save() will take the shutdown lock - result = flash_param_save(filter); - params_unsaved.reset(); - param_unlock_writer(); - perf_end(param_export_perf); - return result; - } - - param_wbuf_s *s = nullptr; - bson_encoder_s encoder{}; + PX4_DEBUG("param_export"); int shutdown_lock_ret = px4_shutdown_lock(); - if (shutdown_lock_ret) { + if (shutdown_lock_ret != 0) { PX4_ERR("px4_shutdown_lock() failed (%i)", shutdown_lock_ret); } @@ -1404,10 +1420,41 @@ param_export(int fd, param_filter_func filter) param_lock_reader(); + int fd = ::open(filename, O_RDWR | O_CREAT, PX4_O_MODE_666); + int result = PX4_ERROR; + + perf_begin(param_export_perf); + + if (fd > -1) { + result = param_export_internal(fd, filter); + + } else { + result = flash_param_save(filter); + } + + perf_end(param_export_perf); + + param_unlock_reader(); + px4_sem_post(¶m_sem_save); + + if (shutdown_lock_ret == 0) { + px4_shutdown_unlock(); + } + + return result; +} + +// internal parameter export, caller is responsible for locking +static int param_export_internal(int fd, param_filter_func filter) +{ + PX4_DEBUG("param_export_internal"); + + int result = -1; + param_wbuf_s *s = nullptr; + bson_encoder_s encoder{}; uint8_t bson_buffer[256]; if (bson_encoder_init_buf_file(&encoder, fd, &bson_buffer, sizeof(bson_buffer)) != 0) { - result = -1; goto out; } @@ -1484,33 +1531,12 @@ param_export(int fd, param_filter_func filter) out: if (result == 0) { - if (bson_encoder_fini(&encoder) == PX4_OK) { - // verify exported bson - if (param_verify(fd) == 0) { - if (!filter) { - params_unsaved.reset(); - } - - } else { - result = -1; - } - - } else { + if (bson_encoder_fini(&encoder) != PX4_OK) { PX4_ERR("BSON encoder finalize failed"); result = -1; } } - param_unlock_reader(); - - px4_sem_post(¶m_sem_save); - - if (shutdown_lock_ret == 0) { - px4_shutdown_unlock(); - } - - perf_end(param_export_perf); - return result; } @@ -1542,7 +1568,7 @@ param_import_callback(bson_decoder_t decoder, bson_node_t node) if (param_type(param) == PARAM_TYPE_INT32) { int32_t i = node->i32; param_set_internal(param, &i, true, true); - PX4_DEBUG("Imported %s with value %d", param_name(param), i); + PX4_DEBUG("Imported %s with value %" PRIi32, param_name(param), i); } else { PX4_WARN("unexpected type for %s", node->name); @@ -1639,18 +1665,20 @@ param_import_internal(int fd) } } else { - PX4_ERR("param import failed (%d) attempt %d, retrying", result, attempt); + PX4_ERR("param import failed (%d) attempt %d", result, attempt); } } else { - PX4_ERR("param import bson decoder init failed attempt %d, retrying", attempt); + PX4_ERR("param import bson decoder init failed (attempt %d)", attempt); } - if (lseek(fd, 0, SEEK_SET) != 0) { - PX4_ERR("import lseek failed (%d)", errno); - } + if (attempt != MAX_ATTEMPTS) { + if (lseek(fd, 0, SEEK_SET) != 0) { + PX4_ERR("import lseek failed (%d)", errno); + } - px4_usleep(10000); // wait at least 10 milliseconds before trying again + px4_usleep(10000); // wait at least 10 milliseconds before trying again + } } return -1; diff --git a/src/modules/commander/factory_calibration_storage.cpp b/src/modules/commander/factory_calibration_storage.cpp index f64fbce346..e4bfeeba99 100644 --- a/src/modules/commander/factory_calibration_storage.cpp +++ b/src/modules/commander/factory_calibration_storage.cpp @@ -59,17 +59,13 @@ FactoryCalibrationStorage::FactoryCalibrationStorage() int FactoryCalibrationStorage::open() { - if (_fd >= 0) { - cleanup(); - } - if (!_enabled) { return 0; } - _fd = ::open(CALIBRATION_STORAGE, O_RDWR); + int ret = ::access(CALIBRATION_STORAGE, R_OK | W_OK); - if (_fd == -1) { + if (ret != 0) { return -errno; } @@ -84,7 +80,7 @@ int FactoryCalibrationStorage::store() return 0; } - int ret = param_export(_fd, filter_calibration_params); + int ret = param_export(CALIBRATION_STORAGE, filter_calibration_params); if (ret != 0) { PX4_ERR("param export failed (%i)", ret); @@ -98,10 +94,5 @@ void FactoryCalibrationStorage::cleanup() if (_enabled) { param_control_autosave(true); } - - if (_fd >= 0) { - close(_fd); - _fd = -1; - } } diff --git a/src/modules/commander/factory_calibration_storage.h b/src/modules/commander/factory_calibration_storage.h index bfb75966d7..1896c9c6d6 100644 --- a/src/modules/commander/factory_calibration_storage.h +++ b/src/modules/commander/factory_calibration_storage.h @@ -60,5 +60,4 @@ private: void cleanup(); bool _enabled{false}; - int _fd{-1}; }; diff --git a/src/systemcmds/param/param.cpp b/src/systemcmds/param/param.cpp index 1422c6fe71..44ccdb4292 100644 --- a/src/systemcmds/param/param.cpp +++ b/src/systemcmds/param/param.cpp @@ -433,21 +433,9 @@ param_main(int argc, char *argv[]) static int do_save(const char *param_file_name) { - /* create the file */ - int fd = open(param_file_name, O_RDWR | O_CREAT, PX4_O_MODE_666); - - if (fd < 0) { - PX4_ERR("open '%s' failed (%i)", param_file_name, errno); - return 1; - } - - int result = param_export(fd, nullptr); - close(fd); + int result = param_export(param_file_name, nullptr); if (result < 0) { -#ifndef __PX4_QURT - (void)unlink(param_file_name); -#endif PX4_ERR("exporting to '%s' failed (%i)", param_file_name, result); return 1; } diff --git a/src/systemcmds/tests/test_parameters.cpp b/src/systemcmds/tests/test_parameters.cpp index be7fa1063c..1d15eae785 100644 --- a/src/systemcmds/tests/test_parameters.cpp +++ b/src/systemcmds/tests/test_parameters.cpp @@ -401,23 +401,14 @@ bool ParameterTest::exportImportAll() // backup current parameters const char *param_file_name = PX4_STORAGEDIR "/param_backup"; - int fd = open(param_file_name, O_RDWR | O_CREAT, PX4_O_MODE_666); - if (fd < 0) { - PX4_ERR("open '%s' failed (%i)", param_file_name, errno); - return false; - } - - int result = param_export(fd, nullptr); + int result = param_export(param_file_name, nullptr); if (result != PX4_OK) { PX4_ERR("param_export failed"); - close(fd); return false; } - close(fd); - bool ret = true; int N = param_count(); @@ -554,7 +545,7 @@ bool ParameterTest::exportImportAll() param_reset_all(); // restore original params - fd = open(param_file_name, O_RDONLY); + int fd = open(param_file_name, O_RDONLY); if (fd < 0) { PX4_ERR("open '%s' failed (%i)", param_file_name, errno);