diff --git a/hal/nrf52.c b/hal/nrf52.c
index bb4d2423..166bb6dd 100644
--- a/hal/nrf52.c
+++ b/hal/nrf52.c
@@ -53,46 +53,30 @@ static void flash_wait_complete(void)
 
 int hal_flash_write(uint32_t address, const uint8_t *data, int len)
 {
-    int i;
-    uint8_t off = address & 0x03;
-    int words, align;
+    int i = 0;
+    uint32_t *src, *dst;
 
-    if (off != 0) {
-        uint32_t first = *((uint32_t *)(address - off));
-        uint8_t *firstbytes = (uint8_t *)(&first);
-        for (i = 0; i < 4 - off; i++) {
-            firstbytes[i + off] = data[i];
-        }
-        NVMC_CONFIG = NVMC_CONFIG_WEN;
-        flash_wait_complete();
-        *((uint32_t *)(address - off)) = first;
-        flash_wait_complete();
-        address += 4 - off;
-        data += 4 - off;
-        len -= 4 - off;
-    }
-    if (len > 3) {
-        uint32_t *src = (uint32_t *)data;
-        uint32_t *dst = (uint32_t *)address;
-        words = len / 4;
-        align = words * 4;
-        for (i = 0; i < words; i ++) {
+    while (i < len) {
+        if ((len - i > 3) && ((((address + i) & 0x03) == 0)  && ((((uint32_t)data) + i) & 0x03) == 0)) {
+            src = (uint32_t *)data;
+            dst = (uint32_t *)address;
             NVMC_CONFIG = NVMC_CONFIG_WEN;
             flash_wait_complete();
-            dst[i] = src[i];
+            dst[i >> 2] = src[i >> 2];
             flash_wait_complete();
-        }
-        if (len > align) {
-            uint32_t last = dst[words];
-            uint8_t *lastbytes = (uint8_t *)(&last);
-            for (i = off; i < 4; i++) {
-                if (i < len - align)
-                    lastbytes[3 - i] = data[align + i];
-            }
+            i+=4;
+        } else {
+            uint32_t val;
+            uint8_t *vbytes = (uint8_t *)(&val);
+            int off = (address + i) - (((address + i) >> 2) << 2);
+            dst = (uint32_t *)(address - off);
+            val = dst[i >> 2];
+            vbytes[off] = data[i];
             NVMC_CONFIG = NVMC_CONFIG_WEN;
             flash_wait_complete();
-            dst[words] = last;
+            dst[i >> 2] = val;
             flash_wait_complete();
+            i++;
         }
     }
     return 0;
@@ -109,7 +93,7 @@ void hal_flash_lock(void)
 
 int hal_flash_erase(uint32_t address, int len)
 {
-    uint32_t end = address + len;
+    uint32_t end = address + len - 1;
     uint32_t p;
     for (p = address; p <= end; p += FLASH_PAGE_SIZE) {
         NVMC_CONFIG = NVMC_CONFIG_EEN;
diff --git a/src/image.c b/src/image.c
index eedaee64..50e9823f 100644
--- a/src/image.c
+++ b/src/image.c
@@ -195,15 +195,17 @@ int wolfBoot_verify_authenticity(struct wolfBoot_image *img)
 
 int wolfBoot_copy(uint32_t src, uint32_t dst, uint32_t size)
 {
-    uint32_t *content;
+    uint32_t *orig, *copy;
     uint32_t pos = 0;
     if (src == dst)
         return 0;
     if ((src & 0x03) || (dst & 0x03))
         return -1;
     while (pos < size) {
-        content = (uint32_t *)(src + pos);
-        hal_flash_write(dst + pos, (void *)content, sizeof(uint32_t));
+        orig = (uint32_t *)(src + pos);
+        copy = (uint32_t *)(dst + pos);
+        while (*orig != *copy)
+            hal_flash_write(dst + pos, (void *)orig, sizeof(uint32_t));
         pos += sizeof(uint32_t);
     }
     return pos;
diff --git a/src/loader.c b/src/loader.c
index 5c5acbd8..f49f7d32 100644
--- a/src/loader.c
+++ b/src/loader.c
@@ -27,21 +27,40 @@ extern void do_boot(const uint32_t *app_offset);
 
 static int wolfBoot_update(void)
 {
-    uint32_t total_size;
+    uint32_t total_size = 0;
     uint32_t sector_size = WOLFBOOT_SECTOR_SIZE;
     uint32_t sector = 0;
     uint8_t flag, st;
-    struct wolfBoot_image update;
+    struct wolfBoot_image boot, update;
 
-    if ((wolfBoot_open_image(&update, PART_UPDATE) < 0) ||
-            (wolfBoot_verify_integrity(&update) < 0)  ||
-            (wolfBoot_verify_authenticity(&update) < 0)) {
+    /* Use biggest size for the swap */
+    if ((wolfBoot_open_image(&update, PART_UPDATE) == 0) && (update.fw_size + IMAGE_HEADER_SIZE) > total_size)
+            total_size = update.fw_size + IMAGE_HEADER_SIZE;
+    if ((wolfBoot_open_image(&boot, PART_BOOT) == 0) && (boot.fw_size + IMAGE_HEADER_SIZE) > total_size)
+            total_size = boot.fw_size + IMAGE_HEADER_SIZE;
+
+    if (total_size < IMAGE_HEADER_SIZE)
         return -1;
+
+    /* Check the first sector to detect interrupted update */
+    if ((wolfBoot_get_sector_flag(PART_UPDATE, 0, &flag) < 0) || (flag == SECT_FLAG_NEW))
+    {
+        /* In case this is a new update, check 
+         * integrity/authenticity of the firmware update 
+         * before starting the swap
+         */
+        if (!update.hdr_ok || (wolfBoot_verify_integrity(&update) < 0)  
+                || (wolfBoot_verify_authenticity(&update) < 0)) {
+            return -1;
+        }
     }
 
-    total_size = update.fw_size + IMAGE_HEADER_SIZE;
     hal_flash_unlock();
 
+    /* Interruptible swap
+     * The status is saved in the sector flags of the update partition.
+     * If something goes wrong, the operation will be resumed upon reboot.
+     */
     while ((sector * sector_size) < total_size) {
         if ((wolfBoot_get_sector_flag(PART_UPDATE, sector, &flag) != 0) || (flag == SECT_FLAG_NEW)) {
            flag = SECT_FLAG_SWAPPING;
@@ -94,12 +113,16 @@ static int wolfBoot_update(void)
 static void wolfBoot_start(void)
 {
     uint8_t st;
-    struct wolfBoot_image boot;
+    struct wolfBoot_image boot, update;
     if ((wolfBoot_get_partition_state(PART_UPDATE, &st) == 0) && (st == IMG_STATE_UPDATING)) {
         wolfBoot_update();
     } else if ((wolfBoot_get_partition_state(PART_BOOT, &st) == 0) && (st == IMG_STATE_TESTING)) {
         wolfBoot_update_trigger();
-        wolfBoot_update();
+        if ((wolfBoot_open_image(&update, PART_UPDATE) < 0) ||
+                (wolfBoot_verify_integrity(&update) < 0)  ||
+                (wolfBoot_verify_authenticity(&update) < 0)) {
+            wolfBoot_update();
+        }
     }
     if ((wolfBoot_open_image(&boot, PART_BOOT) < 0) ||
             (wolfBoot_verify_integrity(&boot) < 0)  ||