i2c_spi_buses: add support for multiple instances of the same device on a bus

This also simplifies the API a bit, since we anyway have to change the
drivers to pass additional information (the bus device index).

The orientation flag is merged with the rotation.

platforms/common/i2c_spi_buses.cpp

@@ -49,6 +49,31 @@
 static List<I2CSPIInstance *> i2c_spi_module_instances; ///< list of currently running instances
 static pthread_mutex_t i2c_spi_module_instances_mutex = PTHREAD_MUTEX_INITIALIZER;
 
+
+I2CSPIDriverConfig::I2CSPIDriverConfig(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+				       const px4::wq_config_t &wq_config_)
+	: module_name(iterator.moduleName()),
+	  devid_driver_index(iterator.devidDriverIndex()),
+	  bus_option(iterator.configuredBusOption()),
+	  bus_type(iterator.busType()),
+	  bus(iterator.bus()),
+	  i2c_address(cli.i2c_address),
+	  bus_frequency(cli.bus_frequency),
+	  drdy_gpio(iterator.DRDYGPIO()),
+	  spi_mode(cli.spi_mode),
+	  spi_devid(iterator.devid()),
+	  bus_device_index(iterator.busDeviceIndex()),
+	  rotation(cli.rotation),
+	  quiet_start(cli.quiet_start),
+	  keep_running(cli.keep_running),
+	  custom1(cli.custom1),
+	  custom2(cli.custom2),
+	  custom_data(cli.custom_data),
+	  wq_config(wq_config_)
+{
+
+}
+
 const char *BusCLIArguments::parseDefaultArguments(int argc, char *argv[])
 {
 	if (getOpt(argc, argv, "") == EOF) {
@@ -223,7 +248,7 @@ bool BusCLIArguments::validateConfiguration()
 
 BusInstanceIterator::BusInstanceIterator(const char *module_name,
 		const BusCLIArguments &cli_arguments, uint16_t devid_driver_index)
-	: _module_name(module_name), _bus_option(cli_arguments.bus_option), _type(cli_arguments.type),
+	: _module_name(module_name), _bus_option(cli_arguments.bus_option), _devid_driver_index(devid_driver_index),
 	  _i2c_address(cli_arguments.i2c_address),
 	  _spi_bus_iterator(spiFilter(cli_arguments.bus_option),
 			    cli_arguments.bus_option == I2CSPIBusOption::SPIExternal ? cli_arguments.chipselect_index : devid_driver_index,
@@ -256,7 +281,7 @@ bool BusInstanceIterator::next()
 
 		while (_current_instance != i2c_spi_module_instances.end()) {
 			if (strcmp((*_current_instance)->_module_name, _module_name) == 0 &&
-			    _type == (*_current_instance)->_type) {
+			    _devid_driver_index == (*_current_instance)->_devid_driver_index) {
 				return true;
 			}
 
@@ -287,7 +312,8 @@ bool BusInstanceIterator::next()
 			}
 
 			if (_bus_option == (*_current_instance)->_bus_option && bus == (*_current_instance)->_bus &&
-			    _type == (*_current_instance)->_type &&
+			    _devid_driver_index == (*_current_instance)->_devid_driver_index &&
+			    busDeviceIndex() == (*_current_instance)->_bus_device_index &&
 			    (!is_i2c || _i2c_address == (*_current_instance)->_i2c_address)) {
 				break;
 			}
@@ -422,6 +448,15 @@ int BusInstanceIterator::externalBusIndex() const
 	}
 }
 
+int BusInstanceIterator::busDeviceIndex() const
+{
+	if (busType() == BOARD_SPI_BUS) {
+		return _spi_bus_iterator.busDeviceIndex();
+	}
+
+	return -1;
+}
+
 I2CBusIterator::FilterType BusInstanceIterator::i2cFilter(I2CSPIBusOption bus_option)
 {
 	switch (bus_option) {
@@ -451,8 +486,7 @@ SPIBusIterator::FilterType BusInstanceIterator::spiFilter(I2CSPIBusOption bus_op
 }
 
 struct I2CSPIDriverInitializing {
-	const BusCLIArguments &cli;
-	const BusInstanceIterator &iterator;
+	const I2CSPIDriverConfig &config;
 	I2CSPIDriverBase::instantiate_method instantiate;
 	int runtime_instance;
 	I2CSPIDriverBase *instance{nullptr};
@@ -461,7 +495,7 @@ struct I2CSPIDriverInitializing {
 static void initializer_trampoline(void *argument)
 {
 	I2CSPIDriverInitializing *data = (I2CSPIDriverInitializing *)argument;
-	data->instance = data->instantiate(data->cli, data->iterator, data->runtime_instance);
+	data->instance = data->instantiate(data->config, data->runtime_instance);
 }
 
 int I2CSPIDriverBase::module_start(const BusCLIArguments &cli, BusInstanceIterator &iterator,
@@ -493,10 +527,13 @@ int I2CSPIDriverBase::module_start(const BusCLIArguments &cli, BusInstanceIterat
 		case BOARD_INVALID_BUS: device_id.devid_s.bus_type = device::Device::DeviceBusType_UNKNOWN; break;
 		}
 
+
+		const px4::wq_config_t &wq_config = px4::device_bus_to_wq(device_id.devid);
+		I2CSPIDriverConfig driver_config{cli, iterator, wq_config};
 		const int runtime_instance = iterator.runningInstancesCount();
-		I2CSPIDriverInitializing initializer_data{cli, iterator, instantiate, runtime_instance};
+		I2CSPIDriverInitializing initializer_data{driver_config, instantiate, runtime_instance};
 		// initialize the object and bus on the work queue thread - this will also probe for the device
-		px4::WorkItemSingleShot initializer(px4::device_bus_to_wq(device_id.devid), initializer_trampoline, &initializer_data);
+		px4::WorkItemSingleShot initializer(wq_config, initializer_trampoline, &initializer_data);
 		initializer.ScheduleNow();
 		initializer.wait();
 		I2CSPIDriverBase *instance = initializer_data.instance;
@@ -556,6 +593,10 @@ int I2CSPIDriverBase::module_start(const BusCLIArguments &cli, BusInstanceIterat
 
 	if (!started && !cli.quiet_start) {
 		PX4_WARN("%s: no instance started (no device on bus?)", px4_get_taskname());
+
+		if (iterator.busType() == BOARD_I2C_BUS && cli.i2c_address == 0) {
+			PX4_ERR("%s: driver does not set i2c address", px4_get_taskname());
+		}
 	}
 
 	return started ? 0 : -1;

platforms/common/include/px4_platform_common/i2c_spi_buses.h

@@ -54,6 +54,37 @@ enum class I2CSPIBusOption : uint8_t {
 	SPIExternal,
 };
 
+class BusCLIArguments;
+class BusInstanceIterator;
+
+struct I2CSPIDriverConfig {
+	I2CSPIDriverConfig(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+			   const px4::wq_config_t &wq_config_);
+
+	const char *module_name;
+	uint16_t devid_driver_index;
+	I2CSPIBusOption bus_option;
+	board_bus_types bus_type;
+	int bus;
+	uint8_t i2c_address;
+	int bus_frequency;
+	spi_drdy_gpio_t drdy_gpio;
+	spi_mode_e spi_mode;
+	uint32_t spi_devid;
+	int bus_device_index;
+
+	Rotation rotation;
+
+	bool quiet_start;
+	bool keep_running;
+
+	int custom1;
+	int custom2;
+	void *custom_data;
+
+	const px4::wq_config_t &wq_config;
+};
+
 /**
  * @class I2CSPIInstance
  * I2C/SPI driver instance used by BusInstanceIterator to find running instances.
@@ -64,8 +95,10 @@ public:
 	virtual ~I2CSPIInstance() = default;
 
 private:
-	I2CSPIInstance(const char *module_name, I2CSPIBusOption bus_option, int bus, uint8_t i2c_address, uint16_t type)
-		: _module_name(module_name), _bus_option(bus_option), _bus(bus), _type(type), _i2c_address(i2c_address) {}
+	I2CSPIInstance(const I2CSPIDriverConfig &config)
+		: _module_name(config.module_name), _bus_option(config.bus_option), _bus(config.bus),
+		  _devid_driver_index(config.devid_driver_index), _bus_device_index(config.bus_device_index),
+		  _i2c_address(config.i2c_address) {}
 
 	friend class BusInstanceIterator;
 	friend class I2CSPIDriverBase;
@@ -73,7 +106,8 @@ private:
 	const char *_module_name;
 	const I2CSPIBusOption _bus_option;
 	const int _bus;
-	const int16_t _type; ///< device type (driver-specific)
+	const uint16_t _devid_driver_index;
+	const int8_t _bus_device_index;
 	const int8_t _i2c_address; ///< I2C address (optional)
 };
 
@@ -102,17 +136,15 @@ public:
 
 
 	I2CSPIBusOption bus_option{I2CSPIBusOption::All};
-	uint16_t type{0}; ///< device type (driver-specific)
 	int requested_bus{-1};
 	int chipselect_index{1};
-	Rotation rotation{ROTATION_NONE};
 	int bus_frequency{0};
 	spi_mode_e spi_mode{SPIDEV_MODE3};
-	uint8_t i2c_address{0}; ///< optional I2C address: a driver can set this to allow configuring the I2C address
+	uint8_t i2c_address{0}; ///< I2C address (a driver must set the default address)
 	bool quiet_start{false}; ///< do not print a message when startup fails
 	bool keep_running{false}; ///< keep driver running even if no device is detected on startup
 
-	uint8_t orientation{0}; ///< distance_sensor_s::ROTATION_*
+	Rotation rotation{ROTATION_NONE}; ///< sensor rotation (MAV_SENSOR_ROTATION_* or distance_sensor_s::ROTATION_*)
 
 	int custom1{0}; ///< driver-specific custom argument
 	int custom2{0}; ///< driver-specific custom argument
@@ -158,15 +190,20 @@ public:
 	spi_drdy_gpio_t DRDYGPIO() const;
 	bool external() const;
 	int externalBusIndex() const;
+	int busDeviceIndex() const;
 
 	void addInstance(I2CSPIInstance *instance);
 
 	static I2CBusIterator::FilterType i2cFilter(I2CSPIBusOption bus_option);
 	static SPIBusIterator::FilterType spiFilter(I2CSPIBusOption bus_option);
+
+	const char *moduleName() const { return _module_name; }
+	uint16_t devidDriverIndex() const { return _devid_driver_index; }
+
 private:
 	const char *_module_name;
 	const I2CSPIBusOption _bus_option;
-	const uint16_t _type;
+	const uint16_t _devid_driver_index;
 	const uint8_t _i2c_address;
 	SPIBusIterator _spi_bus_iterator;
 	I2CBusIterator _i2c_bus_iterator;
@@ -180,18 +217,16 @@ private:
 class I2CSPIDriverBase : public px4::ScheduledWorkItem, public I2CSPIInstance
 {
 public:
-	I2CSPIDriverBase(const char *module_name, const px4::wq_config_t &config, I2CSPIBusOption bus_option, int bus,
-			 uint8_t i2c_address, uint16_t type)
-		: ScheduledWorkItem(module_name, config),
-		  I2CSPIInstance(module_name, bus_option, bus, i2c_address, type) {}
+	I2CSPIDriverBase(const I2CSPIDriverConfig &config)
+		: ScheduledWorkItem(config.module_name, config.wq_config),
+		  I2CSPIInstance(config) {}
 
 	static int module_stop(BusInstanceIterator &iterator);
 	static int module_status(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);
+	using instantiate_method = I2CSPIDriverBase * (*)(const I2CSPIDriverConfig &config, int runtime_instance);
 protected:
 	virtual ~I2CSPIDriverBase() = default;
 
@@ -230,13 +265,12 @@ class I2CSPIDriver : public I2CSPIDriverBase
 public:
 	static int module_start(const BusCLIArguments &cli, BusInstanceIterator &iterator)
 	{
-		return I2CSPIDriverBase::module_start(cli, iterator, &T::print_usage, &T::instantiate);
+		return I2CSPIDriverBase::module_start(cli, iterator, &T::print_usage, InstantiateHelper<T>::m);
 	}
 
 protected:
-	I2CSPIDriver(const char *module_name, const px4::wq_config_t &config, I2CSPIBusOption bus_option, int bus,
-		     uint8_t i2c_address = 0, uint16_t type = 0)
-		: I2CSPIDriverBase(module_name, config, bus_option, bus, i2c_address, type) {}
+	I2CSPIDriver(const I2CSPIDriverConfig &config)
+		: I2CSPIDriverBase(config) {}
 
 	virtual ~I2CSPIDriver() = default;
 
@@ -251,4 +285,33 @@ protected:
 	}
 	// *INDENT-ON*
 private:
+
+	// SFINAE to use R::instantiate if it exists, and R::instantiate_default otherwise
+	template <typename R>
+	class InstantiateHelper
+	{
+		template <typename C>
+		static constexpr I2CSPIDriverBase::instantiate_method get(decltype(&C::instantiate)) { return &C::instantiate; }
+		template <typename C>
+		static constexpr I2CSPIDriverBase::instantiate_method get(...) { return &C::instantiate_default; }
+	public:
+		static constexpr I2CSPIDriverBase::instantiate_method m = get<R>(0);
+	};
+
+	static I2CSPIDriverBase *instantiate_default(const I2CSPIDriverConfig &config, int runtime_instance)
+	{
+		T *instance = new T(config);
+
+		if (!instance) {
+			PX4_ERR("alloc failed");
+			return nullptr;
+		}
+
+		if (OK != instance->init()) {
+			delete instance;
+			return nullptr;
+		}
+
+		return instance;
+	}
 };

platforms/common/include/px4_platform_common/spi.h

@@ -158,11 +158,13 @@ public:
 
 	bool external() const { return px4_spi_bus_external(bus()); }
 
+	int busDeviceIndex() const { return _bus_device_index; }
+
 private:
 	const FilterType _filter;
 	const uint16_t _devid_driver_index;
 	const int _bus;
-	int _index{-1};
-	int _external_bus_counter{0};
-	int _bus_device_index{0};
+	int _index{0};
+	int _external_bus_counter{1};
+	int _bus_device_index{-1};
 };

platforms/common/spi.cpp

@@ -109,8 +109,7 @@ bool px4_spi_bus_external(const px4_spi_bus_t &bus)
 
 bool SPIBusIterator::next()
 {
-	// we have at most 1 match per bus, so we can directly jump to the next bus
-	while (++_index < SPI_BUS_MAX_BUS_ITEMS && px4_spi_buses[_index].bus != -1) {
+	while (_index < SPI_BUS_MAX_BUS_ITEMS && px4_spi_buses[_index].bus != -1) {
 		const px4_spi_bus_t &bus_data = px4_spi_buses[_index];
 
 		if (!board_has_bus(BOARD_SPI_BUS, bus_data.bus)) {
@@ -127,7 +126,7 @@ bool SPIBusIterator::next()
 			if (!bus_data.is_external) {
 				if (_bus == bus_data.bus || _bus == -1) {
 					// find device id
-					for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
+					for (int i = _bus_device_index + 1; i < SPI_BUS_MAX_DEVICES; ++i) {
 						if (PX4_SPI_DEVICE_ID == PX4_SPIDEVID_TYPE(bus_data.devices[i].devid) &&
 						    _devid_driver_index == bus_data.devices[i].devtype_driver) {
 							_bus_device_index = i;
@@ -141,11 +140,10 @@ bool SPIBusIterator::next()
 
 		case FilterType::ExternalBus:
 			if (bus_data.is_external) {
-				++_external_bus_counter;
 				uint16_t cs_index = _devid_driver_index - 1;
 
 				if (_bus == _external_bus_counter && cs_index < SPI_BUS_MAX_DEVICES &&
-				    bus_data.devices[cs_index].cs_gpio != 0) {
+				    bus_data.devices[cs_index].cs_gpio != 0 && cs_index != _bus_device_index) {
 					// we know that bus_data.devices[cs_index].devtype_driver == cs_index
 					_bus_device_index = cs_index;
 					return true;
@@ -154,6 +152,13 @@ bool SPIBusIterator::next()
 
 			break;
 		}
+
+		if (bus_data.is_external) {
+			++_external_bus_counter;
+		}
+
+		++_index;
+		_bus_device_index = -1;
 	}
 
 	return false;

platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/spi_hw_description.h

@@ -73,16 +73,22 @@ static inline constexpr px4_spi_bus_t initSPIBus(SPI::Bus bus, const px4_spi_bus
 	for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
 		ret.devices[i] = devices.devices[i];
 
-		// check that the same device is configured only once (the chip-select code depends on that)
-		for (int j = i + 1; j < SPI_BUS_MAX_DEVICES; ++j) {
-			if (ret.devices[j].cs_gpio != 0) {
-				constexpr_assert(ret.devices[i].devid != ret.devices[j].devid, "Same device configured multiple times");
-			}
-		}
 
 		if (ret.devices[i].cs_gpio != 0) {
-			// A bus potentially requires locking if it is accessed by non-PX4 devices (i.e. NuttX drivers)
-			if (PX4_SPI_DEVICE_ID != PX4_SPIDEVID_TYPE(ret.devices[i].devid)) {
+			if (PX4_SPI_DEVICE_ID == PX4_SPIDEVID_TYPE(ret.devices[i].devid)) {
+				int same_devices_count = 0;
+
+				for (int j = 0; j < i; ++j) {
+					if (ret.devices[j].cs_gpio != 0) {
+						same_devices_count += (ret.devices[i].devid & 0xff) == (ret.devices[j].devid & 0xff);
+					}
+				}
+
+				// increment the 2. LSB byte to allow multiple devices of the same type
+				ret.devices[i].devid |= same_devices_count << 8;
+
+			} else {
+				// A bus potentially requires locking if it is accessed by non-PX4 devices (i.e. NuttX drivers)
 				ret.requires_locking = true;
 			}
 		}

platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/spi_hw_description.h

@@ -67,16 +67,22 @@ static inline constexpr px4_spi_bus_t initSPIBus(SPI::Bus bus, const px4_spi_bus
 	for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
 		ret.devices[i] = devices.devices[i];
 
-		// check that the same device is configured only once (the chip-select code depends on that)
-		for (int j = i + 1; j < SPI_BUS_MAX_DEVICES; ++j) {
-			if (ret.devices[j].cs_gpio != 0) {
-				constexpr_assert(ret.devices[i].devid != ret.devices[j].devid, "Same device configured multiple times");
-			}
-		}
 
 		if (ret.devices[i].cs_gpio != 0) {
-			// A bus potentially requires locking if it is accessed by non-PX4 devices (i.e. NuttX drivers)
-			if (PX4_SPI_DEVICE_ID != PX4_SPIDEVID_TYPE(ret.devices[i].devid)) {
+			if (PX4_SPI_DEVICE_ID == PX4_SPIDEVID_TYPE(ret.devices[i].devid)) {
+				int same_devices_count = 0;
+
+				for (int j = 0; j < i; ++j) {
+					if (ret.devices[j].cs_gpio != 0) {
+						same_devices_count += (ret.devices[i].devid & 0xff) == (ret.devices[j].devid & 0xff);
+					}
+				}
+
+				// increment the 2. LSB byte to allow multiple devices of the same type
+				ret.devices[i].devid |= same_devices_count << 8;
+
+			} else {
+				// A bus potentially requires locking if it is accessed by non-PX4 devices (i.e. NuttX drivers)
 				ret.requires_locking = true;
 			}
 		}

platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/spi_hw_description.h

@@ -69,16 +69,22 @@ static inline constexpr px4_spi_bus_t initSPIBus(SPI::Bus bus, const px4_spi_bus
 	for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) {
 		ret.devices[i] = devices.devices[i];
 
-		// check that the same device is configured only once (the chip-select code depends on that)
-		for (int j = i + 1; j < SPI_BUS_MAX_DEVICES; ++j) {
-			if (ret.devices[j].cs_gpio != 0) {
-				constexpr_assert(ret.devices[i].devid != ret.devices[j].devid, "Same device configured multiple times");
-			}
-		}
 
 		if (ret.devices[i].cs_gpio != 0) {
-			// A bus potentially requires locking if it is accessed by non-PX4 devices (i.e. NuttX drivers)
-			if (PX4_SPI_DEVICE_ID != PX4_SPIDEVID_TYPE(ret.devices[i].devid)) {
+			if (PX4_SPI_DEVICE_ID == PX4_SPIDEVID_TYPE(ret.devices[i].devid)) {
+				int same_devices_count = 0;
+
+				for (int j = 0; j < i; ++j) {
+					if (ret.devices[j].cs_gpio != 0) {
+						same_devices_count += (ret.devices[i].devid & 0xff) == (ret.devices[j].devid & 0xff);
+					}
+				}
+
+				// increment the 2. LSB byte to allow multiple devices of the same type
+				ret.devices[i].devid |= same_devices_count << 8;
+
+			} else {
+				// A bus potentially requires locking if it is accessed by non-PX4 devices (i.e. NuttX drivers)
 				ret.requires_locking = true;
 			}
 		}