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

@@ -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 <px4_arch/io_timer.h>
 
+#include <px4_platform_common/sem.hpp>
+
 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);

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 <board_pwm_out.h>
 #include <drivers/drv_hrt.h>
 
+#include <px4_platform_common/sem.hpp>
+
 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);
 	}

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 <cmath>
 #include "PCA9685.h"
 
+#include <px4_platform_common/sem.hpp>
+
 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;
 	}
-}
+}

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 <px4_platform_common/sem.hpp>
+
 #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);
 	}

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 <px4_platform_common/sem.hpp>
+
 pthread_mutex_t pwm_out_module_mutex = PTHREAD_MUTEX_INITIALIZER;
 static px4::atomic<PWMOut *> _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;
 }
 

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 <uORB/Subscription.hpp>
 #include <uORB/topics/parameter_update.h>
 
+#include <px4_platform_common/sem.hpp>
+
 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;
 }
 

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;
 		}

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 <px4_platform_common/sem.hpp>
+
 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;
 }
 

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);

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;
-}

src/lib/mixer_module/mixer_module.hpp

@@ -40,7 +40,6 @@
 #include <drivers/drv_pwm_output.h>
 #include <lib/mixer/MixerGroup.hpp>
 #include <lib/perf/perf_counter.h>
-#include <px4_platform_common/atomic.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <uORB/Publication.hpp>
@@ -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<int> 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