diff --git a/lib/main/STM32F7/Drivers/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal.c b/lib/main/STM32F7/Drivers/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal.c index 93a0e68527..48e13cd1f5 100644 --- a/lib/main/STM32F7/Drivers/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal.c +++ b/lib/main/STM32F7/Drivers/STM32F7xx_HAL_Driver/Src/stm32f7xx_hal.c @@ -158,10 +158,6 @@ HAL_StatusTypeDef HAL_Init(void) __HAL_FLASH_ART_ENABLE(); #endif /* ART_ACCLERATOR_ENABLE */ -#if (PREFETCH_ENABLE != 0U) - __HAL_FLASH_PREFETCH_BUFFER_ENABLE(); -#endif /* PREFETCH_ENABLE */ - /* Set Interrupt Group Priority */ HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITYGROUP_4); diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c index 055a52429c..dcfbd22485 100644 --- a/src/main/drivers/pwm_output_dshot_hal.c +++ b/src/main/drivers/pwm_output_dshot_hal.c @@ -69,10 +69,8 @@ FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value) { bufferSize = loadDmaBuffer(motor->dmaBuffer, 1, packet); motor->timer->timerDmaSources |= motor->timerDmaSource; - // @todo LL_DMA_SetDataLength - MODIFY_REG(motor->timerHardware->dmaRef->NDTR, DMA_SxNDT, bufferSize); - // @todo LL_DMA_EnableStream - SET_BIT(motor->timerHardware->dmaRef->CR, DMA_SxCR_EN); + LL_EX_DMA_SetDataLength(motor->timerHardware->dmaRef, bufferSize); + LL_EX_DMA_EnableStream(motor->timerHardware->dmaRef); } } @@ -83,10 +81,8 @@ FAST_CODE void pwmCompleteDshotMotorUpdate(uint8_t motorCount) for (int i = 0; i < dmaMotorTimerCount; i++) { #ifdef USE_DSHOT_DMAR if (useBurstDshot) { - // @todo LL_DMA_SetDataLength - MODIFY_REG(dmaMotorTimers[i].dmaBurstRef->NDTR, DMA_SxNDT, dmaMotorTimers[i].dmaBurstLength); - // @todo LL_DMA_EnableStream - SET_BIT(dmaMotorTimers[i].dmaBurstRef->CR, DMA_SxCR_EN); + LL_EX_DMA_SetDataLength(dmaMotorTimers[i].dmaBurstRef, dmaMotorTimers[i].dmaBurstLength); + LL_EX_DMA_EnableStream(dmaMotorTimers[i].dmaBurstRef); /* configure the DMA Burst Mode */ LL_TIM_ConfigDMABurst(dmaMotorTimers[i].timer, LL_TIM_DMABURST_BASEADDR_CCR1, LL_TIM_DMABURST_LENGTH_4TRANSFERS); @@ -98,7 +94,7 @@ FAST_CODE void pwmCompleteDshotMotorUpdate(uint8_t motorCount) /* Reset timer counter */ LL_TIM_SetCounter(dmaMotorTimers[i].timer, 0); /* Enable channel DMA requests */ - SET_BIT(dmaMotorTimers[i].timer->DIER, dmaMotorTimers[i].timerDmaSources); + LL_EX_TIM_EnableIT(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources); dmaMotorTimers[i].timerDmaSources = 0; } } @@ -111,15 +107,13 @@ static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) #ifdef USE_DSHOT_DMAR if (useBurstDshot) { - // @todo LL_DMA_DisableStream - CLEAR_BIT(motor->timerHardware->dmaTimUPRef->CR, DMA_SxCR_EN); + LL_EX_DMA_DisableStream(motor->timerHardware->dmaTimUPRef); LL_TIM_DisableDMAReq_UPDATE(motor->timerHardware->tim); } else #endif { - // @todo LL_DMA_DisableStream - CLEAR_BIT(motor->timerHardware->dmaRef->CR, DMA_SxCR_EN); - CLEAR_BIT(motor->timerHardware->tim->DIER, motor->timerDmaSource); + LL_EX_DMA_DisableStream(motor->timerHardware->dmaRef); + LL_EX_TIM_DisableIT(motor->timerHardware->tim, motor->timerDmaSource); } DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); @@ -155,7 +149,6 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m const uint8_t timerIndex = getTimerIndex(timer); const bool configureTimer = (timerIndex == dmaMotorTimerCount - 1); - // @note original DSHOT for F7 has pulldown instead of pullup IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLDOWN), timerHardware->alternateFunction); if (configureTimer) { @@ -195,12 +188,10 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m } LL_TIM_OC_Init(timer, channel, &oc_init); LL_TIM_OC_EnablePreload(timer, channel); - // @note original DSHOT for F7 LL_TIM_OC_DisableFast(timer, channel); if (output & TIMER_OUTPUT_N_CHANNEL) { - // @todo quick hack to get TIM_CCER_CCxNE from TIM_CCER_CCxE - LL_TIM_CC_EnableChannel(timer, 4 * channel); + LL_EX_TIM_CC_EnableNChannel(timer, channel); } else { LL_TIM_CC_EnableChannel(timer, channel); } @@ -208,7 +199,6 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m if (configureTimer) { LL_TIM_EnableAllOutputs(timer); LL_TIM_EnableARRPreload(timer); - // @note original DSHOT for F7 LL_TIM_EnableCounter(timer); } @@ -229,28 +219,7 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m motor->timer->timerDmaSources &= ~motor->timerDmaSource; } - DMA_TypeDef *dma; - uint32_t stream; - if (dmaRef == DMA1_Stream0) { dma = DMA1; stream = LL_DMA_STREAM_0; } - else if (dmaRef == DMA1_Stream1) { dma = DMA1; stream = LL_DMA_STREAM_1; } - else if (dmaRef == DMA1_Stream2) { dma = DMA1; stream = LL_DMA_STREAM_2; } - else if (dmaRef == DMA1_Stream3) { dma = DMA1; stream = LL_DMA_STREAM_3; } - else if (dmaRef == DMA1_Stream4) { dma = DMA1; stream = LL_DMA_STREAM_4; } - else if (dmaRef == DMA1_Stream5) { dma = DMA1; stream = LL_DMA_STREAM_5; } - else if (dmaRef == DMA1_Stream6) { dma = DMA1; stream = LL_DMA_STREAM_6; } - else if (dmaRef == DMA1_Stream7) { dma = DMA1; stream = LL_DMA_STREAM_7; } - else if (dmaRef == DMA2_Stream0) { dma = DMA2; stream = LL_DMA_STREAM_0; } - else if (dmaRef == DMA2_Stream1) { dma = DMA2; stream = LL_DMA_STREAM_1; } - else if (dmaRef == DMA2_Stream2) { dma = DMA2; stream = LL_DMA_STREAM_2; } - else if (dmaRef == DMA2_Stream3) { dma = DMA2; stream = LL_DMA_STREAM_3; } - else if (dmaRef == DMA2_Stream4) { dma = DMA2; stream = LL_DMA_STREAM_4; } - else if (dmaRef == DMA2_Stream5) { dma = DMA2; stream = LL_DMA_STREAM_5; } - else if (dmaRef == DMA2_Stream6) { dma = DMA2; stream = LL_DMA_STREAM_6; } - else if (dmaRef == DMA2_Stream7) { dma = DMA2; stream = LL_DMA_STREAM_7; } - - LL_DMA_DisableStream(dma, stream); - //CLEAR_BIT(dmaRef->CR, DMA_SxCR_EN); - LL_DMA_DeInit(dma, stream); + LL_EX_DMA_DeInit(dmaRef); LL_DMA_StructInit(&dma_init); #ifdef USE_DSHOT_DMAR @@ -260,19 +229,8 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m dma_init.Channel = timerHardware->dmaTimUPChannel; dma_init.MemoryOrM2MDstAddress = (uint32_t)motor->timer->dmaBurstBuffer; - dma_init.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH; - dma_init.FIFOMode = LL_DMA_FIFOMODE_ENABLE; dma_init.FIFOThreshold = LL_DMA_FIFOTHRESHOLD_FULL; - dma_init.MemBurst = LL_DMA_MBURST_SINGLE; - dma_init.PeriphBurst = LL_DMA_PBURST_SINGLE; dma_init.PeriphOrM2MSrcAddress = (uint32_t)&timerHardware->tim->DMAR; - dma_init.NbData = (pwmProtocolType == PWM_TYPE_PROSHOT1000) ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE; // XXX - dma_init.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; - dma_init.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT; - dma_init.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_WORD; - dma_init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_WORD; - dma_init.Mode = LL_DMA_MODE_NORMAL; - dma_init.Priority = LL_DMA_PRIORITY_HIGH; } else #endif { @@ -281,25 +239,24 @@ void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m dma_init.Channel = timerHardware->dmaChannel; dma_init.MemoryOrM2MDstAddress = (uint32_t)motor->dmaBuffer; - dma_init.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH; - // @note original DSHOT for F7 disabled FIFO for non-burst - dma_init.FIFOMode = LL_DMA_FIFOMODE_ENABLE; dma_init.FIFOThreshold = LL_DMA_FIFOTHRESHOLD_1_4; - dma_init.MemBurst = LL_DMA_MBURST_SINGLE; - dma_init.PeriphBurst = LL_DMA_PBURST_SINGLE; dma_init.PeriphOrM2MSrcAddress = (uint32_t)timerChCCR(timerHardware); - dma_init.NbData = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE; - dma_init.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; - dma_init.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT; - dma_init.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_WORD; - dma_init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_WORD; - dma_init.Mode = LL_DMA_MODE_NORMAL; - dma_init.Priority = LL_DMA_PRIORITY_HIGH; } - // XXX Consolidate common settings in the next refactor - LL_DMA_Init(dma, stream, &dma_init); - LL_DMA_EnableIT_TC(dma, stream); + dma_init.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH; + dma_init.FIFOMode = LL_DMA_FIFOMODE_ENABLE; + dma_init.MemBurst = LL_DMA_MBURST_SINGLE; + dma_init.PeriphBurst = LL_DMA_PBURST_SINGLE; + dma_init.NbData = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE; + dma_init.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT; + dma_init.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT; + dma_init.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_WORD; + dma_init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_WORD; + dma_init.Mode = LL_DMA_MODE_NORMAL; + dma_init.Priority = LL_DMA_PRIORITY_HIGH; + + LL_EX_DMA_Init(dmaRef, &dma_init); + LL_EX_DMA_EnableIT_TC(dmaRef); motor->configured = true; } diff --git a/src/main/drivers/stm32f7xx_ll_ex.h b/src/main/drivers/stm32f7xx_ll_ex.h new file mode 100644 index 0000000000..2f3943c7e8 --- /dev/null +++ b/src/main/drivers/stm32f7xx_ll_ex.h @@ -0,0 +1,104 @@ +/* + * This file is part of Betaflight. + * + * Betaflight 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. + * + * 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 Betaflight. If not, see . + */ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "stm32f7xx.h" +#include "common/utils.h" + +#define DMA_STREAM_MASK 0xFFU + +__STATIC_INLINE DMA_TypeDef *LL_EX_DMA_Stream_to_DMA(DMA_Stream_TypeDef *DMAx_Streamy) +{ + // clear stream address + return (DMA_TypeDef *) (((uint32_t) DMAx_Streamy) & ((uint32_t) ~DMA_STREAM_MASK)); +} + +__STATIC_INLINE uint32_t LL_EX_DMA_Stream_to_Stream(DMA_Stream_TypeDef *DMAx_Streamy) +{ + const size_t firstStreamOffset = sizeof(DMA_TypeDef); + const size_t streamSize = sizeof(DMA_Stream_TypeDef); + + STATIC_ASSERT(DMA1_Stream0_BASE - DMA1_BASE == firstStreamOffset, DMA_TypeDef_has_padding); + STATIC_ASSERT(DMA1_Stream1_BASE - DMA1_Stream0_BASE == streamSize, DMA_Stream_TypeDef_has_padding); + + return (((uint32_t) DMAx_Streamy & DMA_STREAM_MASK) - firstStreamOffset) / streamSize; +} + +#undef DMA_STREAM_MASK + +__STATIC_INLINE uint32_t LL_EX_DMA_Init(DMA_Stream_TypeDef *DMAx_Streamy, LL_DMA_InitTypeDef *DMA_InitStruct) { + DMA_TypeDef *DMA = LL_EX_DMA_Stream_to_DMA(DMAx_Streamy); + const uint32_t Stream = LL_EX_DMA_Stream_to_Stream(DMAx_Streamy); + + return LL_DMA_Init(DMA, Stream, DMA_InitStruct); +} + +__STATIC_INLINE uint32_t LL_EX_DMA_DeInit(DMA_Stream_TypeDef *DMAx_Streamy) { + DMA_TypeDef *DMA = LL_EX_DMA_Stream_to_DMA(DMAx_Streamy); + const uint32_t Stream = LL_EX_DMA_Stream_to_Stream(DMAx_Streamy); + + return LL_DMA_DeInit(DMA, Stream); +} + +__STATIC_INLINE void LL_EX_DMA_SetChannelSelection(DMA_Stream_TypeDef *DMAx_Streamy, uint32_t Channel) +{ + MODIFY_REG(DMAx_Streamy->CR, DMA_SxCR_CHSEL, Channel); +} + +__STATIC_INLINE void LL_EX_DMA_EnableStream(DMA_Stream_TypeDef *DMAx_Streamy) +{ + SET_BIT(DMAx_Streamy->CR, DMA_SxCR_EN); +} + +__STATIC_INLINE void LL_EX_DMA_DisableStream(DMA_Stream_TypeDef *DMAx_Streamy) +{ + CLEAR_BIT(DMAx_Streamy->CR, DMA_SxCR_EN); +} + +__STATIC_INLINE void LL_EX_DMA_EnableIT_TC(DMA_Stream_TypeDef *DMAx_Streamy) +{ + SET_BIT(DMAx_Streamy->CR, DMA_SxCR_TCIE); +} + +__STATIC_INLINE void LL_EX_DMA_SetDataLength(DMA_Stream_TypeDef* DMAx_Streamy, uint32_t NbData) +{ + MODIFY_REG(DMAx_Streamy->NDTR, DMA_SxNDT, NbData); +} + +__STATIC_INLINE void LL_EX_TIM_EnableIT(TIM_TypeDef *TIMx, uint32_t Sources) +{ + SET_BIT(TIMx->DIER, Sources); +} + +__STATIC_INLINE void LL_EX_TIM_DisableIT(TIM_TypeDef *TIMx, uint32_t Sources) +{ + CLEAR_BIT(TIMx->DIER, Sources); +} + +__STATIC_INLINE void LL_EX_TIM_CC_EnableNChannel(TIM_TypeDef *TIMx, uint32_t Channel) +{ + LL_TIM_CC_EnableChannel(TIMx, 4 * Channel); +} + +#ifdef __cplusplus +} +#endif diff --git a/src/main/platform.h b/src/main/platform.h index 671a35a0df..afe83877f3 100644 --- a/src/main/platform.h +++ b/src/main/platform.h @@ -34,6 +34,8 @@ #include "stm32f7xx_ll_rcc.h" #include "stm32f7xx_ll_bus.h" #include "stm32f7xx_ll_tim.h" +#include "stm32f7xx_ll_system.h" +#include "drivers/stm32f7xx_ll_ex.h" // Chip Unique ID on F7 #if defined(STM32F722xx) diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index e4798e6638..89d441d542 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -105,11 +105,11 @@ #ifdef STM32F4 // Data in RAM which is guaranteed to not be reset on hot reboot -#define PERSISTENT __attribute__ ((section(".persistent_data"), aligned(4))) +#define PERSISTENT __attribute__ ((section(".persistent_data"), aligned(4))) #endif #ifdef USE_SRAM2 -#define SRAM2 __attribute__ ((section(".sram2"), aligned(4))) +#define SRAM2 __attribute__ ((section(".sram2"), aligned(4))) #else #define SRAM2 #endif diff --git a/src/main/target/system_stm32f7xx.c b/src/main/target/system_stm32f7xx.c index 0154865c9f..bd22beadef 100644 --- a/src/main/target/system_stm32f7xx.c +++ b/src/main/target/system_stm32f7xx.c @@ -363,6 +363,10 @@ void SystemInit(void) SCB_EnableDCache(); } + if (PREFETCH_ENABLE) { + LL_FLASH_EnablePrefetch(); + } + /* Configure the system clock to specified frequency */ SystemClock_Config();