From 6151ded96120c5b333035006a5c97cec13f341eb Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 23 Jul 2017 20:53:45 +1000 Subject: [PATCH] Low level driver for SPI for F7 --- Makefile | 2 +- .../STM32F7xx_HAL_Driver/Inc/stm32f7xx_ll_rcc.h | 1 + .../STM32F7xx_HAL_Driver/Inc/stm32f7xx_ll_spi.h | 6 +- .../STM32F7xx_HAL_Driver/Src/stm32f7xx_ll_rcc.c | 2 + make/mcu/STM32F7.mk | 8 +- src/main/drivers/bus_spi_hal.c | 2 +- src/main/drivers/bus_spi_ll.c | 335 +++++++++++++++++++++ src/main/main.c | 1 - src/main/target/NERO/target.h | 9 +- src/main/target/stm32f7xx_hal_conf.h | 1 + 10 files changed, 354 insertions(+), 13 deletions(-) create mode 100644 src/main/drivers/bus_spi_ll.c diff --git a/Makefile b/Makefile index c088d0c61..f371e3d52 100644 --- a/Makefile +++ b/Makefile @@ -284,7 +284,7 @@ $(TARGET_ELF): $(TARGET_OBJS) ifeq ($(DEBUG),GDB) $(OBJECT_DIR)/$(TARGET)/%.o: %.c $(V1) mkdir -p $(dir $@) - $(V1) echo "%% $(notdir $<)" "$(STDOUT)" && \ + $(V1) echo "%% (debug) $(notdir $<)" "$(STDOUT)" && \ $(CROSS_CC) -c -o $@ $(CFLAGS) $(CC_DEBUG_OPTIMISATION) $< else $(OBJECT_DIR)/$(TARGET)/%.o: %.c diff --git a/lib/main/STM32F7xx_HAL_Driver/Inc/stm32f7xx_ll_rcc.h b/lib/main/STM32F7xx_HAL_Driver/Inc/stm32f7xx_ll_rcc.h index b3fa6eb93..b9c9db88f 100644 --- a/lib/main/STM32F7xx_HAL_Driver/Inc/stm32f7xx_ll_rcc.h +++ b/lib/main/STM32F7xx_HAL_Driver/Inc/stm32f7xx_ll_rcc.h @@ -2757,6 +2757,7 @@ __STATIC_INLINE uint32_t LL_RCC_GetI2CClockSource(uint32_t I2Cx) */ __STATIC_INLINE uint32_t LL_RCC_GetLPTIMClockSource(uint32_t LPTIMx) { + (void)LPTIMx; return (uint32_t)(READ_BIT(RCC->DCKCFGR2, RCC_DCKCFGR2_LPTIM1SEL)); } diff --git a/lib/main/STM32F7xx_HAL_Driver/Inc/stm32f7xx_ll_spi.h b/lib/main/STM32F7xx_HAL_Driver/Inc/stm32f7xx_ll_spi.h index d0b4a2088..6144280fd 100644 --- a/lib/main/STM32F7xx_HAL_Driver/Inc/stm32f7xx_ll_spi.h +++ b/lib/main/STM32F7xx_HAL_Driver/Inc/stm32f7xx_ll_spi.h @@ -1373,6 +1373,9 @@ __STATIC_INLINE uint16_t LL_SPI_ReceiveData16(SPI_TypeDef *SPIx) * @param TxData Value between Min_Data=0x00 and Max_Data=0xFF * @retval None */ + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wstrict-aliasing" __STATIC_INLINE void LL_SPI_TransmitData8(SPI_TypeDef *SPIx, uint8_t TxData) { *((__IO uint8_t *)&SPIx->DR) = TxData; @@ -1389,6 +1392,7 @@ __STATIC_INLINE void LL_SPI_TransmitData16(SPI_TypeDef *SPIx, uint16_t TxData) { *((__IO uint16_t *)&SPIx->DR) = TxData; } +#pragma GCC diagnostic pop /** * @} @@ -1397,7 +1401,7 @@ __STATIC_INLINE void LL_SPI_TransmitData16(SPI_TypeDef *SPIx, uint16_t TxData) /** @defgroup SPI_LL_EF_Init Initialization and de-initialization functions * @{ */ - + ErrorStatus LL_SPI_DeInit(SPI_TypeDef *SPIx); ErrorStatus LL_SPI_Init(SPI_TypeDef *SPIx, LL_SPI_InitTypeDef *SPI_InitStruct); void LL_SPI_StructInit(LL_SPI_InitTypeDef *SPI_InitStruct); diff --git a/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_ll_rcc.c b/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_ll_rcc.c index c3e7b8366..67935a4c9 100644 --- a/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_ll_rcc.c +++ b/lib/main/STM32F7xx_HAL_Driver/Src/stm32f7xx_ll_rcc.c @@ -1168,6 +1168,7 @@ uint32_t LL_RCC_GetDSIClockFreq(uint32_t DSIxSource) */ uint32_t LL_RCC_GetLTDCClockFreq(uint32_t LTDCxSource) { + (void)LTDCxSource; uint32_t ltdc_frequency = LL_RCC_PERIPH_FREQUENCY_NO; /* Check parameter */ @@ -1192,6 +1193,7 @@ uint32_t LL_RCC_GetLTDCClockFreq(uint32_t LTDCxSource) */ uint32_t LL_RCC_GetSPDIFRXClockFreq(uint32_t SPDIFRXxSource) { + (void)SPDIFRXxSource; uint32_t spdifrx_frequency = LL_RCC_PERIPH_FREQUENCY_NO; /* Check parameter */ diff --git a/make/mcu/STM32F7.mk b/make/mcu/STM32F7.mk index e79092888..cb670684c 100644 --- a/make/mcu/STM32F7.mk +++ b/make/mcu/STM32F7.mk @@ -66,14 +66,11 @@ EXCLUDES = stm32f7xx_hal_can.c \ stm32f7xx_ll_i2c.c \ stm32f7xx_ll_lptim.c \ stm32f7xx_ll_pwr.c \ - stm32f7xx_ll_rcc.c \ stm32f7xx_ll_rng.c \ stm32f7xx_ll_rtc.c \ stm32f7xx_ll_sdmmc.c \ - stm32f7xx_ll_spi.c \ stm32f7xx_ll_tim.c \ - stm32f7xx_ll_usart.c \ - stm32f7xx_ll_utils.c + stm32f7xx_ll_usart.c STDPERIPH_SRC := $(filter-out ${EXCLUDES}, $(STDPERIPH_SRC)) @@ -116,7 +113,7 @@ endif #Flags ARCH_FLAGS = -mthumb -mcpu=cortex-m7 -mfloat-abi=hard -mfpu=fpv5-sp-d16 -fsingle-precision-constant -Wdouble-promotion -DEVICE_FLAGS = -DUSE_HAL_DRIVER +DEVICE_FLAGS = -DUSE_HAL_DRIVER -DUSE_FULL_LL_DRIVER ifeq ($(TARGET),$(filter $(TARGET),$(F7X5XG_TARGETS))) DEVICE_FLAGS += -DSTM32F745xx LD_SCRIPT = $(LINKER_DIR)/stm32_flash_f745.ld @@ -155,6 +152,7 @@ MCU_COMMON_SRC = \ drivers/gpio_stm32f7xx.c \ drivers/light_ws2811strip_hal.c \ drivers/bus_spi_hal.c \ + drivers/bus_spi_ll.c \ drivers/pwm_output_dshot_hal.c \ drivers/timer_hal.c \ drivers/timer_stm32f7xx.c \ diff --git a/src/main/drivers/bus_spi_hal.c b/src/main/drivers/bus_spi_hal.c index 6199ee03e..805c3bf2f 100644 --- a/src/main/drivers/bus_spi_hal.c +++ b/src/main/drivers/bus_spi_hal.c @@ -21,7 +21,7 @@ #include -#ifdef USE_SPI +#if defined(USE_SPI) && !defined(USE_LOWLEVEL_DRIVER) #include "common/utils.h" diff --git a/src/main/drivers/bus_spi_ll.c b/src/main/drivers/bus_spi_ll.c new file mode 100644 index 000000000..ec6c8b146 --- /dev/null +++ b/src/main/drivers/bus_spi_ll.c @@ -0,0 +1,335 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include +#include +#include + +#include + +#if defined(USE_SPI) && defined(USE_LOWLEVEL_DRIVER) + +#include "common/utils.h" + +#include "drivers/bus.h" +#include "drivers/bus_spi.h" +#include "drivers/bus_spi_impl.h" +#include "drivers/dma.h" +#include "drivers/io.h" +#include "drivers/nvic.h" +#include "drivers/rcc.h" + +spiDevice_t spiDevice[SPIDEV_COUNT]; + +#ifndef SPI2_SCK_PIN +#define SPI2_NSS_PIN PB12 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 +#endif + +#ifndef SPI3_SCK_PIN +#define SPI3_NSS_PIN PA15 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 +#endif + +#ifndef SPI4_SCK_PIN +#define SPI4_NSS_PIN PA15 +#define SPI4_SCK_PIN PB3 +#define SPI4_MISO_PIN PB4 +#define SPI4_MOSI_PIN PB5 +#endif + +#ifndef SPI1_NSS_PIN +#define SPI1_NSS_PIN NONE +#endif +#ifndef SPI2_NSS_PIN +#define SPI2_NSS_PIN NONE +#endif +#ifndef SPI3_NSS_PIN +#define SPI3_NSS_PIN NONE +#endif +#ifndef SPI4_NSS_PIN +#define SPI4_NSS_PIN NONE +#endif + +#define SPI_DEFAULT_TIMEOUT 10 + +SPIDevice spiDeviceByInstance(SPI_TypeDef *instance) +{ +#ifdef USE_SPI_DEVICE_1 + if (instance == SPI1) + return SPIDEV_1; +#endif + +#ifdef USE_SPI_DEVICE_2 + if (instance == SPI2) + return SPIDEV_2; +#endif + +#ifdef USE_SPI_DEVICE_3 + if (instance == SPI3) + return SPIDEV_3; +#endif + +#ifdef USE_SPI_DEVICE_4 + if (instance == SPI4) + return SPIDEV_4; +#endif + + return SPIINVALID; +} + +SPI_TypeDef *spiInstanceByDevice(SPIDevice device) +{ + if (device >= SPIDEV_COUNT) { + return NULL; + } + + return spiDevice[device].dev; +} + +void spiInitDevice(SPIDevice device) +{ + spiDevice_t *spi = &(spiDevice[device]); + +#ifdef SDCARD_SPI_INSTANCE + if (spi->dev == SDCARD_SPI_INSTANCE) { + spi->leadingEdge = true; + } +#endif +#ifdef RX_SPI_INSTANCE + if (spi->dev == RX_SPI_INSTANCE) { + spi->leadingEdge = true; + } +#endif + + // Enable SPI clock + RCC_ClockCmd(spi->rcc, ENABLE); + RCC_ResetCmd(spi->rcc, ENABLE); + + IOInit(IOGetByTag(spi->sck), OWNER_SPI_SCK, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(spi->miso), OWNER_SPI_MISO, RESOURCE_INDEX(device)); + IOInit(IOGetByTag(spi->mosi), OWNER_SPI_MOSI, RESOURCE_INDEX(device)); + + if (spi->leadingEdge == true) + IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_LOW, spi->sckAF); + else + IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->sckAF); + IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->misoAF); + IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->mosiAF); + + LL_SPI_Disable(spi->dev); + LL_SPI_DeInit(spi->dev); + + LL_SPI_InitTypeDef init = + { + .TransferDirection = SPI_DIRECTION_2LINES, + .Mode = SPI_MODE_MASTER, + .DataWidth = SPI_DATASIZE_8BIT, + .ClockPolarity = spi->leadingEdge ? SPI_POLARITY_LOW : SPI_POLARITY_HIGH, + .ClockPhase = spi->leadingEdge ? SPI_PHASE_1EDGE : SPI_PHASE_2EDGE, + .NSS = SPI_NSS_SOFT, + .BaudRate = SPI_BAUDRATEPRESCALER_8, + .BitOrder = SPI_FIRSTBIT_MSB, + .CRCPoly = 7, + .CRCCalculation = SPI_CRCCALCULATION_DISABLE, + }; + LL_SPI_SetRxFIFOThreshold(spi->dev, SPI_RXFIFO_THRESHOLD_QF); + + LL_SPI_Init(spi->dev, &init); + LL_SPI_Enable(spi->dev); +} + +bool spiInit(SPIDevice device) +{ + switch (device) { + case SPIINVALID: + return false; + case SPIDEV_1: +#if defined(USE_SPI_DEVICE_1) + spiInitDevice(device); + return true; +#else + break; +#endif + case SPIDEV_2: +#if defined(USE_SPI_DEVICE_2) + spiInitDevice(device); + return true; +#else + break; +#endif + case SPIDEV_3: +#if defined(USE_SPI_DEVICE_3) + spiInitDevice(device); + return true; +#else + break; +#endif + case SPIDEV_4: +#if defined(USE_SPI_DEVICE_4) + spiInitDevice(device); + return true; +#else + break; +#endif + } + return false; +} + +uint32_t spiTimeoutUserCallback(SPI_TypeDef *instance) +{ + SPIDevice device = spiDeviceByInstance(instance); + if (device == SPIINVALID) { + return -1; + } + spiDevice[device].errorCount++; + return spiDevice[device].errorCount; +} + +uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t txByte) +{ + uint16_t spiTimeout = 1000; + + while (!LL_SPI_IsActiveFlag_TXE(instance)) + if ((spiTimeout--) == 0) + return spiTimeoutUserCallback(instance); + + LL_SPI_TransmitData8(instance, txByte); + + spiTimeout = 1000; + while (!LL_SPI_IsActiveFlag_RXNE(instance)) + if ((spiTimeout--) == 0) + return spiTimeoutUserCallback(instance); + + return (uint8_t)LL_SPI_ReceiveData8(instance); +} + +/** + * Return true if the bus is currently in the middle of a transmission. + */ +bool spiIsBusBusy(SPI_TypeDef *instance) +{ + return LL_SPI_GetTxFIFOLevel(instance) != LL_SPI_TX_FIFO_EMPTY + || LL_SPI_IsActiveFlag_BSY(instance); +} + +bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len) +{ + uint16_t spiTimeout = 1000; + + uint8_t b; + instance->DR; + while (len--) { + b = txData ? *(txData++) : 0xFF; + while (!LL_SPI_IsActiveFlag_TXE(instance)) { + if ((spiTimeout--) == 0) + return spiTimeoutUserCallback(instance); + } + LL_SPI_TransmitData8(instance, b); + + spiTimeout = 1000; + while (!LL_SPI_IsActiveFlag_RXNE(instance)) { + if ((spiTimeout--) == 0) + return spiTimeoutUserCallback(instance); + } + b = LL_SPI_ReceiveData8(instance); + + if (rxData) + *(rxData++) = b; + } + + return true; +} + +bool spiBusTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int length) +{ + IOLo(bus->busdev_u.spi.csnPin); + spiTransfer(bus->busdev_u.spi.instance, txData, rxData, length); + IOHi(bus->busdev_u.spi.csnPin); + return true; +} + +void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor) +{ + LL_SPI_Disable(instance); +#define BR_CLEAR_MASK 0xFFC7 + + const uint16_t tempRegister = (instance->CR1 & BR_CLEAR_MASK); + instance->CR1 = (tempRegister | ((ffs(divisor | 0x100) - 2) << 3)); + + //LL_SPI_SetBaudRatePrescaler(instance, baudRatePrescaler); + LL_SPI_Enable(instance); +} + +uint16_t spiGetErrorCounter(SPI_TypeDef *instance) +{ + SPIDevice device = spiDeviceByInstance(instance); + if (device == SPIINVALID) { + return 0; + } + return spiDevice[device].errorCount; +} + +void spiResetErrorCounter(SPI_TypeDef *instance) +{ + SPIDevice device = spiDeviceByInstance(instance); + if (device != SPIINVALID) { + spiDevice[device].errorCount = 0; + } +} + +bool spiBusWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) +{ + IOLo(bus->busdev_u.spi.csnPin); + spiTransferByte(bus->busdev_u.spi.instance, reg); + spiTransferByte(bus->busdev_u.spi.instance, data); + IOHi(bus->busdev_u.spi.csnPin); + + return true; +} + +bool spiBusReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length) +{ + IOLo(bus->busdev_u.spi.csnPin); + spiTransferByte(bus->busdev_u.spi.instance, reg | 0x80); // read transaction + spiTransfer(bus->busdev_u.spi.instance, NULL, data, length); + IOHi(bus->busdev_u.spi.csnPin); + + return true; +} + +uint8_t spiBusReadRegister(const busDevice_t *bus, uint8_t reg) +{ + uint8_t data; + IOLo(bus->busdev_u.spi.csnPin); + spiTransferByte(bus->busdev_u.spi.instance, reg | 0x80); // read transaction + spiTransfer(bus->busdev_u.spi.instance, NULL, &data, 1); + IOHi(bus->busdev_u.spi.csnPin); + + return data; +} + +void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance) +{ + bus->busdev_u.spi.instance = instance; +} + +#endif diff --git a/src/main/main.c b/src/main/main.c index fdedb9ced..2c1c0fa5a 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -24,7 +24,6 @@ #include "scheduler/scheduler.h" - int main(void) { init(); diff --git a/src/main/target/NERO/target.h b/src/main/target/NERO/target.h index caab21c38..b35837718 100644 --- a/src/main/target/NERO/target.h +++ b/src/main/target/NERO/target.h @@ -62,10 +62,10 @@ // Divide to under 25MHz for normal operation: #define SDCARD_SPI_FULL_SPEED_CLOCK_DIVIDER 4 // 21MHz -#define SDCARD_DMA_CHANNEL_TX DMA1_Stream5 -#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF1_5 -#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 -#define SDCARD_DMA_CHANNEL DMA_CHANNEL_0 +//#define SDCARD_DMA_CHANNEL_TX DMA1_Stream5 +//#define SDCARD_DMA_CHANNEL_TX_COMPLETE_FLAG DMA_FLAG_TCIF1_5 +//#define SDCARD_DMA_CLK RCC_AHB1Periph_DMA1 +//#define SDCARD_DMA_CHANNEL DMA_CHANNEL_0 #define USE_I2C #define USE_I2C_DEVICE_1 @@ -98,6 +98,7 @@ #define ESCSERIAL_TIMER_TX_PIN PC7 // (Hardware=0, PPM) #define USE_SPI +#define USE_LOWLEVEL_DRIVER #define USE_SPI_DEVICE_1 #define SPI1_NSS_PIN PC4 diff --git a/src/main/target/stm32f7xx_hal_conf.h b/src/main/target/stm32f7xx_hal_conf.h index e8114a5a0..7a87a45fa 100644 --- a/src/main/target/stm32f7xx_hal_conf.h +++ b/src/main/target/stm32f7xx_hal_conf.h @@ -370,6 +370,7 @@ #ifdef HAL_SPI_MODULE_ENABLED #include "stm32f7xx_hal_spi.h" + #include "stm32f7xx_ll_spi.h" #endif /* HAL_SPI_MODULE_ENABLED */ #ifdef HAL_TIM_MODULE_ENABLED -- 2.11.4.GIT