diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 615470aeb4..e83b60cd95 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -490,7 +490,7 @@ void spiSequence(const extDevice_t *dev, busSegment_t *segments) FAST_CODE void spiProcessSegmentsDMA(const extDevice_t *dev) { // Intialise the init structures for the first transfer - spiInternalInitStream(dev, false); + spiInternalInitStream(dev, dev->bus->curSegment); // Assert Chip Select IOLo(dev->busType_u.spi.csnPin); @@ -499,6 +499,16 @@ FAST_CODE void spiProcessSegmentsDMA(const extDevice_t *dev) spiInternalStartDMA(dev); } +static void spiPreInitStream(const extDevice_t *dev) +{ + // Prepare the init structure for the next segment to reduce inter-segment interval + // (if it's a "buffers" segment, not a "link" segment). + busSegment_t *nextSegment = (busSegment_t *)dev->bus->curSegment + 1; + if (nextSegment->len > 0) { + spiInternalInitStream(dev, nextSegment); + } +} + // Interrupt handler common code for SPI receive DMA completion. // Proceed to next segment as required. FAST_IRQ_HANDLER void spiIrqHandler(const extDevice_t *dev) @@ -512,7 +522,7 @@ FAST_IRQ_HANDLER void spiIrqHandler(const extDevice_t *dev) // Repeat the last DMA segment bus->curSegment--; // Reinitialise the cached init values as segment is not progressing - spiInternalInitStream(dev, true); + spiPreInitStream(dev); break; case BUS_ABORT: @@ -557,7 +567,7 @@ FAST_IRQ_HANDLER void spiIrqHandler(const extDevice_t *dev) // After the completion of the first segment setup the init structure for the subsequent segment if (bus->initSegment) { - spiInternalInitStream(dev, false); + spiInternalInitStream(dev, bus->curSegment); bus->initSegment = false; } @@ -570,7 +580,7 @@ FAST_IRQ_HANDLER void spiIrqHandler(const extDevice_t *dev) spiInternalStartDMA(dev); // Prepare the init structures ready for the next segment to reduce inter-segment time - spiInternalInitStream(dev, true); + spiPreInitStream(dev); } } diff --git a/src/main/drivers/bus_spi_impl.h b/src/main/drivers/bus_spi_impl.h index 4b5106c8c4..abf925d447 100644 --- a/src/main/drivers/bus_spi_impl.h +++ b/src/main/drivers/bus_spi_impl.h @@ -87,7 +87,7 @@ typedef struct SPIDevice_s { extern spiDevice_t spiDevice[SPIDEV_COUNT]; void spiInitDevice(SPIDevice device); -void spiInternalInitStream(const extDevice_t *dev, bool preInit); +void spiInternalInitStream(const extDevice_t *dev, volatile busSegment_t *segment); void spiInternalStartDMA(const extDevice_t *dev); void spiInternalStopDMA (const extDevice_t *dev); void spiInternalResetStream(dmaChannelDescriptor_t *descriptor); diff --git a/src/platform/APM32/bus_spi_apm32.c b/src/platform/APM32/bus_spi_apm32.c index 6c58c5e8da..cf49043e58 100644 --- a/src/platform/APM32/bus_spi_apm32.c +++ b/src/platform/APM32/bus_spi_apm32.c @@ -156,23 +156,11 @@ FAST_CODE bool spiInternalReadWriteBufPolled(SPI_TypeDef *instance, const uint8_ return true; } -void spiInternalInitStream(const extDevice_t *dev, bool preInit) +void spiInternalInitStream(const extDevice_t *dev, volatile busSegment_t *segment) { STATIC_DMA_DATA_AUTO uint8_t dummyTxByte = 0xff; STATIC_DMA_DATA_AUTO uint8_t dummyRxByte; busDevice_t *bus = dev->bus; - - busSegment_t *segment = (busSegment_t *)bus->curSegment; - - if (preInit) { - // Prepare the init structure for the next segment to reduce inter-segment interval - segment++; - if(segment->len == 0) { - // There's no following segment - return; - } - } - int len = segment->len; uint8_t *txData = segment->u.buffers.txData; diff --git a/src/platform/AT32/bus_spi_at32bsp.c b/src/platform/AT32/bus_spi_at32bsp.c index fed36d685e..7a96e06139 100644 --- a/src/platform/AT32/bus_spi_at32bsp.c +++ b/src/platform/AT32/bus_spi_at32bsp.c @@ -156,23 +156,11 @@ bool spiInternalReadWriteBufPolled(spi_type *instance, const uint8_t *txData, ui return true; } -void spiInternalInitStream(const extDevice_t *dev, bool preInit) +void spiInternalInitStream(const extDevice_t *dev, volatile busSegment_t *segment) { STATIC_DMA_DATA_AUTO uint8_t dummyTxByte = 0xff; STATIC_DMA_DATA_AUTO uint8_t dummyRxByte; busDevice_t *bus = dev->bus; - - volatile busSegment_t *segment = bus->curSegment; - - if (preInit) { - // Prepare the init structure for the next segment to reduce inter-segment interval - segment++; - if(segment->len == 0) { - // There's no following segment - return; - } - } - int len = segment->len; uint8_t *txData = segment->u.buffers.txData; diff --git a/src/platform/PICO/bus_spi_pico.c b/src/platform/PICO/bus_spi_pico.c index acfa6cc4bd..a16aa3ef52 100644 --- a/src/platform/PICO/bus_spi_pico.c +++ b/src/platform/PICO/bus_spi_pico.c @@ -156,6 +156,8 @@ void spiPinConfigure(const struct spiPinConfig_s *pConfig) if (pDev->sck && pDev->miso && pDev->mosi) { pDev->dev = hw->reg; pDev->leadingEdge = false; + bprintf("spiPinConfigure got dev %p sck %d mosi %d miso %d", + pDev->dev, pDev->sck, pDev->mosi, pDev->miso); } } } @@ -163,6 +165,7 @@ void spiPinConfigure(const struct spiPinConfig_s *pConfig) static void spiSetClockFromSpeed(spi_inst_t *spi, uint16_t speed) { uint32_t freq = spiCalculateClock(speed); + bprintf("spiSetClockFromSpeed %p %d -> %d",spi, speed, freq); spi_set_baudrate(spi, freq); } @@ -207,6 +210,7 @@ Must be SPI_MSB_FIRST, no other values supported on the PL022 void spiInitDevice(SPIDevice device) { + bprintf("pico spiInitDevice %d",device); const spiDevice_t *spi = &spiDevice[device]; if (!spi->dev) { @@ -223,6 +227,8 @@ void spiInitDevice(SPIDevice device) gpio_set_function(IO_PINBYTAG(spi->miso), GPIO_FUNC_SPI); gpio_set_function(IO_PINBYTAG(spi->mosi), GPIO_FUNC_SPI); gpio_set_function(IO_PINBYTAG(spi->sck), GPIO_FUNC_SPI); + bprintf("spi initialised device %p [sck %d mosi %d miso %d]", + spi->dev, IO_PINBYTAG(spi->sck), IO_PINBYTAG(spi->mosi), IO_PINBYTAG(spi->miso)); } void spiInternalStopDMA (const extDevice_t *dev) @@ -232,16 +238,17 @@ void spiInternalStopDMA (const extDevice_t *dev) if (dmaRx && dma_channel_is_busy(dmaRx->channel)) { // Abort active DMA - this should never happen + bprintf("\n *** pico spiInternalStopDMA RX busy\n"); dma_channel_abort(dmaRx->channel); } if (dmaTx && dma_channel_is_busy(dmaTx->channel)) { // Abort active DMA - this should never happen as Tx should complete before Rx + bprintf("\n *** pico spiInternalStopDMA TX busy\n"); dma_channel_abort(dmaTx->channel); } } - // Interrupt handler for SPI receive DMA completion FAST_IRQ_HANDLER static void spiRxIrqHandler(dmaChannelDescriptor_t* descriptor) { @@ -321,39 +328,52 @@ void spiInternalResetStream(dmaChannelDescriptor_t *descriptor) bool spiInternalReadWriteBufPolled(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len) { // TODO optimise with 16-bit transfers as per stm bus_spi_ll code - int bytesProcessed = spi_write_read_blocking(SPI_INST(instance), txData, rxData, len); + int bytesProcessed = 0; + if (txData && rxData) { + bytesProcessed = spi_write_read_blocking(SPI_INST(instance), txData, rxData, len); + } else if (txData) { + bytesProcessed = spi_write_blocking(SPI_INST(instance), txData, len); + } else if (rxData) { + // NB tx data: "Generally this can be 0, but some devices require a specific value here, e.g. SD cards expect 0xff" (pico-sdk spi.c). + uint8_t repeated_tx_data = 0xff; // cf. dummyTxByte in stm bus_spi_ll.c and for DMA here + bytesProcessed = spi_read_blocking(SPI_INST(instance), repeated_tx_data, rxData, len); + } else { + bprintf("\n*** unexpected spiInternalReadWriteBufPolled with no tx, rx"); + } + return bytesProcessed == len; } -// Initialise DMA before first segment transfer -void spiInternalInitStream(const extDevice_t *dev, bool preInit) +void spiInternalInitStream(const extDevice_t *dev, volatile busSegment_t *segment) { #ifndef USE_DMA UNUSED(dev); - UNUSED(preInit); + UNUSED(segment); #else - UNUSED(preInit); - busDevice_t *bus = dev->bus; + spi_inst_t *spi = SPI_INST(bus->busType_u.spi.instance); - volatile busSegment_t *segment = bus->curSegment; + // Prepare config, store in dmaInitTx/Rx, to be used in the following spiInternalStartDMA. + // To keep everything uniform (always both TX and RX channels active, callback always on RX completion), if there is + // no TX or RX buffer to read from / write to, treat as a single dummy byte with no increment. + // (cf. same idea on other platform implementations) + bool isTX = segment->u.buffers.txData != NULL; + bool isRX = segment->u.buffers.rxData != NULL; - const spiDevice_t *spi = &spiDevice[spiDeviceByInstance(dev->bus->busType_u.spi.instance)]; - dma_channel_config config = dma_channel_get_default_config(dev->bus->dmaTx->channel); + dma_channel_config config = dma_channel_get_default_config(bus->dmaTx->channel); channel_config_set_transfer_data_size(&config, DMA_SIZE_8); - channel_config_set_read_increment(&config, true); + channel_config_set_read_increment(&config, isTX); channel_config_set_write_increment(&config, false); - channel_config_set_dreq(&config, spi_get_dreq(SPI_INST(spi->dev), true)); + channel_config_set_dreq(&config, spi_get_dreq(spi, true)); + *(bus->dmaInitTx) = config; - dma_channel_configure(dev->bus->dmaTx->channel, &config, &spi_get_hw(SPI_INST(spi->dev))->dr, segment->u.buffers.txData, 0, false); - - config = dma_channel_get_default_config(dev->bus->dmaRx->channel); + config = dma_channel_get_default_config(bus->dmaRx->channel); channel_config_set_transfer_data_size(&config, DMA_SIZE_8); channel_config_set_read_increment(&config, false); - channel_config_set_write_increment(&config, true); - channel_config_set_dreq(&config, spi_get_dreq(SPI_INST(spi->dev), false)); + channel_config_set_write_increment(&config, isRX); + channel_config_set_dreq(&config, spi_get_dreq(spi, false)); + *(bus->dmaInitRx) = config; - dma_channel_configure(dev->bus->dmaRx->channel, &config, segment->u.buffers.rxData, &spi_get_hw(SPI_INST(spi->dev))->dr, 0, false); #endif } @@ -363,22 +383,31 @@ void spiInternalStartDMA(const extDevice_t *dev) #ifndef USE_DMA UNUSED(dev); #else - dev->bus->dmaRx->userParam = (uint32_t)dev; + static uint8_t dummyTxByte = 0xff; + static uint8_t dummyRxByte; - // TODO check correct, was len + 1 now len - dma_channel_set_trans_count(dev->bus->dmaTx->channel, dev->bus->curSegment->len, false); - dma_channel_set_trans_count(dev->bus->dmaRx->channel, dev->bus->curSegment->len, false); + busDevice_t *bus = dev->bus; + spi_inst_t *spi = SPI_INST(bus->busType_u.spi.instance); + bus->dmaRx->userParam = (uint32_t)dev; + volatile busSegment_t *segment = bus->curSegment; + int xferLen = segment->len; - dma_start_channel_mask((1 << dev->bus->dmaTx->channel) | (1 << dev->bus->dmaRx->channel)); + const uint8_t *txBuffer = segment->u.buffers.txData; + uint8_t *rxBuffer = segment->u.buffers.rxData; + + // Configure channels using the config that was created in spiInternalInitStream + io_rw_32 *dr_ptr = &spi_get_hw(spi)->dr; + dma_channel_configure(bus->dmaTx->channel, bus->dmaInitTx, dr_ptr, txBuffer ? txBuffer : &dummyTxByte, xferLen, false); + dma_channel_configure(bus->dmaRx->channel, bus->dmaInitRx, rxBuffer ? rxBuffer : &dummyRxByte, dr_ptr, xferLen, false); + + uint32_t channelMask = (1 << bus->dmaTx->channel) | (1 << bus->dmaRx->channel); + dma_start_channel_mask(channelMask); #endif } // DMA transfer setup and start void spiSequenceStart(const extDevice_t *dev) { - //TODO: implementation for PICO - // base on STM32/bus_spi_ll.c - busDevice_t *bus = dev->bus; SPI_TypeDef *instance = bus->busType_u.spi.instance; spiDevice_t *spi = &spiDevice[spiDeviceByInstance(instance)]; @@ -388,10 +417,7 @@ void spiSequenceStart(const extDevice_t *dev) #endif uint32_t xferLen = 0; uint32_t segmentCount = 0; - - // bus->initSegment = true; - // Switch bus speed if (dev->busType_u.spi.speed != bus->busType_u.spi.speed) { diff --git a/src/platform/PICO/include/platform/platform.h b/src/platform/PICO/include/platform/platform.h index 0110506a0c..d6e5d3fb2e 100644 --- a/src/platform/PICO/include/platform/platform.h +++ b/src/platform/PICO/include/platform/platform.h @@ -45,9 +45,9 @@ typedef enum {DISABLE = 0, ENABLE = !DISABLE} FunctionalState; //#define GPIO_InitTypeDef #define TIM_TypeDef void* //#define TIM_OCInitTypeDef + #define DMA_TypeDef void* -#define DMA_InitTypeDef void* -//#define DMA_Channel_TypeDef +#define DMA_InitTypeDef dma_channel_config #define ADC_TypeDef void* #define USART_TypeDef uart_inst_t diff --git a/src/platform/STM32/bus_spi_ll.c b/src/platform/STM32/bus_spi_ll.c index 2a12575335..71857318a4 100644 --- a/src/platform/STM32/bus_spi_ll.c +++ b/src/platform/STM32/bus_spi_ll.c @@ -268,23 +268,11 @@ FAST_CODE bool spiInternalReadWriteBufPolled(SPI_TypeDef *instance, const uint8_ return true; } -void spiInternalInitStream(const extDevice_t *dev, bool preInit) +void spiInternalInitStream(const extDevice_t *dev, volatile busSegment_t *segment) { STATIC_DMA_DATA_AUTO uint8_t dummyTxByte = 0xff; STATIC_DMA_DATA_AUTO uint8_t dummyRxByte; busDevice_t *bus = dev->bus; - - busSegment_t *segment = (busSegment_t *)bus->curSegment; - - if (preInit) { - // Prepare the init structure for the next segment to reduce inter-segment interval - segment++; - if(segment->len == 0) { - // There's no following segment - return; - } - } - int len = segment->len; uint8_t *txData = segment->u.buffers.txData; diff --git a/src/platform/STM32/bus_spi_stdperiph.c b/src/platform/STM32/bus_spi_stdperiph.c index 9bb02fba80..5c37d8f5e6 100644 --- a/src/platform/STM32/bus_spi_stdperiph.c +++ b/src/platform/STM32/bus_spi_stdperiph.c @@ -162,23 +162,11 @@ bool spiInternalReadWriteBufPolled(SPI_TypeDef *instance, const uint8_t *txData, return true; } -void spiInternalInitStream(const extDevice_t *dev, bool preInit) +void spiInternalInitStream(const extDevice_t *dev, volatile busSegment_t *segment) { STATIC_DMA_DATA_AUTO uint8_t dummyTxByte = 0xff; STATIC_DMA_DATA_AUTO uint8_t dummyRxByte; busDevice_t *bus = dev->bus; - - volatile busSegment_t *segment = bus->curSegment; - - if (preInit) { - // Prepare the init structure for the next segment to reduce inter-segment interval - segment++; - if(segment->len == 0) { - // There's no following segment - return; - } - } - int len = segment->len; uint8_t *txData = segment->u.buffers.txData;