Merge pull request #2662 from jepler/issue2332

Fix unaligned disk reads properly

4 files changed, 43 insertions, 13 deletions

diff --git a/lib/oofatfs/ff.c b/lib/oofatfs/ff.c
index 71bd19702..e961c789d 100644
--- a/lib/oofatfs/ff.c
+++ b/lib/oofatfs/ff.c
@@ -3382,11 +3382,7 @@ FRESULT f_read (
             if (!sect) ABORT(fs, FR_INT_ERR);
             sect += csect;
             cc = btr / SS(fs);                  /* When remaining bytes >= sector size, */
-            if (cc
-#if _FS_DISK_READ_ALIGNED
-                    && (((int)rbuff & 3) == 0)
-#endif
-                    ) {/* Read maximum contiguous sectors directly */
+            if (cc) {/* Read maximum contiguous sectors directly */
                 if (csect + cc > fs->csize) {   /* Clip at cluster boundary */
                     cc = fs->csize - csect;
                 }
diff --git a/lib/oofatfs/ffconf.h b/lib/oofatfs/ffconf.h
index a3795fa3f..214311b4c 100644
--- a/lib/oofatfs/ffconf.h
+++ b/lib/oofatfs/ffconf.h
@@ -343,12 +343,6 @@
 /  SemaphoreHandle_t and etc.. A header file for O/S definitions needs to be
 /  included somewhere in the scope of ff.h. */
 
-// Set to nonzero if buffers passed to disk_read have a word alignment
-// restriction
-#ifndef _FS_DISK_READ_ALIGNED
-#define _FS_DISK_READ_ALIGNED 0
-#endif
-
 /* #include <windows.h> // O/S definitions  */
 
 
diff --git a/ports/nrf/Makefile b/ports/nrf/Makefile
index 47a5b8653..89d88ce36 100755
--- a/ports/nrf/Makefile
+++ b/ports/nrf/Makefile
@@ -102,7 +102,6 @@ CFLAGS += -Wno-undef
 CFLAGS += -Wno-cast-align
 
 NRF_DEFINES += -DCONFIG_GPIO_AS_PINRESET
-NRF_DEFINES += -D_FS_DISK_READ_ALIGNED=1
 CFLAGS += $(NRF_DEFINES)
 
 CFLAGS += \
diff --git a/ports/nrf/supervisor/qspi_flash.c b/ports/nrf/supervisor/qspi_flash.c
index bb449d881..90260b091 100644
--- a/ports/nrf/supervisor/qspi_flash.c
+++ b/ports/nrf/supervisor/qspi_flash.c
@@ -83,11 +83,52 @@ bool spi_flash_sector_command(uint8_t command, uint32_t address) {
 }
 
 bool spi_flash_write_data(uint32_t address, uint8_t* data, uint32_t length) {
+    // TODO: In theory, this also needs to handle unaligned data and
+    // non-multiple-of-4 length.  (in practice, I don't think the fat layer
+    // generates such writes)
     return nrfx_qspi_write(data, length, address) == NRFX_SUCCESS;
 }
 
 bool spi_flash_read_data(uint32_t address, uint8_t* data, uint32_t length) {
-    return nrfx_qspi_read(data, length, address) == NRFX_SUCCESS;
+    int misaligned = ((intptr_t)data) & 3;
+    // If the data is misaligned, we need to read 4 bytes
+    // into an aligned buffer, and then copy 1, 2, or 3 bytes from the aligned
+    // buffer to data.
+    if(misaligned) {
+        int sz = 4 - misaligned;
+        __attribute__((aligned(4))) uint8_t buf[4];
+
+        if(nrfx_qspi_read(buf, 4, address) != NRFX_SUCCESS) {
+            return false;
+        }
+        memcpy(data, buf, sz);
+        data += sz;
+        address += sz;
+        length -= sz;
+    }
+
+    // nrfx_qspi_read works in 4 byte increments, though it doesn't
+    // signal an error if sz is not a multiple of 4.  Read (directly into data)
+    // all but the last 1, 2, or 3 bytes depending on the (remaining) length.
+    uint32_t sz = length & ~(uint32_t)3;
+    if(nrfx_qspi_read(data, sz, address) != NRFX_SUCCESS) {
+        return false;
+    }
+    data += sz;
+    address += sz;
+    length -= sz;
+
+    // Now, if we have any bytes left over, we must do a final read of 4
+    // bytes and copy 1, 2, or 3 bytes to data.
+    if(length) {
+        __attribute__((aligned(4))) uint8_t buf[4];
+        if(nrfx_qspi_read(buf, 4, address) != NRFX_SUCCESS) {
+            return false;
+        }
+        memcpy(data, buf, length);
+    }
+
+    return true;
 }
 
 void spi_flash_init(void) {