Merge remote-tracking branch 'upstream/master' into list_remove

1 files changed, 15 insertions, 2 deletions

diff --git a/stm/storage.c b/stm/storage.c
index 6878de22e6..daee4adb5e 100644
--- a/stm/storage.c
+++ b/stm/storage.c
@@ -49,6 +49,18 @@ static uint8_t *cache_get_addr_for_write(uint32_t flash_addr) {
     return (uint8_t*)CACHE_MEM_START_ADDR + flash_addr - flash_sector_start;
 }
 
+static uint8_t *cache_get_addr_for_read(uint32_t flash_addr) {
+    uint32_t flash_sector_start;
+    uint32_t flash_sector_size;
+    uint32_t flash_sector_id = flash_get_sector_info(flash_addr, &flash_sector_start, &flash_sector_size);
+    if (cache_flash_sector_id == flash_sector_id) {
+        // in cache, copy from there
+        return (uint8_t*)CACHE_MEM_START_ADDR + flash_addr - flash_sector_start;
+    }
+    // not in cache, copy straight from flash
+    return (uint8_t*)flash_addr;
+}
+
 void storage_init(void) {
     if (!is_initialised) {
         cache_flash_sector_id = 0;
@@ -131,8 +143,9 @@ bool storage_read_block(uint8_t *dest, uint32_t block) {
         return true;
 
     } else if (FLASH_PART1_START_BLOCK <= block && block < FLASH_PART1_START_BLOCK + FLASH_PART1_NUM_BLOCKS) {
-        // non-MBR block, just copy straight from flash
-        uint8_t *src = (uint8_t*)FLASH_MEM_START_ADDR + (block - FLASH_PART1_START_BLOCK) * BLOCK_SIZE;
+        // non-MBR block, get data from flash memory, possibly via cache
+        uint32_t flash_addr = FLASH_MEM_START_ADDR + (block - FLASH_PART1_START_BLOCK) * BLOCK_SIZE;
+        uint8_t *src = cache_get_addr_for_read(flash_addr);
         memcpy(dest, src, BLOCK_SIZE);
         return true;