Pic32c: fix unaligned read, always check flash done after flash operations

hal/hal.c

@@ -178,7 +178,46 @@ int hal_flash_test_align(void)
     wolfBoot_printf("Unaligned write test passed\n");
     return 0;
 }
-#endif
+
+int hal_flash_test_unaligned_src(void)
+{
+    uint32_t src[9];
+    unsigned int i;
+    uint8_t *ptr;
+    int ret;
+
+    /* force unaligned pointer */
+    ptr = (uint8_t*)(uintptr_t)src;
+    ptr++;
+
+    for (i = 0; i < sizeof(src); i++) {
+        ptr[i] = i & 0xff;
+    }
+
+    hal_flash_unlock();
+    ret = hal_flash_erase(TEST_ADDRESS, TEST_SZ);
+    hal_flash_lock();
+    if (ret != 0) {
+        wolfBoot_printf("Erase Sector failed: Ret %d\n", ret);
+        return -1;
+    }
+
+    hal_flash_unlock();
+    ret = hal_flash_write(TEST_ADDRESS, ptr, sizeof(src) - 1);
+    hal_flash_lock();
+    if (ret != 0) {
+        wolfBoot_printf("writing for unaligned source failed: Ret %d\n", ret);
+        return -1;
+    }
+    if (memcmp(ptr, (uint8_t*)TEST_ADDRESS, sizeof(src) - 1) != 0) {
+        wolfBoot_printf("unaligned source verification failed\n");
+        return -1;
+    }
+
+    return 0;
+}
+
+#endif /* TEST_FLASH_READONLY */
 
 /* This test can be run only if swapping the flash do not reboot the board */
 #if defined(DUALBANK_SWAP) && !defined(TEST_FLASH_READONLY)

hal/pic32c.c

@@ -76,6 +76,7 @@
 #define FCW_MUTEX_LOCK_MASK 0x1
 
 #define FCW_WRITE_SIZE (4 * 8)
+#define FCW_WRITE_WORD_SIZE (8)
 static uint32_t pic32_last_err = 0;
 
 #define OSCCTRL_STATUS (*(volatile uint32_t *)(OSCCTRL_BASE + 0x10U))
@@ -170,23 +171,26 @@ static void pic32_fcw_wait_complete(void)
     while (FCW_STATUS & FCW_BUSY_MASK) {}
 }
 
-static int pic32_write_dqword_aligned(uint32_t addr, const uint8_t *data)
+static int pic32_write_dqword_aligned(uint32_t addr, const uint32_t *data)
 {
-    uint32_t i;
-    uint32_t *_data = (uint32_t *)data;
     uint32_t err;
+    uint32_t i;
 
     pic32_fcw_wait_complete();
     FCW_ADDR = addr;
     for (i = 0; i < 8; i++) {
-        FCW_DATA[i] = _data[i];
+        FCW_DATA[i] = data[i];
     }
     FCW_KEY = FCW_UNLOCK_WRKEY;
     pic32_fcw_start_op(FCW_OP_QUAD_DOUBLE_WORD_WRITE);
     pic32_fcw_wait_complete();
     err = pic32_get_errs();
     pic32_last_err = err;
-    err &= ~FCW_INTFLAG_DONE_BIT;
+    if (!(err & FCW_INTFLAG_DONE_BIT)) {
+        err = -1;
+    } else {
+        err &= ~FCW_INTFLAG_DONE_BIT;
+    }
     pic32_clear_errs();
     return err;
 }
@@ -201,14 +205,6 @@ static uint32_t pic32_addr_dqword_align(uint32_t addr)
     return (addr & ~0x1F);
 }
 
-static void pic32_copy_dqword(uint8_t *dst, uint32_t addr)
-{
-    int i;
-    for (i = 0; i < FCW_WRITE_SIZE; i++) {
-        dst[i] = *(volatile uint8_t *)(addr + i);
-    }
-}
-
 static int pic32_fcw_erase_sector(uint32_t addr)
 {
     uint32_t err;
@@ -218,9 +214,13 @@ static int pic32_fcw_erase_sector(uint32_t addr)
     pic32_fcw_start_op(FCW_OP_ERASE_SECTOR);
     pic32_fcw_wait_complete();
     err = pic32_get_errs();
-    pic32_clear_errs();
     pic32_last_err = err;
-    err &= ~FCW_INTFLAG_DONE_BIT;
+    if (!(err & FCW_INTFLAG_DONE_BIT)) {
+        err = -1;
+    } else {
+        err &= ~FCW_INTFLAG_DONE_BIT;
+    }
+    pic32_clear_errs();
     return err;
 }
 
@@ -239,7 +239,8 @@ static uint8_t pic32_mask_zeros(uint8_t programmed, uint8_t to_program)
 
 int pic32_flash_write(uint32_t address, const uint8_t *data, int len)
 {
-    uint8_t buff[FCW_WRITE_SIZE], curr[FCW_WRITE_SIZE];
+    uint32_t buff[FCW_WRITE_WORD_SIZE], curr[FCW_WRITE_WORD_SIZE];
+    uint8_t  *p_buff, *p_curr;
     uint32_t _addr;
     uint8_t i;
     int ret;
@@ -256,11 +257,13 @@ int pic32_flash_write(uint32_t address, const uint8_t *data, int len)
              * is at least WOLFBOOT_SECTOR_SIZE, an erase was already performed,
              * so we can write data directly.
              */
-            pic32_copy_dqword(curr, _addr);
-            memset(buff, 0xff, FCW_WRITE_SIZE);
+            memcpy(curr, (uint8_t*)(uintptr_t)_addr, sizeof(curr));
+            memset(buff, 0xff, sizeof(buff));
             i = address - _addr;
+            p_curr = (uint8_t*)curr;
+            p_buff = (uint8_t*)buff;
             for (; i < FCW_WRITE_SIZE && len > 0; i++, len--) {
-                buff[i] = pic32_mask_zeros(curr[i], *data);
+                p_buff[i] = pic32_mask_zeros(p_curr[i], *data);
                 data++;
                 address++;
             }
@@ -269,7 +272,15 @@ int pic32_flash_write(uint32_t address, const uint8_t *data, int len)
                 return ret;
             continue;
         }
-        ret = pic32_write_dqword_aligned(address, data);
+
+        /* move data in aligned buffer */
+        if (!pic32_addr_is_dqword_aligned((uint32_t)(uintptr_t)data)) {
+            memcpy(buff, data, sizeof(buff));
+            ret = pic32_write_dqword_aligned(address, buff);
+        } else {
+            ret = pic32_write_dqword_aligned(address, (uint32_t*)data);
+        }
+
         if (ret != 0)
             return ret;
         address += FCW_WRITE_SIZE;
@@ -406,6 +417,7 @@ int hal_flash_test_align(void);
 int hal_flash_test_write_once(void);
 int hal_flash_test(void);
 int hal_flash_test_dualbank(void);
+int hal_flash_test_unaligned_src(void);
 void pic32_flash_test(void)
 {
     int ret;
@@ -419,6 +431,12 @@ void pic32_flash_test(void)
     ret = hal_flash_test_write_once();
     if (ret != 0)
         wolfBoot_panic();
+    /* enable unaligned access fault for testing */
+    ret = *(volatile uint32_t*)0xE000ED14;
+    *(volatile uint32_t*)0xE000ED14 = ret | 8;
+    ret = hal_flash_test_unaligned_src();
+    if (ret != 0)
+        wolfBoot_panic();
 #ifdef DUALBANK_SWAP
     ret = hal_flash_test_dualbank();
     if (ret != 0)