diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 0b463d6532..63cc4891a9 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -331,7 +331,7 @@ static const char * const lookupTableBusType[] = { #ifdef USE_MAX7456 static const char * const lookupTableMax7456Clock[] = { - "HALF", "DEFAULT", "FULL" + "HALF", "NOMINAL", "DOUBLE" }; #endif diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 3a8b7fa37e..cf19e9acbf 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -217,10 +217,7 @@ bool mpuAccReadSPI(accDev_t *acc) case GYRO_EXTI_INT: case GYRO_EXTI_NO_INT: { - // Ensure any prior DMA has completed before continuing - spiWaitClaim(&acc->gyro->dev); - - acc->gyro->dev.txBuf[0] = acc->gyro->accDataReg | 0x80; + acc->gyro->dev.txBuf[0] = MPU_RA_ACCEL_XOUT_H | 0x80; busSegment_t segments[] = { {NULL, NULL, 7, true, NULL}, @@ -274,8 +271,6 @@ bool mpuGyroReadSPI(gyroDev_t *gyro) gyro->gyroDmaMaxDuration = 5; if (gyro->detectedEXTI > GYRO_EXTI_DETECT_THRESHOLD) { if (spiUseDMA(&gyro->dev)) { - // Indicate that the bus on which this device resides may initiate DMA transfers from interrupt context - spiSetAtomicWait(&gyro->dev); gyro->dev.callbackArg = (uint32_t)gyro; gyro->dev.txBuf[0] = gyro->accDataReg | 0x80; gyro->segments[0].len = gyro->gyroDataReg - gyro->accDataReg + 7; @@ -299,10 +294,7 @@ bool mpuGyroReadSPI(gyroDev_t *gyro) case GYRO_EXTI_INT: case GYRO_EXTI_NO_INT: { - // Ensure any prior DMA has completed before continuing - spiWaitClaim(&gyro->dev); - - gyro->dev.txBuf[0] = gyro->gyroDataReg | 0x80; + gyro->dev.txBuf[0] = MPU_RA_GYRO_XOUT_H | 0x80; busSegment_t segments[] = { {NULL, NULL, 7, true, NULL}, diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c b/src/main/drivers/accgyro/accgyro_spi_bmi270.c index 00dc806ed6..85a4a7c826 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi270.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.c @@ -316,9 +316,6 @@ static bool bmi270AccRead(accDev_t *acc) case GYRO_EXTI_INT: case GYRO_EXTI_NO_INT: { - // Ensure any prior DMA has completed before continuing - spiWaitClaim(&acc->gyro->dev); - acc->gyro->dev.txBuf[0] = BMI270_REG_ACC_DATA_X_LSB | 0x80; busSegment_t segments[] = { @@ -374,8 +371,6 @@ static bool bmi270GyroReadRegister(gyroDev_t *gyro) // Using DMA for gyro access upsets the scheduler on the F4 if (gyro->detectedEXTI > GYRO_EXTI_DETECT_THRESHOLD) { if (spiUseDMA(&gyro->dev)) { - // Indicate that the bus on which this device resides may initiate DMA transfers from interrupt context - spiSetAtomicWait(&gyro->dev); gyro->dev.callbackArg = (uint32_t)gyro; gyro->dev.txBuf[0] = BMI270_REG_ACC_DATA_X_LSB | 0x80; gyro->segments[0].len = 14; @@ -399,9 +394,6 @@ static bool bmi270GyroReadRegister(gyroDev_t *gyro) case GYRO_EXTI_INT: case GYRO_EXTI_NO_INT: { - // Ensure any prior DMA has completed before continuing - spiWaitClaim(&gyro->dev); - gyro->dev.txBuf[0] = BMI270_REG_GYR_DATA_X_LSB | 0x80; busSegment_t segments[] = { diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 0525c7bf5c..2db5006458 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -60,7 +60,6 @@ typedef struct busDevice_s { } mpuSlave; } busType_u; bool useDMA; - bool useAtomicWait; uint8_t deviceCount; dmaChannelDescriptor_t *dmaTx; dmaChannelDescriptor_t *dmaRx; diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 99d3c9549f..3f8001e168 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -39,6 +39,8 @@ #include "nvic.h" #include "pg/bus_spi.h" +#define NUM_QUEUE_SEGS 5 + static uint8_t spiRegisteredDeviceCount = 0; spiDevice_t spiDevice[SPIDEV_COUNT]; @@ -157,34 +159,6 @@ bool spiIsBusy(const extDevice_t *dev) 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 -void spiSetAtomicWait(const extDevice_t *dev) -{ - dev->bus->useAtomicWait = true; -} - -// Wait for DMA completion and claim the bus driver -void spiWaitClaim(const extDevice_t *dev) -{ - // If there is a device on the bus whose driver might call spiSequence from an ISR then an - // atomic access is required to claim the bus, however if not, then interrupts need not be - // disabled as this can result in edge triggered interrupts being missed - - if (dev->bus->useAtomicWait) { - // Prevent race condition where the bus appears free, but a gyro interrupt starts a transfer - do { - 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 *)BUS_SPI_LOCKED); - } else { - // Wait for completion - while (dev->bus->curSegment != (busSegment_t *)BUS_SPI_FREE); - } -} - // Wait for DMA completion void spiWait(const extDevice_t *dev) { @@ -201,9 +175,6 @@ void spiReadWriteBuf(const extDevice_t *dev, uint8_t *txData, uint8_t *rxData, i {NULL, NULL, 0, true, NULL}, }; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(dev); - spiSequence(dev, &segments[0]); spiWait(dev); @@ -233,9 +204,6 @@ uint8_t spiReadWrite(const extDevice_t *dev, uint8_t data) {NULL, NULL, 0, true, NULL}, }; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(dev); - spiSequence(dev, &segments[0]); spiWait(dev); @@ -255,9 +223,6 @@ uint8_t spiReadWriteReg(const extDevice_t *dev, uint8_t reg, uint8_t data) {NULL, NULL, 0, true, NULL}, }; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(dev); - spiSequence(dev, &segments[0]); spiWait(dev); @@ -274,9 +239,6 @@ void spiWrite(const extDevice_t *dev, uint8_t data) {NULL, NULL, 0, true, NULL}, }; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(dev); - spiSequence(dev, &segments[0]); spiWait(dev); @@ -292,9 +254,6 @@ void spiWriteReg(const extDevice_t *dev, uint8_t reg, uint8_t data) {NULL, NULL, 0, true, NULL}, }; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(dev); - spiSequence(dev, &segments[0]); spiWait(dev); @@ -323,9 +282,6 @@ void spiReadRegBuf(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t l {NULL, NULL, 0, true, NULL}, }; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(dev); - spiSequence(dev, &segments[0]); spiWait(dev); @@ -360,9 +316,6 @@ void spiWriteRegBuf(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint32_t {NULL, NULL, 0, true, NULL}, }; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(dev); - spiSequence(dev, &segments[0]); spiWait(dev); @@ -379,9 +332,6 @@ uint8_t spiReadReg(const extDevice_t *dev, uint8_t reg) {NULL, NULL, 0, true, NULL}, }; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(dev); - spiSequence(dev, &segments[0]); spiWait(dev); @@ -414,6 +364,19 @@ uint16_t spiCalculateDivider(uint32_t freq) return divisor; } +uint32_t spiCalculateClock(uint16_t spiClkDivisor) +{ +#if defined(STM32F4) || defined(STM32G4) || defined(STM32F7) + uint32_t spiClk = SystemCoreClock / 2; +#elif defined(STM32H7) + uint32_t spiClk = 100000000; +#else +#error "Base SPI clock not defined for this architecture" +#endif + + return spiClk / spiClkDivisor; +} + // Interrupt handler for SPI receive DMA completion static void spiIrqHandler(const extDevice_t *dev) { @@ -448,9 +411,10 @@ static void spiIrqHandler(const extDevice_t *dev) 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); + bus->curSegment = nextSegments; + nextSegment->txData = NULL; + spiSequenceStart(nextDev); } else { // The end of the segment list has been reached, so mark transactions as complete bus->curSegment = (busSegment_t *)BUS_SPI_FREE; @@ -559,7 +523,6 @@ bool spiSetBusInstance(extDevice_t *dev, uint32_t device) bus->busType = BUS_TYPE_SPI; bus->useDMA = false; - bus->useAtomicWait = false; bus->deviceCount = 1; bus->initTx = &dev->initTx; bus->initRx = &dev->initRx; @@ -743,28 +706,62 @@ uint8_t spiGetExtDeviceCount(const extDevice_t *dev) void spiSequence(const extDevice_t *dev, busSegment_t *segments) { busDevice_t *bus = dev->bus; + busSegment_t *queuedEndSegments[NUM_QUEUE_SEGS]; + uint8_t queuedEndSegmentsIndex = 0; 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 (spiIsBusy(dev)) { + busSegment_t *endSegment; - if (endSegment) { - // Find the last segment of the current transfer - for (; endSegment->len; endSegment++); + // Defer this transfer to be triggered upon completion of the current transfer + // Find the last segment of the current transfer + for (endSegment = segments; endSegment->len; endSegment++); + + busSegment_t *endCmpSegment = bus->curSegment; + + if (endCmpSegment) { + while (true) { + // Find the last segment of the current transfer + for (; endCmpSegment->len; endCmpSegment++); + + if (endCmpSegment == endSegment) { + // Attempt to use the new segment list twice in the same queue. Abort. + return; + } + + for (uint8_t n = 0; n < queuedEndSegmentsIndex; n++) { + if (endCmpSegment == queuedEndSegments[n]) { + // Attempt to use the same segment list twice in the same queue. Abort. + return; + } + } + + queuedEndSegments[queuedEndSegmentsIndex++] = endCmpSegment; + + if (queuedEndSegmentsIndex == NUM_QUEUE_SEGS) { + // Queue is too long. Abort. + return; + } + + if (endCmpSegment->txData == NULL) { + break; + } else { + endCmpSegment = (busSegment_t *)endCmpSegment->rxData; + } + } // Record the dev and segments parameters in the terminating segment entry - endSegment->txData = (uint8_t *)dev; - endSegment->rxData = (uint8_t *)segments; + endCmpSegment->txData = (uint8_t *)dev; + endCmpSegment->rxData = (uint8_t *)segments; return; } + } else { + // Claim the bus with this list of segments + bus->curSegment = segments; } } - spiSequenceStart(dev, segments); + spiSequenceStart(dev); } #endif diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 4e0268129f..efd4c45906 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -116,6 +116,8 @@ SPI_TypeDef *spiInstanceByDevice(SPIDevice device); bool spiSetBusInstance(extDevice_t *dev, uint32_t device); // Determine the divisor to use for a given bus frequency uint16_t spiCalculateDivider(uint32_t freq); +// Return the SPI clock based on the given divisor +uint32_t spiCalculateClock(uint16_t spiClkDivisor); // Set the clock divisor to be used for accesses by the given device void spiSetClkDivisor(const extDevice_t *dev, uint16_t divider); // Set the clock phase/polarity to be used for accesses by the given device @@ -127,10 +129,6 @@ void spiDmaEnable(const extDevice_t *dev, bool enable); void spiSequence(const extDevice_t *dev, busSegment_t *segments); // Wait for DMA completion void spiWait(const extDevice_t *dev); -// Indicate that the bus on which this device resides may initiate DMA transfers from interrupt context -void spiSetAtomicWait(const extDevice_t *dev); -// Wait for DMA completion and claim the bus driver - use this when waiting for a prior access to complete before starting a new one -void spiWaitClaim(const extDevice_t *dev); // Return true if DMA engine is busy bool spiIsBusy(const extDevice_t *dev); diff --git a/src/main/drivers/bus_spi_impl.h b/src/main/drivers/bus_spi_impl.h index 57d55dcf94..fdc7aac8d7 100644 --- a/src/main/drivers/bus_spi_impl.h +++ b/src/main/drivers/bus_spi_impl.h @@ -33,7 +33,6 @@ #endif #define BUS_SPI_FREE 0x0 -#define BUS_SPI_LOCKED 0x4 typedef struct spiPinDef_s { ioTag_t pin; @@ -90,5 +89,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); +void spiSequenceStart(const extDevice_t *dev); diff --git a/src/main/drivers/bus_spi_ll.c b/src/main/drivers/bus_spi_ll.c index f8c2d27336..01f9ac7062 100644 --- a/src/main/drivers/bus_spi_ll.c +++ b/src/main/drivers/bus_spi_ll.c @@ -539,7 +539,7 @@ void spiInternalStopDMA (const extDevice_t *dev) } // DMA transfer setup and start -void spiSequenceStart(const extDevice_t *dev, busSegment_t *segments) +void spiSequenceStart(const extDevice_t *dev) { busDevice_t *bus = dev->bus; SPI_TypeDef *instance = bus->busType_u.spi.instance; @@ -549,7 +549,6 @@ void spiSequenceStart(const extDevice_t *dev, busSegment_t *segments) uint32_t segmentCount = 0; bus->initSegment = true; - bus->curSegment = segments; // Switch bus speed #if !defined(STM32H7) @@ -689,8 +688,10 @@ void spiSequenceStart(const extDevice_t *dev, busSegment_t *segments) 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); + busSegment_t *endSegment = bus->curSegment; + bus->curSegment = nextSegments; + endSegment->txData = NULL; + spiSequenceStart(nextDev); } else { // The end of the segment list has been reached, so mark transactions as complete bus->curSegment = (busSegment_t *)BUS_SPI_FREE; diff --git a/src/main/drivers/bus_spi_stdperiph.c b/src/main/drivers/bus_spi_stdperiph.c index 61d70b9f7f..d775e3530e 100644 --- a/src/main/drivers/bus_spi_stdperiph.c +++ b/src/main/drivers/bus_spi_stdperiph.c @@ -320,7 +320,7 @@ void spiInternalStopDMA (const extDevice_t *dev) } // DMA transfer setup and start -void spiSequenceStart(const extDevice_t *dev, busSegment_t *segments) +void spiSequenceStart(const extDevice_t *dev) { busDevice_t *bus = dev->bus; SPI_TypeDef *instance = bus->busType_u.spi.instance; @@ -329,7 +329,6 @@ void spiSequenceStart(const extDevice_t *dev, busSegment_t *segments) uint32_t segmentCount = 0; dev->bus->initSegment = true; - dev->bus->curSegment = segments; SPI_Cmd(instance, DISABLE); @@ -415,8 +414,10 @@ void spiSequenceStart(const extDevice_t *dev, busSegment_t *segments) 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); + busSegment_t *endSegment = bus->curSegment; + bus->curSegment = nextSegments; + endSegment->txData = NULL; + spiSequenceStart(nextDev); } else { // The end of the segment list has been reached, so mark transactions as complete bus->curSegment = (busSegment_t *)BUS_SPI_FREE; diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index 69dc1fb570..195731664a 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -288,7 +288,7 @@ static void m25p16_eraseSector(flashDevice_t *fdevice, uint32_t address) }; // Ensure any prior DMA has completed before continuing - spiWaitClaim(fdevice->io.handle.dev); + spiWait(fdevice->io.handle.dev); m25p16_setCommandAddress(§orErase[1], address, fdevice->isLargeFlash); @@ -312,9 +312,6 @@ static void m25p16_eraseCompletely(flashDevice_t *fdevice) {NULL, NULL, 0, true, NULL}, }; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(fdevice->io.handle.dev); - spiSequence(fdevice->io.handle.dev, segments); // Block pending completion of SPI access, but the erase will be ongoing @@ -346,7 +343,7 @@ static uint32_t m25p16_pageProgramContinue(flashDevice_t *fdevice, uint8_t const }; // Ensure any prior DMA has completed before continuing - spiWaitClaim(fdevice->io.handle.dev); + spiWait(fdevice->io.handle.dev); // Patch the pageProgram segment segments[PAGE_PROGRAM].len = fdevice->isLargeFlash ? 5 : 4; @@ -427,7 +424,7 @@ static int m25p16_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *b STATIC_DMA_DATA_AUTO uint8_t readBytes[5] = { M25P16_INSTRUCTION_READ_BYTES }; // Ensure any prior DMA has completed before continuing - spiWaitClaim(fdevice->io.handle.dev); + spiWait(fdevice->io.handle.dev); busSegment_t segments[] = { {readStatus, readyStatus, sizeof(readStatus), true, m25p16_callbackReady}, diff --git a/src/main/drivers/flash_w25n01g.c b/src/main/drivers/flash_w25n01g.c index 26f6c968e2..da47b6e419 100644 --- a/src/main/drivers/flash_w25n01g.c +++ b/src/main/drivers/flash_w25n01g.c @@ -154,9 +154,6 @@ static void w25n01g_performOneByteCommand(flashDeviceIO_t *io, uint8_t command) {NULL, NULL, 0, true, NULL}, }; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(dev); - spiSequence(dev, &segments[0]); // Block pending completion of SPI access @@ -182,9 +179,6 @@ static void w25n01g_performCommandWithPageAddress(flashDeviceIO_t *io, uint8_t c {NULL, NULL, 0, true, NULL}, }; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(dev); - spiSequence(dev, &segments[0]); // Block pending completion of SPI access @@ -419,9 +413,6 @@ static void w25n01g_programDataLoad(flashDevice_t *fdevice, uint16_t columnAddre {NULL, NULL, 0, true, NULL}, }; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(dev); - spiSequence(dev, &segments[0]); // Block pending completion of SPI access @@ -453,9 +444,6 @@ static void w25n01g_randomProgramDataLoad(flashDevice_t *fdevice, uint16_t colum {NULL, NULL, 0, true, NULL}, }; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(dev); - spiSequence(dev, &segments[0]); // Block pending completion of SPI access @@ -699,9 +687,6 @@ int w25n01g_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer, {NULL, NULL, 0, true, NULL}, }; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(dev); - spiSequence(dev, &segments[0]); // Block pending completion of SPI access @@ -867,9 +852,6 @@ void w25n01g_readBBLUT(flashDevice_t *fdevice, bblut_t *bblut, int lutsize) {NULL, NULL, 0, true, NULL}, }; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(dev); - spiSequence(dev, &segments[0]); // Block pending completion of SPI access diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 3547fe275a..e6a099b68e 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -56,6 +56,7 @@ #define DEBUG_MAX7456_SPICLOCK_OVERCLOCK 0 #define DEBUG_MAX7456_SPICLOCK_DEVTYPE 1 #define DEBUG_MAX7456_SPICLOCK_DIVISOR 2 +#define DEBUG_MAX7456_SPICLOCK_X100 3 // VM0 bits #define VIDEO_BUFFER_DISABLE 0x01 @@ -192,7 +193,7 @@ extDevice_t max7456Device; extDevice_t *dev = &max7456Device; static bool max7456DeviceDetected = false; -static uint16_t max7456SpiClock; +static uint16_t max7456SpiClockDiv; uint16_t maxScreenSize = VIDEO_BUFFER_CHARS_PAL; @@ -394,32 +395,35 @@ max7456InitStatus_e max7456Init(const max7456Config_t *max7456Config, const vcdP } #if defined(USE_OVERCLOCK) + max7456SpiClockDiv = spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ); + // Determine SPI clock divisor based on config and the device type. switch (max7456Config->clockConfig) { case MAX7456_CLOCK_CONFIG_HALF: - max7456SpiClock = spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ / 2); + max7456SpiClockDiv <<= 1; break; - case MAX7456_CLOCK_CONFIG_OC: - max7456SpiClock = (cpuOverclock && (max7456DeviceType == MAX7456_DEVICE_TYPE_MAX)) ? spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ / 2) : spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ); + case MAX7456_CLOCK_CONFIG_NOMINAL: + default: break; - case MAX7456_CLOCK_CONFIG_FULL: - max7456SpiClock = spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ); + case MAX7456_CLOCK_CONFIG_DOUBLE: + max7456SpiClockDiv >>= 1; break; } DEBUG_SET(DEBUG_MAX7456_SPICLOCK, DEBUG_MAX7456_SPICLOCK_OVERCLOCK, cpuOverclock); DEBUG_SET(DEBUG_MAX7456_SPICLOCK, DEBUG_MAX7456_SPICLOCK_DEVTYPE, max7456DeviceType); - DEBUG_SET(DEBUG_MAX7456_SPICLOCK, DEBUG_MAX7456_SPICLOCK_DIVISOR, max7456SpiClock); + DEBUG_SET(DEBUG_MAX7456_SPICLOCK, DEBUG_MAX7456_SPICLOCK_DIVISOR, max7456SpiClockDiv); + DEBUG_SET(DEBUG_MAX7456_SPICLOCK, DEBUG_MAX7456_SPICLOCK_X100, spiCalculateClock(max7456SpiClockDiv) / 10000); #else UNUSED(max7456Config); UNUSED(cpuOverclock); - max7456SpiClock = spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ); + max7456SpiClockDiv = spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ); #endif - spiSetClkDivisor(dev, max7456SpiClock); + spiSetClkDivisor(dev, max7456SpiClockDiv); // force soft reset on Max7456 spiWriteReg(dev, MAX7456ADD_VM0, MAX7456_RESET); @@ -631,9 +635,6 @@ bool max7456DrawScreen(void) return true; } - // Ensure any prior DMA has completed before overwriting the buffer - spiWaitClaim(dev); - // Allow for 8 bytes followed by an ESCAPE and reset of DMM at end of buffer maxSpiBufStartIndex -= 12; diff --git a/src/main/drivers/rx/rx_spi.c b/src/main/drivers/rx/rx_spi.c index 3778fcf2ba..58d11953cd 100644 --- a/src/main/drivers/rx/rx_spi.c +++ b/src/main/drivers/rx/rx_spi.c @@ -98,8 +98,6 @@ bool rxSpiDeviceInit(const rxSpiConfig_t *rxSpiConfig) return false; } - spiSetAtomicWait(dev); - const IO_t rxCsPin = IOGetByTag(rxSpiConfig->csnTag); IOInit(rxCsPin, OWNER_RX_SPI_CS, 0); IOConfigGPIO(rxCsPin, SPI_IO_CS_CFG); diff --git a/src/main/drivers/sdcard_spi.c b/src/main/drivers/sdcard_spi.c index df6f9a3106..63d941f654 100644 --- a/src/main/drivers/sdcard_spi.c +++ b/src/main/drivers/sdcard_spi.c @@ -174,9 +174,6 @@ static bool sdcard_waitForIdle(int maxBytesToWait) sdcard.idleCount = maxBytesToWait; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(&sdcard.dev); - spiSequence(&sdcard.dev, &segments[0]); // Block pending completion of SPI access @@ -202,9 +199,6 @@ static uint8_t sdcard_waitForNonIdleByte(int maxDelay) sdcard.idleCount = maxDelay; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(&sdcard.dev); - spiSequence(&sdcard.dev, &segments[0]); // Block pending completion of SPI access @@ -248,9 +242,6 @@ static uint8_t sdcard_sendCommand(uint8_t commandCode, uint32_t commandArgument) sdcard.idleCount = SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(&sdcard.dev); - spiSequence(&sdcard.dev, &segments[0]); // Block pending completion of SPI access @@ -374,9 +365,6 @@ static sdcardReceiveBlockStatus_e sdcard_receiveDataBlock(uint8_t *buffer, int c {NULL, NULL, 0, false, NULL}, }; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(&sdcard.dev); - spiSequence(&sdcard.dev, &segments[0]); // Block pending completion of SPI access @@ -396,9 +384,6 @@ static bool sdcard_sendDataBlockFinish(void) {NULL, NULL, 0, false, NULL}, }; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(&sdcard.dev); - spiSequence(&sdcard.dev, &segments[0]); // Block pending completion of SPI access @@ -440,9 +425,6 @@ static void sdcard_sendDataBlockBegin(uint8_t *buffer, bool multiBlockWrite) segments[2].txData = buffer; segments[2].len = spiUseDMA(&sdcard.dev) ? SDCARD_BLOCK_SIZE : SDCARD_NON_DMA_CHUNK_SIZE; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(&sdcard.dev); - spiSequence(&sdcard.dev, &segments[0]); // Don't block pending completion of SPI access @@ -584,9 +566,6 @@ static void sdcardSpi_init(const sdcardConfig_t *config, const spiPinConfig_t *s {NULL, NULL, 0, false, NULL}, }; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(&sdcard.dev); - spiSequence(&sdcard.dev, &segments[0]); // Block pending completion of SPI access @@ -639,9 +618,6 @@ static sdcardOperationStatus_e sdcard_endWriteBlocks(void) {NULL, NULL, 0, false, NULL}, }; - // Ensure any prior DMA has completed before continuing - spiWaitClaim(&sdcard.dev); - spiSequence(&sdcard.dev, &segments[0]); // Block pending completion of SPI access diff --git a/src/main/pg/max7456.c b/src/main/pg/max7456.c index cc584673ca..82d3291fda 100644 --- a/src/main/pg/max7456.c +++ b/src/main/pg/max7456.c @@ -34,7 +34,7 @@ PG_REGISTER_WITH_RESET_FN(max7456Config_t, max7456Config, PG_MAX7456_CONFIG, 0); void pgResetFn_max7456Config(max7456Config_t *config) { - config->clockConfig = MAX7456_CLOCK_CONFIG_DEFAULT; + config->clockConfig = MAX7456_CLOCK_CONFIG_NOMINAL; config->csTag = IO_TAG(MAX7456_SPI_CS_PIN); config->spiDevice = SPI_DEV_TO_CFG(spiDeviceByInstance(MAX7456_SPI_INSTANCE)); config->preInitOPU = false; diff --git a/src/main/pg/max7456.h b/src/main/pg/max7456.h index 42cc2ba8bf..3ea55d2d67 100644 --- a/src/main/pg/max7456.h +++ b/src/main/pg/max7456.h @@ -24,15 +24,15 @@ #include "pg/pg.h" typedef struct max7456Config_s { - uint8_t clockConfig; // SPI clock based on device type and overclock state (MAX7456_CLOCK_CONFIG_xxxx) + uint8_t clockConfig; // SPI clock modifier ioTag_t csTag; uint8_t spiDevice; bool preInitOPU; } max7456Config_t; // clockConfig values -#define MAX7456_CLOCK_CONFIG_HALF 0 // Force half clock -#define MAX7456_CLOCK_CONFIG_OC 1 // Half clock if OC -#define MAX7456_CLOCK_CONFIG_FULL 2 // Force full clock +#define MAX7456_CLOCK_CONFIG_HALF 0 // Force half clock +#define MAX7456_CLOCK_CONFIG_NOMINAL 1 // Nominal clock (default) +#define MAX7456_CLOCK_CONFIG_DOUBLE 2 // Double clock PG_DECLARE(max7456Config_t, max7456Config);