Fixed update + write to the right bank

Author: danielinux

hal/stm32_tz.c

@@ -48,8 +48,6 @@ static void RAMFUNCTION hal_flash_nonsecure_unlock(void)
         DMB();
         FLASH_NS_KEYR = FLASH_KEY2;
         DMB();
-        while ((FLASH_NS_CR & FLASH_CR_LOCK) != 0)
-            ;
     }
 }
 
@@ -97,37 +95,42 @@ void hal_tz_claim_nonsecure_area(uint32_t address, int len)
     int page_n, reg_idx;
     uint32_t reg;
     uint32_t end = address + len;
+    uint32_t start_address = address;
     uint32_t bank = 0;
     int pos;
 
     if (!is_range_nonsecure(address, len))
         return;
-    while (address < end) {
-        if (address < FLASH_BANK2_BASE) {
-            page_n = (address - ARCH_FLASH_OFFSET) / FLASH_PAGE_SIZE;
-            bank = 0;
-        } else {
-            page_n = (address - FLASH_BANK2_BASE) / FLASH_PAGE_SIZE;
-            bank = 1;
-        }
 
+    if (address < FLASH_BANK2_BASE) {
+        page_n = (address - ARCH_FLASH_OFFSET) / FLASH_PAGE_SIZE;
+        bank = 0;
+    } else {
+        page_n = (address - FLASH_BANK2_BASE) / FLASH_PAGE_SIZE;
+        bank = 1;
+    }
 #ifdef PLATFORM_stm32h5
-        /* Take into account current swap configuration */
-        if ((FLASH_OPTSR_CUR & FLASH_OPTSR_SWAP_BANK) >> 31)
-            bank = !bank;
+    /* Take into account current swap configuration */
+    if ((FLASH_OPTSR_CUR & FLASH_OPTSR_SWAP_BANK) >> 31)
+        bank = !bank;
 #endif
+    while (address < end) {
         reg_idx = page_n / 32;
         pos = page_n % 32;
         hal_flash_wait_complete(bank);
         hal_flash_clear_errors(bank);
-        hal_flash_nonsecure_unlock();
         if (bank == 0)
             FLASH_SECBB1[reg_idx] |= ( 1 << pos);
         else
             FLASH_SECBB2[reg_idx] |= ( 1 << pos);
         ISB();
         hal_flash_wait_complete(bank);
-        hal_flash_nonsecure_lock();
+        address += FLASH_PAGE_SIZE;
+        page_n++;
+    }
+
+    address = start_address;
+    while (address < end) {
         /* Erase claimed non-secure page, in secure mode */
 #ifndef PLATFORM_stm32h5
         reg = FLASH_CR & (~((FLASH_CR_PNB_MASK << FLASH_CR_PNB_SHIFT) | FLASH_CR_PER | FLASH_CR_BKER | FLASH_CR_PG | FLASH_CR_MER1 | FLASH_CR_MER2));
@@ -156,9 +159,48 @@ void hal_tz_claim_nonsecure_area(uint32_t address, int len)
 #if defined (__ARM_FEATURE_CMSE) && (__ARM_FEATURE_CMSE == 3U)
 void hal_tz_release_nonsecure_area(void)
 {
+#ifndef DUALBANK_SWAP
     int i;
     for (i = 0; i < FLASH_SECBB_NREGS; i++)
         FLASH_SECBB2[i] = 0;
+#else
+    uint32_t addr;
+    int bank_swp = 0;
+    /* Take into account current swap configuration */
+    if ((FLASH_OPTSR_CUR & FLASH_OPTSR_SWAP_BANK) >> 31)
+        bank_swp = 1;
+
+    /* Bank 1 */
+    for(addr = WOLFBOOT_PARTITION_BOOT_ADDRESS;
+            addr < FLASH_BANK2_BASE; addr += FLASH_PAGE_SIZE) {
+        uint32_t page_n = (addr - FLASHMEM_ADDRESS_SPACE) / FLASH_PAGE_SIZE;
+        uint32_t reg_idx = page_n / 32;
+        uint32_t pos = page_n % 32;
+        hal_flash_wait_complete(0);
+        hal_flash_clear_errors(0);
+        if (!bank_swp)
+            FLASH_SECBB1[reg_idx] &= ~( 1 << pos);
+        else
+            FLASH_SECBB2[reg_idx] &= ~( 1 << pos);
+        ISB();
+        hal_flash_wait_complete(0);
+    }
+    /* Bank 2 */
+    for(addr = WOLFBOOT_PARTITION_UPDATE_ADDRESS;
+            addr < FLASH_TOP; addr += FLASH_PAGE_SIZE) {
+        uint32_t page_n = (addr - FLASH_BANK2_BASE) / FLASH_PAGE_SIZE;
+        uint32_t reg_idx = page_n / 32;
+        uint32_t pos = page_n % 32;
+        hal_flash_wait_complete(1);
+        hal_flash_clear_errors(1);
+        if (!bank_swp)
+            FLASH_SECBB2[reg_idx] &= ~( 1 << pos);
+        else
+            FLASH_SECBB1[reg_idx] &= ~( 1 << pos);
+        ISB();
+        hal_flash_wait_complete(1);
+    }
+#endif
 }
 #else
 #define release_nonsecure_area(...) do{}while(0)
@@ -243,13 +285,14 @@ void hal_tz_sau_init(void)
 {
     uint32_t page_n = 0;
     /* SAU is set up before staging. Set up all areas as secure. */
+
     /* Non-secure callable: NSC functions area */
     sau_init_region(0, 0x0C038000, 0x0C040000, 1);
 
-    /* Non-Secure: application flash area (first bank) */
+    /* Secure: application flash area (first bank) */
     sau_init_region(1, WOLFBOOT_PARTITION_BOOT_ADDRESS, FLASH_BANK2_BASE - 1, 0);
 
-    /* Non-Secure: application flash area (second bank) */
+    /* Secure: application flash area (second bank) */
     sau_init_region(2, WOLFBOOT_PARTITION_UPDATE_ADDRESS, FLASH_TOP -1, 0);
 
     /* Secure RAM regions in SRAM1/SRAM2 */

hal/stm32h5.c

@@ -94,6 +94,8 @@ int RAMFUNCTION hal_flash_write(uint32_t address, const uint8_t *data, int len)
     uint32_t *src, *dst;
     uint32_t dword[2];
     volatile uint32_t *sr, *cr;
+    uint32_t off = 0;
+    uint32_t una_len = 0;
 
     cr = &FLASH_CR;
     sr = &FLASH_SR;
@@ -105,28 +107,32 @@ int RAMFUNCTION hal_flash_write(uint32_t address, const uint8_t *data, int len)
 #if (TZ_SECURE())
     if (is_flash_nonsecure(address)) {
         hal_tz_claim_nonsecure_area(address, len);
-    }
-    /* Convert into secure address space */
-    if (((uint32_t)dst & 0x0F000000) == 0x08000000) {
+    } else if (((uint32_t)dst & 0x0F000000) == 0x08000000) {
+        /* Convert into secure address space */
         dst = (uint32_t *)((address & (~FLASHMEM_ADDRESS_SPACE)) | FLASH_SECURE_MMAP_BASE);
     }
 #endif
-
     while (i < len) {
         dword[0] = src[i >> 2];
-        dword[1] = src[(i >> 2) + 1];
+        if (len > i + 1)
+            dword[1] = src[(i >> 2) + 1];
+        else
+            dword[1] = 0xFFFFFFFF;
         *cr |= FLASH_CR_PG;
         dst[i >> 2] = dword[0];
         ISB();
         dst[(i >> 2) + 1] = dword[1];
+        ISB();
         hal_flash_wait_complete(0);
         if ((*sr & FLASH_SR_EOP) != 0)
             *sr |= FLASH_SR_EOP;
         *cr &= ~FLASH_CR_PG;
         i+=8;
     }
 #if (TZ_SECURE())
-    hal_tz_release_nonsecure_area();
+    if (is_flash_nonsecure(address)) {
+        hal_tz_release_nonsecure_area();
+    }
 #endif
     return 0;
 }
@@ -194,29 +200,29 @@ int RAMFUNCTION hal_flash_erase(uint32_t address, int len)
         uint32_t bnksel = 0;
         base = FLASHMEM_ADDRESS_SPACE;
         reg = FLASH_CR & (~((FLASH_CR_PNB_MASK << FLASH_CR_PNB_SHIFT) | FLASH_CR_BER));
+        if(p >= (FLASH_BANK2_BASE) && (p <= (FLASH_TOP) ))
+        {
+            base = FLASH_BANK2_BASE;
+            bnksel = 1;
+        }
 
 #if TZ_SECURE()
         /* When in secure mode, skip erasing non-secure pages: will be erased upon claim */
         if (is_flash_nonsecure(address)) {
             return 0;
         }
 #endif
-        if(p >= (FLASH_BANK2_BASE) && (p <= (FLASH_TOP) ))
-        {
-            base = FLASH_BANK2_BASE;
-            bnksel = 1;
-        } else {
-            FLASH_CR &= ~FLASH_CR_SER ;
-            return 0; /* Address out of range */
-        }
-
         /* Check for swapped banks to invert bnksel */
         if ((FLASH_OPTSR_CUR & FLASH_OPTSR_SWAP_BANK) >> 31)
             bnksel = !bnksel;
 
+#if !TZ_SECURE() && !defined(__FLASH_OTP_PRIMER)
+        printf("Erasing bank %d, page %d\r\n", bnksel, (p - base) >> 13);
+#endif
+
         reg |= ((((p - base)  >> 13) << FLASH_CR_PNB_SHIFT) | FLASH_CR_SER | (bnksel << 31));
         FLASH_CR = reg;
-        DMB();
+        ISB();
         FLASH_CR |= FLASH_CR_STRT;
         hal_flash_wait_complete(0);
     }
@@ -547,21 +553,21 @@ static void fork_bootloader(void)
 #include "uart_drv.h"
 void hal_init(void)
 {
+    clock_pll_on();
 #if TZ_SECURE()
-    hal_tz_sau_init();
     hal_gtzc_init();
+    hal_tz_sau_init();
 #endif
-    clock_pll_on();
 
 #if defined(DUALBANK_SWAP) && defined(__WOLFBOOT)
-    if ((FLASH_OPTSR_CUR & (FLASH_OPTSR_SWAP_BANK)) == 0)
-        fork_bootloader();
+    fork_bootloader();
 #endif
 
 
 }
 
 
+
 void hal_prepare_boot(void)
 {
 

include/wolfboot/wolfboot.h

@@ -301,6 +301,8 @@ int wolfBoot_fallback_is_possible(void);
 int wolfBoot_dualboot_candidate(void);
 
 int wolfBoot_dualboot_candidate_addr(void**);
+int wolfBoot_get_partition_state(uint8_t part, uint8_t *st);
+
 
 #ifdef EXT_ENCRYPTED
 /* Encryption support */

test-app/Makefile

@@ -162,6 +162,7 @@ ifeq ($(TARGET),stm32h5)
     LSCRIPT_TEMPLATE=ARM-stm32h5.ld
   endif
   CFLAGS+=-DAPP_HAS_SYSTICK
+  CFLAGS+=-DRAMFUNCTION='__attribute__((used,section(".ramcode")))'
   CFLAGS+=-mcpu=cortex-m33 -ffunction-sections -fdata-sections -fno-common
   LDFLAGS+=-mcpu=cortex-m33
   LDFLAGS+=-Wl,-gc-sections -Wl,-Map=image.map

test-app/app_stm32h5.c

@@ -368,6 +368,9 @@ static int cmd_update_xmodem(const char *args)
     update_ver = wolfBoot_update_firmware_version();
     if (update_ver != 0) {
         printf("New firmware version: 0x%lx\r\n", update_ver);
+        printf("Triggering update...\r\n");
+        wolfBoot_update_trigger();
+        printf("Update completed successfully.\r\n");
     } else {
         printf("No valid image in update partition\r\n");
     }
@@ -387,25 +390,60 @@ static int cmd_help(const char *args)
     return 0;
 }
 
+const char part_state_names[6][16] = {
+    "NEW",
+    "UPDATING",
+    "FFLAGS",
+    "TESTING",
+    "CONFIRMED",
+    "[Invalid state]"
+};
+
+static const char *part_state_name(uint8_t state)
+{
+    switch(state) {
+        case IMG_STATE_NEW:
+            return part_state_names[0];
+        case IMG_STATE_UPDATING:
+            return part_state_names[1];
+        case IMG_STATE_FINAL_FLAGS:
+            return part_state_names[2];
+        case IMG_STATE_TESTING:
+            return part_state_names[3];
+        case IMG_STATE_SUCCESS:
+            return part_state_names[4];
+        default:
+            return part_state_names[5];
+    }
+}
+
 static int cmd_info(const char *args)
 {
     int i, j;
     uint32_t cur_fw_version, update_fw_version;
     uint32_t n_keys;
     uint16_t hdrSz;
+    uint8_t boot_part_state = IMG_STATE_NEW, update_part_state = IMG_STATE_NEW;
 
     cur_fw_version = wolfBoot_current_firmware_version();
     update_fw_version = wolfBoot_update_firmware_version();
 
+    wolfBoot_get_partition_state(PART_BOOT, &boot_part_state);
+    wolfBoot_get_partition_state(PART_UPDATE, &update_part_state);
+
     printf("\r\n");
     printf("System information\r\n");
     printf("====================================\r\n");
     printf("Flash banks are %sswapped.\r\n", ((FLASH_OPTSR_CUR & (FLASH_OPTSR_SWAP_BANK)) == 0)?"not ":"");
     printf("Firmware version : 0x%lx\r\n", wolfBoot_current_firmware_version());
+    printf("Current firmware state: %s\r\n", part_state_name(boot_part_state));
     if (update_fw_version != 0) {
-        printf("Candidate firmware version : 0x%lx\r\n", update_fw_version);
+        if (update_part_state == IMG_STATE_UPDATING)
+            printf("Candidate firmware version : 0x%lx\r\n", update_fw_version);
+        else
+            printf("Backup firmware version : 0x%lx\r\n", update_fw_version);
+        printf("Update state: %s\r\n", part_state_name(update_part_state));
         if (update_fw_version > cur_fw_version) {
-            wolfBoot_update_trigger();
             printf("'reboot' to initiate update.\r\n");
         } else {
             printf("Update image older than current.\r\n");