From 7ef7795944896063b955488b1946f97f5a9ab26d Mon Sep 17 00:00:00 2001 From: Steve Evans Date: Sun, 3 Oct 2021 01:14:58 +0100 Subject: [PATCH] Support dual gyros sharing a common SPI bus --- src/main/drivers/accgyro/accgyro.h | 1 + src/main/drivers/accgyro/accgyro_mpu.c | 19 +++---- src/main/drivers/bus.h | 6 +++ src/main/drivers/bus_spi.c | 65 ++++++++++++++++++----- src/main/drivers/bus_spi_impl.h | 4 ++ src/main/drivers/bus_spi_ll.c | 14 +++-- src/main/drivers/bus_spi_stdperiph.c | 13 ++++- src/main/drivers/compass/compass_ak8963.c | 3 ++ src/main/drivers/exti.c | 33 ++++++------ src/main/drivers/flash_m25p16.c | 1 + 10 files changed, 114 insertions(+), 45 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index 39c169913b..1e49961ae8 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -112,6 +112,7 @@ typedef struct gyroDev_s { uint32_t gyroSyncEXTI; int32_t gyroShortPeriod; int32_t gyroDmaMaxDuration; + busSegment_t segments[2]; #endif volatile bool dataReady; bool gyro_high_fsr; diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 18d9f93707..a2318ba3af 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -129,11 +129,6 @@ busStatus_e mpuIntcallback(uint32_t arg) static void mpuIntExtiHandler(extiCallbackRec_t *cb) { - // Non-blocking, so this needs to be static - static busSegment_t segments[] = { - {NULL, NULL, 15, true, mpuIntcallback}, - {NULL, NULL, 0, true, NULL}, - }; gyroDev_t *gyro = container_of(cb, gyroDev_t, exti); // Ideally we'd use a time to capture such information, but unfortunately the port used for EXTI interrupt does @@ -147,12 +142,7 @@ static void mpuIntExtiHandler(extiCallbackRec_t *cb) gyro->gyroLastEXTI = nowCycles; if (gyro->gyroModeSPI == GYRO_EXTI_INT_DMA) { - segments[0].txData = gyro->dev.txBuf;; - segments[0].rxData = &gyro->dev.rxBuf[1]; - - if (!spiIsBusy(&gyro->dev)) { - spiSequence(&gyro->dev, &segments[0]); - } + spiSequence(&gyro->dev, gyro->segments); } gyro->detectedEXTI++; @@ -287,9 +277,14 @@ bool mpuGyroReadSPI(gyroDev_t *gyro) if (spiUseDMA(&gyro->dev)) { // Indicate that the bus on which this device resides may initiate DMA transfers from interrupt context spiSetAtomicWait(&gyro->dev); - gyro->gyroModeSPI = GYRO_EXTI_INT_DMA; gyro->dev.callbackArg = (uint32_t)gyro; gyro->dev.txBuf[0] = MPU_RA_ACCEL_XOUT_H | 0x80; + gyro->segments[0].len = 15; + gyro->segments[0].callback = mpuIntcallback; + gyro->segments[0].txData = gyro->dev.txBuf; + gyro->segments[0].rxData = &gyro->dev.rxBuf[1]; + gyro->segments[0].negateCS = true; + gyro->gyroModeSPI = GYRO_EXTI_INT_DMA; } else { // Interrupts are present, but no DMA gyro->gyroModeSPI = GYRO_EXTI_INT; diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 598982d8d1..0525c7bf5c 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -82,7 +82,13 @@ typedef struct busDevice_s { * is defined by a segment, with optional callback after each is completed */ typedef struct busSegment_s { + /* Note that txData may point to the transmit buffer, or in the case of the final segment to + * a const extDevice_t * structure to link to the next transfer. + */ uint8_t *txData; + /* Note that rxData may point to the receive buffer, or in the case of the final segment to + * a busSegment_t * structure to link to the next transfer. + */ uint8_t *rxData; int len; bool negateCS; // Should CS be negated at the end of this segment diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 2fbddc9c51..eb1c93e1b0 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -137,7 +137,7 @@ bool spiInit(SPIDevice device) // Return true if DMA engine is busy bool spiIsBusy(const extDevice_t *dev) { - return (dev->bus->curSegment != (busSegment_t *)NULL); + return (dev->bus->curSegment != (busSegment_t *)BUS_SPI_FREE); } // Indicate that the bus on which this device resides may initiate DMA transfers from interrupt context @@ -156,15 +156,15 @@ void spiWaitClaim(const extDevice_t *dev) if (dev->bus->useAtomicWait) { // Prevent race condition where the bus appears free, but a gyro interrupt starts a transfer do { - ATOMIC_BLOCK(NVIC_PRIO_MPU_INT_EXTI) { - if (dev->bus->curSegment == (busSegment_t *)NULL) { - dev->bus->curSegment = (busSegment_t *)0x04; + ATOMIC_BLOCK(NVIC_PRIO_MAX) { + if (dev->bus->curSegment == (busSegment_t *)BUS_SPI_FREE) { + dev->bus->curSegment = (busSegment_t *)BUS_SPI_LOCKED; } } - } while (dev->bus->curSegment != (busSegment_t *)0x04); + } while (dev->bus->curSegment != (busSegment_t *)BUS_SPI_LOCKED); } else { // Wait for completion - while (dev->bus->curSegment != (busSegment_t *)NULL); + while (dev->bus->curSegment != (busSegment_t *)BUS_SPI_FREE); } } @@ -172,7 +172,7 @@ void spiWaitClaim(const extDevice_t *dev) void spiWait(const extDevice_t *dev) { // Wait for completion - while (dev->bus->curSegment != (busSegment_t *)NULL); + while (dev->bus->curSegment != (busSegment_t *)BUS_SPI_FREE); } // Wait for bus to become free, then read/write block of data @@ -407,6 +407,7 @@ static void spiRxIrqHandler(dmaChannelDescriptor_t* descriptor) } busDevice_t *bus = dev->bus; + busSegment_t *nextSegment; if (bus->curSegment->negateCS) { // Negate Chip Select @@ -451,12 +452,23 @@ static void spiRxIrqHandler(dmaChannelDescriptor_t* descriptor) } // Advance through the segment list - bus->curSegment++; + nextSegment = bus->curSegment + 1; - if (bus->curSegment->len == 0) { - // The end of the segment list has been reached, so mark transactions as complete - bus->curSegment = (busSegment_t *)NULL; + if (nextSegment->len == 0) { + // If a following transaction has been linked, start it + if (nextSegment->txData) { + const extDevice_t *nextDev = (const extDevice_t *)nextSegment->txData; + busSegment_t *nextSegments = (busSegment_t *)nextSegment->rxData; + nextSegment->txData = NULL; + // The end of the segment list has been reached + spiSequenceStart(nextDev, nextSegments); + } else { + // The end of the segment list has been reached, so mark transactions as complete + bus->curSegment = (busSegment_t *)NULL; + } } else { + bus->curSegment = nextSegment; + // After the completion of the first segment setup the init structure for the subsequent segment if (bus->initSegment) { spiInternalInitStream(dev, false); @@ -479,6 +491,7 @@ bool spiSetBusInstance(extDevice_t *dev, uint32_t device) } dev->bus = &spiBusDevice[SPI_CFG_TO_DEV(device)]; + dev->useDMA = true; if (dev->bus->busType == BUS_TYPE_SPI) { // This bus has already been initialised @@ -495,7 +508,6 @@ bool spiSetBusInstance(extDevice_t *dev, uint32_t device) } bus->busType = BUS_TYPE_SPI; - dev->useDMA = true; bus->useDMA = false; bus->useAtomicWait = false; bus->deviceCount = 1; @@ -637,4 +649,33 @@ uint8_t spiGetExtDeviceCount(const extDevice_t *dev) { return dev->bus->deviceCount; } + +// DMA transfer setup and start +void spiSequence(const extDevice_t *dev, busSegment_t *segments) +{ + busDevice_t *bus = dev->bus; + + ATOMIC_BLOCK(NVIC_PRIO_MAX) { + if ((bus->curSegment != (busSegment_t *)BUS_SPI_LOCKED) && spiIsBusy(dev)) { + /* Defer this transfer to be triggered upon completion of the current transfer. Blocking calls + * and those from non-interrupt context will have already called spiWaitClaim() so this will + * only happen for non-blocking calls called from an ISR. + */ + busSegment_t *endSegment = bus->curSegment; + + if (endSegment) { + // Find the last segment of the current transfer + for (; endSegment->len; endSegment++); + + // Record the dev and segments parameters in the terminating segment entry + endSegment->txData = (uint8_t *)dev; + endSegment->rxData = (uint8_t *)segments; + + return; + } + } + } + + spiSequenceStart(dev, segments); +} #endif diff --git a/src/main/drivers/bus_spi_impl.h b/src/main/drivers/bus_spi_impl.h index fe07f32e60..57d55dcf94 100644 --- a/src/main/drivers/bus_spi_impl.h +++ b/src/main/drivers/bus_spi_impl.h @@ -32,6 +32,9 @@ #error Unknown MCU family #endif +#define BUS_SPI_FREE 0x0 +#define BUS_SPI_LOCKED 0x4 + typedef struct spiPinDef_s { ioTag_t pin; #if defined(STM32F7) || defined(STM32H7) || defined(STM32G4) @@ -87,4 +90,5 @@ void spiInternalStartDMA(const extDevice_t *dev); void spiInternalStopDMA (const extDevice_t *dev); void spiInternalResetStream(dmaChannelDescriptor_t *descriptor); void spiInternalResetDescriptors(busDevice_t *bus); +void spiSequenceStart(const extDevice_t *dev, busSegment_t *segments); diff --git a/src/main/drivers/bus_spi_ll.c b/src/main/drivers/bus_spi_ll.c index a98f20d335..3dbf7cb13d 100644 --- a/src/main/drivers/bus_spi_ll.c +++ b/src/main/drivers/bus_spi_ll.c @@ -34,7 +34,6 @@ #include "drivers/bus_spi_impl.h" #include "drivers/dma.h" #include "drivers/io.h" -#include "drivers/nvic.h" #include "drivers/rcc.h" #ifndef SPI2_SCK_PIN @@ -471,7 +470,7 @@ void spiInternalStopDMA (const extDevice_t *dev) } // DMA transfer setup and start -void spiSequence(const extDevice_t *dev, busSegment_t *segments) +void spiSequenceStart(const extDevice_t *dev, busSegment_t *segments) { busDevice_t *bus = dev->bus; SPI_TypeDef *instance = bus->busType_u.spi.instance; @@ -612,7 +611,16 @@ void spiSequence(const extDevice_t *dev, busSegment_t *segments) bus->curSegment++; } - bus->curSegment = (busSegment_t *)NULL; + // If a following transaction has been linked, start it + if (bus->curSegment->txData) { + const extDevice_t *nextDev = (const extDevice_t *)bus->curSegment->txData; + busSegment_t *nextSegments = (busSegment_t *)bus->curSegment->rxData; + bus->curSegment->txData = NULL; + spiSequenceStart(nextDev, nextSegments); + } else { + // The end of the segment list has been reached, so mark transactions as complete + bus->curSegment = (busSegment_t *)NULL; + } } } #endif diff --git a/src/main/drivers/bus_spi_stdperiph.c b/src/main/drivers/bus_spi_stdperiph.c index 5dd1584317..ffd616d91b 100644 --- a/src/main/drivers/bus_spi_stdperiph.c +++ b/src/main/drivers/bus_spi_stdperiph.c @@ -270,7 +270,7 @@ void spiInternalStopDMA (const extDevice_t *dev) } // DMA transfer setup and start -void spiSequence(const extDevice_t *dev, busSegment_t *segments) +void spiSequenceStart(const extDevice_t *dev, busSegment_t *segments) { busDevice_t *bus = dev->bus; SPI_TypeDef *instance = bus->busType_u.spi.instance; @@ -360,7 +360,16 @@ void spiSequence(const extDevice_t *dev, busSegment_t *segments) bus->curSegment++; } - bus->curSegment = (busSegment_t *)NULL; + // If a following transaction has been linked, start it + if (bus->curSegment->txData) { + const extDevice_t *nextDev = (const extDevice_t *)bus->curSegment->txData; + busSegment_t *nextSegments = (busSegment_t *)bus->curSegment->rxData; + bus->curSegment->txData = NULL; + spiSequenceStart(nextDev, nextSegments); + } else { + // The end of the segment list has been reached, so mark transactions as complete + bus->curSegment = (busSegment_t *)NULL; + } } } #endif diff --git a/src/main/drivers/compass/compass_ak8963.c b/src/main/drivers/compass/compass_ak8963.c index 3f6372956a..621d419257 100644 --- a/src/main/drivers/compass/compass_ak8963.c +++ b/src/main/drivers/compass/compass_ak8963.c @@ -392,6 +392,9 @@ void ak8963BusInit(const extDevice_t *dev) case BUS_TYPE_MPU_SLAVE: rescheduleTask(TASK_COMPASS, TASK_PERIOD_HZ(40)); + // Disable DMA on gyro as this upsets slave access timing + spiDmaEnable(dev->bus->busType_u.mpuSlave.master, false); + // initialize I2C master via SPI bus ak8963SpiWriteRegisterDelay(dev->bus->busType_u.mpuSlave.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); ak8963SpiWriteRegisterDelay(dev->bus->busType_u.mpuSlave.master, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz diff --git a/src/main/drivers/exti.c b/src/main/drivers/exti.c index 8bc57a00a2..0bebb135f1 100644 --- a/src/main/drivers/exti.c +++ b/src/main/drivers/exti.c @@ -234,39 +234,40 @@ void EXTIEnable(IO_t io, bool enable) #define EXTI_EVENT_MASK 0xFFFF // first 16 bits only, see also definition of extiChannelRecs. -void EXTI_IRQHandler(void) +void EXTI_IRQHandler(uint32_t mask) { - uint32_t exti_active = (EXTI_REG_IMR & EXTI_REG_PR) & EXTI_EVENT_MASK; + uint32_t exti_active = (EXTI_REG_IMR & EXTI_REG_PR) & mask; + + EXTI_REG_PR = exti_active; // clear pending mask (by writing 1) while (exti_active) { unsigned idx = 31 - __builtin_clz(exti_active); uint32_t mask = 1 << idx; extiChannelRecs[idx].handler->fn(extiChannelRecs[idx].handler); - EXTI_REG_PR = mask; // clear pending mask (by writing 1) exti_active &= ~mask; } } -#define _EXTI_IRQ_HANDLER(name) \ - void name(void) { \ - EXTI_IRQHandler(); \ - } \ - struct dummy \ +#define _EXTI_IRQ_HANDLER(name, mask) \ + void name(void) { \ + EXTI_IRQHandler(mask & EXTI_EVENT_MASK); \ + } \ + struct dummy \ /**/ -_EXTI_IRQ_HANDLER(EXTI0_IRQHandler); -_EXTI_IRQ_HANDLER(EXTI1_IRQHandler); +_EXTI_IRQ_HANDLER(EXTI0_IRQHandler, 0x0001); +_EXTI_IRQ_HANDLER(EXTI1_IRQHandler, 0x0002); #if defined(STM32F1) || defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) -_EXTI_IRQ_HANDLER(EXTI2_IRQHandler); +_EXTI_IRQ_HANDLER(EXTI2_IRQHandler, 0x0004); #elif defined(STM32F3) -_EXTI_IRQ_HANDLER(EXTI2_TS_IRQHandler); +_EXTI_IRQ_HANDLER(EXTI2_TS_IRQHandler, 0x0004); #else # warning "Unknown CPU" #endif -_EXTI_IRQ_HANDLER(EXTI3_IRQHandler); -_EXTI_IRQ_HANDLER(EXTI4_IRQHandler); -_EXTI_IRQ_HANDLER(EXTI9_5_IRQHandler); -_EXTI_IRQ_HANDLER(EXTI15_10_IRQHandler); +_EXTI_IRQ_HANDLER(EXTI3_IRQHandler, 0x0008); +_EXTI_IRQ_HANDLER(EXTI4_IRQHandler, 0x0010); +_EXTI_IRQ_HANDLER(EXTI9_5_IRQHandler, 0x03e0); +_EXTI_IRQ_HANDLER(EXTI15_10_IRQHandler, 0xfc00); #endif diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index b1864def04..c58d6ad9b7 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -361,6 +361,7 @@ static uint32_t m25p16_pageProgramContinue(flashDevice_t *fdevice, uint8_t const segments[DATA1].negateCS = true; segments[DATA1].callback = m25p16_callbackWriteComplete; // Mark segment following data as being of zero length + segments[DATA2].txData = (uint8_t *)NULL; segments[DATA2].len = 0; } else if (bufferCount == 2) { segments[DATA1].negateCS = false;