drivers/memory/spiflash: Fix bugs in and clean up read/write functions.

mp_spiflash_read had a bug in it where "dest" and "addr" were incremented
twice for a certain special case.  This was fixed, which then allowed the
function to be simplified to reduce code size.

mp_spiflash_write had a bug in it where "src" was not incremented correctly
for the case where the data to be written included the caching buffer as
well as some bytes after this buffer.  This was fixed and the resulting
code simplified.

1 files changed, 55 insertions, 70 deletions

diff --git a/drivers/memory/spiflash.c b/drivers/memory/spiflash.c
index c7f333044..d72603f64 100644
--- a/drivers/memory/spiflash.c
+++ b/drivers/memory/spiflash.c
@@ -233,50 +233,32 @@ void mp_spiflash_read(mp_spiflash_t *self, uint32_t addr, size_t len, uint8_t *d
     mp_spiflash_acquire_bus(self);
     if (bufuser == self && bufsec != 0xffffffff) {
         uint32_t bis = addr / SECTOR_SIZE;
-        int rest = 0;
-        if (bis < bufsec) {
-            rest = bufsec * SECTOR_SIZE - addr;
-            if (rest > len) {
-                rest = len;
-            }
-            mp_spiflash_read_data(self, addr, rest, dest);
-            len -= rest;
-            if (len <= 0) {
-                mp_spiflash_release_bus(self);
-                return;
-            } else {
-                // Something from buffer...
-                addr = bufsec * SECTOR_SIZE;
-                dest += rest;
-                if (len > SECTOR_SIZE) {
-                    rest = SECTOR_SIZE;
-                } else {
-                    rest = len;
-                }
-                memcpy(dest, buf, rest);
+        uint32_t bie = (addr + len - 1) / SECTOR_SIZE;
+        if (bis <= bufsec && bufsec <= bie) {
+            // Read straddles current buffer
+            size_t rest = 0;
+            if (bis < bufsec) {
+                // Read direct from flash for first part
+                rest = bufsec * SECTOR_SIZE - addr;
+                mp_spiflash_read_data(self, addr, rest, dest);
                 len -= rest;
-                if (len <= 0) {
-                    mp_spiflash_release_bus(self);
-                    return;
-                }
                 dest += rest;
                 addr += rest;
             }
-        } else if (bis == bufsec) {
-            uint32_t offset = addr & (SECTOR_SIZE-1);
+            uint32_t offset = addr & (SECTOR_SIZE - 1);
             rest = SECTOR_SIZE - offset;
             if (rest > len) {
                 rest = len;
             }
             memcpy(dest, &buf[offset], rest);
             len -= rest;
-            if (len <= 0) {
+            if (len == 0) {
                 mp_spiflash_release_bus(self);
                 return;
             }
+            dest += rest;
+            addr += rest;
         }
-        dest += rest;
-        addr += rest;
     }
     // Read rest direct from flash
     mp_spiflash_read_data(self, addr, len, dest);
@@ -395,64 +377,67 @@ int mp_spiflash_write(mp_spiflash_t *self, uint32_t addr, size_t len, const uint
     uint32_t bie = (addr + len - 1) / SECTOR_SIZE;
 
     mp_spiflash_acquire_bus(self);
+
     if (bufuser == self && bis <= bufsec && bie >= bufsec) {
-        // Current buffer affected, handle this part first
-        uint32_t taddr = (bufsec + 1) * SECTOR_SIZE;
-        int32_t offset = addr - bufsec * SECTOR_SIZE;
-        int32_t pre = bufsec * SECTOR_SIZE - addr;
-        if (offset < 0) {
+        // Write straddles current buffer
+        uint32_t pre;
+        uint32_t offset;
+        if (bufsec * SECTOR_SIZE >= addr) {
+            pre = bufsec * SECTOR_SIZE - addr;
             offset = 0;
         } else {
             pre = 0;
+            offset = addr - bufsec * SECTOR_SIZE;
         }
-        int32_t rest = len - pre;
-        int32_t trail = 0;
-        if (rest > SECTOR_SIZE - offset) {
-            trail = rest - (SECTOR_SIZE - offset);
-            rest = SECTOR_SIZE - offset;
+
+        // Write buffered part first
+        uint32_t len_in_buf = len - pre;
+        len = 0;
+        if (len_in_buf > SECTOR_SIZE - offset) {
+            len = len_in_buf - (SECTOR_SIZE - offset);
+            len_in_buf = SECTOR_SIZE - offset;
         }
-        memcpy(&buf[offset], &src[pre], rest);
+        memcpy(&buf[offset], &src[pre], len_in_buf);
         self->flags |= 1; // Mark dirty
-        if ((pre | trail) == 0) {
-            mp_spiflash_release_bus(self);
-            return 0;
-        }
-        const uint8_t *p = src;
+
+        // Write part before buffer sector
         while (pre) {
             int rest = pre & (SECTOR_SIZE - 1);
             if (rest == 0) {
                 rest = SECTOR_SIZE;
             }
-            mp_spiflash_write_part(self, addr, rest, p);
-            p += rest;
+            int ret = mp_spiflash_write_part(self, addr, rest, src);
+            if (ret != 0) {
+                mp_spiflash_release_bus(self);
+                return ret;
+            }
+            src += rest;
             addr += rest;
             pre -= rest;
         }
-        while (trail) {
-            int rest = trail;
-            if (rest > SECTOR_SIZE) {
-                rest = SECTOR_SIZE;
-            }
-            mp_spiflash_write_part(self, taddr, rest, src);
-            src += rest;
-            taddr += rest;
-            trail -= rest;
+        src += len_in_buf;
+        addr += len_in_buf;
+
+        // Fall through to write remaining part
+    }
+
+    uint32_t offset = addr & (SECTOR_SIZE - 1);
+    while (len) {
+        int rest = SECTOR_SIZE - offset;
+        if (rest > len) {
+            rest = len;
         }
-    } else {
-        // Current buffer not affected, business as usual
-        uint32_t offset = addr & (SECTOR_SIZE - 1);
-        while (len) {
-            int rest = SECTOR_SIZE - offset;
-            if (rest > len) {
-                rest = len;
-            }
-            mp_spiflash_write_part(self, addr, rest, src);
-            len -= rest;
-            addr += rest;
-            src += rest;
-            offset = 0;
+        int ret = mp_spiflash_write_part(self, addr, rest, src);
+        if (ret != 0) {
+            mp_spiflash_release_bus(self);
+            return ret;
         }
+        len -= rest;
+        addr += rest;
+        src += rest;
+        offset = 0;
     }
+
     mp_spiflash_release_bus(self);
     return 0;
 }