mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-12 19:10:32 +03:00
G4 SPI DMA support
This commit is contained in:
parent
c283981d54
commit
ab66795eeb
10 changed files with 75 additions and 289 deletions
0
lib/main/STM32G4/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc.c
Executable file → Normal file
0
lib/main/STM32G4/Middlewares/ST/STM32_USB_Device_Library/Class/MSC/Src/usbd_msc.c
Executable file → Normal file
|
@ -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 \
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* HAL version resurrected from v3.1.7 (by jflyper)
|
||||
*/
|
||||
|
||||
// Note that the HAL driver is polled only
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <strings.h>
|
||||
|
||||
#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
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue