espressif: Fix flash_area_read support for unaligned accesses
Signed-off-by: Gustavo Henrique Nihei <gustavo.nihei@espressif.com>
diff --git a/boot/espressif/port/esp_mcuboot.c b/boot/espressif/port/esp_mcuboot.c
index a80a859..1281d2e 100644
--- a/boot/espressif/port/esp_mcuboot.c
+++ b/boot/espressif/port/esp_mcuboot.c
@@ -16,7 +16,33 @@
#include "bootloader_flash.h"
#include "bootloader_flash_priv.h"
-#define ARRAY_SIZE(arr) sizeof(arr)/sizeof(arr[0])
+#ifndef ARRAY_SIZE
+# define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0]))
+#endif
+
+#ifndef MIN
+# define MIN(a, b) (((a) < (b)) ? (a) : (b))
+#endif
+
+#ifndef ALIGN_UP
+# define ALIGN_UP(num, align) (((num) + ((align) - 1)) & ~((align) - 1))
+#endif
+
+#ifndef ALIGN_DOWN
+# define ALIGN_DOWN(num, align) ((num) & ~((align) - 1))
+#endif
+
+#ifndef ALIGN_OFFSET
+# define ALIGN_OFFSET(num, align) ((num) & ((align) - 1))
+#endif
+
+#ifndef IS_ALIGNED
+# define IS_ALIGNED(num, align) (ALIGN_OFFSET((num), (align)) == 0)
+#endif
+
+#define FLASH_BUFFER_SIZE 256 /* SPI Flash block size */
+
+_Static_assert(IS_ALIGNED(FLASH_BUFFER_SIZE, 4), "Buffer size for SPI Flash operations must be 4-byte aligned.");
#define BOOTLOADER_START_ADDRESS 0x1000
#define BOOTLOADER_SIZE CONFIG_ESP_BOOTLOADER_SIZE
@@ -86,30 +112,73 @@
}
+static bool aligned_flash_read(uintptr_t addr, void *dest, size_t size)
+{
+ if (IS_ALIGNED(addr, 4) && IS_ALIGNED((uintptr_t)dest, 4) && IS_ALIGNED(size, 4)) {
+ /* A single read operation is enough when when all parameters are aligned */
+
+ return bootloader_flash_read(addr, dest, size, true) == ESP_OK;
+ }
+
+ const uint32_t aligned_addr = ALIGN_DOWN(addr, 4);
+ const uint32_t addr_offset = ALIGN_OFFSET(addr, 4);
+ uint32_t bytes_remaining = size;
+ uint8_t read_data[FLASH_BUFFER_SIZE] = {0};
+
+ /* Align the read address to 4-byte boundary and ensure read size is a multiple of 4 bytes */
+
+ uint32_t bytes = MIN(bytes_remaining + addr_offset, sizeof(read_data));
+ if (bootloader_flash_read(aligned_addr, read_data, ALIGN_UP(bytes, 4), true) != ESP_OK) {
+ return false;
+ }
+
+ /* Skip non-useful data which may have been read for adjusting the alignment */
+
+ uint32_t bytes_read = bytes - addr_offset;
+ memcpy(dest, &read_data[addr_offset], bytes_read);
+
+ bytes_remaining -= bytes_read;
+
+ /* Read remaining data from Flash in case requested size is greater than buffer size */
+
+ uint32_t offset = bytes;
+
+ while (bytes_remaining != 0) {
+ bytes = MIN(bytes_remaining, sizeof(read_data));
+ if (bootloader_flash_read(aligned_addr + offset, read_data, ALIGN_UP(bytes, 4), true) != ESP_OK) {
+ return false;
+ }
+
+ memcpy(&((uint8_t *)dest)[bytes_read], read_data, bytes);
+
+ offset += bytes;
+ bytes_read += bytes;
+ bytes_remaining -= bytes;
+ }
+
+ return true;
+}
+
int flash_area_read(const struct flash_area *fa, uint32_t off, void *dst,
uint32_t len)
{
- uint32_t internal_data = 0, read_len = 0;
- void *read_ptr;
if (fa->fa_device_id != FLASH_DEVICE_INTERNAL_FLASH) {
return -1;
}
const uint32_t end_offset = off + len;
-
if (end_offset > fa->fa_size) {
MCUBOOT_LOG_ERR("%s: Out of Bounds (0x%x vs 0x%x)", __func__, end_offset, fa->fa_size);
return -1;
}
- read_len = (len < 4) ? sizeof(uint32_t) : len;
- read_ptr = (len < 4) ? (void *)&internal_data : dst;
- if (bootloader_flash_read(fa->fa_off + off, read_ptr, read_len, true) != ESP_OK) {
+
+ bool success = aligned_flash_read(fa->fa_off + off, dst, len);
+ if (!success) {
MCUBOOT_LOG_ERR("%s: Flash read failed", __func__);
+
return -1;
}
- if (len < 4) {
- memcpy(dst, read_ptr, len);
- }
+
return 0;
}