From 6e79712906bfd43fcc204aae07aaee202dfc18dd Mon Sep 17 00:00:00 2001 From: Steve Evans Date: Wed, 6 Nov 2024 19:33:32 +0000 Subject: [PATCH] Add PRBS checking of BB FLASH with USE_FLASH_TEST_PRBS (#13987) * Add PRBS checking of BB FLASH with USE_FLASH_TEST_PRBS * Update src/main/io/flashfs.c Co-authored-by: Petr Ledvina --------- Co-authored-by: Petr Ledvina --- src/main/blackbox/blackbox.c | 17 ++++++ src/main/drivers/flash/flash_w25n.c | 45 +++++++++------- src/main/io/flashfs.c | 104 ++++++++++++++++++++++++------------ 3 files changed, 112 insertions(+), 54 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index f8f744afd..df7a622e3 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -91,6 +91,12 @@ #include "sensors/gyro_init.h" #include "sensors/rangefinder.h" + +#ifdef USE_FLASH_TEST_PRBS +void checkFlashStart(void); +void checkFlashStop(void); +#endif + #if !defined(DEFAULT_BLACKBOX_DEVICE) #define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_NONE #endif @@ -596,10 +602,21 @@ static void blackboxSetState(BlackboxState newState) break; case BLACKBOX_STATE_RUNNING: blackboxSlowFrameIterationTimer = blackboxSInterval; //Force a slow frame to be written on the first iteration +#ifdef USE_FLASH_TEST_PRBS + // Start writing a known pattern as the running state is entered + checkFlashStart(); +#endif break; case BLACKBOX_STATE_SHUTTING_DOWN: xmitState.u.startTime = millis(); break; + +#ifdef USE_FLASH_TEST_PRBS + case BLACKBOX_STATE_STOPPED: + // Now that the log is shut down, verify it + checkFlashStop(); + break; +#endif default: ; } diff --git a/src/main/drivers/flash/flash_w25n.c b/src/main/drivers/flash/flash_w25n.c index cb1c05594..d88b95371 100644 --- a/src/main/drivers/flash/flash_w25n.c +++ b/src/main/drivers/flash/flash_w25n.c @@ -537,6 +537,25 @@ static uint32_t programStartAddress; static uint32_t programLoadAddress; bool bufferDirty = false; +// Called in ISR context +// Check if the status was busy and if so repeat the poll +busStatus_e w25n_callbackReady(uint32_t arg) +{ + flashDevice_t *fdevice = (flashDevice_t *)arg; + extDevice_t *dev = fdevice->io.handle.dev; + + uint8_t readyPoll = dev->bus->curSegment->u.buffers.rxData[2]; + + if (readyPoll & W25N_STATUS_FLAG_BUSY) { + return BUS_BUSY; + } + + // Bus is now known not to be busy + fdevice->couldBeBusy = false; + + return BUS_READY; +} + #ifdef USE_QUADSPI bool isProgramming = false; @@ -620,25 +639,6 @@ void w25n_pageProgramBegin(flashDevice_t *fdevice, uint32_t address, void (*call static uint32_t currentPage = UINT32_MAX; // Called in ISR context -// Check if the status was busy and if so repeat the poll -busStatus_e w25n_callbackReady(uint32_t arg) -{ - flashDevice_t *fdevice = (flashDevice_t *)arg; - extDevice_t *dev = fdevice->io.handle.dev; - - uint8_t readyPoll = dev->bus->curSegment->u.buffers.rxData[2]; - - if (readyPoll & W25N_STATUS_FLAG_BUSY) { - return BUS_BUSY; - } - - // Bus is now known not to be busy - fdevice->couldBeBusy = false; - - return BUS_READY; -} - -// Called in ISR context // A write enable has just been issued busStatus_e w25n_callbackWriteEnable(uint32_t arg) { @@ -843,6 +843,9 @@ int w25n_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer, ui { uint32_t targetPage = W25N_LINEAR_TO_PAGE(address); + // As data is buffered before being written a flush must be performed before attempting a read + w25n_flush(fdevice); + if (currentPage != targetPage) { if (!w25n_waitForReady(fdevice)) { return 0; @@ -872,6 +875,9 @@ int w25n_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer, ui if (fdevice->io.mode == FLASHIO_SPI) { extDevice_t *dev = fdevice->io.handle.dev; + uint8_t readStatus[] = { W25N_INSTRUCTION_READ_STATUS_REG, W25N_STAT_REG, 0 }; + uint8_t readyStatus[3]; + uint8_t cmd[4]; cmd[0] = W25N_INSTRUCTION_READ_DATA; cmd[1] = (column >> 8) & 0xff; @@ -879,6 +885,7 @@ int w25n_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer, ui cmd[3] = 0; busSegment_t segments[] = { + {.u.buffers = {readStatus, readyStatus}, sizeof(readStatus), true, w25n_callbackReady}, {.u.buffers = {cmd, NULL}, sizeof(cmd), false, NULL}, {.u.buffers = {NULL, buffer}, length, true, NULL}, {.u.link = {NULL, NULL}, 0, true, NULL}, diff --git a/src/main/io/flashfs.c b/src/main/io/flashfs.c index dcbf14432..3359a2b42 100644 --- a/src/main/io/flashfs.c +++ b/src/main/io/flashfs.c @@ -41,6 +41,7 @@ #if defined(USE_FLASHFS) #include "build/debug.h" +#include "common/maths.h" #include "common/printf.h" #include "drivers/flash/flash.h" #include "drivers/light_led.h" @@ -82,20 +83,69 @@ static volatile uint8_t bufferTail = 0; */ static volatile bool dataWritten = true; -//#define CHECK_FLASH +// The position of the buffer's tail in the overall flash address space: +static uint32_t tailAddress = 0; + -#ifdef CHECK_FLASH +#ifdef USE_FLASH_TEST_PRBS // Write an incrementing sequence of bytes instead of the requested data and verify -DMA_DATA uint8_t checkFlashBuffer[FLASHFS_WRITE_BUFFER_SIZE]; -uint32_t checkFlashPtr = 0; -uint32_t checkFlashLen = 0; -uint8_t checkFlashWrite = 0x00; -uint8_t checkFlashExpected = 0x00; -uint32_t checkFlashErrors = 0; -#endif +static DMA_DATA uint8_t checkFlashBuffer[FLASHFS_WRITE_BUFFER_SIZE]; +static uint32_t checkFlashPtr = 0; +static uint32_t checkFlashLen = 0; +static uint32_t checkFlashErrors = 0; +static bool checkFlashActive = false; +static uint32_t checkFlashSeedPRBS; + +static uint8_t checkFlashNextByte(void) +{ + uint32_t newLSB = ((checkFlashSeedPRBS >> 31) ^ (checkFlashSeedPRBS >> 28)) & 1; -// The position of the buffer's tail in the overall flash address space: -static uint32_t tailAddress = 0; + checkFlashSeedPRBS = (checkFlashSeedPRBS << 1) | newLSB; + + return checkFlashSeedPRBS & 0xff; +} + +// Called from blackboxSetState() to start/stop writing of pseudo-random data to FLASH +void checkFlashStart(void) +{ + checkFlashSeedPRBS = 0xdeadbeef; + checkFlashPtr = tailAddress; + checkFlashLen = 0; + checkFlashActive = true; +} + +void checkFlashStop(void) +{ + checkFlashSeedPRBS = 0xdeadbeef; + checkFlashErrors = 0; + + debug[6] = checkFlashLen / flashGeometry->pageSize; + + // Verify the data written since flashfsSeekAbs() last called + while (checkFlashLen) { + uint32_t checkLen = MIN(checkFlashLen, sizeof(checkFlashBuffer)); + + // Don't read over a page boundary + checkLen = MIN(checkLen, flashGeometry->pageSize - (checkFlashPtr & (flashGeometry->pageSize - 1))); + + flashReadBytes(checkFlashPtr, checkFlashBuffer, checkLen); + + for (uint32_t i = 0; i < checkLen; i++) { + uint8_t expected = checkFlashNextByte(); + if (checkFlashBuffer[i] != expected) { + checkFlashErrors++; // <-- insert breakpoint here to catch errors + } + } + + checkFlashPtr += checkLen; + checkFlashLen -= checkLen; + } + + debug[7] = checkFlashErrors; + + checkFlashActive = false; +} +#endif static void flashfsClearBuffer(void) { @@ -265,10 +315,6 @@ static uint32_t flashfsWriteBuffers(uint8_t const **buffers, uint32_t *bufferSiz return 0; } -#ifdef CHECK_FLASH - checkFlashPtr = tailAddress; -#endif - flashPageProgramBegin(tailAddress, flashfsWriteCallback); /* Mark that data has yet to be written. There is no race condition as the DMA engine is known @@ -278,10 +324,6 @@ static uint32_t flashfsWriteBuffers(uint8_t const **buffers, uint32_t *bufferSiz bytesWritten = flashPageProgramContinue(buffers, bufferSizes, bufferCount); -#ifdef CHECK_FLASH - checkFlashLen = bytesWritten; -#endif - flashPageProgramFinish(); return bytesWritten; @@ -361,20 +403,6 @@ bool flashfsFlushAsync(bool force) return false; } -#ifdef CHECK_FLASH - // Verify the data written last time - if (checkFlashLen) { - while (!flashIsReady()); - flashReadBytes(checkFlashPtr, checkFlashBuffer, checkFlashLen); - - for (uint32_t i = 0; i < checkFlashLen; i++) { - if (checkFlashBuffer[i] != checkFlashExpected++) { - checkFlashErrors++; // <-- insert breakpoint here to catch errors - } - } - } -#endif - bufCount = flashfsGetDirtyDataBuffers(buffers, bufferSizes); uint32_t bufferedBytes = bufferSizes[0] + bufferSizes[1]; @@ -443,8 +471,11 @@ void flashfsSeekAbs(uint32_t offset) */ void flashfsWriteByte(uint8_t byte) { -#ifdef CHECK_FLASH - byte = checkFlashWrite++; +#ifdef USE_FLASH_TEST_PRBS + if (checkFlashActive) { + byte = checkFlashNextByte(); + checkFlashLen++; + } #endif flashWriteBuffer[bufferHead++] = byte; @@ -504,6 +535,9 @@ int flashfsReadAbs(uint32_t address, uint8_t *buffer, unsigned int len) len = flashfsSize - address; } + // Don't read across a page boundary + len = MIN(len, flashGeometry->pageSize - (address & (flashGeometry->pageSize - 1))); + // Since the read could overlap data in our dirty buffers, force a sync to clear those first flashfsFlushSync(); -- 2.11.4.GIT