i2c+spi: add module base class and bus iterators

platforms/common/CMakeLists.txt

@@ -41,10 +41,13 @@ endif()
 
 add_library(px4_platform
 	board_identity.c
+	#i2c.cpp
+	#i2c_spi_buses.cpp
 	module.cpp
 	px4_getopt.c
 	px4_cli.cpp
 	shutdown.cpp
+	#spi.cpp
 	${SRCS}
 	)
 add_dependencies(px4_platform prebuild_targets)

platforms/common/i2c.cpp

@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2020 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <board_config.h>
+#ifndef BOARD_DISABLE_I2C_SPI
+
+#include <px4_platform_common/i2c.h>
+
+#ifndef BOARD_OVERRIDE_I2C_BUS_EXTERNAL
+bool px4_i2c_bus_external(const px4_i2c_bus_t &bus)
+{
+	return bus.is_external;
+}
+#endif
+
+bool I2CBusIterator::next()
+{
+	while (++_index < I2C_BUS_MAX_BUS_ITEMS && px4_i2c_buses[_index].bus != -1) {
+		const px4_i2c_bus_t &bus_data = px4_i2c_buses[_index];
+
+		if (!board_has_bus(BOARD_I2C_BUS, bus_data.bus)) {
+			continue;
+		}
+
+		switch (_filter) {
+		case FilterType::All:
+			if (_bus == bus_data.bus || _bus == -1) {
+				return true;
+			}
+
+			break;
+
+		case FilterType::InternalBus:
+			if (!px4_i2c_bus_external(bus_data)) {
+				if (_bus == bus_data.bus || _bus == -1) {
+					return true;
+				}
+			}
+
+			break;
+
+		case FilterType::ExternalBus:
+			if (px4_i2c_bus_external(bus_data)) {
+				++_external_bus_counter;
+
+				if (_bus == _external_bus_counter || _bus == -1) {
+					return true;
+				}
+			}
+
+			break;
+		}
+	}
+
+	return false;
+}
+
+#endif /* BOARD_DISABLE_I2C_SPI */

platforms/common/i2c_spi_buses.cpp

@@ -0,0 +1,393 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2020 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <board_config.h>
+#ifndef BOARD_DISABLE_I2C_SPI
+
+#ifndef MODULE_NAME
+#define MODULE_NAME "SPI_I2C"
+#endif
+
+#include <lib/drivers/device/Device.hpp>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <px4_platform_common/log.h>
+
+BusInstanceIterator::BusInstanceIterator(I2CSPIInstance **instances, int max_num_instances,
+		const BusCLIArguments &cli_arguments, uint16_t devid_driver_index)
+	: _instances(instances), _max_num_instances(max_num_instances),
+	  _bus_option(cli_arguments.bus_option),
+	  _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)
+{
+}
+
+bool BusInstanceIterator::next()
+{
+	int bus = -1;
+
+	if (busType() == BOARD_INVALID_BUS) {
+		while (++_current_instance < _max_num_instances && _instances[_current_instance] == nullptr) {}
+
+		return _current_instance < _max_num_instances;
+
+	} else if (busType() == BOARD_SPI_BUS) {
+		if (_spi_bus_iterator.next()) {
+			bus = _spi_bus_iterator.bus().bus;
+		}
+
+	} else {
+		if (_i2c_bus_iterator.next()) {
+			bus = _i2c_bus_iterator.bus().bus;
+		}
+	}
+
+	if (bus != -1) {
+		// find matching runtime instance
+		_current_instance = -1;
+
+		for (int i = 0; i < _max_num_instances; ++i) {
+			if (!_instances[i]) {
+				continue;
+			}
+
+			if (_bus_option == _instances[i]->_bus_option && bus == _instances[i]->_bus) {
+				_current_instance = i;
+			}
+		}
+
+		return true;
+	}
+
+	return false;
+}
+
+int BusInstanceIterator::nextFreeInstance() const
+{
+	for (int i = 0; i < _max_num_instances; ++i) {
+		if (_instances[i] == nullptr) {
+			return i;
+		}
+	}
+
+	return -1;
+}
+
+I2CSPIInstance *BusInstanceIterator::instance() const
+{
+	if (_current_instance < 0 || _current_instance >= _max_num_instances) {
+		return nullptr;
+	}
+
+	return _instances[_current_instance];
+}
+
+void BusInstanceIterator::resetInstance()
+{
+	if (_current_instance >= 0 && _current_instance < _max_num_instances) {
+		_instances[_current_instance] = nullptr;
+	}
+}
+
+board_bus_types BusInstanceIterator::busType() const
+{
+	switch (_bus_option) {
+	case I2CSPIBusOption::All:
+		return BOARD_INVALID_BUS;
+
+	case I2CSPIBusOption::I2CInternal:
+	case I2CSPIBusOption::I2CExternal:
+		return BOARD_I2C_BUS;
+
+	case I2CSPIBusOption::SPIInternal:
+	case I2CSPIBusOption::SPIExternal:
+		return BOARD_SPI_BUS;
+	}
+
+	return BOARD_INVALID_BUS;
+}
+
+int BusInstanceIterator::bus() const
+{
+	if (busType() == BOARD_INVALID_BUS) {
+		return -1;
+
+	} else if (busType() == BOARD_SPI_BUS) {
+		return _spi_bus_iterator.bus().bus;
+
+	} else {
+		return _i2c_bus_iterator.bus().bus;
+	}
+}
+
+uint32_t BusInstanceIterator::devid() const
+{
+	if (busType() == BOARD_INVALID_BUS) {
+		return 0;
+
+	} else if (busType() == BOARD_SPI_BUS) {
+		return _spi_bus_iterator.devid();
+
+	} else {
+		return 0;
+	}
+}
+
+uint32_t BusInstanceIterator::DRDYGPIO() const
+{
+	if (busType() == BOARD_INVALID_BUS) {
+		return 0;
+
+	} else if (busType() == BOARD_SPI_BUS) {
+		return _spi_bus_iterator.DRDYGPIO();
+
+	} else {
+		return 0;
+	}
+}
+
+bool BusInstanceIterator::external() const
+{
+	if (busType() == BOARD_INVALID_BUS) {
+		return false;
+
+	} else if (busType() == BOARD_SPI_BUS) {
+		return _spi_bus_iterator.external();
+
+	} else {
+		return _i2c_bus_iterator.external();
+	}
+}
+
+I2CBusIterator::FilterType BusInstanceIterator::i2cFilter(I2CSPIBusOption bus_option)
+{
+	switch (bus_option) {
+	case I2CSPIBusOption::All: return I2CBusIterator::FilterType::All;
+
+	case I2CSPIBusOption::I2CExternal: return I2CBusIterator::FilterType::ExternalBus;
+
+	case I2CSPIBusOption::I2CInternal: return I2CBusIterator::FilterType::InternalBus;
+
+	default: break;
+	}
+
+	return I2CBusIterator::FilterType::All;
+}
+
+SPIBusIterator::FilterType BusInstanceIterator::spiFilter(I2CSPIBusOption bus_option)
+{
+	switch (bus_option) {
+	case I2CSPIBusOption::SPIExternal: return SPIBusIterator::FilterType::ExternalBus;
+
+	case I2CSPIBusOption::SPIInternal: return SPIBusIterator::FilterType::InternalBus;
+
+	default: break;
+	}
+
+	return SPIBusIterator::FilterType::InternalBus;
+}
+
+
+int I2CSPIDriverBase::module_start(const BusCLIArguments &cli, BusInstanceIterator &iterator,
+				   void(*print_usage)(),
+				   instantiate_method instantiate, I2CSPIInstance **instances)
+{
+	if (iterator.configuredBusOption() == I2CSPIBusOption::All) {
+		PX4_ERR("need to specify a bus type");
+		print_usage();
+		return -1;
+	}
+
+	bool started = false;
+
+	while (iterator.next()) {
+		if (iterator.instance()) {
+			continue; // already running
+		}
+
+		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();
+
+		switch (iterator.busType()) {
+		case BOARD_I2C_BUS: device_id.devid_s.bus_type = device::Device::DeviceBusType_I2C; break;
+
+		case BOARD_SPI_BUS: device_id.devid_s.bus_type = device::Device::DeviceBusType_SPI; break;
+
+		case BOARD_INVALID_BUS: device_id.devid_s.bus_type = device::Device::DeviceBusType_UNKNOWN; break;
+		}
+
+		// 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);
+		initializer.ScheduleNow();
+		initializer.wait();
+		I2CSPIDriverBase *instance = initializer.instance();
+
+		if (!instance) {
+			PX4_DEBUG("instantiate failed (no device on bus %i (devid 0x%x)?)", iterator.bus(), iterator.devid());
+			continue;
+		}
+
+		instances[free_index] = 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)" : "");
+			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()),
+				     iterator.external() ? " (external)" : "");
+			break;
+
+		case BOARD_INVALID_BUS:
+			break;
+		}
+	}
+
+	return started ? 0 : -1;
+}
+
+
+int I2CSPIDriverBase::module_stop(BusInstanceIterator &iterator)
+{
+	bool is_running = false;
+
+	while (iterator.next()) {
+		if (iterator.instance()) {
+			I2CSPIDriverBase *instance = (I2CSPIDriverBase *)iterator.instance();
+			instance->request_stop_and_wait();
+			delete iterator.instance();
+			iterator.resetInstance();
+			is_running = true;
+		}
+	}
+
+	if (!is_running) {
+		PX4_ERR("Not running");
+		return -1;
+	}
+
+	return 0;
+}
+
+int I2CSPIDriverBase::module_status(BusInstanceIterator &iterator)
+{
+	bool is_running = false;
+
+	while (iterator.next()) {
+		if (iterator.instance()) {
+			I2CSPIDriverBase *instance = (I2CSPIDriverBase *)iterator.instance();
+			instance->print_status();
+			is_running = true;
+		}
+	}
+
+	if (!is_running) {
+		PX4_INFO("Not running");
+		return -1;
+	}
+
+	return 0;
+}
+
+int I2CSPIDriverBase::module_custom_method(const BusCLIArguments &cli, BusInstanceIterator &iterator)
+{
+	while (iterator.next()) {
+		if (iterator.instance()) {
+			I2CSPIDriverBase *instance = (I2CSPIDriverBase *)iterator.instance();
+			instance->custom_method(cli);
+		}
+	}
+
+	return 0;
+}
+
+void I2CSPIDriverBase::print_status()
+{
+	bool is_i2c_bus = _bus_option == I2CSPIBusOption::I2CExternal || _bus_option == I2CSPIBusOption::I2CInternal;
+	PX4_INFO("Running on %s Bus %i", is_i2c_bus ? "I2C" : "SPI", _bus);
+}
+
+void I2CSPIDriverBase::request_stop_and_wait()
+{
+	_task_should_exit.store(true);
+	ScheduleNow(); // wake up the task (in case it is not scheduled anymore or just to be faster)
+	unsigned int i = 0;
+
+	do {
+		px4_usleep(20000); // 20 ms
+		// wait at most 2 sec
+	} while (++i < 100 && !_task_exited.load());
+
+	if (i >= 100) {
+		PX4_ERR("Module did not respond to stop request");
+	}
+}
+
+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/board_common.h

@@ -124,6 +124,17 @@
 #    error BOARD_NUMBER_I2C_BUSES not supported
 #  endif
 #endif
+
+#ifdef BOARD_SPI_BUS_MAX_BUS_ITEMS
+#define SPI_BUS_MAX_BUS_ITEMS BOARD_SPI_BUS_MAX_BUS_ITEMS
+#else
+#define SPI_BUS_MAX_BUS_ITEMS 6
+#endif
+
+#ifndef BOARD_NUM_SPI_CFG_HW_VERSIONS
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 1
+#endif
+
 /* ADC defining tools
  * We want to normalize the V5 Sensing to V = (adc_dn) * ADC_V5_V_FULL_SCALE/(2 ^ ADC_BITS) * ADC_V5_SCALE)
  */
@@ -1060,6 +1071,7 @@ __EXPORT bool px4_spi_bus_external(int bus);
  ************************************************************************************/
 
 enum board_bus_types {
+	BOARD_INVALID_BUS = 0,
 	BOARD_SPI_BUS = 1,
 	BOARD_I2C_BUS = 2
 };

platforms/common/include/px4_platform_common/i2c.h

@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <board_config.h>
+
+#define I2C_BUS_MAX_BUS_ITEMS PX4_NUMBER_I2C_BUSES
+
+struct px4_i2c_bus_t {
+	int bus{-1}; ///< physical bus number (1, ...) (-1 means this is unused)
+	bool is_external; ///< static external configuration. Use px4_i2c_bus_external() to check if a bus is really external
+};
+
+__EXPORT extern const px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS]; ///< board-specific I2C bus configuration
+
+/**
+ * runtime-check if a board has a specific bus as external.
+ * This can be overridden by a board to add run-time checks.
+ */
+__EXPORT bool px4_i2c_bus_external(const px4_i2c_bus_t &bus);
+
+/**
+ * runtime-check if a board has a specific bus as external.
+ */
+static inline bool px4_i2c_bus_external(int bus)
+{
+	for (int i = 0; i < I2C_BUS_MAX_BUS_ITEMS; ++i) {
+		if (px4_i2c_buses[i].bus == bus) {
+			return px4_i2c_bus_external(px4_i2c_buses[i]);
+		}
+	}
+
+	return true;
+}
+
+
+/**
+ * @class I2CBusIterator
+ * Iterate over configured I2C buses by the board
+ */
+class I2CBusIterator
+{
+public:
+	enum class FilterType {
+		All, ///< specific or all buses
+		InternalBus, ///< specific or all internal buses
+		ExternalBus, ///< specific or all external buses
+	};
+
+	/**
+	 * @param bus specify bus: starts with 1, -1=all. Internal: arch-specific bus numbering is used,
+	 *             external: n-th external bus
+	 */
+	I2CBusIterator(FilterType filter, int bus = -1)
+		: _filter(filter), _bus(bus) {}
+
+	bool next();
+
+	const px4_i2c_bus_t &bus() const { return px4_i2c_buses[_index]; }
+
+	bool external() const { return px4_i2c_bus_external(bus()); }
+
+private:
+	const FilterType _filter;
+	const int _bus;
+	int _index{-1};
+	int _external_bus_counter{0};
+};
+

platforms/common/include/px4_platform_common/i2c_spi_buses.h

@@ -0,0 +1,229 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "i2c.h"
+#include "spi.h"
+
+#include <stdint.h>
+
+#include <lib/conversion/rotation.h>
+#include <px4_platform_common/atomic.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <px4_platform_common/sem.h>
+#include <board_config.h>
+
+enum class I2CSPIBusOption : uint8_t {
+	All = 0, ///< select all runnning instances
+	I2CInternal,
+	I2CExternal,
+	SPIInternal,
+	SPIExternal,
+};
+
+/**
+ * @class I2CSPIInstance
+ * I2C/SPI driver instance used by BusInstanceIterator to find running instances.
+ */
+class I2CSPIInstance
+{
+public:
+	I2CSPIInstance(I2CSPIBusOption bus_option, int bus)
+		: _bus_option(bus_option), _bus(bus) {}
+
+	virtual ~I2CSPIInstance() = default;
+
+private:
+	friend class BusInstanceIterator;
+	friend class I2CSPIDriverBase;
+
+	const I2CSPIBusOption _bus_option;
+	const int _bus;
+};
+
+struct BusCLIArguments {
+	I2CSPIBusOption bus_option{I2CSPIBusOption::All};
+	int requested_bus{-1};
+	int chipselect_index{1};
+	Rotation rotation{ROTATION_NONE};
+	int custom1; ///< driver-specific custom argument
+	int custom2; ///< driver-specific custom argument
+};
+
+/**
+ * @class BusInstanceIterator
+ * Iterate over running instances and/or configured I2C/SPI buses with given filter options.
+ */
+class BusInstanceIterator
+{
+public:
+	BusInstanceIterator(I2CSPIInstance **instances, int max_num_instances,
+			    const BusCLIArguments &cli_arguments, uint16_t devid_driver_index);
+	~BusInstanceIterator() = default;
+
+	I2CSPIBusOption configuredBusOption() const { return _bus_option; }
+
+	int nextFreeInstance() const;
+
+	bool next();
+
+	I2CSPIInstance *instance() const;
+	void resetInstance();
+	board_bus_types busType() const;
+	int bus() const;
+	uint32_t devid() const;
+	uint32_t DRDYGPIO() const;
+	bool external() const;
+
+	static I2CBusIterator::FilterType i2cFilter(I2CSPIBusOption bus_option);
+	static SPIBusIterator::FilterType spiFilter(I2CSPIBusOption bus_option);
+private:
+	I2CSPIInstance **_instances;
+	const int _max_num_instances;
+	const I2CSPIBusOption _bus_option;
+	SPIBusIterator _spi_bus_iterator;
+	I2CBusIterator _i2c_bus_iterator;
+	int _current_instance{-1};
+};
+
+/**
+ * @class I2CSPIDriverBase
+ * Base class for I2C/SPI driver modules (non-templated, used by I2CSPIDriver)
+ */
+class I2CSPIDriverBase : public px4::ScheduledWorkItem, public I2CSPIInstance
+{
+public:
+	I2CSPIDriverBase(const char *module_name, const px4::wq_config_t &config, I2CSPIBusOption bus_option, int bus)
+		: ScheduledWorkItem(module_name, config),
+		  I2CSPIInstance(bus_option, bus) {}
+
+	static int module_stop(BusInstanceIterator &iterator);
+	static int module_status(BusInstanceIterator &iterator);
+	static int module_custom_method(const BusCLIArguments &cli, BusInstanceIterator &iterator);
+
+	using instantiate_method = I2CSPIDriverBase * (*)(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+				   int runtime_instance);
+protected:
+	virtual ~I2CSPIDriverBase() = default;
+
+	virtual void print_status();
+
+	virtual void custom_method(const BusCLIArguments &cli) {}
+
+	/**
+	 * Exiting the module. A driver can override this, for example to unregister interrupt callbacks.
+	 * This will be called from the work queue.
+	 * A module overriding this, needs to call I2CSPIDriverBase::exit_and_cleanup() as the very last statement.
+	 */
+	virtual void exit_and_cleanup() { ScheduleClear(); _task_exited.store(true); }
+
+	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);
+
+private:
+	void request_stop_and_wait();
+
+	px4::atomic_bool _task_should_exit{false};
+	px4::atomic_bool _task_exited{false};
+};
+
+/**
+ * @class I2CSPIDriver
+ * Base class for I2C/SPI driver modules
+ */
+template<class T, int MAX_NUM = 3>
+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);
+	}
+
+	static I2CSPIInstance **instances() { return _instances; }
+
+protected:
+	I2CSPIDriver(const char *module_name, const px4::wq_config_t &config, I2CSPIBusOption bus_option, int bus)
+		: I2CSPIDriverBase(module_name, config, bus_option, bus) {}
+
+	virtual ~I2CSPIDriver() = default;
+
+	void Run() final {
+		static_cast<T *>(this)->RunImpl();
+
+		if (should_exit())
+		{
+			exit_and_cleanup();
+		}
+	}
+private:
+	static I2CSPIInstance *_instances[MAX_NUM];
+};
+
+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;
+};

platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp

@@ -75,6 +75,8 @@ public:
 	 */
 	bool ChangeWorkQeue(const wq_config_t &config) { return Init(config); }
 
+	const char *ItemName() const { return _item_name; }
+
 protected:
 
 	explicit WorkItem(const char *name, const wq_config_t &config);

platforms/common/include/px4_platform_common/spi.h

@@ -0,0 +1,165 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <stdint.h>
+#include <board_config.h>
+
+/*
+ * Helper macros to handle device ID's. They are used to match drivers against SPI buses and chip-select signals.
+ * They match with corresponding definitions in NuttX.
+ * 'type' is typically PX4_SPI_DEVICE_ID for PX4 drivers, and 'index' is used for the driver (DRV_*) or chip-select index.
+ */
+//#define PX4_SPIDEV_ID(type, index)  ((((type) & 0xffff) << 16) | ((index) & 0xffff))
+//#define PX4_SPI_DEVICE_ID         (1 << 12)
+//#define PX4_SPI_DEV_ID(devid)     ((devid) & 0xffff)
+#define PX4_SPIDEVID_TYPE(devid) (((uint32_t)(devid) >> 16) & 0xffff)
+
+#define SPI_BUS_MAX_DEVICES 5
+struct px4_spi_bus_device_t {
+	uint32_t cs_gpio; ///< chip-select GPIO (0 if this device is not used)
+	uint32_t drdy_gpio; ///< data ready GPIO (0 if not set)
+	uint32_t devid; ///< SPIDEV_ID(type,index). For PX4 devices on NuttX: index is the device type, and for external buses the CS index
+	uint16_t devtype_driver; ///< driver device type, e.g. DRV_IMU_DEVTYPE_ICM20689 (on NuttX: PX4_SPI_DEV_ID(devid) == devtype_driver)
+};
+
+struct px4_spi_bus_devices_t {
+	px4_spi_bus_device_t devices[SPI_BUS_MAX_DEVICES];
+};
+
+struct px4_spi_bus_t {
+	px4_spi_bus_device_t devices[SPI_BUS_MAX_DEVICES];
+	int bus{-1}; ///< physical bus number (1, ...) (-1 means this is unused)
+	uint32_t power_enable_gpio{0}; ///< GPIO (if non-zero) to control the power of the attached devices on this bus (0 means power is off)
+	bool is_external; ///< static external configuration. Use px4_spi_bus_external() to check if a bus is really external
+	bool requires_locking; ///< whether the bus should be locked during transfers (true if NuttX drivers access the bus)
+};
+
+
+struct px4_spi_bus_all_hw_t {
+	px4_spi_bus_t buses[SPI_BUS_MAX_BUS_ITEMS];
+	int board_hw_version; ///< 0=default, >0 for a specific revision (see board_get_hw_version)
+};
+
+#if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1
+/**
+ * initialze px4_spi_buses from px4_spi_buses_all_hw and the hardware version.
+ * Call this on early boot before anything else accesses px4_spi_buses (e.g. from stm32_spiinitialize).
+ */
+__EXPORT void px4_set_spi_buses_from_hw_version();
+
+__EXPORT extern const px4_spi_bus_all_hw_t
+px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS]; ///< board-specific SPI bus configuration all hw versions
+
+__EXPORT extern const px4_spi_bus_t *px4_spi_buses; ///< board-specific SPI bus configuration for current board revision
+#else
+static inline void px4_set_spi_buses_from_hw_version() {}
+__EXPORT extern const px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS]; ///< board-specific SPI bus configuration
+#endif
+
+
+/**
+ * Find a SPI bus given a device ID. Note: only internal buses are checked.
+ * @return the bus or -1
+ */
+__EXPORT int px4_find_spi_bus(uint32_t devid);
+
+/**
+ * Check if a bus requires locking during a SPI transfer (because it is potentially accessed by different threads)
+ */
+__EXPORT bool px4_spi_bus_requires_locking(int bus);
+
+/**
+ * runtime-check if a board has a specific bus as external.
+ * This can be overridden by a board to add run-time checks.
+ */
+__EXPORT bool px4_spi_bus_external(const px4_spi_bus_t &bus);
+
+/**
+ * runtime-check if a board has a specific bus as external.
+ */
+static inline bool px4_spi_bus_external(int bus)
+{
+	for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) {
+		if (px4_spi_buses[i].bus == bus) {
+			return px4_spi_bus_external(px4_spi_buses[i]);
+		}
+	}
+
+	return true;
+}
+
+
+/**
+ * @class SPIBusIterator
+ * Iterate over configured SPI buses by the board
+ */
+class SPIBusIterator
+{
+public:
+	enum class FilterType {
+		InternalBus, ///< specific or all internal buses
+		ExternalBus, ///< specific external bus + CS index
+	};
+
+	/**
+	 * Constructor
+	 * Note: only for devices of type PX4_SPI_DEVICE_ID
+	 * @param filter
+	 * @param devid_driver_index DRV_* or chip-select index starting from 1
+	 * @param bus starts with 1 (-1=all, but only for internal). Numbering for internal is arch-specific, for external
+	 *            it is the n-th external bus.
+	 */
+	SPIBusIterator(FilterType filter, uint16_t devid_driver_index, int bus = -1)
+		: _filter(filter), _devid_driver_index(devid_driver_index),
+		  _bus(filter == FilterType::ExternalBus && bus == -1 ? 1 : bus) {}
+
+	bool next();
+
+	const px4_spi_bus_t &bus() const { return px4_spi_buses[_index]; }
+	uint32_t DRDYGPIO() const { return px4_spi_buses[_index].devices[_bus_device_index].drdy_gpio; }
+
+	uint32_t devid() const { return px4_spi_buses[_index].devices[_bus_device_index].devid; }
+
+	bool external() const { return px4_spi_bus_external(bus()); }
+
+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};
+};
+

platforms/common/spi.cpp

@@ -0,0 +1,157 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2020 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <board_config.h>
+#ifndef BOARD_DISABLE_I2C_SPI
+
+#include <px4_platform_common/spi.h>
+
+#if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1
+void px4_set_spi_buses_from_hw_version()
+{
+	int hw_version = board_get_hw_version();
+
+	for (int i = 0; i < BOARD_NUM_SPI_CFG_HW_VERSIONS; ++i) {
+		if (!px4_spi_buses && px4_spi_buses_all_hw[i].board_hw_version == 0) {
+			px4_spi_buses = px4_spi_buses_all_hw[i].buses;
+		}
+
+		if (px4_spi_buses_all_hw[i].board_hw_version == hw_version) {
+			px4_spi_buses = px4_spi_buses_all_hw[i].buses;
+		}
+	}
+
+	if (!px4_spi_buses) { // fallback
+		px4_spi_buses = px4_spi_buses_all_hw[0].buses;
+	}
+}
+
+const px4_spi_bus_t *px4_spi_buses{};
+#endif
+
+int px4_find_spi_bus(uint32_t devid)
+{
+	for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) {
+		const px4_spi_bus_t &bus_data = px4_spi_buses[i];
+
+		if (bus_data.bus == -1) {
+			break;
+		}
+
+		if (px4_spi_bus_external(bus_data)) {
+			continue;
+		}
+
+		for (int j = 0; j < SPI_BUS_MAX_DEVICES; ++j) {
+			if (PX4_SPIDEVID_TYPE(devid) == PX4_SPIDEVID_TYPE(bus_data.devices[j].devid) &&
+			    PX4_SPI_DEV_ID(devid) == bus_data.devices[j].devtype_driver) {
+				return bus_data.bus;
+			}
+		}
+	}
+
+	return -1;
+}
+
+bool px4_spi_bus_requires_locking(int bus)
+{
+	for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) {
+		const px4_spi_bus_t &bus_data = px4_spi_buses[i];
+
+		if (bus_data.bus == bus) {
+			return bus_data.requires_locking;
+		}
+	}
+
+	return true;
+}
+
+
+bool px4_spi_bus_external(const px4_spi_bus_t &bus)
+{
+	return bus.is_external;
+}
+
+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) {
+		const px4_spi_bus_t &bus_data = px4_spi_buses[_index];
+
+		if (!board_has_bus(BOARD_SPI_BUS, bus_data.bus)) {
+			continue;
+		}
+
+		// Note: we use bus_data.is_external here instead of px4_spi_bus_external(),
+		// otherwise the chip-select matching does not work if a bus is configured as
+		// external/internal, but at runtime the other way around.
+		// (On boards where a bus can be internal/external at runtime, it should be
+		// configured as external.)
+		switch (_filter) {
+		case FilterType::InternalBus:
+			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) {
+						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;
+							return true;
+						}
+					}
+				}
+			}
+
+			break;
+
+		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) {
+					// we know that bus_data.devices[cs_index].devtype_driver == cs_index
+					_bus_device_index = cs_index;
+					return true;
+				}
+			}
+
+			break;
+		}
+	}
+
+	return false;
+}
+
+#endif /* BOARD_DISABLE_I2C_SPI */

platforms/nuttx/src/px4/stm/stm32_common/spi/spi.cpp

@@ -331,7 +331,7 @@ void board_control_spi_sensors_power(bool enable_power, int bus_mask)
 	const px4_spi_bus_t *buses = px4_spi_buses;
 	// this might be called very early on boot where we have not yet determined the hw version
 	// (we expect all versions to have the same power GPIO)
-#if BOARD_NUM_HW_VERSIONS > 1
+#if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1
 
 	if (!buses) {
 		buses = &px4_spi_buses_all_hw[0].buses[0];
@@ -361,7 +361,7 @@ void board_control_spi_sensors_power_configgpio()
 	const px4_spi_bus_t *buses = px4_spi_buses;
 	// this might be called very early on boot where we have yet not determined the hw version
 	// (we expect all versions to have the same power GPIO)
-#if BOARD_NUM_HW_VERSIONS > 1
+#if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1
 
 	if (!buses) {
 		buses = &px4_spi_buses_all_hw[0].buses[0];