From d82cd5a28ab5c68f6aaf72119a160ab779c7e768 Mon Sep 17 00:00:00 2001 From: Jay Blackman Date: Mon, 6 Jan 2025 04:51:45 +1100 Subject: [PATCH] Move STM (and clone) specific IO initialisation to platform (#14123) * Move stm (and clone) specific io initialisation to platform * Removed unused includes and implemented get by tag for SITL * Update sitl.c Corrected return value * Update io_impl.c Corrected licence file --- src/main/drivers/io.c | 44 ----------------- src/platform/APM32/mk/APM32F4.mk | 1 + src/platform/AT32/mk/AT32F4.mk | 1 + src/platform/SIMULATOR/sitl.c | 11 +++++ src/platform/STM32/mk/STM32_COMMON.mk | 5 +- src/platform/common/stm32/io_impl.c | 70 +++++++++++++++++++++++++++ 6 files changed, 87 insertions(+), 45 deletions(-) create mode 100644 src/platform/common/stm32/io_impl.c diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c index bd11be4de1..66c82aa2ba 100644 --- a/src/main/drivers/io.c +++ b/src/main/drivers/io.c @@ -129,14 +129,6 @@ bool IOIsFreeOrPreinit(IO_t io) return false; } -#if DEFIO_PORT_USED_COUNT > 0 -static const uint16_t ioDefUsedMask[DEFIO_PORT_USED_COUNT] = { DEFIO_PORT_USED_LIST }; -static const uint8_t ioDefUsedOffset[DEFIO_PORT_USED_COUNT] = { DEFIO_PORT_OFFSET_LIST }; -#else -// Avoid -Wpedantic warning -static const uint16_t ioDefUsedMask[1] = {0}; -static const uint8_t ioDefUsedOffset[1] = {0}; -#endif #if DEFIO_IO_USED_COUNT ioRec_t ioRecs[DEFIO_IO_USED_COUNT]; #else @@ -144,42 +136,6 @@ ioRec_t ioRecs[DEFIO_IO_USED_COUNT]; ioRec_t ioRecs[1]; #endif -// initialize all ioRec_t structures from ROM -// currently only bitmask is used, this may change in future -void IOInitGlobal(void) -{ - ioRec_t *ioRec = ioRecs; - - for (unsigned port = 0; port < ARRAYLEN(ioDefUsedMask); port++) { - for (unsigned pin = 0; pin < sizeof(ioDefUsedMask[0]) * 8; pin++) { - if (ioDefUsedMask[port] & (1 << pin)) { - ioRec->gpio = (GPIO_TypeDef *)(GPIOA_BASE + (port << 10)); // ports are 0x400 apart - ioRec->pin = 1 << pin; - ioRec++; - } - } - } -} - -IO_t IOGetByTag(ioTag_t tag) -{ - const int portIdx = DEFIO_TAG_GPIOID(tag); - const int pinIdx = DEFIO_TAG_PIN(tag); - - if (portIdx < 0 || portIdx >= DEFIO_PORT_USED_COUNT) { - return NULL; - } - // check if pin exists - if (!(ioDefUsedMask[portIdx] & (1 << pinIdx))) { - return NULL; - } - // count bits before this pin on single port - int offset = popcount(((1 << pinIdx) - 1) & ioDefUsedMask[portIdx]); - // and add port offset - offset += ioDefUsedOffset[portIdx]; - return ioRecs + offset; -} - void IOTraversePins(IOTraverseFuncPtr_t fnPtr) { for (int i = 0; i < DEFIO_IO_USED_COUNT; i++) { diff --git a/src/platform/APM32/mk/APM32F4.mk b/src/platform/APM32/mk/APM32F4.mk index c222e67ec0..9ea1025baf 100644 --- a/src/platform/APM32/mk/APM32F4.mk +++ b/src/platform/APM32/mk/APM32F4.mk @@ -147,6 +147,7 @@ endif MCU_COMMON_SRC = \ common/stm32/system.c \ + common/stm32/io_impl.c \ APM32/startup/system_apm32f4xx.c \ drivers/inverter.c \ drivers/dshot_bitbang_decode.c \ diff --git a/src/platform/AT32/mk/AT32F4.mk b/src/platform/AT32/mk/AT32F4.mk index 3053712fdd..cc5f9955f1 100644 --- a/src/platform/AT32/mk/AT32F4.mk +++ b/src/platform/AT32/mk/AT32F4.mk @@ -82,6 +82,7 @@ DEVICE_FLAGS += -DUSE_ATBSP_DRIVER -DAT32F43x -DHSE_VALUE=$(HSE_VALUE) -DAT32 MCU_COMMON_SRC = \ common/stm32/system.c \ + common/stm32/io_impl.c \ AT32/startup/at32f435_437_clock.c \ AT32/startup/system_at32f435_437.c \ AT32/adc_at32f43x.c \ diff --git a/src/platform/SIMULATOR/sitl.c b/src/platform/SIMULATOR/sitl.c index f53482f8f6..c96c268fed 100644 --- a/src/platform/SIMULATOR/sitl.c +++ b/src/platform/SIMULATOR/sitl.c @@ -757,3 +757,14 @@ void IOLo(IO_t io) { UNUSED(io); } + +void IOInitGlobal(void) +{ + // NOOP +} + +IO_t IOGetByTag(ioTag_t tag) +{ + UNUSED(tag); + return NULL; +} diff --git a/src/platform/STM32/mk/STM32_COMMON.mk b/src/platform/STM32/mk/STM32_COMMON.mk index 24950c9e5f..1e45a60ae2 100644 --- a/src/platform/STM32/mk/STM32_COMMON.mk +++ b/src/platform/STM32/mk/STM32_COMMON.mk @@ -4,9 +4,12 @@ INCLUDE_DIRS += $(PLATFORM_DIR)/common/stm32 MCU_COMMON_SRC += \ common/stm32/bus_spi_pinconfig.c \ common/stm32/bus_spi_hw.c \ + common/stm32/io_impl.c SIZE_OPTIMISED_SRC += \ common/stm32/bus_spi_pinconfig.c SPEED_OPTIMISED_SRC += \ - common/stm32/bus_spi_hw.c + common/stm32/bus_spi_hw.c \ + common/stm32/io_impl.c + diff --git a/src/platform/common/stm32/io_impl.c b/src/platform/common/stm32/io_impl.c new file mode 100644 index 0000000000..da511e06d7 --- /dev/null +++ b/src/platform/common/stm32/io_impl.c @@ -0,0 +1,70 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include "platform.h" + +#include "drivers/io.h" +#include "drivers/io_impl.h" + +#if DEFIO_PORT_USED_COUNT > 0 +static const uint16_t ioDefUsedMask[DEFIO_PORT_USED_COUNT] = { DEFIO_PORT_USED_LIST }; +static const uint8_t ioDefUsedOffset[DEFIO_PORT_USED_COUNT] = { DEFIO_PORT_OFFSET_LIST }; +#else +// Avoid -Wpedantic warning +static const uint16_t ioDefUsedMask[1] = {0}; +static const uint8_t ioDefUsedOffset[1] = {0}; +#endif + +// initialize all ioRec_t structures from ROM +// currently only bitmask is used, this may change in future +void IOInitGlobal(void) +{ + ioRec_t *ioRec = ioRecs; + + for (unsigned port = 0; port < ARRAYLEN(ioDefUsedMask); port++) { + for (unsigned pin = 0; pin < sizeof(ioDefUsedMask[0]) * 8; pin++) { + if (ioDefUsedMask[port] & (1 << pin)) { + ioRec->gpio = (GPIO_TypeDef *)(GPIOA_BASE + (port << 10)); // ports are 0x400 apart + ioRec->pin = 1 << pin; + ioRec++; + } + } + } +} + +IO_t IOGetByTag(ioTag_t tag) +{ + const int portIdx = DEFIO_TAG_GPIOID(tag); + const int pinIdx = DEFIO_TAG_PIN(tag); + + if (portIdx < 0 || portIdx >= DEFIO_PORT_USED_COUNT) { + return NULL; + } + // check if pin exists + if (!(ioDefUsedMask[portIdx] & (1 << pinIdx))) { + return NULL; + } + // count bits before this pin on single port + int offset = popcount(((1 << pinIdx) - 1) & ioDefUsedMask[portIdx]); + // and add port offset + offset += ioDefUsedOffset[portIdx]; + return ioRecs + offset; +}