From 19e7ba63eee330da07da68e4da650cb273534d98 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=83=C2=BCng?= Date: Mon, 11 Sep 2017 15:17:32 +0200 Subject: [PATCH] param: use separate param save lock param save is an expensive operation that can take several 100ms. And previously it was possible that a param_get() caller was blocked on a save operation. If this happened due to a param change notification, important modules, such as sensors, could have been blocked for a longer period (and affecting the flight performance). With this patch, this situation is not possible anymore, because a param save now uses the reader lock and a separate file lock. However it is still possible that a param_set() needs to wait for a save operation, thus blocking for a longer time. param_set() thus needs to be avoided in important modules when the system is armed. In the case of mavlink it works, since it does not affect the flight if the mavlink receiver is blocked over a longer time. It is only problematic if a joystick is used as input or in offboard control. --- src/modules/systemlib/param/param.c | 26 ++++++++++++++++++-------- 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 28340107d3..6e3e87133d 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -167,10 +167,15 @@ static param_t param_find_internal(const char *name, bool notification); // the following implements an RW-lock using 2 semaphores (used as mutexes). It gives // priority to readers, meaning a writer could suffer from starvation, but in our use-case // we only have short periods of reads and writes are rare. -static px4_sem_t param_sem; ///< this protects against concurrent access to param_values and param save +static px4_sem_t param_sem; ///< this protects against concurrent access to param_values static int reader_lock_holders = 0; static px4_sem_t reader_lock_holders_lock; ///< this protects against concurrent access to reader_lock_holders +static px4_sem_t param_sem_save; ///< this protects against concurrent param saves (file or flash access). +///< we use a separate lock to allow concurrent param reads and saves. +///< a param_set could still be blocked by a param save, because it +///< needs to take the reader lock + /** lock the parameter store for read access */ static void param_lock_reader(void) @@ -228,6 +233,7 @@ void param_init(void) { px4_sem_init(¶m_sem, 0, 1); + px4_sem_init(¶m_sem_save, 0, 1); px4_sem_init(&reader_lock_holders_lock, 0, 1); } @@ -1014,7 +1020,10 @@ param_export(int fd, bool only_unsaved) PX4_ERR("px4_shutdown_lock() failed (%i)", shutdown_lock_ret); } - param_lock_writer(); + // take the file lock + do {} while (px4_sem_wait(¶m_sem_save) != 0); + + param_lock_reader(); bson_encoder_init_file(&encoder, fd); @@ -1099,18 +1108,19 @@ param_export(int fd, bool only_unsaved) result = 0; out: - param_unlock_writer(); - fsync(fd); // make sure the data is flushed before releasing the shutdown lock + if (result == 0) { + result = bson_encoder_fini(&encoder); // this will call fsync + } + + param_unlock_reader(); + + px4_sem_post(¶m_sem_save); if (shutdown_lock_ret == 0) { px4_shutdown_unlock(); } - if (result == 0) { - result = bson_encoder_fini(&encoder); - } - return result; }