From 5e05d98fe2beffc45d784e31cfb00b6e89f6a98a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 14 Apr 2022 13:52:11 -0400 Subject: [PATCH] output modules simplify locking for mixer reset and load - fixes the deadlock in px4io ioctl mixer reset - px4io Run() locks (CDev semaphore) - mixer load goes through px4io ioctl MIXERIOCRESET which calls MixingOutput::resetMixerThreadSafe() - MixingOutput::resetMixerThreadSafe() stores a Command::Type::resetMixer command in an atomic variable, schedules a work queue cycle, then sleep spins until the command is cleared - the execution of the cycle eventually calls back into PX4IO::updateOutputs(), which tries to lock (and waits forever) --- src/drivers/dshot/DShot.cpp | 16 ++-- src/drivers/linux_pwm_out/linux_pwm_out.cpp | 16 ++-- src/drivers/pca9685_pwm_out/PCA9685.cpp | 6 +- src/drivers/pca9685_pwm_out/main.cpp | 17 ++-- src/drivers/pwm_out/PWMOut.cpp | 16 ++-- src/drivers/pwm_out_sim/PWMSim.cpp | 17 ++-- src/drivers/px4io/px4io.cpp | 14 ++-- src/drivers/tap_esc/TAP_ESC.cpp | 16 ++-- src/drivers/uavcan/uavcan_main.cpp | 10 +-- src/lib/mixer_module/mixer_module.cpp | 90 --------------------- src/lib/mixer_module/mixer_module.hpp | 26 +----- 11 files changed, 72 insertions(+), 172 deletions(-) diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index da702025aa..46beadf08a 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +35,8 @@ #include +#include + char DShot::_telemetry_device[] {}; px4::atomic_bool DShot::_request_telemetry_init{false}; @@ -487,6 +489,8 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], void DShot::Run() { + SmartLock lock_guard(_lock); + if (should_exit()) { ScheduleClear(); _mixing_output.unregister(); @@ -640,21 +644,21 @@ void DShot::update_params() int DShot::ioctl(file *filp, int cmd, unsigned long arg) { + SmartLock lock_guard(_lock); + int ret = OK; PX4_DEBUG("dshot ioctl cmd: %d, arg: %ld", cmd, arg); - lock(); - switch (cmd) { case MIXERIOCRESET: - _mixing_output.resetMixerThreadSafe(); + _mixing_output.resetMixer(); break; case MIXERIOCLOADBUF: { const char *buf = (const char *)arg; unsigned buflen = strlen(buf); - ret = _mixing_output.loadMixerThreadSafe(buf, buflen); + ret = _mixing_output.loadMixer(buf, buflen); break; } @@ -664,8 +668,6 @@ int DShot::ioctl(file *filp, int cmd, unsigned long arg) break; } - unlock(); - // if nobody wants it, let CDev have it if (ret == -ENOTTY) { ret = CDev::ioctl(filp, cmd, arg); diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.cpp b/src/drivers/linux_pwm_out/linux_pwm_out.cpp index 538626e95a..63a8b1b5da 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.cpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2021-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,6 +36,8 @@ #include #include +#include + using namespace pwm_out; LinuxPWMOut::LinuxPWMOut() : @@ -124,6 +126,8 @@ bool LinuxPWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS void LinuxPWMOut::Run() { + SmartLock lock_guard(_lock); + if (should_exit()) { ScheduleClear(); _mixing_output.unregister(); @@ -301,21 +305,21 @@ void LinuxPWMOut::update_params() int LinuxPWMOut::ioctl(device::file_t *filp, int cmd, unsigned long arg) { + SmartLock lock_guard(_lock); + int ret = OK; PX4_DEBUG("ioctl cmd: %d, arg: %ld", cmd, arg); - lock(); - switch (cmd) { case MIXERIOCRESET: - _mixing_output.resetMixerThreadSafe(); + _mixing_output.resetMixer(); break; case MIXERIOCLOADBUF: { const char *buf = (const char *)arg; unsigned buflen = strlen(buf); - ret = _mixing_output.loadMixerThreadSafe(buf, buflen); + ret = _mixing_output.loadMixer(buf, buflen); update_params(); break; } @@ -325,8 +329,6 @@ int LinuxPWMOut::ioctl(device::file_t *filp, int cmd, unsigned long arg) break; } - unlock(); - if (ret == -ENOTTY) { ret = CDev::ioctl(filp, cmd, arg); } diff --git a/src/drivers/pca9685_pwm_out/PCA9685.cpp b/src/drivers/pca9685_pwm_out/PCA9685.cpp index a8e95b0f1d..c2a1af4b16 100644 --- a/src/drivers/pca9685_pwm_out/PCA9685.cpp +++ b/src/drivers/pca9685_pwm_out/PCA9685.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +35,8 @@ #include #include "PCA9685.h" +#include + using namespace drv_pca9685_pwm; PCA9685::PCA9685(int bus, int addr): @@ -255,4 +257,4 @@ void PCA9685::triggerRestart() PX4_DEBUG("triggerRestart: i2c::transfer returned %d", ret); return; } -} \ No newline at end of file +} diff --git a/src/drivers/pca9685_pwm_out/main.cpp b/src/drivers/pca9685_pwm_out/main.cpp index 25bcf86e25..5e8300a878 100644 --- a/src/drivers/pca9685_pwm_out/main.cpp +++ b/src/drivers/pca9685_pwm_out/main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -49,6 +49,8 @@ #include "PCA9685.h" +#include + #define PCA9685_DEFAULT_IICBUS 1 #define PCA9685_DEFAULT_ADDRESS (0x40) @@ -374,6 +376,8 @@ bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned void PCA9685Wrapper::Run() { + SmartLock lock_guard(_lock); + if (should_exit()) { ScheduleClear(); _mixing_output.unregister(); @@ -464,21 +468,20 @@ void PCA9685Wrapper::Run() int PCA9685Wrapper::ioctl(cdev::file_t *filep, int cmd, unsigned long arg) { - int ret = OK; + SmartLock lock_guard(_lock); - lock(); + int ret = OK; switch (cmd) { case MIXERIOCRESET: - _mixing_output.resetMixerThreadSafe(); + _mixing_output.resetMixer(); break; case MIXERIOCLOADBUF: { const char *buf = (const char *)arg; unsigned buflen = strlen(buf); - - ret = _mixing_output.loadMixerThreadSafe(buf, buflen); + ret = _mixing_output.loadMixer(buf, buflen); break; } @@ -501,8 +504,6 @@ int PCA9685Wrapper::ioctl(cdev::file_t *filep, int cmd, unsigned long arg) break; } - unlock(); - if (ret == -ENOTTY) { ret = CDev::ioctl(filep, cmd, arg); } diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index 6b6da82b74..ecf7de378b 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,6 +33,8 @@ #include "PWMOut.hpp" +#include + pthread_mutex_t pwm_out_module_mutex = PTHREAD_MUTEX_INITIALIZER; static px4::atomic _objects[PWM_OUT_MAX_INSTANCES] {}; static px4::atomic_bool _require_arming[PWM_OUT_MAX_INSTANCES] {}; @@ -437,6 +439,8 @@ bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], void PWMOut::Run() { + SmartLock lock_guard(_lock); + if (should_exit()) { ScheduleClear(); _mixing_output.unregister(); @@ -688,6 +692,8 @@ void PWMOut::update_params() int PWMOut::ioctl(device::file_t *filp, int cmd, unsigned long arg) { + SmartLock lock_guard(_lock); + int ret = pwm_ioctl(filp, cmd, arg); /* if nobody wants it, let CDev have it */ @@ -704,8 +710,6 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) PX4_DEBUG("pwm_out%u: ioctl cmd: %d, arg: %ld", _instance, cmd, arg); - lock(); - switch (cmd) { case PWM_SERVO_ARM: update_pwm_out_state(true); @@ -912,14 +916,14 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) break; case MIXERIOCRESET: - _mixing_output.resetMixerThreadSafe(); + _mixing_output.resetMixer(); break; case MIXERIOCLOADBUF: { const char *buf = (const char *)arg; unsigned buflen = strlen(buf); - ret = _mixing_output.loadMixerThreadSafe(buf, buflen); + ret = _mixing_output.loadMixer(buf, buflen); update_params(); break; @@ -930,8 +934,6 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) break; } - unlock(); - return ret; } diff --git a/src/drivers/pwm_out_sim/PWMSim.cpp b/src/drivers/pwm_out_sim/PWMSim.cpp index 0cac5ad29f..fd87c08ab6 100644 --- a/src/drivers/pwm_out_sim/PWMSim.cpp +++ b/src/drivers/pwm_out_sim/PWMSim.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,6 +39,8 @@ #include #include +#include + PWMSim::PWMSim(bool hil_mode_enabled) : CDev(PWM_OUTPUT0_DEVICE_PATH), OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default) @@ -56,6 +58,8 @@ PWMSim::PWMSim(bool hil_mode_enabled) : void PWMSim::Run() { + SmartLock lock_guard(_lock); + if (should_exit()) { ScheduleClear(); _mixing_output.unregister(); @@ -120,9 +124,9 @@ PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigne int PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) { - int ret = OK; + SmartLock lock_guard(_lock); - lock(); + int ret = OK; switch (cmd) { case PWM_SERVO_ARM: @@ -231,24 +235,21 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) break; case MIXERIOCRESET: - _mixing_output.resetMixerThreadSafe(); + _mixing_output.resetMixer(); break; case MIXERIOCLOADBUF: { const char *buf = (const char *)arg; unsigned buflen = strlen(buf); - ret = _mixing_output.loadMixerThreadSafe(buf, buflen); + ret = _mixing_output.loadMixer(buf, buflen); break; } - default: ret = -ENOTTY; break; } - unlock(); - return ret; } diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 9ccdbc0d4e..ed139aca3b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -379,8 +379,6 @@ PX4IO::~PX4IO() bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { - SmartLock lock_guard(_lock); - if (!_test_fmu_fail) { /* output to the servos */ io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, num_outputs); @@ -391,6 +389,8 @@ bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], int PX4IO::init() { + SmartLock lock_guard(_lock); + /* do regular cdev init */ int ret = CDev::init(); @@ -525,6 +525,8 @@ void PX4IO::updateFailsafe() void PX4IO::Run() { + SmartLock lock_guard(_lock); + if (should_exit()) { ScheduleClear(); _mixing_output.unregister(); @@ -546,8 +548,6 @@ void PX4IO::Run() _mixing_output.update(); } - SmartLock lock_guard(_lock); - if (hrt_elapsed_time(&_poll_last) >= 20_ms) { /* run at 50 */ _poll_last = hrt_absolute_time(); @@ -1698,7 +1698,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) case MIXERIOCRESET: PX4_DEBUG("MIXERIOCRESET"); - _mixing_output.resetMixerThreadSafe(); + _mixing_output.resetMixer(); break; case MIXERIOCLOADBUF: { @@ -1706,7 +1706,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) const char *buf = (const char *)arg; unsigned buflen = strlen(buf); - ret = _mixing_output.loadMixerThreadSafe(buf, buflen); + ret = _mixing_output.loadMixer(buf, buflen); break; } diff --git a/src/drivers/tap_esc/TAP_ESC.cpp b/src/drivers/tap_esc/TAP_ESC.cpp index b059ffd346..7598f97a4f 100644 --- a/src/drivers/tap_esc/TAP_ESC.cpp +++ b/src/drivers/tap_esc/TAP_ESC.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,6 +33,8 @@ #include "TAP_ESC.hpp" +#include + TAP_ESC::TAP_ESC(char const *const device, uint8_t channels_count): CDev(TAP_ESC_DEVICE_PATH), OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(device)), @@ -325,6 +327,8 @@ bool TAP_ESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], u void TAP_ESC::Run() { + SmartLock lock_guard(_lock); + if (should_exit()) { ScheduleClear(); _mixing_output.unregister(); @@ -409,19 +413,19 @@ void TAP_ESC::Run() int TAP_ESC::ioctl(device::file_t *filp, int cmd, unsigned long arg) { - int ret = OK; + SmartLock lock_guard(_lock); - lock(); + int ret = OK; switch (cmd) { case MIXERIOCRESET: - _mixing_output.resetMixerThreadSafe(); + _mixing_output.resetMixer(); break; case MIXERIOCLOADBUF: { const char *buf = (const char *)arg; unsigned buflen = strlen(buf); - ret = _mixing_output.loadMixerThreadSafe(buf, buflen); + ret = _mixing_output.loadMixer(buf, buflen); break; } @@ -431,8 +435,6 @@ int TAP_ESC::ioctl(device::file_t *filp, int cmd, unsigned long arg) break; } - unlock(); - return ret; } diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 42724c6212..4c035a01aa 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014-2017, 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -946,7 +946,7 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) { int ret = OK; - lock(); + pthread_mutex_lock(&_node_mutex); switch (cmd) { case PWM_SERVO_SET_ARM_OK: @@ -956,14 +956,14 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) break; case MIXERIOCRESET: - _mixing_interface_esc.mixingOutput().resetMixerThreadSafe(); + _mixing_interface_esc.mixingOutput().resetMixer(); break; case MIXERIOCLOADBUF: { const char *buf = (const char *)arg; unsigned buflen = strlen(buf); - ret = _mixing_interface_esc.mixingOutput().loadMixerThreadSafe(buf, buflen); + ret = _mixing_interface_esc.mixingOutput().loadMixer(buf, buflen); } break; @@ -972,7 +972,7 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) break; } - unlock(); + pthread_mutex_unlock(&_node_mutex); if (ret == -ENOTTY) { ret = CDev::ioctl(filp, cmd, arg); diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 553c8899a9..9ec0064392 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -331,7 +331,6 @@ bool MixingOutput::updateSubscriptionsStaticMixer(bool allow_wq_switch, bool lim return true; } - void MixingOutput::cleanupFunctions() { if (_subscription_callback) { @@ -645,7 +644,6 @@ bool MixingOutput::update() bool MixingOutput::updateStaticMixer() { if (!_mixers) { - handleCommands(); // do nothing until we have a valid mixer return false; } @@ -707,7 +705,6 @@ bool MixingOutput::updateStaticMixer() setAndPublishActuatorOutputs(num_motor_test, actuator_outputs); } - handleCommands(); return true; } } @@ -749,8 +746,6 @@ bool MixingOutput::updateStaticMixer() updateLatencyPerfCounter(actuator_outputs); } - handleCommands(); - return true; } @@ -1248,88 +1243,3 @@ int MixingOutput::loadMixer(const char *buf, unsigned len) _interface.mixerChanged(); return ret; } - -void MixingOutput::handleCommands() -{ - if ((Command::Type)_command.command.load() == Command::Type::None) { - return; - } - - switch ((Command::Type)_command.command.load()) { - case Command::Type::loadMixer: - _command.result = loadMixer(_command.mixer_buf, _command.mixer_buf_length); - break; - - case Command::Type::resetMixer: - resetMixer(); - _command.result = 0; - break; - - default: - break; - } - - // mark as done - _command.command.store((int)Command::Type::None); -} - -void MixingOutput::resetMixerThreadSafe() -{ - if (_use_dynamic_mixing) { - PX4_ERR("mixer reset unavailable, not using static mixers"); - return; - } - - if ((Command::Type)_command.command.load() != Command::Type::None) { - // Cannot happen, because we expect only one other thread to call this. - // But as a safety precaution we return here. - PX4_ERR("Command not None"); - return; - } - - lock(); - - _command.command.store((int)Command::Type::resetMixer); - - _interface.ScheduleNow(); - - unlock(); - - // wait until processed - while ((Command::Type)_command.command.load() != Command::Type::None) { - usleep(1000); - } - -} - -int MixingOutput::loadMixerThreadSafe(const char *buf, unsigned len) -{ - if (_use_dynamic_mixing) { - PX4_ERR("mixer load unavailable, not using static mixers"); - return -1; - } - - if ((Command::Type)_command.command.load() != Command::Type::None) { - // Cannot happen, because we expect only one other thread to call this. - // But as a safety precaution we return here. - PX4_ERR("Command not None"); - return -1; - } - - lock(); - - _command.mixer_buf = buf; - _command.mixer_buf_length = len; - _command.command.store((int)Command::Type::loadMixer); - - _interface.ScheduleNow(); - - unlock(); - - // wait until processed - while ((Command::Type)_command.command.load() != Command::Type::None) { - usleep(1000); - } - - return _command.result; -} diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 3cbe3bcb65..b717855577 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -40,7 +40,6 @@ #include #include #include -#include #include #include #include @@ -157,20 +156,14 @@ public: void setMaxTopicUpdateRate(unsigned max_topic_update_interval_us); /** - * Reset (unload) the complete mixer, called from another thread. - * This is thread-safe, as long as only one other thread at a time calls this. + * Reset (unload) the complete mixer. */ - void resetMixerThreadSafe(); - void resetMixer(); /** - * Load (append) a new mixer from a buffer, called from another thread. - * This is thread-safe, as long as only one other thread at a time calls this. + * Load (append) a new mixer from a buffer. * @return 0 on success, <0 error otherwise */ - int loadMixerThreadSafe(const char *buf, unsigned len); - int loadMixer(const char *buf, unsigned len); const actuator_armed_s &armed() const { return _armed; } @@ -230,8 +223,6 @@ private: bool updateStaticMixer(); bool updateDynamicMixer(); - void handleCommands(); - bool armNoThrottle() const { return (_armed.prearmed && !_armed.armed) || _armed.in_esc_calibration_mode; @@ -270,19 +261,6 @@ private: Betaflight = 1 }; - struct Command { - enum class Type : int { - None, - resetMixer, - loadMixer - }; - px4::atomic command{(int)Type::None}; - const char *mixer_buf; - unsigned mixer_buf_length; - int result; - }; - Command _command; ///< incoming commands (from another thread) - /** * Reorder outputs according to _param_mot_ordering * @param values values to reorder