1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 16:25:31 +03:00

Allow for HALF/NOMINAL/DOUBLE frequency of MAX7456 SPI clock

This commit is contained in:
Steve Evans 2021-12-29 23:05:16 +00:00
parent 96ac7953ae
commit 5ef34f79d5
16 changed files with 101 additions and 168 deletions

View file

@ -331,7 +331,7 @@ static const char * const lookupTableBusType[] = {
#ifdef USE_MAX7456 #ifdef USE_MAX7456
static const char * const lookupTableMax7456Clock[] = { static const char * const lookupTableMax7456Clock[] = {
"HALF", "DEFAULT", "FULL" "HALF", "NOMINAL", "DOUBLE"
}; };
#endif #endif

View file

@ -217,10 +217,7 @@ bool mpuAccReadSPI(accDev_t *acc)
case GYRO_EXTI_INT: case GYRO_EXTI_INT:
case GYRO_EXTI_NO_INT: case GYRO_EXTI_NO_INT:
{ {
// Ensure any prior DMA has completed before continuing acc->gyro->dev.txBuf[0] = MPU_RA_ACCEL_XOUT_H | 0x80;
spiWaitClaim(&acc->gyro->dev);
acc->gyro->dev.txBuf[0] = acc->gyro->accDataReg | 0x80;
busSegment_t segments[] = { busSegment_t segments[] = {
{NULL, NULL, 7, true, NULL}, {NULL, NULL, 7, true, NULL},
@ -274,8 +271,6 @@ bool mpuGyroReadSPI(gyroDev_t *gyro)
gyro->gyroDmaMaxDuration = 5; gyro->gyroDmaMaxDuration = 5;
if (gyro->detectedEXTI > GYRO_EXTI_DETECT_THRESHOLD) { if (gyro->detectedEXTI > GYRO_EXTI_DETECT_THRESHOLD) {
if (spiUseDMA(&gyro->dev)) { 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.callbackArg = (uint32_t)gyro;
gyro->dev.txBuf[0] = gyro->accDataReg | 0x80; gyro->dev.txBuf[0] = gyro->accDataReg | 0x80;
gyro->segments[0].len = gyro->gyroDataReg - gyro->accDataReg + 7; 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_INT:
case GYRO_EXTI_NO_INT: case GYRO_EXTI_NO_INT:
{ {
// Ensure any prior DMA has completed before continuing gyro->dev.txBuf[0] = MPU_RA_GYRO_XOUT_H | 0x80;
spiWaitClaim(&gyro->dev);
gyro->dev.txBuf[0] = gyro->gyroDataReg | 0x80;
busSegment_t segments[] = { busSegment_t segments[] = {
{NULL, NULL, 7, true, NULL}, {NULL, NULL, 7, true, NULL},

View file

@ -316,9 +316,6 @@ static bool bmi270AccRead(accDev_t *acc)
case GYRO_EXTI_INT: case GYRO_EXTI_INT:
case GYRO_EXTI_NO_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; acc->gyro->dev.txBuf[0] = BMI270_REG_ACC_DATA_X_LSB | 0x80;
busSegment_t segments[] = { busSegment_t segments[] = {
@ -374,8 +371,6 @@ static bool bmi270GyroReadRegister(gyroDev_t *gyro)
// Using DMA for gyro access upsets the scheduler on the F4 // Using DMA for gyro access upsets the scheduler on the F4
if (gyro->detectedEXTI > GYRO_EXTI_DETECT_THRESHOLD) { if (gyro->detectedEXTI > GYRO_EXTI_DETECT_THRESHOLD) {
if (spiUseDMA(&gyro->dev)) { 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.callbackArg = (uint32_t)gyro;
gyro->dev.txBuf[0] = BMI270_REG_ACC_DATA_X_LSB | 0x80; gyro->dev.txBuf[0] = BMI270_REG_ACC_DATA_X_LSB | 0x80;
gyro->segments[0].len = 14; gyro->segments[0].len = 14;
@ -399,9 +394,6 @@ static bool bmi270GyroReadRegister(gyroDev_t *gyro)
case GYRO_EXTI_INT: case GYRO_EXTI_INT:
case GYRO_EXTI_NO_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; gyro->dev.txBuf[0] = BMI270_REG_GYR_DATA_X_LSB | 0x80;
busSegment_t segments[] = { busSegment_t segments[] = {

View file

@ -60,7 +60,6 @@ typedef struct busDevice_s {
} mpuSlave; } mpuSlave;
} busType_u; } busType_u;
bool useDMA; bool useDMA;
bool useAtomicWait;
uint8_t deviceCount; uint8_t deviceCount;
dmaChannelDescriptor_t *dmaTx; dmaChannelDescriptor_t *dmaTx;
dmaChannelDescriptor_t *dmaRx; dmaChannelDescriptor_t *dmaRx;

View file

@ -39,6 +39,8 @@
#include "nvic.h" #include "nvic.h"
#include "pg/bus_spi.h" #include "pg/bus_spi.h"
#define NUM_QUEUE_SEGS 5
static uint8_t spiRegisteredDeviceCount = 0; static uint8_t spiRegisteredDeviceCount = 0;
spiDevice_t spiDevice[SPIDEV_COUNT]; spiDevice_t spiDevice[SPIDEV_COUNT];
@ -157,34 +159,6 @@ bool spiIsBusy(const extDevice_t *dev)
return (dev->bus->curSegment != (busSegment_t *)BUS_SPI_FREE); 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 // Wait for DMA completion
void spiWait(const extDevice_t *dev) 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}, {NULL, NULL, 0, true, NULL},
}; };
// Ensure any prior DMA has completed before continuing
spiWaitClaim(dev);
spiSequence(dev, &segments[0]); spiSequence(dev, &segments[0]);
spiWait(dev); spiWait(dev);
@ -233,9 +204,6 @@ uint8_t spiReadWrite(const extDevice_t *dev, uint8_t data)
{NULL, NULL, 0, true, NULL}, {NULL, NULL, 0, true, NULL},
}; };
// Ensure any prior DMA has completed before continuing
spiWaitClaim(dev);
spiSequence(dev, &segments[0]); spiSequence(dev, &segments[0]);
spiWait(dev); spiWait(dev);
@ -255,9 +223,6 @@ uint8_t spiReadWriteReg(const extDevice_t *dev, uint8_t reg, uint8_t data)
{NULL, NULL, 0, true, NULL}, {NULL, NULL, 0, true, NULL},
}; };
// Ensure any prior DMA has completed before continuing
spiWaitClaim(dev);
spiSequence(dev, &segments[0]); spiSequence(dev, &segments[0]);
spiWait(dev); spiWait(dev);
@ -274,9 +239,6 @@ void spiWrite(const extDevice_t *dev, uint8_t data)
{NULL, NULL, 0, true, NULL}, {NULL, NULL, 0, true, NULL},
}; };
// Ensure any prior DMA has completed before continuing
spiWaitClaim(dev);
spiSequence(dev, &segments[0]); spiSequence(dev, &segments[0]);
spiWait(dev); spiWait(dev);
@ -292,9 +254,6 @@ void spiWriteReg(const extDevice_t *dev, uint8_t reg, uint8_t data)
{NULL, NULL, 0, true, NULL}, {NULL, NULL, 0, true, NULL},
}; };
// Ensure any prior DMA has completed before continuing
spiWaitClaim(dev);
spiSequence(dev, &segments[0]); spiSequence(dev, &segments[0]);
spiWait(dev); 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}, {NULL, NULL, 0, true, NULL},
}; };
// Ensure any prior DMA has completed before continuing
spiWaitClaim(dev);
spiSequence(dev, &segments[0]); spiSequence(dev, &segments[0]);
spiWait(dev); 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}, {NULL, NULL, 0, true, NULL},
}; };
// Ensure any prior DMA has completed before continuing
spiWaitClaim(dev);
spiSequence(dev, &segments[0]); spiSequence(dev, &segments[0]);
spiWait(dev); spiWait(dev);
@ -379,9 +332,6 @@ uint8_t spiReadReg(const extDevice_t *dev, uint8_t reg)
{NULL, NULL, 0, true, NULL}, {NULL, NULL, 0, true, NULL},
}; };
// Ensure any prior DMA has completed before continuing
spiWaitClaim(dev);
spiSequence(dev, &segments[0]); spiSequence(dev, &segments[0]);
spiWait(dev); spiWait(dev);
@ -414,6 +364,19 @@ uint16_t spiCalculateDivider(uint32_t freq)
return divisor; 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 // Interrupt handler for SPI receive DMA completion
static void spiIrqHandler(const extDevice_t *dev) static void spiIrqHandler(const extDevice_t *dev)
{ {
@ -448,9 +411,10 @@ static void spiIrqHandler(const extDevice_t *dev)
if (nextSegment->txData) { if (nextSegment->txData) {
const extDevice_t *nextDev = (const extDevice_t *)nextSegment->txData; const extDevice_t *nextDev = (const extDevice_t *)nextSegment->txData;
busSegment_t *nextSegments = (busSegment_t *)nextSegment->rxData; busSegment_t *nextSegments = (busSegment_t *)nextSegment->rxData;
nextSegment->txData = NULL;
// The end of the segment list has been reached // The end of the segment list has been reached
spiSequenceStart(nextDev, nextSegments); bus->curSegment = nextSegments;
nextSegment->txData = NULL;
spiSequenceStart(nextDev);
} else { } else {
// The end of the segment list has been reached, so mark transactions as complete // The end of the segment list has been reached, so mark transactions as complete
bus->curSegment = (busSegment_t *)BUS_SPI_FREE; 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->busType = BUS_TYPE_SPI;
bus->useDMA = false; bus->useDMA = false;
bus->useAtomicWait = false;
bus->deviceCount = 1; bus->deviceCount = 1;
bus->initTx = &dev->initTx; bus->initTx = &dev->initTx;
bus->initRx = &dev->initRx; bus->initRx = &dev->initRx;
@ -743,28 +706,62 @@ uint8_t spiGetExtDeviceCount(const extDevice_t *dev)
void spiSequence(const extDevice_t *dev, busSegment_t *segments) void spiSequence(const extDevice_t *dev, busSegment_t *segments)
{ {
busDevice_t *bus = dev->bus; busDevice_t *bus = dev->bus;
busSegment_t *queuedEndSegments[NUM_QUEUE_SEGS];
uint8_t queuedEndSegmentsIndex = 0;
ATOMIC_BLOCK(NVIC_PRIO_MAX) { ATOMIC_BLOCK(NVIC_PRIO_MAX) {
if ((bus->curSegment != (busSegment_t *)BUS_SPI_LOCKED) && spiIsBusy(dev)) { if (spiIsBusy(dev)) {
/* Defer this transfer to be triggered upon completion of the current transfer. Blocking calls busSegment_t *endSegment;
* 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) { // Defer this transfer to be triggered upon completion of the current transfer
// Find the last segment of the current transfer // Find the last segment of the current transfer
for (; endSegment->len; endSegment++); 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 // Record the dev and segments parameters in the terminating segment entry
endSegment->txData = (uint8_t *)dev; endCmpSegment->txData = (uint8_t *)dev;
endSegment->rxData = (uint8_t *)segments; endCmpSegment->rxData = (uint8_t *)segments;
return; return;
} }
} else {
// Claim the bus with this list of segments
bus->curSegment = segments;
} }
} }
spiSequenceStart(dev, segments); spiSequenceStart(dev);
} }
#endif #endif

View file

@ -116,6 +116,8 @@ SPI_TypeDef *spiInstanceByDevice(SPIDevice device);
bool spiSetBusInstance(extDevice_t *dev, uint32_t device); bool spiSetBusInstance(extDevice_t *dev, uint32_t device);
// Determine the divisor to use for a given bus frequency // Determine the divisor to use for a given bus frequency
uint16_t spiCalculateDivider(uint32_t freq); 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 // Set the clock divisor to be used for accesses by the given device
void spiSetClkDivisor(const extDevice_t *dev, uint16_t divider); void spiSetClkDivisor(const extDevice_t *dev, uint16_t divider);
// Set the clock phase/polarity to be used for accesses by the given device // 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); void spiSequence(const extDevice_t *dev, busSegment_t *segments);
// Wait for DMA completion // Wait for DMA completion
void spiWait(const extDevice_t *dev); 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 // Return true if DMA engine is busy
bool spiIsBusy(const extDevice_t *dev); bool spiIsBusy(const extDevice_t *dev);

View file

@ -33,7 +33,6 @@
#endif #endif
#define BUS_SPI_FREE 0x0 #define BUS_SPI_FREE 0x0
#define BUS_SPI_LOCKED 0x4
typedef struct spiPinDef_s { typedef struct spiPinDef_s {
ioTag_t pin; ioTag_t pin;
@ -90,5 +89,5 @@ void spiInternalStartDMA(const extDevice_t *dev);
void spiInternalStopDMA (const extDevice_t *dev); void spiInternalStopDMA (const extDevice_t *dev);
void spiInternalResetStream(dmaChannelDescriptor_t *descriptor); void spiInternalResetStream(dmaChannelDescriptor_t *descriptor);
void spiInternalResetDescriptors(busDevice_t *bus); void spiInternalResetDescriptors(busDevice_t *bus);
void spiSequenceStart(const extDevice_t *dev, busSegment_t *segments); void spiSequenceStart(const extDevice_t *dev);

View file

@ -539,7 +539,7 @@ void spiInternalStopDMA (const extDevice_t *dev)
} }
// DMA transfer setup and start // 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; busDevice_t *bus = dev->bus;
SPI_TypeDef *instance = bus->busType_u.spi.instance; 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; uint32_t segmentCount = 0;
bus->initSegment = true; bus->initSegment = true;
bus->curSegment = segments;
// Switch bus speed // Switch bus speed
#if !defined(STM32H7) #if !defined(STM32H7)
@ -689,8 +688,10 @@ void spiSequenceStart(const extDevice_t *dev, busSegment_t *segments)
if (bus->curSegment->txData) { if (bus->curSegment->txData) {
const extDevice_t *nextDev = (const extDevice_t *)bus->curSegment->txData; const extDevice_t *nextDev = (const extDevice_t *)bus->curSegment->txData;
busSegment_t *nextSegments = (busSegment_t *)bus->curSegment->rxData; busSegment_t *nextSegments = (busSegment_t *)bus->curSegment->rxData;
bus->curSegment->txData = NULL; busSegment_t *endSegment = bus->curSegment;
spiSequenceStart(nextDev, nextSegments); bus->curSegment = nextSegments;
endSegment->txData = NULL;
spiSequenceStart(nextDev);
} else { } else {
// The end of the segment list has been reached, so mark transactions as complete // The end of the segment list has been reached, so mark transactions as complete
bus->curSegment = (busSegment_t *)BUS_SPI_FREE; bus->curSegment = (busSegment_t *)BUS_SPI_FREE;

View file

@ -320,7 +320,7 @@ void spiInternalStopDMA (const extDevice_t *dev)
} }
// DMA transfer setup and start // 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; busDevice_t *bus = dev->bus;
SPI_TypeDef *instance = bus->busType_u.spi.instance; 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; uint32_t segmentCount = 0;
dev->bus->initSegment = true; dev->bus->initSegment = true;
dev->bus->curSegment = segments;
SPI_Cmd(instance, DISABLE); SPI_Cmd(instance, DISABLE);
@ -415,8 +414,10 @@ void spiSequenceStart(const extDevice_t *dev, busSegment_t *segments)
if (bus->curSegment->txData) { if (bus->curSegment->txData) {
const extDevice_t *nextDev = (const extDevice_t *)bus->curSegment->txData; const extDevice_t *nextDev = (const extDevice_t *)bus->curSegment->txData;
busSegment_t *nextSegments = (busSegment_t *)bus->curSegment->rxData; busSegment_t *nextSegments = (busSegment_t *)bus->curSegment->rxData;
bus->curSegment->txData = NULL; busSegment_t *endSegment = bus->curSegment;
spiSequenceStart(nextDev, nextSegments); bus->curSegment = nextSegments;
endSegment->txData = NULL;
spiSequenceStart(nextDev);
} else { } else {
// The end of the segment list has been reached, so mark transactions as complete // The end of the segment list has been reached, so mark transactions as complete
bus->curSegment = (busSegment_t *)BUS_SPI_FREE; bus->curSegment = (busSegment_t *)BUS_SPI_FREE;

View file

@ -288,7 +288,7 @@ static void m25p16_eraseSector(flashDevice_t *fdevice, uint32_t address)
}; };
// Ensure any prior DMA has completed before continuing // Ensure any prior DMA has completed before continuing
spiWaitClaim(fdevice->io.handle.dev); spiWait(fdevice->io.handle.dev);
m25p16_setCommandAddress(&sectorErase[1], address, fdevice->isLargeFlash); m25p16_setCommandAddress(&sectorErase[1], address, fdevice->isLargeFlash);
@ -312,9 +312,6 @@ static void m25p16_eraseCompletely(flashDevice_t *fdevice)
{NULL, NULL, 0, true, NULL}, {NULL, NULL, 0, true, NULL},
}; };
// Ensure any prior DMA has completed before continuing
spiWaitClaim(fdevice->io.handle.dev);
spiSequence(fdevice->io.handle.dev, segments); spiSequence(fdevice->io.handle.dev, segments);
// Block pending completion of SPI access, but the erase will be ongoing // 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 // Ensure any prior DMA has completed before continuing
spiWaitClaim(fdevice->io.handle.dev); spiWait(fdevice->io.handle.dev);
// Patch the pageProgram segment // Patch the pageProgram segment
segments[PAGE_PROGRAM].len = fdevice->isLargeFlash ? 5 : 4; 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 }; STATIC_DMA_DATA_AUTO uint8_t readBytes[5] = { M25P16_INSTRUCTION_READ_BYTES };
// Ensure any prior DMA has completed before continuing // Ensure any prior DMA has completed before continuing
spiWaitClaim(fdevice->io.handle.dev); spiWait(fdevice->io.handle.dev);
busSegment_t segments[] = { busSegment_t segments[] = {
{readStatus, readyStatus, sizeof(readStatus), true, m25p16_callbackReady}, {readStatus, readyStatus, sizeof(readStatus), true, m25p16_callbackReady},

View file

@ -154,9 +154,6 @@ static void w25n01g_performOneByteCommand(flashDeviceIO_t *io, uint8_t command)
{NULL, NULL, 0, true, NULL}, {NULL, NULL, 0, true, NULL},
}; };
// Ensure any prior DMA has completed before continuing
spiWaitClaim(dev);
spiSequence(dev, &segments[0]); spiSequence(dev, &segments[0]);
// Block pending completion of SPI access // 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}, {NULL, NULL, 0, true, NULL},
}; };
// Ensure any prior DMA has completed before continuing
spiWaitClaim(dev);
spiSequence(dev, &segments[0]); spiSequence(dev, &segments[0]);
// Block pending completion of SPI access // 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}, {NULL, NULL, 0, true, NULL},
}; };
// Ensure any prior DMA has completed before continuing
spiWaitClaim(dev);
spiSequence(dev, &segments[0]); spiSequence(dev, &segments[0]);
// Block pending completion of SPI access // 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}, {NULL, NULL, 0, true, NULL},
}; };
// Ensure any prior DMA has completed before continuing
spiWaitClaim(dev);
spiSequence(dev, &segments[0]); spiSequence(dev, &segments[0]);
// Block pending completion of SPI access // 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}, {NULL, NULL, 0, true, NULL},
}; };
// Ensure any prior DMA has completed before continuing
spiWaitClaim(dev);
spiSequence(dev, &segments[0]); spiSequence(dev, &segments[0]);
// Block pending completion of SPI access // 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}, {NULL, NULL, 0, true, NULL},
}; };
// Ensure any prior DMA has completed before continuing
spiWaitClaim(dev);
spiSequence(dev, &segments[0]); spiSequence(dev, &segments[0]);
// Block pending completion of SPI access // Block pending completion of SPI access

View file

@ -56,6 +56,7 @@
#define DEBUG_MAX7456_SPICLOCK_OVERCLOCK 0 #define DEBUG_MAX7456_SPICLOCK_OVERCLOCK 0
#define DEBUG_MAX7456_SPICLOCK_DEVTYPE 1 #define DEBUG_MAX7456_SPICLOCK_DEVTYPE 1
#define DEBUG_MAX7456_SPICLOCK_DIVISOR 2 #define DEBUG_MAX7456_SPICLOCK_DIVISOR 2
#define DEBUG_MAX7456_SPICLOCK_X100 3
// VM0 bits // VM0 bits
#define VIDEO_BUFFER_DISABLE 0x01 #define VIDEO_BUFFER_DISABLE 0x01
@ -192,7 +193,7 @@ extDevice_t max7456Device;
extDevice_t *dev = &max7456Device; extDevice_t *dev = &max7456Device;
static bool max7456DeviceDetected = false; static bool max7456DeviceDetected = false;
static uint16_t max7456SpiClock; static uint16_t max7456SpiClockDiv;
uint16_t maxScreenSize = VIDEO_BUFFER_CHARS_PAL; uint16_t maxScreenSize = VIDEO_BUFFER_CHARS_PAL;
@ -394,32 +395,35 @@ max7456InitStatus_e max7456Init(const max7456Config_t *max7456Config, const vcdP
} }
#if defined(USE_OVERCLOCK) #if defined(USE_OVERCLOCK)
max7456SpiClockDiv = spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ);
// Determine SPI clock divisor based on config and the device type. // Determine SPI clock divisor based on config and the device type.
switch (max7456Config->clockConfig) { switch (max7456Config->clockConfig) {
case MAX7456_CLOCK_CONFIG_HALF: case MAX7456_CLOCK_CONFIG_HALF:
max7456SpiClock = spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ / 2); max7456SpiClockDiv <<= 1;
break; break;
case MAX7456_CLOCK_CONFIG_OC: case MAX7456_CLOCK_CONFIG_NOMINAL:
max7456SpiClock = (cpuOverclock && (max7456DeviceType == MAX7456_DEVICE_TYPE_MAX)) ? spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ / 2) : spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ); default:
break; break;
case MAX7456_CLOCK_CONFIG_FULL: case MAX7456_CLOCK_CONFIG_DOUBLE:
max7456SpiClock = spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ); max7456SpiClockDiv >>= 1;
break; break;
} }
DEBUG_SET(DEBUG_MAX7456_SPICLOCK, DEBUG_MAX7456_SPICLOCK_OVERCLOCK, cpuOverclock); 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_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 #else
UNUSED(max7456Config); UNUSED(max7456Config);
UNUSED(cpuOverclock); UNUSED(cpuOverclock);
max7456SpiClock = spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ); max7456SpiClockDiv = spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ);
#endif #endif
spiSetClkDivisor(dev, max7456SpiClock); spiSetClkDivisor(dev, max7456SpiClockDiv);
// force soft reset on Max7456 // force soft reset on Max7456
spiWriteReg(dev, MAX7456ADD_VM0, MAX7456_RESET); spiWriteReg(dev, MAX7456ADD_VM0, MAX7456_RESET);
@ -631,9 +635,6 @@ bool max7456DrawScreen(void)
return true; 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 // Allow for 8 bytes followed by an ESCAPE and reset of DMM at end of buffer
maxSpiBufStartIndex -= 12; maxSpiBufStartIndex -= 12;

View file

@ -98,8 +98,6 @@ bool rxSpiDeviceInit(const rxSpiConfig_t *rxSpiConfig)
return false; return false;
} }
spiSetAtomicWait(dev);
const IO_t rxCsPin = IOGetByTag(rxSpiConfig->csnTag); const IO_t rxCsPin = IOGetByTag(rxSpiConfig->csnTag);
IOInit(rxCsPin, OWNER_RX_SPI_CS, 0); IOInit(rxCsPin, OWNER_RX_SPI_CS, 0);
IOConfigGPIO(rxCsPin, SPI_IO_CS_CFG); IOConfigGPIO(rxCsPin, SPI_IO_CS_CFG);

View file

@ -174,9 +174,6 @@ static bool sdcard_waitForIdle(int maxBytesToWait)
sdcard.idleCount = maxBytesToWait; sdcard.idleCount = maxBytesToWait;
// Ensure any prior DMA has completed before continuing
spiWaitClaim(&sdcard.dev);
spiSequence(&sdcard.dev, &segments[0]); spiSequence(&sdcard.dev, &segments[0]);
// Block pending completion of SPI access // Block pending completion of SPI access
@ -202,9 +199,6 @@ static uint8_t sdcard_waitForNonIdleByte(int maxDelay)
sdcard.idleCount = maxDelay; sdcard.idleCount = maxDelay;
// Ensure any prior DMA has completed before continuing
spiWaitClaim(&sdcard.dev);
spiSequence(&sdcard.dev, &segments[0]); spiSequence(&sdcard.dev, &segments[0]);
// Block pending completion of SPI access // 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; 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]); spiSequence(&sdcard.dev, &segments[0]);
// Block pending completion of SPI access // 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}, {NULL, NULL, 0, false, NULL},
}; };
// Ensure any prior DMA has completed before continuing
spiWaitClaim(&sdcard.dev);
spiSequence(&sdcard.dev, &segments[0]); spiSequence(&sdcard.dev, &segments[0]);
// Block pending completion of SPI access // Block pending completion of SPI access
@ -396,9 +384,6 @@ static bool sdcard_sendDataBlockFinish(void)
{NULL, NULL, 0, false, NULL}, {NULL, NULL, 0, false, NULL},
}; };
// Ensure any prior DMA has completed before continuing
spiWaitClaim(&sdcard.dev);
spiSequence(&sdcard.dev, &segments[0]); spiSequence(&sdcard.dev, &segments[0]);
// Block pending completion of SPI access // 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].txData = buffer;
segments[2].len = spiUseDMA(&sdcard.dev) ? SDCARD_BLOCK_SIZE : SDCARD_NON_DMA_CHUNK_SIZE; 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]); spiSequence(&sdcard.dev, &segments[0]);
// Don't block pending completion of SPI access // 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}, {NULL, NULL, 0, false, NULL},
}; };
// Ensure any prior DMA has completed before continuing
spiWaitClaim(&sdcard.dev);
spiSequence(&sdcard.dev, &segments[0]); spiSequence(&sdcard.dev, &segments[0]);
// Block pending completion of SPI access // Block pending completion of SPI access
@ -639,9 +618,6 @@ static sdcardOperationStatus_e sdcard_endWriteBlocks(void)
{NULL, NULL, 0, false, NULL}, {NULL, NULL, 0, false, NULL},
}; };
// Ensure any prior DMA has completed before continuing
spiWaitClaim(&sdcard.dev);
spiSequence(&sdcard.dev, &segments[0]); spiSequence(&sdcard.dev, &segments[0]);
// Block pending completion of SPI access // Block pending completion of SPI access

View file

@ -34,7 +34,7 @@ PG_REGISTER_WITH_RESET_FN(max7456Config_t, max7456Config, PG_MAX7456_CONFIG, 0);
void pgResetFn_max7456Config(max7456Config_t *config) 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->csTag = IO_TAG(MAX7456_SPI_CS_PIN);
config->spiDevice = SPI_DEV_TO_CFG(spiDeviceByInstance(MAX7456_SPI_INSTANCE)); config->spiDevice = SPI_DEV_TO_CFG(spiDeviceByInstance(MAX7456_SPI_INSTANCE));
config->preInitOPU = false; config->preInitOPU = false;

View file

@ -24,15 +24,15 @@
#include "pg/pg.h" #include "pg/pg.h"
typedef struct max7456Config_s { 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; ioTag_t csTag;
uint8_t spiDevice; uint8_t spiDevice;
bool preInitOPU; bool preInitOPU;
} max7456Config_t; } max7456Config_t;
// clockConfig values // clockConfig values
#define MAX7456_CLOCK_CONFIG_HALF 0 // Force half clock #define MAX7456_CLOCK_CONFIG_HALF 0 // Force half clock
#define MAX7456_CLOCK_CONFIG_OC 1 // Half clock if OC #define MAX7456_CLOCK_CONFIG_NOMINAL 1 // Nominal clock (default)
#define MAX7456_CLOCK_CONFIG_FULL 2 // Force full clock #define MAX7456_CLOCK_CONFIG_DOUBLE 2 // Double clock
PG_DECLARE(max7456Config_t, max7456Config); PG_DECLARE(max7456Config_t, max7456Config);