DRAFT: flash: espressif: erase flash region before writing when flash encryption is enabled

drivers/flash/flash_esp32.c

@@ -34,6 +34,32 @@
 
 #define FLASH_SEM_TIMEOUT (k_is_in_isr() ? K_NO_WAIT : K_FOREVER)
 
+#ifdef CONFIG_EFUSE_VIRTUAL_KEEP_IN_FLASH
+#define ENCRYPTION_IS_VIRTUAL (!efuse_hal_flash_encryption_enabled())
+#else
+#define ENCRYPTION_IS_VIRTUAL 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
+
 struct flash_esp32_dev_config {
 	spi_dev_t *controller;
 };
@@ -76,6 +102,8 @@
 #include <stdint.h>
 #include <string.h>
 
+static bool aligned_flash_erase(const struct device *dev, size_t addr, size_t size);
+
 #ifdef CONFIG_MCUBOOT
 #define READ_BUFFER_SIZE 32
 static bool flash_esp32_is_aligned(off_t address, void *buffer, size_t length)
@@ -85,6 +113,46 @@
 }
 #endif
 
+bool aligned_flash_read_(off_t address, void *buffer, size_t length, bool read_encrypted)
+{
+	int ret = 0;
+
+	if (read_encrypted) {
+		LOG_INF("Flash read ENCRYPTED - address 0x%lx size 0x%x", address, length);
+		ret = esp_flash_read_encrypted(NULL, address, buffer, length);
+	} else {
+		LOG_INF("Flash read RAW - address 0x%lx size 0x%x", address, length);
+		ret = esp_flash_read(NULL, buffer, address, length);
+	}
+
+	if (ret != 0) {
+		LOG_ERR("Flash read error: %d", ret);
+		return false;
+	}
+
+	return true;
+}
+
+static int flash_esp32_read_check_enc(off_t address, void *buffer, size_t length)
+{
+	int ret = 0;
+
+	if (esp_flash_encryption_enabled()) {
+		LOG_DBG("Flash read ENCRYPTED - address 0x%lx size 0x%x", address, length);
+		ret = esp_flash_read_encrypted(NULL, address, buffer, length);
+	} else {
+		LOG_DBG("Flash read RAW - address 0x%lx size 0x%x", address, length);
+		ret = esp_flash_read(NULL, buffer, address, length);
+	}
+
+	if (ret != 0) {
+		LOG_ERR("Flash read error: %d", ret);
+		return -EIO;
+	}
+
+	return 0;
+}
+
 static int flash_esp32_read(const struct device *dev, off_t address, void *buffer, size_t length)
 {
 	int ret = 0;
@@ -139,11 +207,7 @@
 #else
 	flash_esp32_sem_take(dev);
 
-	if (esp_flash_encryption_enabled()) {
-		ret = esp_flash_read_encrypted(NULL, address, buffer, length);
-	} else {
-		ret = esp_flash_read(NULL, buffer, address, length);
-	}
+	ret = flash_esp32_read_check_enc(address, buffer, length);
 
 	flash_esp32_sem_give(dev);
 #endif
@@ -156,6 +220,26 @@
 	return 0;
 }
 
+static int flash_esp32_write_check_enc(off_t address, const void *buffer, size_t length)
+{
+	int ret = 0;
+
+	if (esp_flash_encryption_enabled() && !ENCRYPTION_IS_VIRTUAL) {
+		LOG_DBG("Flash write ENCRYPTED - address 0x%lx size 0x%x", address, length);
+		ret = esp_flash_write_encrypted(NULL, address, buffer, length);
+	} else {
+		LOG_DBG("Flash write RAW - address 0x%lx size 0x%x", address, length);
+		ret = esp_flash_write(NULL, buffer, address, length);
+	}
+
+	if (ret != 0) {
+		LOG_ERR("Flash write error: %d", ret);
+		return -EIO;
+	}
+
+	return 0;
+}
+
 static int flash_esp32_write(const struct device *dev,
 			     off_t address,
 			     const void *buffer,
@@ -176,9 +260,17 @@
 	flash_esp32_sem_take(dev);
 
 	if (esp_flash_encryption_enabled()) {
-		ret = esp_flash_write_encrypted(NULL, address, buffer, length);
-	} else {
-		ret = esp_flash_write(NULL, buffer, address, length);
+		/* Ensuring flash region has been erased before writing in order to
+		 * avoid inconsistences when hardware flash encryption is enabled.
+		 */
+		if (!aligned_flash_erase(dev, address, length)) {
+			LOG_ERR("%s: Flash erase before write failed", __func__);
+			ret = -1;
+		}
+	}
+
+	if (ret == 0) {
+		ret = flash_esp32_write_check_enc(address, buffer, length);
 	}
 
 	flash_esp32_sem_give(dev);
@@ -192,6 +284,71 @@
 	return 0;
 }
 
+static bool aligned_flash_erase(const struct device *dev, size_t addr, size_t size)
+{
+    if (IS_ALIGNED(addr, FLASH_SECTOR_SIZE) && IS_ALIGNED(size, FLASH_SECTOR_SIZE)) {
+        /* A single write operation is enough when all parameters are aligned */
+
+        return esp_flash_erase_region(NULL, addr, size) == ESP_OK;
+    }
+
+    const uint32_t aligned_addr = ALIGN_DOWN(addr, FLASH_SECTOR_SIZE);
+    const uint32_t addr_offset = ALIGN_OFFSET(addr, FLASH_SECTOR_SIZE);
+    uint32_t bytes_remaining = size;
+    uint8_t write_data[FLASH_SECTOR_SIZE] = {0};
+
+    /* Perform a read operation considering an offset not aligned to 4-byte boundary */
+
+    uint32_t bytes = MIN(bytes_remaining + addr_offset, sizeof(write_data));
+    if (flash_esp32_read_check_enc(aligned_addr, write_data, ALIGN_UP(bytes, FLASH_SECTOR_SIZE)) != ESP_OK) {
+        return false;
+    }
+
+    if (esp_flash_erase_region(NULL, aligned_addr, ALIGN_UP(bytes, FLASH_SECTOR_SIZE)) != ESP_OK) {
+        LOG_ERR("%s: Flash erase failed", __func__);
+        return -1;
+    }
+
+    uint32_t bytes_written = bytes - addr_offset;
+
+    /* Write first part of non-erased data */
+    if(addr_offset > 0) {
+        flash_esp32_write_check_enc(aligned_addr, write_data, addr_offset);
+    }
+
+    if(bytes < sizeof(write_data)) {
+        flash_esp32_write_check_enc(aligned_addr + bytes, write_data + bytes, sizeof(write_data) - bytes);
+    }
+
+    bytes_remaining -= bytes_written;
+
+    /* Write remaining data to Flash if any */
+
+    uint32_t offset = bytes;
+
+    while (bytes_remaining != 0) {
+        bytes = MIN(bytes_remaining, sizeof(write_data));
+        if (flash_esp32_read_check_enc(aligned_addr + offset, write_data, ALIGN_UP(bytes, FLASH_SECTOR_SIZE)) != ESP_OK) {
+            return false;
+        }
+
+        if (esp_flash_erase_region(NULL, aligned_addr + offset, ALIGN_UP(bytes, FLASH_SECTOR_SIZE)) != ESP_OK) {
+            LOG_ERR("%s: Flash erase failed", __func__);
+            return -1;
+        }
+
+        if(bytes < sizeof(write_data)) {
+            flash_esp32_write_check_enc(aligned_addr + offset + bytes, write_data + bytes, sizeof(write_data) - bytes);
+        }
+
+        offset += bytes;
+        bytes_written += bytes;
+        bytes_remaining -= bytes;
+    }
+
+    return true;
+}
+
 static int flash_esp32_erase(const struct device *dev, off_t start, size_t len)
 {
 	int ret = 0;
@@ -200,7 +357,20 @@
 	ret = esp_rom_flash_erase_range(start, len);
 #else
 	flash_esp32_sem_take(dev);
-	ret = esp_flash_erase_region(NULL, start, len);
+
+    if ((len % FLASH_SECTOR_SIZE) != 0 || (start % FLASH_SECTOR_SIZE) != 0) {
+        LOG_DBG("%s: Not aligned on sector Offset: 0x%x Length: 0x%x",
+                     __func__, (int)start, (int)len);
+
+        if(!aligned_flash_erase(dev, start, len)) {
+			ret = -EIO;
+		}
+    } else {
+		LOG_DBG("%s: Aligned Addr: 0x%08x Length: %d", __func__, (int)start, (int)len);
+
+		ret = esp_flash_erase_region(NULL, start, len);
+	}
+
 	flash_esp32_sem_give(dev);
 #endif
 	if (ret != 0) {

samples/subsys/mgmt/mcumgr/smp_svr/boards/esp32s3_devkitm_procpu.overlay

@@ -0,0 +1,4 @@
+&flash0 {
+       write-block-size = <32>;
+};
+

samples/subsys/mgmt/mcumgr/smp_svr/overlay-bt.conf

@@ -14,7 +14,7 @@ CONFIG_MCUMGR_TRANSPORT_BT_CONN_PARAM_CONTROL=y
 # Enable the Shell mcumgr transport.
 CONFIG_BASE64=y
 CONFIG_CRC=y
-CONFIG_SHELL=y
+# CONFIG_SHELL=y
 CONFIG_SHELL_BACKEND_SERIAL=y
 CONFIG_MCUMGR_TRANSPORT_SHELL=y
 

samples/subsys/mgmt/mcumgr/smp_svr/prj.conf

@@ -8,7 +8,7 @@ CONFIG_FLASH_MAP=y
 
 # Some command handlers require a large stack.
 CONFIG_SYSTEM_WORKQUEUE_STACK_SIZE=2304
-CONFIG_MAIN_STACK_SIZE=2048
+CONFIG_MAIN_STACK_SIZE=8192
 
 # Ensure an MCUboot-compatible binary is generated.
 CONFIG_BOOTLOADER_MCUBOOT=y
@@ -35,7 +35,15 @@ CONFIG_MCUMGR_GRP_STAT=y
 
 # Enable logging
 CONFIG_LOG=y
-CONFIG_MCUBOOT_UTIL_LOG_LEVEL_WRN=y
+CONFIG_MCUBOOT_UTIL_LOG_LEVEL_INF=y
+CONFIG_LOG_MODE_IMMEDIATE=y
+# CONFIG_LOG_BUFFER_SIZE=4096
 
 # Disable debug logging
 CONFIG_LOG_MAX_LEVEL=3
+
+CONFIG_REBOOT=y
+
+# Ensure asset is aligned and padded correctly for flash encryption.
+CONFIG_MCUBOOT_EXTRA_IMGTOOL_ARGS="--align 32 --max-align 32 --pad --pad-sig"
+CONFIG_MCUBOOT_GENERATE_CONFIRMED_IMAGE=y

samples/subsys/mgmt/mcumgr/smp_svr/src/main.c

@@ -8,6 +8,7 @@
 #include <zephyr/kernel.h>
 #include <zephyr/stats/stats.h>
 #include <zephyr/usb/usb_device.h>
+#include <zephyr/dfu/mcuboot.h>
 
 #ifdef CONFIG_MCUMGR_GRP_FS
 #include <zephyr/device.h>
@@ -18,8 +19,14 @@
 #include <zephyr/mgmt/mcumgr/grp/stat_mgmt/stat_mgmt.h>
 #endif
 
+#include <zephyr/sys/reboot.h>
+#include <stdlib.h>
+#include <stdio.h>
+
 #define LOG_LEVEL LOG_LEVEL_DBG
 #include <zephyr/logging/log.h>
+#include <zephyr/logging/log_ctrl.h>
+
 LOG_MODULE_REGISTER(smp_sample);
 
 #include "common.h"
@@ -50,6 +57,42 @@ static struct fs_mount_t littlefs_mnt = {
 };
 #endif
 
+int latch_ota_if_required()
+{
+	// boot_request_upgrade(false);
+
+	// LOG_PANIC();
+	// k_sleep(K_SECONDS(2));
+	// sys_reboot(SYS_REBOOT_COLD);
+
+	LOG_INF("latch_ota_if_required");
+
+	if (boot_is_img_confirmed()) {
+		LOG_INF("No update is currently being tested");
+		return 0;
+	}
+
+	LOG_INF("latch_ota_if_required 2");
+
+	int ret = boot_write_img_confirmed();
+	if (ret) {
+		LOG_ERR("Current image is being tested, but failed to confirm! Rollback imminent!"
+			"(err %d)",
+			ret);
+		return ret;
+	}
+
+	LOG_INF("boot_write_img_confirmed() appears to have been successful");
+
+	if (boot_is_img_confirmed()) {
+		LOG_INF("boot_write_img_confirmed worked");
+	} else {
+		LOG_ERR("boot_write_img_confirmed did not work -- this shouldn't happen");
+	}
+
+	return 0;
+}
+
 int main(void)
 {
 	int rc = STATS_INIT_AND_REG(smp_svr_stats, STATS_SIZE_32,
@@ -85,6 +128,8 @@ int main(void)
 	 */
 	LOG_INF("build time: " __DATE__ " " __TIME__);
 
+	latch_ota_if_required();
+
 	/* The system work queue handles all incoming mcumgr requests.  Let the
 	 * main thread idle while the mcumgr server runs.
 	 */

subsys/dfu/boot/mcuboot.c

@@ -27,7 +27,7 @@
 
 #include "mcuboot_priv.h"
 
-LOG_MODULE_REGISTER(mcuboot_dfu, LOG_LEVEL_DBG);
+LOG_MODULE_REGISTER(mcuboot_dfu, LOG_LEVEL_INF);
 
 /*
  * Helpers for image headers and trailers, as defined by mcuboot.
@@ -305,10 +305,18 @@ bool boot_is_img_confirmed(void)
 
 	rc = boot_read_swap_state(fa, &state);
 	if (rc != 0) {
+		LOG_ERR("boot_read_swap_state ERROR");
 		return false;
 	}
 
+	LOG_INF("boot_is_img_confirmed flash_area id: 0x%x offset: 0x%lx", fa->fa_id, fa->fa_off);
+
+	LOG_INF("boot_is_img_confirmed swap_state magic=0x%x swap_type=0x%x copy_done=0x%x image_ok=0x%x image_num=0x%x",
+		state.magic, state.swap_type, state.copy_done, state.image_ok, state.image_num);
+
 	if (state.magic == BOOT_MAGIC_UNSET) {
+		LOG_INF("boot_is_img_confirmed MAGIC UNSET, treating as CONFIRMED ");
+
 		/* This is initial/preprogramed image.
 		 * Such image can neither be reverted nor physically confirmed.
 		 * Treat this image as confirmed which ensures consistency
@@ -329,6 +337,8 @@ int boot_write_img_confirmed(void)
 		return -EIO;
 	}
 
+	LOG_INF("boot_write_img_confirmed flash_area id: 0x%x offset: 0x%lx", fa->fa_id, fa->fa_off);
+
 	rc = boot_set_next(fa, true, true);
 
 	flash_area_close(fa);