mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 16:25:31 +03:00
parent
bc773d8a49
commit
ab2088dc24
5 changed files with 64 additions and 58 deletions
|
@ -48,13 +48,17 @@
|
|||
#define SPI3_MOSI_PIN PC12
|
||||
|
||||
#define USE_FLASH_M25P16
|
||||
#define FLASH_CS_PIN PB3
|
||||
#define FLASH_SPI_INSTANCE SPI3
|
||||
|
||||
#define I2C1_SCL_PIN PB8
|
||||
#define I2C1_SDA_PIN PB9
|
||||
#define I2C1_SCL_PIN PB8
|
||||
#define I2C1_SDA_PIN PB9
|
||||
|
||||
#define I2C2_SCL_PIN PB10
|
||||
#define I2C2_SDA_PIN PB11
|
||||
#define I2C2_SCL_PIN PB10
|
||||
#define I2C2_SDA_PIN PB11
|
||||
|
||||
#define BARO_I2C_INSTANCE I2C2
|
||||
#define MAG_I2C_INSTANCE I2C1
|
||||
#define USE_MAG
|
||||
|
||||
#define BARO_I2C_INSTANCE I2C1
|
||||
#define USE_BARO
|
||||
#define USE_BARO_MS5611
|
||||
|
|
|
@ -36,6 +36,9 @@
|
|||
#include "drivers/io.h"
|
||||
#include "drivers/rcc.h"
|
||||
|
||||
// Use DMA if possible if this many bytes are to be transferred
|
||||
#define SPI_DMA_THRESHOLD 8
|
||||
|
||||
static spi_init_type defaultInit = {
|
||||
.master_slave_mode = SPI_MODE_MASTER,
|
||||
.transmission_mode = SPI_TRANSMIT_FULL_DUPLEX,
|
||||
|
@ -86,13 +89,13 @@ void spiInitDevice(SPIDevice device)
|
|||
// Init SPI hardware
|
||||
spi_i2s_reset(spi->dev);
|
||||
|
||||
spi_i2s_dma_transmitter_enable(spi->dev,TRUE);
|
||||
spi_i2s_dma_receiver_enable(spi->dev,TRUE);
|
||||
spi_i2s_dma_transmitter_enable(spi->dev, TRUE);
|
||||
spi_i2s_dma_receiver_enable(spi->dev, TRUE);
|
||||
|
||||
spi_init(spi->dev,&defaultInit);
|
||||
spi_crc_polynomial_set(spi->dev,7);
|
||||
spi_init(spi->dev, &defaultInit);
|
||||
spi_crc_polynomial_set(spi->dev, 7);
|
||||
|
||||
spi_enable(spi->dev,TRUE);
|
||||
spi_enable(spi->dev, TRUE);
|
||||
}
|
||||
|
||||
void spiInternalResetDescriptors(busDevice_t *bus)
|
||||
|
@ -138,12 +141,12 @@ static bool spiInternalReadWriteBufPolled(spi_type *instance, const uint8_t *txD
|
|||
while (len--) {
|
||||
b = txData ? *(txData++) : 0xFF;
|
||||
|
||||
while(spi_i2s_flag_get(instance,SPI_I2S_TDBE_FLAG) == RESET);
|
||||
spi_i2s_data_transmit(instance,b);
|
||||
while (spi_i2s_flag_get(instance, SPI_I2S_TDBE_FLAG) == RESET);
|
||||
spi_i2s_data_transmit(instance, b);
|
||||
|
||||
|
||||
while(spi_i2s_flag_get(instance,SPI_I2S_RDBF_FLAG) == RESET);
|
||||
b=spi_i2s_data_receive(instance);
|
||||
while (spi_i2s_flag_get(instance, SPI_I2S_RDBF_FLAG) == RESET);
|
||||
b = (uint8_t)spi_i2s_data_receive(instance);
|
||||
|
||||
if (rxData) {
|
||||
*(rxData++) = b;
|
||||
|
@ -279,10 +282,10 @@ void spiInternalStopDMA (const extDevice_t *dev)
|
|||
spi_i2s_dma_receiver_enable(instance, FALSE);
|
||||
} else {
|
||||
// Ensure the current transmission is complete
|
||||
while(spi_i2s_flag_get(instance,SPI_I2S_BF_FLAG));
|
||||
while (spi_i2s_flag_get(instance, SPI_I2S_BF_FLAG));
|
||||
|
||||
// Drain the RX buffer
|
||||
while(spi_i2s_flag_get(instance,SPI_I2S_RDBF_FLAG)) {
|
||||
while (spi_i2s_flag_get(instance, SPI_I2S_RDBF_FLAG)) {
|
||||
instance->dt;
|
||||
}
|
||||
|
||||
|
@ -324,67 +327,72 @@ void spiSequenceStart(const extDevice_t *dev)
|
|||
bus->busType_u.spi.leadingEdge = dev->busType_u.spi.leadingEdge;
|
||||
}
|
||||
|
||||
spi_enable(instance,TRUE);
|
||||
spi_enable(instance, TRUE);
|
||||
|
||||
// Check that any there are no attempts to DMA to/from CCD SRAM
|
||||
// Count segments
|
||||
for (busSegment_t *checkSegment = (busSegment_t *)bus->curSegment; checkSegment->len; checkSegment++) {
|
||||
// Check there is no receive data as only transmit DMA is available
|
||||
if (((checkSegment->u.buffers.rxData) && (IS_CCM(checkSegment->u.buffers.rxData) ||
|
||||
(bus->dmaRx == (dmaChannelDescriptor_t *)NULL))) ||
|
||||
((checkSegment->u.buffers.txData) && IS_CCM(checkSegment->u.buffers.txData))) {
|
||||
dmaSafe = false;
|
||||
break;
|
||||
}
|
||||
// Note that these counts are only valid if dmaSafe is true
|
||||
segmentCount++;
|
||||
xferLen += checkSegment->len;
|
||||
}
|
||||
|
||||
// Use DMA if possible
|
||||
// If there are more than one segments, or a single segment with negateCS negated then force DMA irrespective of length
|
||||
if (bus->useDMA && dmaSafe && ((segmentCount > 1) || (xferLen >= 8) || !bus->curSegment->negateCS)) {
|
||||
// If there are more than one segments, or a single segment with negateCS negated in the list terminator then force DMA irrespective of length
|
||||
if (bus->useDMA && dmaSafe && ((segmentCount > 1) ||
|
||||
(xferLen >= SPI_DMA_THRESHOLD) ||
|
||||
!bus->curSegment[segmentCount].negateCS)) {
|
||||
// Intialise the init structures for the first transfer
|
||||
spiInternalInitStream(dev, false);
|
||||
|
||||
// Assert Chip Select
|
||||
IOLo(dev->busType_u.spi.csnPin);
|
||||
|
||||
// Start the transfers
|
||||
spiInternalStartDMA(dev);
|
||||
} else {
|
||||
busSegment_t *lastSegment = NULL;
|
||||
bool segmentComplete;
|
||||
|
||||
// Manually work through the segment list performing a transfer for each
|
||||
while (bus->curSegment->len) {
|
||||
if (!lastSegment || lastSegment->negateCS) {
|
||||
// Assert Chip Select if necessary - it's costly so only do so if necessary
|
||||
IOLo(dev->busType_u.spi.csnPin);
|
||||
}
|
||||
|
||||
spiInternalReadWriteBufPolled(
|
||||
bus->busType_u.spi.instance,
|
||||
bus->curSegment->u.buffers.txData,
|
||||
bus->curSegment->u.buffers.rxData,
|
||||
bus->curSegment->len
|
||||
);
|
||||
spiInternalReadWriteBufPolled(bus->busType_u.spi.instance,
|
||||
bus->curSegment->u.buffers.txData,
|
||||
bus->curSegment->u.buffers.rxData,
|
||||
bus->curSegment->len);
|
||||
|
||||
if (bus->curSegment->negateCS) {
|
||||
// Negate Chip Select
|
||||
IOHi(dev->busType_u.spi.csnPin);
|
||||
}
|
||||
|
||||
segmentComplete = true;
|
||||
if (bus->curSegment->callback) {
|
||||
switch(bus->curSegment->callback(dev->callbackArg)) {
|
||||
case BUS_BUSY:
|
||||
bus->curSegment--;
|
||||
// Repeat the last DMA segment
|
||||
segmentComplete = false;
|
||||
break;
|
||||
|
||||
case BUS_ABORT:
|
||||
bus->curSegment = (busSegment_t *)BUS_SPI_FREE;
|
||||
segmentComplete = false;
|
||||
return;
|
||||
|
||||
case BUS_READY:
|
||||
default:
|
||||
// Advance to the next DMA segment
|
||||
break;
|
||||
}
|
||||
}
|
||||
lastSegment = (busSegment_t *)bus->curSegment;
|
||||
bus->curSegment++;
|
||||
}
|
||||
|
||||
if (lastSegment && !lastSegment->negateCS) {
|
||||
IOHi(dev->busType_u.spi.csnPin);
|
||||
if (segmentComplete) {
|
||||
lastSegment = (busSegment_t *)bus->curSegment;
|
||||
bus->curSegment++;
|
||||
}
|
||||
}
|
||||
|
||||
// If a following transaction has been linked, start it
|
||||
|
|
|
@ -82,9 +82,9 @@ typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState;
|
|||
#define DEFAULT_CPU_OVERCLOCK 0
|
||||
#define FAST_IRQ_HANDLER
|
||||
|
||||
#define DMA_DATA_ZERO_INIT __attribute__ ((section(".dmaram_bss"), aligned(32)))
|
||||
#define DMA_DATA __attribute__ ((section(".dmaram_data"), aligned(32)))
|
||||
#define STATIC_DMA_DATA_AUTO static DMA_DATA
|
||||
#define DMA_DATA_ZERO_INIT
|
||||
#define DMA_DATA
|
||||
#define STATIC_DMA_DATA_AUTO static
|
||||
|
||||
#define DMA_RAM
|
||||
#define DMA_RW_AXI
|
||||
|
|
|
@ -381,6 +381,7 @@ void spiSequenceStart(const extDevice_t *dev)
|
|||
spiInternalStartDMA(dev);
|
||||
} else {
|
||||
busSegment_t *lastSegment = NULL;
|
||||
bool segmentComplete;
|
||||
|
||||
// Manually work through the segment list performing a transfer for each
|
||||
while (bus->curSegment->len) {
|
||||
|
@ -400,15 +401,17 @@ void spiSequenceStart(const extDevice_t *dev)
|
|||
IOHi(dev->busType_u.spi.csnPin);
|
||||
}
|
||||
|
||||
segmentComplete = true;
|
||||
if (bus->curSegment->callback) {
|
||||
switch(bus->curSegment->callback(dev->callbackArg)) {
|
||||
case BUS_BUSY:
|
||||
// Repeat the last DMA segment
|
||||
bus->curSegment--;
|
||||
segmentComplete = false;
|
||||
break;
|
||||
|
||||
case BUS_ABORT:
|
||||
bus->curSegment = (busSegment_t *)BUS_SPI_FREE;
|
||||
segmentComplete = false;
|
||||
return;
|
||||
|
||||
case BUS_READY:
|
||||
|
@ -417,8 +420,10 @@ void spiSequenceStart(const extDevice_t *dev)
|
|||
break;
|
||||
}
|
||||
}
|
||||
lastSegment = (busSegment_t *)bus->curSegment;
|
||||
bus->curSegment++;
|
||||
if (segmentComplete) {
|
||||
lastSegment = (busSegment_t *)bus->curSegment;
|
||||
bus->curSegment++;
|
||||
}
|
||||
}
|
||||
|
||||
// If a following transaction has been linked, start it
|
||||
|
|
|
@ -72,18 +72,7 @@
|
|||
#undef USE_RX_SPI
|
||||
#undef USE_RX_CC2500
|
||||
#undef USE_RX_EXPRESSLRS
|
||||
//#undef USE_CMS
|
||||
//#undef USE_OSD
|
||||
#undef USE_BLACKBOX
|
||||
//#undef USE_SDCARD
|
||||
//#undef USE_BARO
|
||||
//#undef USE_MAG
|
||||
#undef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
|
||||
#undef USE_SERIAL_4WAY_SK_BOOTLOADER
|
||||
|
||||
// remove all flash
|
||||
#undef USE_FLASH
|
||||
#undef USE_FLASHFS
|
||||
#undef USE_FLASH_CHIP
|
||||
|
||||
#define FLASH_PAGE_SIZE ((uint32_t)0x1000) // 4K sectors
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue