From 8d76579c9e7ee1b8288ee746de77ed08b32d72c3 Mon Sep 17 00:00:00 2001 From: Sami Korhonen Date: Sat, 24 Dec 2016 13:28:13 +0200 Subject: [PATCH] F7 performance improvements --- src/main/drivers/bus_i2c_hal.c | 9 ++++++++- src/main/drivers/bus_spi_hal.c | 31 ++++++++++++++---------------- src/main/drivers/dma.h | 3 +++ src/main/drivers/serial_uart_hal.c | 2 ++ src/main/target/system_stm32f7xx.c | 7 +++++-- 5 files changed, 32 insertions(+), 20 deletions(-) diff --git a/src/main/drivers/bus_i2c_hal.c b/src/main/drivers/bus_i2c_hal.c index 040e8a603..eddfdd041 100644 --- a/src/main/drivers/bus_i2c_hal.c +++ b/src/main/drivers/bus_i2c_hal.c @@ -231,7 +231,14 @@ void i2cInit(I2CDevice device) i2cHandle[device].Handle.Instance = i2cHardwareMap[device].dev; /// TODO: HAL check if I2C timing is correct - i2cHandle[device].Handle.Init.Timing = 0x00B01B59; + + if (i2c->overClock) { + // 800khz Maximum speed tested on various boards without issues + i2cHandle[device].Handle.Init.Timing = 0x00500D1D; + } else { + //i2cHandle[device].Handle.Init.Timing = 0x00500B6A; + i2cHandle[device].Handle.Init.Timing = 0x00500C6F; + } //i2cHandle[device].Handle.Init.Timing = 0x00D00E28; /* (Rise time = 120ns, Fall time = 25ns) */ i2cHandle[device].Handle.Init.OwnAddress1 = 0x0; i2cHandle[device].Handle.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; diff --git a/src/main/drivers/bus_spi_hal.c b/src/main/drivers/bus_spi_hal.c index 7d86d279a..e27f8fa65 100644 --- a/src/main/drivers/bus_spi_hal.c +++ b/src/main/drivers/bus_spi_hal.c @@ -126,10 +126,8 @@ void SPI4_IRQHandler(void) void spiInitDevice(SPIDevice device) { - static SPI_InitTypeDef spiInit; spiDevice_t *spi = &(spiHardwareMap[device]); - #ifdef SDCARD_SPI_INSTANCE if (spi->dev == SDCARD_SPI_INSTANCE) { spi->leadingEdge = true; @@ -173,27 +171,25 @@ void spiInitDevice(SPIDevice device) // Init SPI hardware HAL_SPI_DeInit(&spiHardwareMap[device].hspi); - spiInit.Mode = SPI_MODE_MASTER; - spiInit.Direction = SPI_DIRECTION_2LINES; - spiInit.DataSize = SPI_DATASIZE_8BIT; - spiInit.NSS = SPI_NSS_SOFT; - spiInit.FirstBit = SPI_FIRSTBIT_MSB; - spiInit.CRCPolynomial = 7; - spiInit.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_256; - spiInit.CRCCalculation = SPI_CRCCALCULATION_DISABLE; - spiInit.TIMode = SPI_TIMODE_DISABLED; + spiHardwareMap[device].hspi.Init.Mode = SPI_MODE_MASTER; + spiHardwareMap[device].hspi.Init.Direction = SPI_DIRECTION_2LINES; + spiHardwareMap[device].hspi.Init.DataSize = SPI_DATASIZE_8BIT; + spiHardwareMap[device].hspi.Init.NSS = SPI_NSS_SOFT; + spiHardwareMap[device].hspi.Init.FirstBit = SPI_FIRSTBIT_MSB; + spiHardwareMap[device].hspi.Init.CRCPolynomial = 7; + spiHardwareMap[device].hspi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_256; + spiHardwareMap[device].hspi.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; + spiHardwareMap[device].hspi.Init.TIMode = SPI_TIMODE_DISABLED; if (spi->leadingEdge) { - spiInit.CLKPolarity = SPI_POLARITY_LOW; - spiInit.CLKPhase = SPI_PHASE_1EDGE; + spiHardwareMap[device].hspi.Init.CLKPolarity = SPI_POLARITY_LOW; + spiHardwareMap[device].hspi.Init.CLKPhase = SPI_PHASE_1EDGE; } else { - spiInit.CLKPolarity = SPI_POLARITY_HIGH; - spiInit.CLKPhase = SPI_PHASE_2EDGE; + spiHardwareMap[device].hspi.Init.CLKPolarity = SPI_POLARITY_HIGH; + spiHardwareMap[device].hspi.Init.CLKPhase = SPI_PHASE_2EDGE; } - spiHardwareMap[device].hspi.Init = spiInit; - if (HAL_SPI_Init(&spiHardwareMap[device].hspi) == HAL_OK) { if (spi->nss) { @@ -394,6 +390,7 @@ DMA_HandleTypeDef* spiSetDMATransmit(DMA_Stream_TypeDef *Stream, uint32_t Channe // DMA TX Interrupt dmaSetHandler(spiHardwareMap[device].dmaIrqHandler, dmaSPIIRQHandler, NVIC_BUILD_PRIORITY(3, 0), (uint32_t)device); + HAL_CLEANCACHE(pData,Size); // And Transmit HAL_SPI_Transmit_DMA(&spiHardwareMap[device].hspi, pData, Size); diff --git a/src/main/drivers/dma.h b/src/main/drivers/dma.h index ce40c758e..5a06b2c9f 100644 --- a/src/main/drivers/dma.h +++ b/src/main/drivers/dma.h @@ -40,6 +40,9 @@ typedef struct dmaChannelDescriptor_s { #if defined(STM32F4) || defined(STM32F7) +#define HAL_CLEANINVALIDATECACHE(addr, size) (SCB_CleanInvalidateDCache_by_Addr((uint32_t*)((uint32_t)addr & ~0x1f), ((uint32_t)(addr + size + 0x1f) & ~0x1f) - ((uint32_t)addr & ~0x1f))) +#define HAL_CLEANCACHE(addr, size) (SCB_CleanDCache_by_Addr((uint32_t*)((uint32_t)addr & ~0x1f), ((uint32_t)(addr + size + 0x1f) & ~0x1f) - ((uint32_t)addr & ~0x1f))) + uint32_t dmaFlag_IT_TCIF(const DMA_Stream_TypeDef *stream); typedef enum { diff --git a/src/main/drivers/serial_uart_hal.c b/src/main/drivers/serial_uart_hal.c index ecbd54a5e..60f717733 100644 --- a/src/main/drivers/serial_uart_hal.c +++ b/src/main/drivers/serial_uart_hal.c @@ -32,6 +32,7 @@ #include "io.h" #include "nvic.h" #include "inverter.h" +#include "dma.h" #include "serial.h" #include "serial_uart.h" @@ -265,6 +266,7 @@ void uartStartTxDMA(uartPort_t *s) s->port.txBufferTail = 0; } s->txDMAEmpty = false; + HAL_CLEANCACHE((uint8_t *)&s->port.txBuffer[fromwhere],size); HAL_UART_Transmit_DMA(&s->Handle, (uint8_t *)&s->port.txBuffer[fromwhere], size); } diff --git a/src/main/target/system_stm32f7xx.c b/src/main/target/system_stm32f7xx.c index 7ea5a23ff..fbc0b9a78 100644 --- a/src/main/target/system_stm32f7xx.c +++ b/src/main/target/system_stm32f7xx.c @@ -197,6 +197,7 @@ |RCC_PERIPHCLK_USART3|RCC_PERIPHCLK_USART6 |RCC_PERIPHCLK_UART4|RCC_PERIPHCLK_UART5 |RCC_PERIPHCLK_UART7|RCC_PERIPHCLK_UART8 + |RCC_PERIPHCLK_I2C1|RCC_PERIPHCLK_I2C3 |RCC_PERIPHCLK_I2C2|RCC_PERIPHCLK_I2C4; PeriphClkInitStruct.Usart1ClockSelection = RCC_USART1CLKSOURCE_PCLK2; PeriphClkInitStruct.Usart2ClockSelection = RCC_USART2CLKSOURCE_PCLK1; @@ -206,7 +207,9 @@ PeriphClkInitStruct.Usart6ClockSelection = RCC_USART6CLKSOURCE_PCLK2; PeriphClkInitStruct.Uart7ClockSelection = RCC_UART7CLKSOURCE_PCLK1; PeriphClkInitStruct.Uart8ClockSelection = RCC_UART8CLKSOURCE_PCLK1; + PeriphClkInitStruct.I2c1ClockSelection = RCC_I2C1CLKSOURCE_PCLK1; PeriphClkInitStruct.I2c2ClockSelection = RCC_I2C2CLKSOURCE_PCLK1; + PeriphClkInitStruct.I2c3ClockSelection = RCC_I2C3CLKSOURCE_PCLK1; PeriphClkInitStruct.I2c4ClockSelection = RCC_I2C4CLKSOURCE_PCLK1; ret = HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct); if (ret != HAL_OK) @@ -268,10 +271,10 @@ void SystemInit(void) #endif /* Enable I-Cache */ - //SCB_EnableICache(); + SCB_EnableICache(); /* Enable D-Cache */ - //SCB_EnableDCache(); + SCB_EnableDCache(); /* Configure the system clock to 216 MHz */ SystemClock_Config();