diff --git a/platforms/common/CMakeLists.txt b/platforms/common/CMakeLists.txt index 499d7408ca..0680758b5d 100644 --- a/platforms/common/CMakeLists.txt +++ b/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) diff --git a/platforms/common/i2c.cpp b/platforms/common/i2c.cpp new file mode 100644 index 0000000000..d71863d26b --- /dev/null +++ b/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 +#ifndef BOARD_DISABLE_I2C_SPI + +#include + +#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 */ diff --git a/platforms/common/i2c_spi_buses.cpp b/platforms/common/i2c_spi_buses.cpp new file mode 100644 index 0000000000..09c1d4b7be --- /dev/null +++ b/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 +#ifndef BOARD_DISABLE_I2C_SPI + +#ifndef MODULE_NAME +#define MODULE_NAME "SPI_I2C" +#endif + +#include +#include +#include + +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("", 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 */ diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index a8752a204f..1b4f5edcf8 100644 --- a/platforms/common/include/px4_platform_common/board_common.h +++ b/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 }; diff --git a/platforms/common/include/px4_platform_common/i2c.h b/platforms/common/include/px4_platform_common/i2c.h new file mode 100644 index 0000000000..9783a11240 --- /dev/null +++ b/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 + +#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}; +}; + diff --git a/platforms/common/include/px4_platform_common/i2c_spi_buses.h b/platforms/common/include/px4_platform_common/i2c_spi_buses.h new file mode 100644 index 0000000000..73c2fdedbe --- /dev/null +++ b/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 + +#include +#include +#include +#include +#include + +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 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(this)->RunImpl(); + + if (should_exit()) + { + exit_and_cleanup(); + } + } +private: + static I2CSPIInstance *_instances[MAX_NUM]; +}; + +template +I2CSPIInstance *I2CSPIDriver::_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; +}; diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp index 498ceea92c..00abbaaf05 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp +++ b/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); diff --git a/platforms/common/include/px4_platform_common/spi.h b/platforms/common/include/px4_platform_common/spi.h new file mode 100644 index 0000000000..d5331c6e5e --- /dev/null +++ b/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 +#include + +/* + * 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}; +}; + diff --git a/platforms/common/spi.cpp b/platforms/common/spi.cpp new file mode 100644 index 0000000000..1e6afe09a4 --- /dev/null +++ b/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 +#ifndef BOARD_DISABLE_I2C_SPI + +#include + +#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 */ diff --git a/platforms/nuttx/src/px4/stm/stm32_common/spi/spi.cpp b/platforms/nuttx/src/px4/stm/stm32_common/spi/spi.cpp index 81300a3cc2..a08a28889b 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/spi/spi.cpp +++ b/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];