Add support for discovering beginning of free space on flash chip

src/main/drivers/flash_m25p16.c

@@ -256,7 +256,9 @@ void m25p16_pageProgram(uint32_t address, const uint8_t *data, int length)
  * Read `length` bytes into the provided `buffer` from the flash starting from the given `address` (which need not lie
  * on a page boundary).
  *
- * The number of bytes actually read is returned, which can be zero if an error occurred.
+ * Waits up to DEFAULT_TIMEOUT_MILLIS milliseconds for the flash to become ready before reading.
+ *
+ * The number of bytes actually read is returned, which can be zero if an error or timeout occurred.
  */
 int m25p16_readBytes(uint32_t address, uint8_t *buffer, int length)
 {

src/main/io/flashfs.c

@@ -18,6 +18,10 @@
 /**
  * This provides a stream interface to a flash chip if one is present.
  *
+ * On statup, call flashfsInit after initialising the flash chip, in order to init the filesystem. This will
+ * result in the file pointer being pointed at the first free block found, or at the end of the device if the
+ * flash chip is full.
+ *
  * Note that bits can only be set to 0 when writing, not back to 1 from 0. You must erase sectors in order
  * to bring bits back to 1 again.
  */
@@ -39,7 +43,7 @@ static uint8_t flashWriteBuffer[FLASHFS_WRITE_BUFFER_SIZE];
  */
 static uint8_t bufferHead = 0, bufferTail = 0;
 
-// The position of our tail in the overall flash address space:
+// The position of the buffer's tail in the overall flash address space:
 static uint32_t tailAddress = 0;
 // The index of the tail within the flash page it is inside
 static uint16_t tailIndexInPage = 0;
@@ -55,7 +59,10 @@ static void flashfsClearBuffer()
 static void flashfsSetTailAddress(uint32_t address)
 {
     tailAddress = address;
-    tailIndexInPage = tailAddress % m25p16_getGeometry()->pageSize;
+
+    if (m25p16_getGeometry()->pageSize > 0) {
+        tailIndexInPage = tailAddress % m25p16_getGeometry()->pageSize;
+    }
 }
 
 void flashfsEraseCompletely()
@@ -163,6 +170,10 @@ static uint32_t flashfsWriteBuffers(uint8_t const **buffers, uint32_t *bufferSiz
             bytesTotalThisIteration = bytesTotalRemaining;
         }
 
+        // Are we at EOF already? Abort.
+        if (tailAddress >= flashfsGetSize())
+            break;
+
         m25p16_pageProgramBegin(tailAddress);
 
         bytesRemainThisIteration = bytesTotalThisIteration;
@@ -228,6 +239,21 @@ static void flashfsGetDirtyDataBuffers(uint8_t const *buffers[], uint32_t buffer
     }
 }
 
+/**
+ * Get the current offset of the file pointer within the volume.
+ */
+uint32_t flashfsGetOffset()
+{
+    uint8_t const * buffers[2];
+    uint32_t bufferSizes[2];
+
+    // Dirty data in the buffers contributes to the offset
+
+    flashfsGetDirtyDataBuffers(buffers, bufferSizes);
+
+    return tailAddress + bufferSizes[0] + bufferSizes[1];
+}
+
 /**
  * Called after bytes have been written from the buffer to advance the position of the tail by the given amount.
  */
@@ -395,23 +421,100 @@ void flashfsWrite(const uint8_t *data, unsigned int len)
 }
 
 /**
- * Read `len` bytes from the current cursor location into the supplied buffer.
+ * Read `len` bytes from the given address into the supplied buffer.
  *
  * Returns the number of bytes actually read which may be less than that requested.
  */
-int flashfsRead(uint8_t *data, unsigned int len)
+int flashfsReadAbs(uint32_t address, uint8_t *buffer, unsigned int len)
 {
     int bytesRead;
 
     // Did caller try to read past the end of the volume?
-    if (tailAddress + len > flashfsGetSize()) {
+    if (address + len > flashfsGetSize()) {
         // Truncate their request
-        len = flashfsGetSize() - tailAddress;
+        len = flashfsGetSize() - address;
     }
 
-    bytesRead = m25p16_readBytes(tailAddress, data, len);
+    // Since the read could overlap data in our dirty buffers, force a sync to clear those first
+    flashfsFlushSync();
 
-    flashfsSetTailAddress(tailAddress + bytesRead);
+    bytesRead = m25p16_readBytes(address, buffer, len);
 
     return bytesRead;
 }
+
+/**
+ * Find the offset of the start of the free space on the device (or the size of the device if it is full).
+ */
+int flashfsIdentifyStartOfFreeSpace()
+{
+    /* Find the start of the free space on the device by examining the beginning of blocks with a binary search,
+     * looking for ones that appear to be erased. We can achieve this with good accuracy because an erased block
+     * is all bits set to 1, which pretty much never appears in reasonable size substrings of blackbox logs.
+     *
+     * To do better we might write a volume header instead, which would mark how much free space remains. But keeping
+     * a header up to date while logging would incur more writes to the flash, which would consume precious write
+     * bandwidth and block more often.
+     */
+
+    enum {
+        /* We can choose whatever power of 2 size we like, which determines how much wastage of free space we'll have
+         * at the end of the last written data. But smaller blocksizes will require more searching.
+         */
+        FREE_BLOCK_SIZE = 65536,
+
+        /* We don't expect valid data to ever contain this many consecutive uint32_t's of all 1 bits: */
+        FREE_BLOCK_TEST_SIZE_INTS = 4, // i.e. 16 bytes
+        FREE_BLOCK_TEST_SIZE_BYTES = FREE_BLOCK_TEST_SIZE_INTS * sizeof(uint32_t),
+    };
+
+    union {
+        uint8_t bytes[FREE_BLOCK_TEST_SIZE_BYTES];
+        uint32_t ints[FREE_BLOCK_TEST_SIZE_INTS];
+    } testBuffer;
+
+    int left = 0;
+    int right = flashfsGetSize() / FREE_BLOCK_SIZE;
+    int mid, result = right;
+    int i;
+    bool blockErased;
+
+    while (left < right) {
+        mid = (left + right) / 2;
+
+        m25p16_readBytes(mid * FREE_BLOCK_SIZE, testBuffer.bytes, FREE_BLOCK_TEST_SIZE_BYTES);
+
+        // Checking the buffer 4 bytes at a time like this is probably faster than byte-by-byte, but I didn't benchmark it :)
+        blockErased = true;
+        for (i = 0; i < FREE_BLOCK_TEST_SIZE_INTS; i++) {
+            if (testBuffer.ints[i] != 0xFFFFFFFF) {
+                blockErased = false;
+                break;
+            }
+        }
+
+        if (blockErased) {
+            /* This erased block might be the leftmost erased block in the volume, but we'll need to continue the
+             * search leftwards to find out:
+             */
+            result = mid;
+
+            right = mid;
+        } else {
+            left = mid + 1;
+        }
+    }
+
+    return result * FREE_BLOCK_SIZE;
+}
+
+/**
+ * Call after initializing the flash chip in order to set up the filesystem.
+ */
+void flashfsInit()
+{
+    if (flashfsGetSize() > 0) {
+        // Start the file pointer off at the beginning of free space so caller can start writing immediately
+        flashfsSeekAbs(flashfsIdentifyStartOfFreeSpace());
+    }
+}

src/main/io/flashfs.h

@@ -31,6 +31,8 @@ void flashfsEraseCompletely();
 void flashfsEraseRange(uint32_t start, uint32_t end);
 
 uint32_t flashfsGetSize();
+uint32_t flashfsGetOffset();
+int flashfsIdentifyStartOfFreeSpace();
 const flashGeometry_t* flashfsGetGeometry();
 
 void flashfsSeekAbs(uint32_t offset);
@@ -39,7 +41,9 @@ void flashfsSeekRel(int32_t offset);
 void flashfsWriteByte(uint8_t byte);
 void flashfsWrite(const uint8_t *data, unsigned int len);
 
-int flashfsRead(uint8_t *data, unsigned int len);
+int flashfsReadAbs(uint32_t offset, uint8_t *data, unsigned int len);
 
 void flashfsFlushAsync();
 void flashfsFlushSync();
+
+void flashfsInit();

src/main/io/serial_cli.c

@@ -753,8 +753,8 @@ static void cliFlashIdent(char *cmdline)
 
     UNUSED(cmdline);
 
-    printf("Flash sectors=%u, sectorSize=%u, pagesPerSector=%u, pageSize=%u, totalSize=%u\r\n",
+    printf("Flash sectors=%u, sectorSize=%u, pagesPerSector=%u, pageSize=%u, totalSize=%u, usedSize=%u\r\n",
-            layout->sectors, layout->sectorSize, layout->pagesPerSector, layout->pageSize, layout->totalSize);
+            layout->sectors, layout->sectorSize, layout->pagesPerSector, layout->pageSize, layout->totalSize, flashfsGetOffset());
 }
 
 static void cliFlashErase(char *cmdline)
@@ -786,7 +786,7 @@ static void cliFlashRead(char *cmdline)
 {
     uint32_t address = atoi(cmdline);
     uint32_t length;
-    uint32_t i;
+    int i;
 
     uint8_t buffer[32];
 
@@ -799,18 +799,22 @@ static void cliFlashRead(char *cmdline)
 
         printf("Reading %u bytes at %u:\r\n", length, address);
 
-        flashfsSeekAbs(address);
-
         while (length > 0) {
-            uint32_t bytesToRead = length < 32 ? length : 32;
+            int bytesRead;
 
-            flashfsRead(buffer, bytesToRead);
+            bytesRead = flashfsReadAbs(address, buffer, length < sizeof(buffer) ? length : sizeof(buffer));
 
-            for (i = 0; i < bytesToRead; i++) {
+            for (i = 0; i < bytesRead; i++) {
-                printf("%c", (char) buffer[i]);
+                cliWrite(buffer[i]);
             }
 
-            length -= bytesToRead;
+            length -= bytesRead;
+            address += bytesRead;
+
+            if (bytesRead == 0) {
+                //Assume we reached the end of the volume or something fatal happened
+                break;
+            }
         }
         printf("\r\n");
     }

src/main/io/serial_msp.c

@@ -533,13 +533,14 @@ reset:
 
 static void serializeDataflashSummaryReply(void)
 {
+    headSerialReply(3 * 4);
 #ifdef FLASHFS
     const flashGeometry_t *geometry = flashfsGetGeometry();
-    headSerialReply(2 * 4);
     serialize32(geometry->sectors);
     serialize32(geometry->totalSize);
+    serialize32(flashfsGetOffset()); // Effectively the current number of bytes stored on the volume
 #else
-    headSerialReply(2 * 4);
+    serialize32(0);
     serialize32(0);
     serialize32(0);
 #endif
@@ -548,22 +549,19 @@ static void serializeDataflashSummaryReply(void)
 #ifdef FLASHFS
 static void serializeDataflashReadReply(uint32_t address, uint8_t size)
 {
-    enum { DATAFLASH_READ_REPLY_CHUNK_SIZE = 128 };
+    uint8_t buffer[128];
-
-    uint8_t buffer[DATAFLASH_READ_REPLY_CHUNK_SIZE];
     int bytesRead;
 
-    if (size > DATAFLASH_READ_REPLY_CHUNK_SIZE) {
+    if (size > sizeof(buffer)) {
-        size = DATAFLASH_READ_REPLY_CHUNK_SIZE;
+        size = sizeof(buffer);
     }
 
     headSerialReply(4 + size);
 
     serialize32(address);
 
-    flashfsSeekAbs(address);
     // bytesRead will be lower than that requested if we reach end of volume
-    bytesRead = flashfsRead(buffer, size);
+    bytesRead = flashfsReadAbs(address, buffer, size);
 
     for (int i = 0; i < bytesRead; i++) {
         serialize8(buffer[i]);

src/main/main.c

@@ -50,6 +50,7 @@
 #include "flight/mixer.h"
 
 #include "io/serial.h"
+#include "io/flashfs.h"
 #include "flight/failsafe.h"
 #include "flight/navigation.h"
 
@@ -355,6 +356,7 @@ void init(void)
         // naze32 rev5 and above have 16mbit of flash available
         m25p16_init();
     #endif
+    flashfsInit();
 #endif
 
 #ifdef BLACKBOX