i2c spi init + custom methods: use WorkItemSingleShot

Use it for custom methods as well (like reset), and run by default on the
work queue, since they typically access the bus.

platforms/common/i2c_spi_buses.cpp

@@ -40,6 +40,7 @@
 
 #include <lib/drivers/device/Device.hpp>
 #include <px4_platform_common/i2c_spi_buses.h>
+#include <px4_platform_common/px4_work_queue/WorkItemSingleShot.hpp>
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/getopt.h>
 
@@ -357,6 +358,19 @@ SPIBusIterator::FilterType BusInstanceIterator::spiFilter(I2CSPIBusOption bus_op
 	return SPIBusIterator::FilterType::InternalBus;
 }
 
+struct I2CSPIDriverInitializing {
+	const BusCLIArguments &cli;
+	const BusInstanceIterator &iterator;
+	I2CSPIDriverBase::instantiate_method instantiate;
+	int runtime_instance;
+	I2CSPIDriverBase *instance{nullptr};
+};
+
+static void initializer_trampoline(void *argument)
+{
+	I2CSPIDriverInitializing *data = (I2CSPIDriverInitializing *)argument;
+	data->instance = data->instantiate(data->cli, data->iterator, data->runtime_instance);
+}
 
 int I2CSPIDriverBase::module_start(const BusCLIArguments &cli, BusInstanceIterator &iterator,
 				   void(*print_usage)(),
@@ -394,11 +408,12 @@ int I2CSPIDriverBase::module_start(const BusCLIArguments &cli, BusInstanceIterat
 		case BOARD_INVALID_BUS: device_id.devid_s.bus_type = device::Device::DeviceBusType_UNKNOWN; break;
 		}
 
+		I2CSPIDriverInitializing initializer_data{cli, iterator, instantiate, free_index};
 		// initialize the object and bus on the work queue thread - this will also probe for the device
-		I2CSPIDriverInitializer initializer(px4::device_bus_to_wq(device_id.devid), cli, iterator, instantiate, free_index);
+		px4::WorkItemSingleShot initializer(px4::device_bus_to_wq(device_id.devid), initializer_trampoline, &initializer_data);
 		initializer.ScheduleNow();
 		initializer.wait();
-		I2CSPIDriverBase *instance = initializer.instance();
+		I2CSPIDriverBase *instance = initializer_data.instance;
 
 		if (!instance) {
 			PX4_DEBUG("instantiate failed (no device on bus %i (devid 0x%x)?)", iterator.bus(), iterator.devid());
@@ -472,12 +487,33 @@ int I2CSPIDriverBase::module_status(BusInstanceIterator &iterator)
 	return 0;
 }
 
-int I2CSPIDriverBase::module_custom_method(const BusCLIArguments &cli, BusInstanceIterator &iterator)
+struct custom_method_data_t {
+	I2CSPIDriverBase *instance;
+	const BusCLIArguments &cli;
+};
+
+void I2CSPIDriverBase::custom_method_trampoline(void *argument)
+{
+	custom_method_data_t *data = (custom_method_data_t *)argument;
+	data->instance->custom_method(data->cli);
+}
+
+int I2CSPIDriverBase::module_custom_method(const BusCLIArguments &cli, BusInstanceIterator &iterator,
+		bool run_on_work_queue)
 {
 	while (iterator.next()) {
 		if (iterator.instance()) {
 			I2CSPIDriverBase *instance = (I2CSPIDriverBase *)iterator.instance();
-			instance->custom_method(cli);
+
+			if (run_on_work_queue) {
+				custom_method_data_t data{instance, cli};
+				px4::WorkItemSingleShot runner(*instance, custom_method_trampoline, &data);
+				runner.ScheduleNow();
+				runner.wait();
+
+			} else {
+				instance->custom_method(cli);
+			}
 		}
 	}
 
@@ -506,28 +542,4 @@ void I2CSPIDriverBase::request_stop_and_wait()
 	}
 }
 
-I2CSPIDriverInitializer::I2CSPIDriverInitializer(const px4::wq_config_t &config, const BusCLIArguments &cli,
-		const BusInstanceIterator &iterator, instantiate_method instantiate, int runtime_instance)
-	: px4::WorkItem("<driver_init>", config),
-	  _cli(cli), _iterator(iterator), _runtime_instance(runtime_instance), _instantiate(instantiate)
-{
-	px4_sem_init(&_sem, 0, 0);
-}
-
-I2CSPIDriverInitializer::~I2CSPIDriverInitializer()
-{
-	px4_sem_destroy(&_sem);
-}
-
-void I2CSPIDriverInitializer::wait()
-{
-	while (px4_sem_wait(&_sem) != 0) {}
-}
-
-void I2CSPIDriverInitializer::Run()
-{
-	_instance = _instantiate(_cli, _iterator, _runtime_instance);
-	px4_sem_post(&_sem);
-}
-
 #endif /* BOARD_DISABLE_I2C_SPI */

platforms/common/include/px4_platform_common/i2c_spi_buses.h

@@ -176,7 +176,8 @@ public:
 
 	static int module_stop(BusInstanceIterator &iterator);
 	static int module_status(BusInstanceIterator &iterator);
-	static int module_custom_method(const BusCLIArguments &cli, BusInstanceIterator &iterator);
+	static int module_custom_method(const BusCLIArguments &cli, BusInstanceIterator &iterator,
+					bool run_on_work_queue = true);
 
 	using instantiate_method = I2CSPIDriverBase * (*)(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
 				   int runtime_instance);
@@ -200,6 +201,8 @@ protected:
 				instantiate_method instantiate, I2CSPIInstance **instances);
 
 private:
+	static void custom_method_trampoline(void *argument);
+
 	void request_stop_and_wait();
 
 	px4::atomic_bool _task_should_exit{false};
@@ -246,31 +249,3 @@ template<class T, int MAX_NUM>
 I2CSPIInstance *I2CSPIDriver<T, MAX_NUM>::_instances[MAX_NUM] {};
 
 
-/**
- * @class I2CSPIDriverInitializer
- * Helper class to initialize a driver: it ensures the object is initialzed on
- * the work queue where the driver is running. This is required so that all
- * bus accesses come from the same thread.
- */
-class I2CSPIDriverInitializer : public px4::WorkItem
-{
-public:
-	using instantiate_method = I2CSPIDriverBase::instantiate_method;
-
-	I2CSPIDriverInitializer(const px4::wq_config_t &config,
-				const BusCLIArguments &cli, const BusInstanceIterator &iterator,
-				instantiate_method instantiate, int runtime_instance);
-	~I2CSPIDriverInitializer();
-	void wait();
-
-	I2CSPIDriverBase *instance() const { return _instance; }
-protected:
-	void Run() override;
-private:
-	I2CSPIDriverBase *_instance{nullptr};
-	const BusCLIArguments &_cli;
-	const BusInstanceIterator &_iterator;
-	const int _runtime_instance;
-	instantiate_method _instantiate;
-	px4_sem_t _sem;
-};