diff --git a/lib/main/STM32G4/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc.c b/lib/main/STM32G4/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc.c
old mode 100755
new mode 100644
diff --git a/make/mcu/STM32G4.mk b/make/mcu/STM32G4.mk
index b7a5fbeb94..6317895cdb 100644
--- a/make/mcu/STM32G4.mk
+++ b/make/mcu/STM32G4.mk
@@ -40,6 +40,7 @@ EXCLUDES = \
stm32g4xx_hal_smartcard.c \
stm32g4xx_hal_smartcard_ex.c \
stm32g4xx_hal_smbus.c \
+ stm32g4xx_hal_spi.c \
stm32g4xx_hal_spi_ex.c \
stm32g4xx_hal_sram.c \
stm32g4xx_hal_timebase_tim_template.c \
@@ -65,7 +66,6 @@ EXCLUDES = \
stm32g4xx_ll_rcc.c \
stm32g4xx_ll_rng.c \
stm32g4xx_ll_rtc.c \
- stm32g4xx_ll_spi.c \
stm32g4xx_ll_ucpd.c \
stm32g4xx_ll_usart.c \
stm32g4xx_ll_utils.c
@@ -161,7 +161,7 @@ MCU_COMMON_SRC = \
drivers/bus_i2c_hal.c \
drivers/bus_i2c_hal_init.c \
drivers/bus_i2c_timing.c \
- drivers/bus_spi_hal.c \
+ drivers/bus_spi_ll.c \
drivers/dma_stm32g4xx.c \
drivers/dshot_bitbang.c \
drivers/dshot_bitbang_decode.c \
diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c b/src/main/drivers/accgyro/accgyro_spi_bmi270.c
index f77805c6fd..62f3a8c11f 100644
--- a/src/main/drivers/accgyro/accgyro_spi_bmi270.c
+++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.c
@@ -282,8 +282,8 @@ static bool bmi270AccRead(accDev_t *acc)
BUFFER_SIZE,
};
- uint8_t bmi270_rx_buf[BUFFER_SIZE];
- static const uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0};
+ STATIC_DMA_DATA_AUTO uint8_t bmi270_rx_buf[BUFFER_SIZE];
+ STATIC_DMA_DATA_AUTO uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0};
spiReadWriteBuf(&acc->gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response
@@ -308,8 +308,8 @@ static bool bmi270GyroReadRegister(gyroDev_t *gyro)
BUFFER_SIZE,
};
- uint8_t bmi270_rx_buf[BUFFER_SIZE];
- static const uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0};
+ STATIC_DMA_DATA_AUTO uint8_t bmi270_rx_buf[BUFFER_SIZE];
+ STATIC_DMA_DATA_AUTO uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0};
spiReadWriteBuf(&gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response
@@ -338,8 +338,8 @@ static bool bmi270GyroReadFifo(gyroDev_t *gyro)
};
bool dataRead = false;
- static const uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_FIFO_LENGTH_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0};
- uint8_t bmi270_rx_buf[BUFFER_SIZE];
+ STATIC_DMA_DATA_AUTO uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_FIFO_LENGTH_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+ STATIC_DMA_DATA_AUTO uint8_t bmi270_rx_buf[BUFFER_SIZE];
// Burst read the FIFO length followed by the next 6 bytes containing the gyro axis data for
// the first sample in the queue. It's possible for the FIFO to be empty so we need to check the
diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h
index 61146bbd97..598982d8d1 100644
--- a/src/main/drivers/bus.h
+++ b/src/main/drivers/bus.h
@@ -64,8 +64,6 @@ typedef struct busDevice_s {
uint8_t deviceCount;
dmaChannelDescriptor_t *dmaTx;
dmaChannelDescriptor_t *dmaRx;
- uint32_t dmaTxChannel;
- uint32_t dmaRxChannel;
#ifndef UNIT_TEST
// Use a reference here as this saves RAM for unused descriptors
#if defined(USE_FULL_LL_DRIVER)
@@ -109,7 +107,7 @@ typedef struct extDevice_s {
} busType_u;
#ifndef UNIT_TEST
// Cache the init structure for the next DMA transfer to reduce inter-segment delay
-#if defined(USE_HAL_DRIVER)
+#if defined(USE_FULL_LL_DRIVER)
LL_DMA_InitTypeDef initTx;
LL_DMA_InitTypeDef initRx;
#else
diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c
index f2713b3c2b..2336586c9b 100644
--- a/src/main/drivers/bus_spi.c
+++ b/src/main/drivers/bus_spi.c
@@ -543,7 +543,9 @@ void spiInitBusDMA()
break;
}
#endif
- bus->dmaTxChannel = dmaTxChannelSpec->channel;
+ bus->dmaTx = dmaGetDescriptorByIdentifier(dmaTxIdentifier);
+ bus->dmaTx->stream = DMA_DEVICE_INDEX(dmaTxIdentifier);
+ bus->dmaTx->channel = dmaTxChannelSpec->channel;
dmaInit(dmaTxIdentifier, OWNER_SPI_MOSI, device + 1);
break;
}
@@ -564,16 +566,15 @@ void spiInitBusDMA()
break;
}
#endif
- bus->dmaRxChannel = dmaRxChannelSpec->channel;
+ bus->dmaRx = dmaGetDescriptorByIdentifier(dmaRxIdentifier);
+ bus->dmaRx->stream = DMA_DEVICE_INDEX(dmaRxIdentifier);
+ bus->dmaRx->channel = dmaRxChannelSpec->channel;
dmaInit(dmaRxIdentifier, OWNER_SPI_MISO, device + 1);
break;
}
}
if (dmaTxIdentifier && dmaRxIdentifier) {
- bus->dmaTx = dmaGetDescriptorByIdentifier(dmaTxIdentifier);
- bus->dmaRx = dmaGetDescriptorByIdentifier(dmaRxIdentifier);
-
// Ensure streams are disabled
spiInternalResetStream(bus->dmaRx);
spiInternalResetStream(bus->dmaTx);
@@ -586,6 +587,10 @@ void spiInitBusDMA()
dmaSetHandler(dmaRxIdentifier, spiRxIrqHandler, NVIC_PRIO_SPI_DMA, 0);
bus->useDMA = true;
+ } else {
+ // Disassociate channels from bus
+ bus->dmaRx = (dmaChannelDescriptor_t *)NULL;
+ bus->dmaTx = (dmaChannelDescriptor_t *)NULL;
}
}
}
diff --git a/src/main/drivers/bus_spi_hal.c b/src/main/drivers/bus_spi_hal.c
deleted file mode 100644
index 913badfa8c..0000000000
--- a/src/main/drivers/bus_spi_hal.c
+++ /dev/null
@@ -1,249 +0,0 @@
-/*
- * This file is part of Cleanflight and Betaflight.
- *
- * Cleanflight and Betaflight are 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.
- *
- * Cleanflight and Betaflight are distributed in the hope that they
- * 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 .
- *
- * HAL version resurrected from v3.1.7 (by jflyper)
- */
-
-// Note that the HAL driver is polled only
-
-#include
-#include
-#include
-
-#include "platform.h"
-
-#ifdef USE_SPI
-
-#include "bus_spi.h"
-#include "bus_spi_impl.h"
-#include "dma.h"
-#include "io.h"
-#include "io_impl.h"
-#include "nvic.h"
-#include "rcc.h"
-
-#define SPI_TIMEOUT_SYS_TICKS (SPI_TIMEOUT_US / 1000)
-
-// Position of Prescaler bits are different from MCU to MCU
-
-static uint32_t baudRatePrescaler[8] = {
- SPI_BAUDRATEPRESCALER_2,
- SPI_BAUDRATEPRESCALER_4,
- SPI_BAUDRATEPRESCALER_8,
- SPI_BAUDRATEPRESCALER_16,
- SPI_BAUDRATEPRESCALER_32,
- SPI_BAUDRATEPRESCALER_64,
- SPI_BAUDRATEPRESCALER_128,
- SPI_BAUDRATEPRESCALER_256,
-};
-
-static void spiDivisorToBRbits(SPI_TypeDef *instance, uint16_t divisor)
-{
- SPIDevice device = spiDeviceByInstance(instance);
-
- spiDevice_t *spi = &(spiDevice[device]);
-
- int prescalerIndex = ffs(divisor) - 2; // prescaler begins at "/2"
-
- if (prescalerIndex < 0 || prescalerIndex >= (int)ARRAYLEN(baudRatePrescaler)) {
- return;
- }
-
- if (spi->hspi.Init.BaudRatePrescaler != baudRatePrescaler[prescalerIndex]) {
- spi->hspi.Init.BaudRatePrescaler = baudRatePrescaler[prescalerIndex];
-
- MODIFY_REG(spi->hspi.Instance->CR1, SPI_CR1_BR_Msk, spi->hspi.Init.BaudRatePrescaler);
- }
-}
-
-static void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor)
-{
- spiDivisorToBRbits(instance, divisor);
-}
-
-void spiInitDevice(SPIDevice device)
-{
- spiDevice_t *spi = &(spiDevice[device]);
-
- if (!spi->dev) {
- return;
- }
-
- // 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 defined(STM32F3)
- IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_CFG, spi->af);
- IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_CFG, spi->af);
- IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af);
-#endif
-
-#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4)
- IOConfigGPIOAF(IOGetByTag(spi->sck), spi->leadingEdge ? SPI_IO_AF_SCK_CFG_LOW : 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);
-#endif
-
-#if defined(STM32F10X)
- IOConfigGPIO(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG);
- IOConfigGPIO(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG);
- IOConfigGPIO(IOGetByTag(spi->mosi), SPI_IO_AF_MOSI_CFG);
-#endif
-
- spi->hspi.Instance = spi->dev;
- // DeInit SPI hardware
- HAL_SPI_DeInit(&spi->hspi);
-
- spi->hspi.Init.Mode = SPI_MODE_MASTER;
- spi->hspi.Init.Direction = SPI_DIRECTION_2LINES;
- spi->hspi.Init.DataSize = SPI_DATASIZE_8BIT;
- spi->hspi.Init.NSS = SPI_NSS_SOFT;
- spi->hspi.Init.FirstBit = SPI_FIRSTBIT_MSB;
- spi->hspi.Init.CRCPolynomial = 7;
- spi->hspi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_256;
- spi->hspi.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
- spi->hspi.Init.TIMode = SPI_TIMODE_DISABLED;
-#if !defined(STM32G4)
- spi->hspi.Init.FifoThreshold = SPI_FIFO_THRESHOLD_01DATA;
- spi->hspi.Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_ENABLE; /* Recommanded setting to avoid glitches */
-#endif
- spi->hspi.Init.CLKPolarity = SPI_POLARITY_LOW;
- spi->hspi.Init.CLKPhase = SPI_PHASE_1EDGE;
-
- // Init SPI hardware
- HAL_SPI_Init(&spi->hspi);
-}
-
-static bool spiInternalReadWriteBufPolled(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len)
-{
- SPIDevice device = spiDeviceByInstance(instance);
- HAL_StatusTypeDef status;
-
- if (!rxData) {
- // Tx only
- status = HAL_SPI_Transmit(&spiDevice[device].hspi, txData, len, SPI_TIMEOUT_SYS_TICKS);
- } else if(!txData) {
- // Rx only
- status = HAL_SPI_Receive(&spiDevice[device].hspi, rxData, len, SPI_TIMEOUT_SYS_TICKS);
- } else {
- // Tx and Rx
- status = HAL_SPI_TransmitReceive(&spiDevice[device].hspi, txData, rxData, len, SPI_TIMEOUT_SYS_TICKS);
- }
-
- return (status == HAL_OK);
-}
-
-void spiInternalInitStream(const extDevice_t *dev, bool preInit)
-{
- UNUSED(dev);
- UNUSED(preInit);
-}
-
-void spiInternalStartDMA(const extDevice_t *dev)
-{
- UNUSED(dev);
-}
-
-void spiInternalStopDMA (const extDevice_t *dev)
-{
- UNUSED(dev);
-}
-
-void spiInternalResetDescriptors(busDevice_t *bus)
-{
- UNUSED(bus);
-}
-
-void spiInternalResetStream(dmaChannelDescriptor_t *descriptor)
-{
- UNUSED(descriptor);
-}
-
-// Transfer setup and start
-void spiSequence(const extDevice_t *dev, busSegment_t *segments)
-{
- busDevice_t *bus = dev->bus;
- SPIDevice device = spiDeviceByInstance(bus->busType_u.spi.instance);
- SPI_HandleTypeDef *hspi = &spiDevice[device].hspi;
-
- bus->initSegment = true;
- bus->curSegment = segments;
-
- // Switch bus speed
- spiSetDivisor(bus->busType_u.spi.instance, dev->busType_u.spi.speed);
-
- // Switch SPI clock polarity/phase if necessary
- if (dev->busType_u.spi.leadingEdge != bus->busType_u.spi.leadingEdge) {
- if (dev->busType_u.spi.leadingEdge){
- hspi->Init.CLKPolarity = SPI_POLARITY_LOW;
- hspi->Init.CLKPhase = SPI_PHASE_1EDGE;
- } else {
- hspi->Init.CLKPolarity = SPI_POLARITY_HIGH;
- hspi->Init.CLKPhase = SPI_PHASE_2EDGE;
- }
- bus->busType_u.spi.leadingEdge = dev->busType_u.spi.leadingEdge;
-
- // Init SPI hardware
- HAL_SPI_Init(hspi);
- }
-
- // Manually work through the segment list performing a transfer for each
- while (bus->curSegment->len) {
- // Assert Chip Select
- IOLo(dev->busType_u.spi.csnPin);
-
- spiInternalReadWriteBufPolled(
- bus->busType_u.spi.instance,
- bus->curSegment->txData,
- bus->curSegment->rxData,
- bus->curSegment->len);
-
- if (bus->curSegment->negateCS) {
- // Negate Chip Select
- IOHi(dev->busType_u.spi.csnPin);
- }
-
- if (bus->curSegment->callback) {
- switch(bus->curSegment->callback(dev->callbackArg)) {
- case BUS_BUSY:
- // Repeat the last DMA segment
- bus->curSegment--;
- break;
-
- case BUS_ABORT:
- bus->curSegment = (busSegment_t *)NULL;
- return;
-
- case BUS_READY:
- default:
- // Advance to the next DMA segment
- break;
- }
- }
- bus->curSegment++;
- }
-
- bus->curSegment = (busSegment_t *)NULL;
-}
-#endif
diff --git a/src/main/drivers/bus_spi_ll.c b/src/main/drivers/bus_spi_ll.c
index ffec81910f..a98f20d335 100644
--- a/src/main/drivers/bus_spi_ll.c
+++ b/src/main/drivers/bus_spi_ll.c
@@ -77,6 +77,8 @@
#define IS_DTCM(p) (((uint32_t)p & 0xfffe0000) == 0x20000000)
#elif defined(STM32F7)
#define IS_DTCM(p) (((uint32_t)p & 0xffff0000) == 0x20000000)
+#elif defined(STM32G4)
+#define IS_CCM(p) ((((uint32_t)p & 0xffff8000) == 0x10000000) || (((uint32_t)p & 0xffff8000) == 0x20018000))
#endif
static LL_SPI_InitTypeDef defaultInit =
{
@@ -167,10 +169,10 @@ void spiInternalResetDescriptors(busDevice_t *bus)
LL_DMA_InitTypeDef *initRx = bus->initRx;
LL_DMA_StructInit(initTx);
-#if defined(STM32H7)
- initTx->PeriphRequest = bus->dmaTxChannel;
+#if defined(STM32G4) || defined(STM32H7)
+ initTx->PeriphRequest = bus->dmaTx->channel;
#else
- initTx->Channel = bus->dmaTxChannel;
+ initTx->Channel = bus->dmaTx->channel;
#endif
initTx->Mode = LL_DMA_MODE_NORMAL;
initTx->Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH;
@@ -185,10 +187,10 @@ void spiInternalResetDescriptors(busDevice_t *bus)
initTx->MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE;
LL_DMA_StructInit(initRx);
-#if defined(STM32H7)
- initRx->PeriphRequest = bus->dmaRxChannel;
+#if defined(STM32G4) || defined(STM32H7)
+ initRx->PeriphRequest = bus->dmaRx->channel;
#else
- initRx->Channel = bus->dmaRxChannel;
+ initRx->Channel = bus->dmaRx->channel;
#endif
initRx->Mode = LL_DMA_MODE_NORMAL;
initRx->Direction = LL_DMA_DIRECTION_PERIPH_TO_MEMORY;
@@ -205,8 +207,13 @@ void spiInternalResetDescriptors(busDevice_t *bus)
void spiInternalResetStream(dmaChannelDescriptor_t *descriptor)
{
// Disable the stream
+#if defined(STM32G4)
+ LL_DMA_DisableChannel(descriptor->dma, descriptor->stream);
+ while (LL_DMA_IsEnabledChannel(descriptor->dma, descriptor->stream));
+#else
LL_DMA_DisableStream(descriptor->dma, descriptor->stream);
while (LL_DMA_IsEnabledStream(descriptor->dma, descriptor->stream));
+#endif
// Clear any pending interrupt flags
DMA_CLEAR_FLAG(descriptor, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
@@ -352,12 +359,6 @@ void spiInternalInitStream(const extDevice_t *dev, bool preInit)
initRx->MemoryOrM2MDstAddress = (uint32_t)&dummyRxByte;
initRx->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_NOINCREMENT;
}
- // If possible use 16 bit memory writes to prevent atomic access issues on gyro data
- if ((initRx->MemoryOrM2MDstAddress & 0x1) || (len & 0x1)) {
- initRx->MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE;
- } else {
- initRx->MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_HALFWORD;
- }
initRx->NbData = len;
}
@@ -371,9 +372,6 @@ void spiInternalStartDMA(const extDevice_t *dev)
dmaChannelDescriptor_t *dmaTx = bus->dmaTx;
dmaChannelDescriptor_t *dmaRx = bus->dmaRx;
- DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref;
- DMA_Stream_TypeDef *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref;
-
// Use the correct callback argument
dmaRx->userParam = (uint32_t)dev;
@@ -381,6 +379,31 @@ void spiInternalStartDMA(const extDevice_t *dev)
DMA_CLEAR_FLAG(dmaTx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
DMA_CLEAR_FLAG(dmaRx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
+#ifdef STM32G4
+ // Disable channels to enable update
+ LL_DMA_DisableChannel(dmaTx->dma, dmaTx->stream);
+ LL_DMA_DisableChannel(dmaRx->dma, dmaRx->stream);
+
+ /* Use the Rx interrupt as this occurs once the SPI operation is complete whereas the Tx interrupt
+ * occurs earlier when the Tx FIFO is empty, but the SPI operation is still in progress
+ */
+ LL_DMA_EnableIT_TC(dmaRx->dma, dmaRx->stream);
+
+ // Update channels
+ LL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->initTx);
+ LL_DMA_Init(dmaRx->dma, dmaRx->stream, bus->initRx);
+
+ LL_SPI_EnableDMAReq_RX(dev->bus->busType_u.spi.instance);
+
+ // Enable channels
+ LL_DMA_EnableChannel(dmaTx->dma, dmaTx->stream);
+ LL_DMA_EnableChannel(dmaRx->dma, dmaRx->stream);
+
+ LL_SPI_EnableDMAReq_TX(dev->bus->busType_u.spi.instance);
+#else
+ DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref;
+ DMA_Stream_TypeDef *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref;
+
// Disable streams to enable update
LL_DMA_WriteReg(streamRegsTx, CR, 0U);
LL_DMA_WriteReg(streamRegsRx, CR, 0U);
@@ -416,6 +439,7 @@ void spiInternalStartDMA(const extDevice_t *dev)
SET_BIT(dev->bus->busType_u.spi.instance->CR2, SPI_CR2_TXDMAEN | SPI_CR2_RXDMAEN);
#endif
+#endif
}
void spiInternalStopDMA (const extDevice_t *dev)
@@ -427,9 +451,15 @@ void spiInternalStopDMA (const extDevice_t *dev)
SPI_TypeDef *instance = bus->busType_u.spi.instance;
// Disable the DMA engine and SPI interface
+#ifdef STM32G4
+ LL_DMA_DisableChannel(dmaTx->dma, dmaTx->stream);
+ LL_DMA_DisableChannel(dmaRx->dma, dmaRx->stream);
+#else
LL_DMA_DisableStream(dmaRx->dma, dmaRx->stream);
LL_DMA_DisableStream(dmaTx->dma, dmaTx->stream);
+#endif
+ // Clear transfer flags
DMA_CLEAR_FLAG(dmaRx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
LL_SPI_DisableDMAReq_TX(instance);
@@ -518,17 +548,17 @@ void spiSequence(const extDevice_t *dev, busSegment_t *segments)
dmaSafe = false;
break;
}
-#elif defined(STM32F4)
+#elif defined(STM32G4)
// Check if RX data can be DMAed
if ((checkSegment->rxData) &&
- // DTCM can't be accessed by DMA1/2 on the F4
- (IS_DTCM(checkSegment->rxData)) {
+ // CCM can't be accessed by DMA1/2 on the G4
+ IS_CCM(checkSegment->rxData)) {
dmaSafe = false;
break;
}
if ((checkSegment->txData) &&
- // DTCM can't be accessed by DMA1/2 on the F4
- (IS_DTCM(checkSegment->txData)) {
+ // CCM can't be accessed by DMA1/2 on the G4
+ IS_CCM(checkSegment->txData)) {
dmaSafe = false;
break;
}
diff --git a/src/main/drivers/bus_spi_stdperiph.c b/src/main/drivers/bus_spi_stdperiph.c
index 47d614f305..5dd1584317 100644
--- a/src/main/drivers/bus_spi_stdperiph.c
+++ b/src/main/drivers/bus_spi_stdperiph.c
@@ -108,7 +108,7 @@ void spiInternalResetDescriptors(busDevice_t *bus)
DMA_InitTypeDef *initRx = bus->initRx;
DMA_StructInit(initTx);
- initTx->DMA_Channel = bus->dmaTxChannel;
+ initTx->DMA_Channel = bus->dmaTx->channel;
initTx->DMA_DIR = DMA_DIR_MemoryToPeripheral;
initTx->DMA_Mode = DMA_Mode_Normal;
initTx->DMA_PeripheralBaseAddr = (uint32_t)&bus->busType_u.spi.instance->DR;
@@ -118,7 +118,7 @@ void spiInternalResetDescriptors(busDevice_t *bus)
initTx->DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
DMA_StructInit(initRx);
- initRx->DMA_Channel = bus->dmaRxChannel;
+ initRx->DMA_Channel = bus->dmaRx->channel;
initRx->DMA_DIR = DMA_DIR_PeripheralToMemory;
initRx->DMA_Mode = DMA_Mode_Normal;
initRx->DMA_PeripheralBaseAddr = (uint32_t)&bus->busType_u.spi.instance->DR;
diff --git a/src/main/drivers/dma.h b/src/main/drivers/dma.h
index 97ff5bc9c9..9aad835796 100644
--- a/src/main/drivers/dma.h
+++ b/src/main/drivers/dma.h
@@ -49,9 +49,10 @@ typedef void (*dmaCallbackHandlerFuncPtr)(struct dmaChannelDescriptor_s *channel
typedef struct dmaChannelDescriptor_s {
DMA_TypeDef* dma;
dmaResource_t *ref;
-#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4)
+#if defined(STM32F4) || defined(STM32F7) || defined(STM32G4) || defined(STM32H7)
uint8_t stream;
#endif
+ uint32_t channel;
dmaCallbackHandlerFuncPtr irqHandlerCallback;
uint8_t flagsShift;
IRQn_Type irqN;
diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h
index da4a754965..82a1b77a25 100644
--- a/src/main/target/common_pre.h
+++ b/src/main/target/common_pre.h
@@ -180,8 +180,9 @@
#define FAST_DATA
#endif // USE_FAST_DATA
-#if defined(STM32F4)
+#if defined(STM32F4) || defined(STM32G4)
// F4 can't DMA to/from CCM (core coupled memory) SRAM (where the stack lives)
+// On G4 there is no specific DMA target memory
#define DMA_DATA_ZERO_INIT
#define DMA_DATA
#define STATIC_DMA_DATA_AUTO static