diff --git a/platforms/common/i2c_spi_buses.cpp b/platforms/common/i2c_spi_buses.cpp index 7a4db76d92..816cba3a0f 100644 --- a/platforms/common/i2c_spi_buses.cpp +++ b/platforms/common/i2c_spi_buses.cpp @@ -44,6 +44,11 @@ #include #include +#include + +static List i2c_spi_module_instances; ///< list of currently running instances +static pthread_mutex_t i2c_spi_module_instances_mutex = PTHREAD_MUTEX_INITIALIZER; + const char *BusCLIArguments::parseDefaultArguments(int argc, char *argv[]) { if (getopt(argc, argv, "") == EOF) { @@ -178,15 +183,25 @@ int BusCLIArguments::getopt(int argc, char *argv[], const char *options) } -BusInstanceIterator::BusInstanceIterator(I2CSPIInstance **instances, int max_num_instances, +BusInstanceIterator::BusInstanceIterator(const char *module_name, const BusCLIArguments &cli_arguments, uint16_t devid_driver_index) - : _instances(instances), _max_num_instances(max_num_instances), - _bus_option(cli_arguments.bus_option), _type(cli_arguments.type), _i2c_address(cli_arguments.i2c_address), + : _module_name(module_name), _bus_option(cli_arguments.bus_option), _type(cli_arguments.type), + _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, cli_arguments.requested_bus), - _i2c_bus_iterator(i2cFilter(cli_arguments.bus_option), cli_arguments.requested_bus) + _i2c_bus_iterator(i2cFilter(cli_arguments.bus_option), cli_arguments.requested_bus), + _current_instance(i2c_spi_module_instances.end()) { + // We lock the module instance list as long as this object is alive, since we iterate over the list. + // Locking could be a bit more fine-grained, but the iterator is mostly only used sequentially, so not an issue. + pthread_mutex_lock(&i2c_spi_module_instances_mutex); + _current_instance = i2c_spi_module_instances.end(); +} + +BusInstanceIterator::~BusInstanceIterator() +{ + pthread_mutex_unlock(&i2c_spi_module_instances_mutex); } bool BusInstanceIterator::next() @@ -194,10 +209,23 @@ bool BusInstanceIterator::next() int bus = -1; if (busType() == BOARD_INVALID_BUS) { - while (++_current_instance < _max_num_instances && (_instances[_current_instance] == nullptr - || _type != _instances[_current_instance]->_type)) {} + if (_current_instance == i2c_spi_module_instances.end()) { // either not initialized, or the first instance was removed + _current_instance = i2c_spi_module_instances.begin(); - return _current_instance < _max_num_instances; + } else { + ++_current_instance; + } + + while (_current_instance != i2c_spi_module_instances.end()) { + if (strcmp((*_current_instance)->_module_name, _module_name) == 0 && + _type == (*_current_instance)->_type) { + return true; + } + + ++_current_instance; + } + + return false; } else if (busType() == BOARD_SPI_BUS) { if (_spi_bus_iterator.next()) { @@ -212,18 +240,18 @@ bool BusInstanceIterator::next() if (bus != -1) { // find matching runtime instance - _current_instance = -1; + bool is_i2c = busType() == BOARD_I2C_BUS; - for (int i = 0; i < _max_num_instances; ++i) { - if (!_instances[i]) { + for (_current_instance = i2c_spi_module_instances.begin(); _current_instance != i2c_spi_module_instances.end(); + ++_current_instance) { + if (strcmp((*_current_instance)->_module_name, _module_name) != 0) { continue; } - bool is_i2c = busType() == BOARD_I2C_BUS; - - if (_bus_option == _instances[i]->_bus_option && bus == _instances[i]->_bus && _type == _instances[i]->_type - && (!is_i2c || _i2c_address == _instances[i]->_i2c_address)) { - _current_instance = i; + if (_bus_option == (*_current_instance)->_bus_option && bus == (*_current_instance)->_bus && + _type == (*_current_instance)->_type && + (!is_i2c || _i2c_address == (*_current_instance)->_i2c_address)) { + break; } } @@ -233,31 +261,44 @@ bool BusInstanceIterator::next() return false; } -int BusInstanceIterator::nextFreeInstance() const +int BusInstanceIterator::runningInstancesCount() const { - for (int i = 0; i < _max_num_instances; ++i) { - if (_instances[i] == nullptr) { - return i; + int num_instances = 0; + + for (const auto &modules : i2c_spi_module_instances) { + if (strcmp(modules->_module_name, _module_name) == 0) { + ++num_instances; } } - return -1; + return num_instances; } I2CSPIInstance *BusInstanceIterator::instance() const { - if (_current_instance < 0 || _current_instance >= _max_num_instances) { + if (_current_instance == i2c_spi_module_instances.end()) { return nullptr; } - return _instances[_current_instance]; + return *_current_instance; } -void BusInstanceIterator::resetInstance() +void BusInstanceIterator::removeInstance() { - if (_current_instance >= 0 && _current_instance < _max_num_instances) { - _instances[_current_instance] = nullptr; + // find previous node + List::Iterator previous = i2c_spi_module_instances.begin(); + + while (previous != i2c_spi_module_instances.end() && (*previous)->getSibling() != *_current_instance) { + ++previous; } + + i2c_spi_module_instances.remove(*_current_instance); + _current_instance = previous; // previous can be i2c_spi_module_instances.end(), which means we removed the first item +} + +void BusInstanceIterator::addInstance(I2CSPIInstance *instance) +{ + i2c_spi_module_instances.add(instance); } board_bus_types BusInstanceIterator::busType() const @@ -373,8 +414,7 @@ static void initializer_trampoline(void *argument) } int I2CSPIDriverBase::module_start(const BusCLIArguments &cli, BusInstanceIterator &iterator, - void(*print_usage)(), - instantiate_method instantiate, I2CSPIInstance **instances) + void(*print_usage)(), instantiate_method instantiate) { if (iterator.configuredBusOption() == I2CSPIBusOption::All) { PX4_ERR("need to specify a bus type"); @@ -390,12 +430,6 @@ int I2CSPIDriverBase::module_start(const BusCLIArguments &cli, BusInstanceIterat continue; } - const int free_index = iterator.nextFreeInstance(); - - if (free_index < 0) { - PX4_ERR("Not enough instances"); - return -1; - } device::Device::DeviceId device_id{}; device_id.devid_s.bus = iterator.bus(); @@ -408,7 +442,8 @@ 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}; + const int runtime_instance = iterator.runningInstancesCount(); + I2CSPIDriverInitializing initializer_data{cli, iterator, 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); initializer.ScheduleNow(); @@ -420,19 +455,19 @@ int I2CSPIDriverBase::module_start(const BusCLIArguments &cli, BusInstanceIterat continue; } - instances[free_index] = instance; + iterator.addInstance(instance); started = true; // print some info that we are running switch (iterator.busType()) { case BOARD_I2C_BUS: PX4_INFO_RAW("%s #%i on I2C bus %d%s\n", - instance->ItemName(), free_index, iterator.bus(), iterator.external() ? " (external)" : ""); + instance->ItemName(), runtime_instance, iterator.bus(), iterator.external() ? " (external)" : ""); break; case BOARD_SPI_BUS: PX4_INFO_RAW("%s #%i on SPI bus %d (devid=0x%x)%s\n", - instance->ItemName(), free_index, iterator.bus(), PX4_SPI_DEV_ID(iterator.devid()), + instance->ItemName(), runtime_instance, iterator.bus(), PX4_SPI_DEV_ID(iterator.devid()), iterator.external() ? " (external)" : ""); break; @@ -454,7 +489,7 @@ int I2CSPIDriverBase::module_stop(BusInstanceIterator &iterator) I2CSPIDriverBase *instance = (I2CSPIDriverBase *)iterator.instance(); instance->request_stop_and_wait(); delete iterator.instance(); - iterator.resetInstance(); + iterator.removeInstance(); is_running = true; } } diff --git a/platforms/common/include/px4_platform_common/i2c_spi_buses.h b/platforms/common/include/px4_platform_common/i2c_spi_buses.h index 667062790c..9e12a60d42 100644 --- a/platforms/common/include/px4_platform_common/i2c_spi_buses.h +++ b/platforms/common/include/px4_platform_common/i2c_spi_buses.h @@ -38,6 +38,7 @@ #include +#include #include #include #include @@ -57,18 +58,19 @@ enum class I2CSPIBusOption : uint8_t { * @class I2CSPIInstance * I2C/SPI driver instance used by BusInstanceIterator to find running instances. */ -class I2CSPIInstance +class I2CSPIInstance : public ListNode { public: virtual ~I2CSPIInstance() = default; private: - I2CSPIInstance(I2CSPIBusOption bus_option, int bus, uint8_t i2c_address, uint16_t type) - : _bus_option(bus_option), _bus(bus), _type(type), _i2c_address(i2c_address) {} + 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) {} friend class BusInstanceIterator; friend class I2CSPIDriverBase; + const char *_module_name; const I2CSPIBusOption _bus_option; const int _bus; const int16_t _type; ///< device type (driver-specific) @@ -131,35 +133,35 @@ private: class BusInstanceIterator { public: - BusInstanceIterator(I2CSPIInstance **instances, int max_num_instances, - const BusCLIArguments &cli_arguments, uint16_t devid_driver_index); - ~BusInstanceIterator() = default; + BusInstanceIterator(const char *module_name, const BusCLIArguments &cli_arguments, uint16_t devid_driver_index); + ~BusInstanceIterator(); I2CSPIBusOption configuredBusOption() const { return _bus_option; } - int nextFreeInstance() const; + int runningInstancesCount() const; bool next(); I2CSPIInstance *instance() const; - void resetInstance(); + void removeInstance(); board_bus_types busType() const; int bus() const; uint32_t devid() const; spi_drdy_gpio_t DRDYGPIO() const; bool external() const; + void addInstance(I2CSPIInstance *instance); + static I2CBusIterator::FilterType i2cFilter(I2CSPIBusOption bus_option); static SPIBusIterator::FilterType spiFilter(I2CSPIBusOption bus_option); private: - I2CSPIInstance **_instances; - const int _max_num_instances; + const char *_module_name; const I2CSPIBusOption _bus_option; const uint16_t _type; const uint8_t _i2c_address; SPIBusIterator _spi_bus_iterator; I2CBusIterator _i2c_bus_iterator; - int _current_instance{-1}; + List::Iterator _current_instance; }; /** @@ -172,7 +174,7 @@ 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(bus_option, bus, i2c_address, type) {} + I2CSPIInstance(module_name, bus_option, bus, i2c_address, type) {} static int module_stop(BusInstanceIterator &iterator); static int module_status(BusInstanceIterator &iterator); @@ -198,7 +200,7 @@ protected: bool should_exit() const { return _task_should_exit.load(); } static int module_start(const BusCLIArguments &cli, BusInstanceIterator &iterator, void(*print_usage)(), - instantiate_method instantiate, I2CSPIInstance **instances); + instantiate_method instantiate); private: static void custom_method_trampoline(void *argument); @@ -213,19 +215,15 @@ private: * @class I2CSPIDriver * Base class for I2C/SPI driver modules */ -template +template class I2CSPIDriver : public I2CSPIDriverBase { public: - static constexpr int max_num_instances = MAX_NUM; - static int module_start(const BusCLIArguments &cli, BusInstanceIterator &iterator) { - return I2CSPIDriverBase::module_start(cli, iterator, &T::print_usage, &T::instantiate, _instances); + return I2CSPIDriverBase::module_start(cli, iterator, &T::print_usage, &T::instantiate); } - static I2CSPIInstance **instances() { return _instances; } - 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) @@ -242,10 +240,6 @@ protected: } } private: - static I2CSPIInstance *_instances[MAX_NUM]; }; -template -I2CSPIInstance *I2CSPIDriver::_instances[MAX_NUM] {}; -