diff --git a/boot/bootutil/src/boot_record.c b/boot/bootutil/src/boot_record.c
index 3fb1fcc..e3a2208 100644
--- a/boot/bootutil/src/boot_record.c
+++ b/boot/bootutil/src/boot_record.c
@@ -35,6 +35,7 @@
 #define SHARED_MEMORY_OVERFLOW      (1)
 #define SHARED_MEMORY_OVERWRITE     (2)
 #define SHARED_MEMORY_GEN_ERROR     (3)
+#define SHARED_MEMORY_CORRUPTED     (4)
 
 /**
  * @var shared_memory_init_done
@@ -52,6 +53,7 @@
                              const uint8_t *data)
 {
     struct shared_data_tlv_entry tlv_entry = {0};
+    uint16_t type = SET_TLV_TYPE(major_type, minor_type);
     struct shared_boot_data *boot_data;
     uint16_t boot_data_size;
     uintptr_t tlv_end, offset;
@@ -60,6 +62,7 @@
         return SHARED_MEMORY_GEN_ERROR;
     }
 
+    assert(((uintptr_t)MCUBOOT_SHARED_DATA_BASE & 3u) == 0u);
     boot_data = (struct shared_boot_data *)MCUBOOT_SHARED_DATA_BASE;
 
     /* Check whether first time to call this function. If does then initialise
@@ -71,6 +74,10 @@
         boot_data->header.tlv_tot_len = SHARED_DATA_HEADER_SIZE;
         shared_memory_init_done = true;
     }
+    else if (boot_data->header.tlv_magic != SHARED_DATA_TLV_INFO_MAGIC ||
+             boot_data->header.tlv_tot_len != SHARED_DATA_HEADER_SIZE) {
+        return SHARED_MEMORY_CORRUPTED;
+    }
 
     /* Check whether TLV entry is already added.
      * Get the boundaries of TLV section
@@ -84,8 +91,7 @@
     while (offset < tlv_end) {
         /* Create local copy to avoid unaligned access */
         memcpy(&tlv_entry, (const void *)offset, SHARED_DATA_ENTRY_HEADER_SIZE);
-        if (GET_MAJOR(tlv_entry.tlv_type) == major_type &&
-            GET_MINOR(tlv_entry.tlv_type) == minor_type) {
+        if (tlv_entry.tlv_type == type) {
             return SHARED_MEMORY_OVERWRITE;
         }
 
@@ -93,7 +99,7 @@
     }
 
     /* Add TLV entry */
-    tlv_entry.tlv_type = SET_TLV_TYPE(major_type, minor_type);
+    tlv_entry.tlv_type = type;
     tlv_entry.tlv_len  = size;
 
     if (!boot_u16_safe_add(&boot_data_size, boot_data->header.tlv_tot_len,
@@ -107,12 +113,12 @@
     }
 
     offset = tlv_end;
-    memcpy((void *)offset, &tlv_entry, SHARED_DATA_ENTRY_HEADER_SIZE);
+    (void)memcpy((void *)offset, &tlv_entry, SHARED_DATA_ENTRY_HEADER_SIZE);
 
     offset += SHARED_DATA_ENTRY_HEADER_SIZE;
-    memcpy((void *)offset, data, size);
+    (void)memcpy((void *)offset, data, size);
 
-    boot_data->header.tlv_tot_len += SHARED_DATA_ENTRY_SIZE(size);
+    boot_data->header.tlv_tot_len = boot_data_size;
 
     return SHARED_MEMORY_OK;
 }
@@ -132,12 +138,16 @@
     uint16_t type;
     uint16_t ias_minor;
     size_t record_len = 0;
-    uint8_t image_hash[32]; /* SHA256 - 32 Bytes */
+    uint8_t image_hash[BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE];
     uint8_t buf[MAX_BOOT_RECORD_SZ];
     bool boot_record_found = false;
     bool hash_found = false;
     int rc;
 
+    if (NULL == hdr || NULL == fap) {
+        return -1;
+    }
+
     /* Manifest data is concatenated to the end of the image.
      * It is encoded in TLV format.
      */
@@ -172,7 +182,7 @@
 
         } else if (type == IMAGE_TLV_SHA256) {
             /* Get the image's hash value from the manifest section. */
-            if (len > sizeof(image_hash)) {
+            if (len != BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE) {
                 return -1;
             }
             rc = flash_area_read(fap, offset, image_hash, len);
@@ -204,11 +214,15 @@
      * part of the boot record TLV). For this reason this field has been
      * filled with zeros during the image signing process.
      */
+    if (record_len < sizeof(image_hash)) {
+        return -1;
+    }
+
     offset = record_len - sizeof(image_hash);
     /* The size of 'buf' has already been checked when
      * the BOOT_RECORD TLV was read, it won't overflow.
      */
-    memcpy(buf + offset, image_hash, sizeof(image_hash));
+    (void)memcpy(buf + offset, image_hash, sizeof(image_hash));
 
     /* Add the CBOR encoded boot record to the shared data area. */
     ias_minor = SET_IAS_MINOR(sw_module, SW_BOOT_RECORD);
diff --git a/boot/bootutil/src/bootutil_misc.c b/boot/bootutil/src/bootutil_misc.c
index 221d8a6..75e53d3 100644
--- a/boot/bootutil/src/bootutil_misc.c
+++ b/boot/bootutil/src/bootutil_misc.c
@@ -129,12 +129,12 @@
 boot_status_entries(int image_index, const struct flash_area *fap)
 {
 #if MCUBOOT_SWAP_USING_SCRATCH
-    if (fap->fa_id == FLASH_AREA_IMAGE_SCRATCH) {
+    if (flash_area_get_id(fap) == FLASH_AREA_IMAGE_SCRATCH) {
         return BOOT_STATUS_STATE_COUNT;
     } else
 #endif
-    if (fap->fa_id == FLASH_AREA_IMAGE_PRIMARY(image_index) ||
-               fap->fa_id == FLASH_AREA_IMAGE_SECONDARY(image_index)) {
+    if (flash_area_get_id(fap) == FLASH_AREA_IMAGE_PRIMARY(image_index) ||
+        flash_area_get_id(fap) == FLASH_AREA_IMAGE_SECONDARY(image_index)) {
         return BOOT_STATUS_STATE_COUNT * BOOT_STATUS_MAX_ENTRIES;
     }
     return -1;
@@ -145,21 +145,22 @@
 boot_status_off(const struct flash_area *fap)
 {
     uint32_t off_from_end;
-    uint8_t elem_sz;
+    size_t elem_sz;
 
     elem_sz = flash_area_align(fap);
+    assert(elem_sz != 0u);
 
     off_from_end = boot_trailer_sz(elem_sz);
 
-    assert(off_from_end <= fap->fa_size);
-    return fap->fa_size - off_from_end;
+    assert(off_from_end <= flash_area_get_size(fap));
+    return flash_area_get_size(fap) - off_from_end;
 }
 #endif
 
 static inline uint32_t
 boot_magic_off(const struct flash_area *fap)
 {
-    return fap->fa_size - BOOT_MAGIC_SZ;
+    return flash_area_get_size(fap) - BOOT_MAGIC_SZ;
 }
 
 #ifndef MCUBOOT_SWAP_USING_STATUS
@@ -275,26 +276,20 @@
 {
     uint32_t off;
     const struct flash_area *fap;
-#if MCUBOOT_SWAP_SAVE_ENCTLV
-    int i;
-#endif
     int rc;
 
     rc = boot_find_status(image_index, &fap);
-    if (rc == 0) {
+    if (0 == rc) {
         off = boot_enc_key_off(fap, slot);
 #if MCUBOOT_SWAP_SAVE_ENCTLV
-        uint8_t aes_iv[BOOTUTIL_CRYPTO_AES_CTR_KEY_SIZE];
+        uint8_t aes_iv[BOOTUTIL_CRYPTO_AES_CTR_BLOCK_SIZE];
 
         rc = flash_area_read(fap, off, bs->enctlv[slot], BOOT_ENC_TLV_ALIGN_SIZE);
-        if (rc == 0) {
-            for (i = 0; i < BOOT_ENC_TLV_ALIGN_SIZE; i++) {
-                if (bs->enctlv[slot][i] != 0xff) {
-                    break;
-                }
-            }
-            /* Only try to decrypt non-erased TLV metadata */
-            if (i != BOOT_ENC_TLV_ALIGN_SIZE) {
+        if (0 == rc) {
+            /* Only try to decrypt initialized TLV metadata */
+            if (!bootutil_buffer_is_filled(bs->enctlv[slot],
+                                           BOOT_UNINITIALIZED_TLV_FILL,
+                                           BOOT_ENC_TLV_ALIGN_SIZE)) {
                 rc = boot_enc_decrypt(bs->enctlv[slot], bs->enckey[slot], 0, aes_iv);
             }
         }
@@ -316,9 +311,9 @@
     uint32_t off;
 
     off = boot_copy_done_off(fap);
-    BOOT_LOG_DBG("writing copy_done; fa_id=%d off=0x%lx (0x%lx)",
-                 fap->fa_id, (unsigned long)off,
-                 (unsigned long)(fap->fa_off + off));
+    BOOT_LOG_DBG("writing copy_done; fa_id=%u off=0x%" PRIx32
+                 " (0x%" PRIx32 ")", (unsigned)flash_area_get_id(fap),
+                 off, flash_area_get_off(fap) + off);
     return boot_write_trailer_flag(fap, off, BOOT_FLAG_SET);
 }
 
@@ -328,9 +323,9 @@
     uint32_t off;
 
     off = boot_swap_size_off(fap);
-    BOOT_LOG_DBG("writing swap_size; fa_id=%d off=0x%lx (0x%lx)",
-                 fap->fa_id, (unsigned long)off,
-                 (unsigned long)fap->fa_off + off);
+    BOOT_LOG_DBG("writing swap_size; fa_id=%u off=0x%" PRIx32
+                 " (0x%" PRIx32 ")", (unsigned)flash_area_get_id(fap),
+                 off, flash_area_get_off(fap) + off);
     return boot_write_trailer(fap, off, (const uint8_t *) &swap_size, 4);
 }
 
@@ -345,9 +340,9 @@
     int rc;
 
     off = boot_enc_key_off(fap, slot);
-    BOOT_LOG_DBG("writing enc_key; fa_id=%d off=0x%lx (0x%lx)",
-                 fap->fa_id, (unsigned long)off,
-                 (unsigned long)fap->fa_off + off);
+    BOOT_LOG_DBG("writing enc_key; fa_id=%u off=0x%" PRIx32
+                 " (0x%" PRIx32 ")", (unsigned)flash_area_get_id(fap),
+                 off, flash_area_get_off(fap) + off);
 #if MCUBOOT_SWAP_SAVE_ENCTLV
     rc = flash_area_write(fap, off, bs->enctlv[slot], BOOT_ENC_TLV_ALIGN_SIZE);
 #else
diff --git a/boot/bootutil/src/bootutil_priv.h b/boot/bootutil/src/bootutil_priv.h
index 37a9350..ac40a6e 100644
--- a/boot/bootutil/src/bootutil_priv.h
+++ b/boot/bootutil/src/bootutil_priv.h
@@ -3,7 +3,7 @@
  *
  * Copyright (c) 2017-2020 Linaro LTD
  * Copyright (c) 2017-2019 JUUL Labs
- * Copyright (c) 2019-2020 Arm Limited
+ * Copyright (c) 2019-2021 Arm Limited
  *
  * Original license:
  *
@@ -28,6 +28,7 @@
 #ifndef H_BOOTUTIL_PRIV_
 #define H_BOOTUTIL_PRIV_
 
+#include <inttypes.h>
 #include <string.h>
 
 #include "sysflash/sysflash.h"
@@ -82,15 +83,17 @@
     uint8_t swap_type;    /* The type of swap in effect */
     uint32_t swap_size;   /* Total size of swapped image */
 #ifdef MCUBOOT_ENC_IMAGES
+#define BOOT_UNINITIALIZED_KEY_FILL 0xFF
     uint8_t enckey[BOOT_NUM_SLOTS][BOOT_ENC_KEY_SIZE];
-#if MCUBOOT_SWAP_SAVE_ENCTLV
+#ifdef MCUBOOT_SWAP_SAVE_ENCTLV
+#define BOOT_UNINITIALIZED_TLV_FILL 0xFF
     uint8_t enctlv[BOOT_NUM_SLOTS][BOOT_ENC_TLV_ALIGN_SIZE];
 #endif
 #endif
     int source;           /* Which slot contains swap status metadata */
 };
 
-#define BOOT_STATUS_IDX_0   1
+#define BOOT_STATUS_IDX_0   1U
 
 #define BOOT_STATUS_STATE_0 1
 #define BOOT_STATUS_STATE_1 2
@@ -143,21 +146,19 @@
 #else
 #define ARE_SLOTS_EQUIVALENT()    1
 
-#if (BOOT_IMAGE_NUMBER != 1)
-#error "The MCUBOOT_DIRECT_XIP and MCUBOOT_RAM_LOAD mode only supports single-image boot (MCUBOOT_IMAGE_NUMBER=1)."
-#endif
-#ifdef MCUBOOT_ENC_IMAGES
-#error "Image encryption (MCUBOOT_ENC_IMAGES) is not supported when MCUBOOT_DIRECT_XIP or MCUBOOT_RAM_LOAD mode is selected."
-#endif
+#if defined(MCUBOOT_DIRECT_XIP) && defined(MCUBOOT_ENC_IMAGES)
+#error "Image encryption (MCUBOOT_ENC_IMAGES) is not supported when MCUBOOT_DIRECT_XIP is selected."
+#endif /* MCUBOOT_DIRECT_XIP && MCUBOOT_ENC_IMAGES */
 #endif /* MCUBOOT_DIRECT_XIP || MCUBOOT_RAM_LOAD */
 
 #define BOOT_MAX_IMG_SECTORS       MCUBOOT_MAX_IMG_SECTORS
 
 #define BOOT_LOG_IMAGE_INFO(slot, hdr)                                    \
-    BOOT_LOG_INF("%-9s slot: version=%u.%u.%u+%u",                        \
+    BOOT_LOG_INF("%-9s slot: "                                            \
+                 "version=%u.%u.%" PRIu16 "+%" PRIu32,                    \
                  ((slot) == BOOT_PRIMARY_SLOT) ? "Primary" : "Secondary", \
-                 (hdr)->ih_ver.iv_major,                                  \
-                 (hdr)->ih_ver.iv_minor,                                  \
+                 (unsigned)(hdr)->ih_ver.iv_major,                        \
+                 (unsigned)(hdr)->ih_ver.iv_minor,                        \
                  (hdr)->ih_ver.iv_revision,                               \
                  (hdr)->ih_ver.iv_build_num)
 
@@ -172,7 +173,7 @@
 #error "Too few sectors, please increase BOOT_MAX_IMG_SECTORS to at least 32"
 #endif
 
-#if MCUBOOT_SWAP_USING_MOVE
+#if defined(MCUBOOT_SWAP_USING_MOVE)
 #define BOOT_STATUS_MOVE_STATE_COUNT    1
 #define BOOT_STATUS_SWAP_STATE_COUNT    2
 #define BOOT_STATUS_STATE_COUNT         (BOOT_STATUS_MOVE_STATE_COUNT + BOOT_STATUS_SWAP_STATE_COUNT)
@@ -210,14 +211,14 @@
         size_t num_sectors;
     } imgs[BOOT_IMAGE_NUMBER][BOOT_NUM_SLOTS];
 
-#if MCUBOOT_SWAP_USING_SCRATCH
+#if defined(MCUBOOT_SWAP_USING_SCRATCH)
     struct {
         const struct flash_area *area;
         boot_sector_t *sectors;
         size_t num_sectors;
     } scratch;
 #endif
-#if MCUBOOT_SWAP_USING_STATUS
+#if defined(MCUBOOT_SWAP_USING_STATUS)
     struct {
         const struct flash_area *area;
         boot_sector_t *sectors;
@@ -242,16 +243,11 @@
 
 fih_int boot_fih_memequal(const void *s1, const void *s2, size_t n);
 
-int boot_magic_compatible_check(uint8_t tbl_val, uint8_t val);
+bool boot_magic_compatible_check(uint8_t tbl_val, uint8_t val);
 uint32_t boot_status_sz(uint32_t min_write_sz);
 uint32_t boot_trailer_sz(uint32_t min_write_sz);
 int boot_status_entries(int image_index, const struct flash_area *fap);
 uint32_t boot_status_off(const struct flash_area *fap);
-uint32_t boot_swap_info_off(const struct flash_area *fap);
-int boot_read_swap_state(const struct flash_area *fap,
-                         struct boot_swap_state *state);
-int boot_read_swap_state_by_id(int flash_area_id,
-                               struct boot_swap_state *state);
 int boot_write_magic(const struct flash_area *fap);
 int boot_write_status(const struct boot_loader_state *state, struct boot_status *bs);
 int boot_write_copy_done(const struct flash_area *fap);
@@ -265,7 +261,7 @@
                             uint8_t flag_val);
 int boot_read_swap_size(int image_index, uint32_t *swap_size);
 int boot_slots_compatible(struct boot_loader_state *state);
-uint32_t boot_status_internal_off(const struct boot_status *bs, int elem_sz);
+uint32_t boot_status_internal_off(const struct boot_status *bs, uint32_t elem_sz);
 int boot_read_image_header(struct boot_loader_state *state, int slot,
                            struct image_header *out_hdr, struct boot_status *bs);
 int boot_copy_region(struct boot_loader_state *state,
@@ -289,11 +285,19 @@
 #endif
 
 /**
+ * Checks that a buffer is filled by the specified value.
+ *
+ * @returns true if all bytes in the buffer are equal to `fill`; false if any
+ * of the bytes does not match, or when buffer is NULL, or when len == 0.
+ */
+bool bootutil_buffer_is_filled(const void *buffer, uint8_t fill, size_t len);
+
+/**
  * Checks that a buffer is erased according to what the erase value for the
  * flash device provided in `flash_area` is.
  *
  * @returns true if the buffer is erased; false if any of the bytes is not
- * erased, or when buffer is NULL, or when len == 0.
+ * erased, or when area is NULL, or when buffer is NULL, or when len == 0.
  */
 bool bootutil_buffer_is_erased(const struct flash_area *area,
                                const void *buffer, size_t len);
@@ -324,11 +328,11 @@
  */
 static inline bool boot_u16_safe_add(uint16_t *dest, uint16_t a, uint16_t b)
 {
-    uint32_t tmp = a + b;
+    uint32_t tmp = (uint32_t)a + b;
     if (tmp > UINT16_MAX) {
         return false;
     } else {
-        *dest = tmp;
+        *dest = (uint16_t) tmp;
         return true;
     }
 }
@@ -336,18 +340,17 @@
 /*
  * Accessors for the contents of struct boot_loader_state.
  */
-
-/* These are macros so they can be used as lvalues. */
 #if (BOOT_IMAGE_NUMBER > 1)
 #define BOOT_CURR_IMG(state) ((state)->curr_img_idx)
 #else
-#define BOOT_CURR_IMG(state) 0
+#define BOOT_CURR_IMG(state) 0u
 #endif
 #ifdef MCUBOOT_ENC_IMAGES
 #define BOOT_CURR_ENC(state) ((state)->enc[BOOT_CURR_IMG(state)])
 #else
 #define BOOT_CURR_ENC(state) NULL
 #endif
+/* These are macros so they can be used as lvalues. */
 #define BOOT_IMG(state, slot) ((state)->imgs[BOOT_CURR_IMG(state)][(slot)])
 #define BOOT_IMG_AREA(state, slot) (BOOT_IMG(state, slot).area)
 #define BOOT_WRITE_SZ(state) ((state)->write_sz)
@@ -377,7 +380,7 @@
 static inline uint32_t
 boot_img_slot_off(struct boot_loader_state *state, size_t slot)
 {
-    return BOOT_IMG(state, slot).area->fa_off;
+    return flash_area_get_off(BOOT_IMG(state, slot).area);
 }
 
 #ifndef MCUBOOT_USE_FLASH_AREA_GET_SECTORS
@@ -386,7 +389,7 @@
 boot_img_sector_size(const struct boot_loader_state *state,
                      size_t slot, size_t sector)
 {
-    return BOOT_IMG(state, slot).sectors[sector].fa_size;
+    return flash_area_get_size(&BOOT_IMG(state, slot).sectors[sector]);
 }
 
 /*
@@ -397,8 +400,8 @@
 boot_img_sector_off(const struct boot_loader_state *state, size_t slot,
                     size_t sector)
 {
-    return BOOT_IMG(state, slot).sectors[sector].fa_off -
-           BOOT_IMG(state, slot).sectors[0].fa_off;
+    return flash_area_get_off(&BOOT_IMG(state, slot).sectors[sector]) -
+           flash_area_get_off(&BOOT_IMG(state, slot).sectors[0]);
 }
 
 #else  /* defined(MCUBOOT_USE_FLASH_AREA_GET_SECTORS) */
@@ -407,24 +410,47 @@
 boot_img_sector_size(const struct boot_loader_state *state,
                      size_t slot, size_t sector)
 {
-    return BOOT_IMG(state, slot).sectors[sector].fs_size;
+    return flash_sector_get_size(&BOOT_IMG(state, slot).sectors[sector]);
 }
 
 static inline uint32_t
 boot_img_sector_off(const struct boot_loader_state *state, size_t slot,
                     size_t sector)
 {
-    return BOOT_IMG(state, slot).sectors[sector].fs_off -
-           BOOT_IMG(state, slot).sectors[0].fs_off;
+    return flash_sector_get_off(&BOOT_IMG(state, slot).sectors[sector]) -
+           flash_sector_get_off(&BOOT_IMG(state, slot).sectors[0]);
 }
 
 #endif  /* !defined(MCUBOOT_USE_FLASH_AREA_GET_SECTORS) */
 
 #ifdef MCUBOOT_RAM_LOAD
+#   ifdef __BOOTSIM__
+
+/* Query for the layout of a RAM buffer appropriate for holding the
+ * image.  This will be per-test-thread, and therefore must be queried
+ * through this call. */
+struct bootsim_ram_info {
+    uint32_t start;
+    uint32_t size;
+    uintptr_t base;
+};
+struct bootsim_ram_info *bootsim_get_ram_info(void);
+
+#define IMAGE_GET_FIELD(field) (bootsim_get_ram_info()->field)
+#define IMAGE_RAM_BASE IMAGE_GET_FIELD(base)
+#define IMAGE_EXECUTABLE_RAM_START IMAGE_GET_FIELD(start)
+#define IMAGE_EXECUTABLE_RAM_SIZE IMAGE_GET_FIELD(size)
+
+#   else
+#       define IMAGE_RAM_BASE ((uintptr_t)0)
+#   endif
+
 #define LOAD_IMAGE_DATA(hdr, fap, start, output, size)       \
-    (memcpy((output),(void*)((hdr)->ih_load_addr + (start)), \
+    (memcpy((output),(void*)(IMAGE_RAM_BASE + (hdr)->ih_load_addr + (start)), \
     (size)), 0)
 #else
+#define IMAGE_RAM_BASE ((uintptr_t)0)
+
 #define LOAD_IMAGE_DATA(hdr, fap, start, output, size)       \
     (flash_area_read((fap), (start), (output), (size)))
 #endif /* MCUBOOT_RAM_LOAD */
diff --git a/boot/bootutil/src/bootutil_public.c b/boot/bootutil/src/bootutil_public.c
index 6057b41..f045fae 100644
--- a/boot/bootutil/src/bootutil_public.c
+++ b/boot/bootutil/src/bootutil_public.c
@@ -3,7 +3,7 @@
  *
  * Copyright (c) 2017-2019 Linaro LTD
  * Copyright (c) 2016-2019 JUUL Labs
- * Copyright (c) 2019-2020 Arm Limited
+ * Copyright (c) 2019-2021 Arm Limited
  * Copyright (c) 2020 Nordic Semiconductor ASA
  *
  * Original license:
@@ -51,10 +51,12 @@
 #include "bootutil/enc_key_public.h"
 #endif
 
+#include "bootutil/boot_public_hooks.h"
+
 #ifdef CONFIG_MCUBOOT
-MCUBOOT_LOG_MODULE_DECLARE(mcuboot);
+BOOT_LOG_MODULE_DECLARE(mcuboot);
 #else
-MCUBOOT_LOG_MODULE_REGISTER(mcuboot_util);
+BOOT_LOG_MODULE_REGISTER(mcuboot_util);
 #endif
 
 const uint32_t boot_img_magic[] = {
@@ -142,7 +144,7 @@
 static inline uint32_t
 boot_magic_off(const struct flash_area *fap)
 {
-    return fap->fa_size - BOOT_MAGIC_SZ;
+    return flash_area_get_size(fap) - BOOT_MAGIC_SZ;
 }
 
 static inline uint32_t
@@ -172,15 +174,14 @@
  * @param val                   The magic value in a trailer, encoded as a
  *                                  BOOT_MAGIC_[...].
  *
- * @return                      1 if the two values are compatible;
- *                              0 otherwise.
+ * @return                      true - if the two values are compatible;
+ *                              false - otherwise.
  */
-int
-boot_magic_compatible_check(uint8_t tbl_val, uint8_t val)
+bool boot_magic_compatible_check(uint8_t tbl_val, uint8_t val)
 {
     switch (tbl_val) {
     case BOOT_MAGIC_ANY:
-        return 1;
+        return true;
 
     case BOOT_MAGIC_NOTGOOD:
         return val != BOOT_MAGIC_GOOD;
@@ -190,20 +191,16 @@
     }
 }
 
-bool bootutil_buffer_is_erased(const struct flash_area *area,
-                               const void *buffer, size_t len)
+bool bootutil_buffer_is_filled(const void *buffer, uint8_t fill, size_t len)
 {
-    size_t i;
-    uint8_t *u8b;
-    uint8_t erased_val;
+    uint8_t *p;
 
     if (buffer == NULL || len == 0) {
         return false;
     }
 
-    erased_val = flash_area_erased_val(area);
-    for (i = 0, u8b = (uint8_t *)buffer; i < len; i++) {
-        if (u8b[i] != erased_val) {
+    for (p = (uint8_t *)buffer; len-- > 0; p++) {
+        if (*p != fill) {
             return false;
         }
     }
@@ -211,16 +208,30 @@
     return true;
 }
 
+bool bootutil_buffer_is_erased(const struct flash_area *area,
+                               const void *buffer, size_t len)
+{
+    uint8_t erased_val;
+
+    if (area == NULL) {
+        return false;
+    }
+
+    erased_val = flash_area_erased_val(area);
+
+    return bootutil_buffer_is_filled(buffer, erased_val, len);
+}
+
 static int
 boot_read_flag(const struct flash_area *fap, uint8_t *flag, uint32_t off)
 {
     int rc;
 
     rc = flash_area_read(fap, off, flag, sizeof *flag);
-    if (rc < 0) {
+    if (rc != 0) {
         return BOOT_EFLASH;
     }
-    if (bootutil_buffer_is_erased(fap, flag, sizeof *flag)) {
+    if (*flag == flash_area_erased_val(fap)) {
         *flag = BOOT_FLAG_UNSET;
     } else {
         *flag = boot_flag_decode(*flag);
@@ -248,7 +259,7 @@
 
     off = boot_magic_off(fap);
     rc = flash_area_read(fap, off, magic, BOOT_MAGIC_SZ);
-    if (rc < 0) {
+    if (rc != 0) {
         return BOOT_EFLASH;
     }
     if (bootutil_buffer_is_erased(fap, magic, BOOT_MAGIC_SZ)) {
@@ -259,7 +270,7 @@
 
     off = boot_swap_info_off(fap);
     rc = flash_area_read(fap, off, &swap_info, sizeof swap_info);
-    if (rc < 0) {
+    if (rc != 0) {
         return BOOT_EFLASH;
     }
 
@@ -267,14 +278,14 @@
     state->swap_type = BOOT_GET_SWAP_TYPE(swap_info);
     state->image_num = BOOT_GET_IMAGE_NUM(swap_info);
 
-    if (bootutil_buffer_is_erased(fap, &swap_info, sizeof swap_info) ||
+    if (swap_info == flash_area_erased_val(fap) ||
             state->swap_type > BOOT_SWAP_TYPE_REVERT) {
         state->swap_type = BOOT_SWAP_TYPE_NONE;
         state->image_num = 0;
     }
 
     rc = boot_read_copy_done(fap, &state->copy_done);
-    if (rc) {
+    if (rc != 0) {
         return BOOT_EFLASH;
     }
 
@@ -286,7 +297,7 @@
 int
 boot_read_swap_state_by_id(int flash_area_id, struct boot_swap_state *state)
 {
-    const struct flash_area *fap;
+    const struct flash_area *fap = NULL;
     int rc;
 
     rc = flash_area_open(flash_area_id, &fap);
@@ -309,9 +320,9 @@
 
     off = boot_magic_off(fap);
 
-    BOOT_LOG_DBG("writing magic; fa_id=%d off=0x%lx (0x%lx)",
-                 fap->fa_id, (unsigned long)off,
-                 (unsigned long)(fap->fa_off + off));
+    BOOT_LOG_DBG("writing magic; fa_id=%u off=0x%" PRIx32
+                 " (0x%" PRIx32 ")", (unsigned)flash_area_get_id(fap),
+                 off, flash_area_get_off(fap) + off);
     rc = flash_area_write(fap, off, boot_img_magic, BOOT_MAGIC_SZ);
     if (rc != 0) {
         return BOOT_EFLASH;
@@ -330,11 +341,14 @@
         const uint8_t *inbuf, uint8_t inlen)
 {
     uint8_t buf[BOOT_MAX_ALIGN];
-    uint8_t align;
+    size_t align;
     uint8_t erased_val;
     int rc;
 
     align = flash_area_align(fap);
+    if (align == 0u) {
+        return BOOT_EFLASH;
+    }
     align = (inlen + align - 1) & ~(align - 1);
     if (align > BOOT_MAX_ALIGN) {
         return -1;
@@ -368,9 +382,9 @@
     uint32_t off;
 
     off = boot_image_ok_off(fap);
-    BOOT_LOG_DBG("writing image_ok; fa_id=%d off=0x%lx (0x%lx)",
-                 fap->fa_id, (unsigned long)off,
-                 (unsigned long)(fap->fa_off + off));
+    BOOT_LOG_DBG("writing image_ok; fa_id=%u off=0x%" PRIx32
+                 " (0x%" PRIx32 ")", (unsigned)flash_area_get_id(fap),
+                 off, flash_area_get_off(fap) + off);
     return boot_write_trailer_flag(fap, off, BOOT_FLAG_SET);
 }
 
@@ -394,23 +408,23 @@
 
     BOOT_SET_SWAP_INFO(swap_info, image_num, swap_type);
     off = boot_swap_info_off(fap);
-    BOOT_LOG_DBG("writing swap_info; fa_id=%d off=0x%lx (0x%lx), swap_type=0x%x"
-                 " image_num=0x%x",
-                 fap->fa_id, (unsigned long)off,
-                 (unsigned long)(fap->fa_off + off), swap_type, image_num);
+    BOOT_LOG_DBG("writing swap_info; fa_id=%u off=0x%" PRIx32
+                 " (0x%" PRIx32 "), swap_type=0x%x image_num=0x%x",
+                 (unsigned)flash_area_get_id(fap), off,
+                 flash_area_get_off(fap) + off,
+                 (unsigned)swap_type, (unsigned)image_num);
     return boot_write_trailer(fap, off, (const uint8_t *) &swap_info, 1);
 }
 
-#define BOOT_LOG_SWAP_STATE(area, state)                            \
-    BOOT_LOG_INF("%s: magic=%s, swap_type=0x%x, copy_done=0x%x, "   \
-                 "image_ok=0x%x",                                   \
-                 (area),                                            \
-                 ((state)->magic == BOOT_MAGIC_GOOD ? "good" :      \
-                  (state)->magic == BOOT_MAGIC_UNSET ? "unset" :    \
-                  "bad"),                                           \
-                 (state)->swap_type,                                \
-                 (state)->copy_done,                                \
-                 (state)->image_ok)
+#define BOOT_LOG_SWAP_STATE(area, state)                                        \
+    BOOT_LOG_INF("%s: magic=%s, swap_type=0x%x, copy_done=0x%x, image_ok=0x%x", \
+                 (area),                                                        \
+                 ((state)->magic == BOOT_MAGIC_GOOD  ? "good" :                 \
+                  (state)->magic == BOOT_MAGIC_UNSET ? "unset" :                \
+                                                       "bad"),                  \
+                 (unsigned)(state)->swap_type,                                  \
+                 (unsigned)(state)->copy_done,                                  \
+                 (unsigned)(state)->image_ok)
 
 int
 boot_swap_type_multi(int image_index)
@@ -421,15 +435,28 @@
     int rc;
     size_t i;
 
-    rc = boot_read_swap_state_by_id(FLASH_AREA_IMAGE_PRIMARY(image_index),
-                                    &primary_slot);
+    rc = BOOT_HOOK_CALL(boot_read_swap_state_primary_slot_hook,
+                        BOOT_HOOK_REGULAR, image_index, &primary_slot);
+    if (rc == BOOT_HOOK_REGULAR)
+    {
+        rc = boot_read_swap_state_by_id(FLASH_AREA_IMAGE_PRIMARY(image_index),
+                                        &primary_slot);
+    }
     if (rc) {
         return BOOT_SWAP_TYPE_PANIC;
     }
 
     rc = boot_read_swap_state_by_id(FLASH_AREA_IMAGE_SECONDARY(image_index),
                                     &secondary_slot);
-    if (rc) {
+    if (rc == BOOT_EFLASH) {
+        BOOT_LOG_INF("Secondary image of image pair (%d.) "
+                     "is unreachable. Treat it as empty", image_index);
+        secondary_slot.magic = BOOT_MAGIC_UNSET;
+        secondary_slot.swap_type = BOOT_SWAP_TYPE_NONE;
+        secondary_slot.copy_done = BOOT_FLAG_UNSET;
+        secondary_slot.image_ok = BOOT_FLAG_UNSET;
+        secondary_slot.image_num = 0;
+    } else if (rc) {
         return BOOT_SWAP_TYPE_PANIC;
     }
 
@@ -478,42 +505,44 @@
 }
 
 /**
- * Marks the image in the secondary slot as pending.  On the next reboot,
- * the system will perform a one-time boot of the the secondary slot image.
+ * Marks the image with the given index in the secondary slot as pending. On the
+ * next reboot, the system will perform a one-time boot of the the secondary
+ * slot image.
+ *
+ * @param image_index       Image pair index.
  *
  * @param permanent         Whether the image should be used permanently or
- *                              only tested once:
- *                                  0=run image once, then confirm or revert.
- *                                  1=run image forever.
+ *                          only tested once:
+ *                               0=run image once, then confirm or revert.
+ *                               1=run image forever.
  *
  * @return                  0 on success; nonzero on failure.
  */
 int
-boot_set_pending(int permanent)
+boot_set_pending_multi(int image_index, int permanent)
 {
-    const struct flash_area *fap;
-    struct boot_swap_state state_secondary_slot;
+    const struct flash_area *fap = NULL;
+    struct boot_swap_state state_secondary_slot = {0};
     uint8_t swap_type;
     int rc;
 
-    rc = boot_read_swap_state_by_id(FLASH_AREA_IMAGE_SECONDARY(0),
-                                    &state_secondary_slot);
+    rc = flash_area_open(FLASH_AREA_IMAGE_SECONDARY(image_index), &fap);
     if (rc != 0) {
-        return rc;
+        return BOOT_EFLASH;
+    }
+
+    rc = boot_read_swap_state(fap, &state_secondary_slot);
+    if (rc != 0) {
+        goto done;
     }
 
     switch (state_secondary_slot.magic) {
     case BOOT_MAGIC_GOOD:
         /* Swap already scheduled. */
-        return 0;
+        break;
 
     case BOOT_MAGIC_UNSET:
-        rc = flash_area_open(FLASH_AREA_IMAGE_SECONDARY(0), &fap);
-        if (rc != 0) {
-            rc = BOOT_EFLASH;
-        } else {
-            rc = boot_write_magic(fap);
-        }
+        rc = boot_write_magic(fap);
 
         if (rc == 0 && permanent) {
             rc = boot_write_image_ok(fap);
@@ -528,46 +557,69 @@
             rc = boot_write_swap_info(fap, swap_type, 0);
         }
 
-        flash_area_close(fap);
-        return rc;
+        break;
 
     case BOOT_MAGIC_BAD:
         /* The image slot is corrupt.  There is no way to recover, so erase the
          * slot to allow future upgrades.
          */
-        rc = flash_area_open(FLASH_AREA_IMAGE_SECONDARY(0), &fap);
-        if (rc != 0) {
-            return BOOT_EFLASH;
-        }
-
-        flash_area_erase(fap, 0, fap->fa_size);
-        flash_area_close(fap);
-        return BOOT_EBADIMAGE;
+        flash_area_erase(fap, 0, flash_area_get_size(fap));
+        rc = BOOT_EBADIMAGE;
+        break;
 
     default:
         assert(0);
-        return BOOT_EBADIMAGE;
+        rc = BOOT_EBADIMAGE;
     }
+
+done:
+    flash_area_close(fap);
+    return rc;
 }
 
 /**
- * Marks the image in the primary slot as confirmed.  The system will continue
- * booting into the image in the primary slot until told to boot from a
- * different slot.
+ * Marks the image with index 0 in the secondary slot as pending. On the next
+ * reboot, the system will perform a one-time boot of the the secondary slot
+ * image. Note that this API is kept for compatibility. The
+ * boot_set_pending_multi() API is recommended.
+ *
+ * @param permanent         Whether the image should be used permanently or
+ *                          only tested once:
+ *                               0=run image once, then confirm or revert.
+ *                               1=run image forever.
  *
  * @return                  0 on success; nonzero on failure.
  */
 int
-boot_set_confirmed(void)
+boot_set_pending(int permanent)
 {
-    const struct flash_area *fap;
-    struct boot_swap_state state_primary_slot;
+    return boot_set_pending_multi(0, permanent);
+}
+
+/**
+ * Marks the image with the given index in the primary slot as confirmed.  The
+ * system will continue booting into the image in the primary slot until told to
+ * boot from a different slot.
+ *
+ * @param image_index       Image pair index.
+ *
+ * @return                  0 on success; nonzero on failure.
+ */
+int
+boot_set_confirmed_multi(int image_index)
+{
+    const struct flash_area *fap = NULL;
+    struct boot_swap_state state_primary_slot = {0};
     int rc;
 
-    rc = boot_read_swap_state_by_id(FLASH_AREA_IMAGE_PRIMARY(0),
-                                    &state_primary_slot);
+    rc = flash_area_open(FLASH_AREA_IMAGE_PRIMARY(image_index), &fap);
     if (rc != 0) {
-        return rc;
+        return BOOT_EFLASH;
+    }
+
+    rc = boot_read_swap_state(fap, &state_primary_slot);
+    if (rc != 0) {
+        goto done;
     }
 
     switch (state_primary_slot.magic) {
@@ -577,25 +629,19 @@
 
     case BOOT_MAGIC_UNSET:
         /* Already confirmed. */
-        return 0;
+        goto done;
 
     case BOOT_MAGIC_BAD:
         /* Unexpected state. */
-        return BOOT_EBADVECT;
-    }
-
-    rc = flash_area_open(FLASH_AREA_IMAGE_PRIMARY(0), &fap);
-    if (rc) {
-        rc = BOOT_EFLASH;
-        goto done;
-    }
-
-    if (state_primary_slot.copy_done == BOOT_FLAG_UNSET) {
-        /* Swap never completed.  This is unexpected. */
         rc = BOOT_EBADVECT;
         goto done;
     }
 
+    /* Intentionally do not check copy_done flag
+     * so can confirm a padded image which was programed using a programing
+     * interface.
+     */
+
     if (state_primary_slot.image_ok != BOOT_FLAG_UNSET) {
         /* Already confirmed. */
         goto done;
@@ -607,3 +653,17 @@
     flash_area_close(fap);
     return rc;
 }
+
+/**
+ * Marks the image with index 0 in the primary slot as confirmed.  The system
+ * will continue booting into the image in the primary slot until told to boot
+ * from a different slot.  Note that this API is kept for compatibility. The
+ * boot_set_confirmed_multi() API is recommended.
+ *
+ * @return                  0 on success; nonzero on failure.
+ */
+int
+boot_set_confirmed(void)
+{
+    return boot_set_confirmed_multi(0);
+}
diff --git a/boot/bootutil/src/caps.c b/boot/bootutil/src/caps.c
index 2033056..4944f46 100644
--- a/boot/bootutil/src/caps.c
+++ b/boot/bootutil/src/caps.c
@@ -2,6 +2,7 @@
  * SPDX-License-Identifier: Apache-2.0
  *
  * Copyright (c) 2017 Linaro Limited
+ * Copyright (c) 2021 Arm Limited
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -70,6 +71,15 @@
 #if defined(MCUBOOT_BOOTSTRAP)
     res |= BOOTUTIL_CAP_BOOTSTRAP;
 #endif
+#if defined(MCUBOOT_AES_256)
+    res |= BOOTUTIL_CAP_AES256;
+#endif
+#if defined(MCUBOOT_RAM_LOAD)
+    res |= BOOTUTIL_CAP_RAM_LOAD;
+#endif
+#if defined(MCUBOOT_DIRECT_XIP)
+    res |= BOOTUTIL_CAP_DIRECT_XIP;
+#endif
 
     return res;
 }
diff --git a/boot/bootutil/src/crc32c.h b/boot/bootutil/src/crc32c.h
index 329b9d3..3a45663 100644
--- a/boot/bootutil/src/crc32c.h
+++ b/boot/bootutil/src/crc32c.h
@@ -22,11 +22,11 @@
  * specific language governing permissions and limitations
  * under the License.
  */
-#ifndef H_CRC32C_
-#define H_CRC32C_
+#ifndef CRC32C_H
+#define CRC32C_H
 
 #include <stdint.h>
 
 uint32_t crc32c_checksum(const uint8_t *address, uint32_t length);
 
-#endif /* H_CRC32C_ */
+#endif /* CRC32C_H */
diff --git a/boot/bootutil/src/encrypted.c b/boot/bootutil/src/encrypted.c
index 12c0a9c..73e1f1f 100644
--- a/boot/bootutil/src/encrypted.c
+++ b/boot/bootutil/src/encrypted.c
@@ -2,7 +2,7 @@
  * SPDX-License-Identifier: Apache-2.0
  *
  * Copyright (c) 2018-2019 JUUL Labs
- * Copyright (c) 2019 Arm Limited
+ * Copyright (c) 2019-2021 Arm Limited
  */
 
 #include "mcuboot_config/mcuboot_config.h"
@@ -14,7 +14,11 @@
 
 #if defined(MCUBOOT_ENCRYPT_RSA)
 #include "mbedtls/rsa.h"
+#if MBEDTLS_VERSION_NUMBER >= 0x03000000
+#include "rsa_alt_helpers.h"
+#else
 #include "mbedtls/rsa_internal.h"
+#endif
 #include "mbedtls/asn1.h"
 #endif
 
@@ -40,6 +44,7 @@
 #include "bootutil/image.h"
 #include "bootutil/enc_key.h"
 #include "bootutil/sign_key.h"
+#include "bootutil/crypto/common.h"
 
 #include "bootutil_priv.h"
 
@@ -74,15 +79,11 @@
 
     bootutil_aes_kw_init(&aes_kw);
     rc = bootutil_aes_kw_set_unwrap_key(&aes_kw, bootutil_enc_key.key, *bootutil_enc_key.len);
-    if (rc != 0) {
-        goto done;
-    }
-    rc = bootutil_aes_kw_unwrap(&aes_kw, wrapped, TLV_ENC_KW_SZ, enckey, BOOT_ENC_KEY_SIZE);
-    if (rc != 0) {
-        goto done;
+
+    if (rc == 0) {
+        rc = bootutil_aes_kw_unwrap(&aes_kw, wrapped, TLV_ENC_KW_SZ, enckey, BOOT_ENC_KEY_SIZE);
     }
 
-done:
     bootutil_aes_kw_drop(&aes_kw);
     return rc;
 }
@@ -105,16 +106,16 @@
 
     /* Non-optional fields. */
     if ( /* version */
-        mbedtls_asn1_get_int(p, end, &ctx->ver) != 0 ||
+        mbedtls_asn1_get_int(p, end, &ctx->MBEDTLS_CONTEXT_MEMBER(ver)) != 0 ||
          /* public modulus */
-        mbedtls_asn1_get_mpi(p, end, &ctx->N) != 0 ||
+        mbedtls_asn1_get_mpi(p, end, &ctx->MBEDTLS_CONTEXT_MEMBER(N)) != 0 ||
          /* public exponent */
-        mbedtls_asn1_get_mpi(p, end, &ctx->E) != 0 ||
+        mbedtls_asn1_get_mpi(p, end, &ctx->MBEDTLS_CONTEXT_MEMBER(E)) != 0 ||
          /* private exponent */
-        mbedtls_asn1_get_mpi(p, end, &ctx->D) != 0 ||
+        mbedtls_asn1_get_mpi(p, end, &ctx->MBEDTLS_CONTEXT_MEMBER(D)) != 0 ||
          /* primes */
-        mbedtls_asn1_get_mpi(p, end, &ctx->P) != 0 ||
-        mbedtls_asn1_get_mpi(p, end, &ctx->Q) != 0) {
+        mbedtls_asn1_get_mpi(p, end, &ctx->MBEDTLS_CONTEXT_MEMBER(P)) != 0 ||
+        mbedtls_asn1_get_mpi(p, end, &ctx->MBEDTLS_CONTEXT_MEMBER(Q)) != 0) {
 
         return -3;
     }
@@ -127,22 +128,26 @@
      */
     if (*p < end) {
         if ( /* d mod (p-1) and d mod (q-1) */
-            mbedtls_asn1_get_mpi(p, end, &ctx->DP) != 0 ||
-            mbedtls_asn1_get_mpi(p, end, &ctx->DQ) != 0 ||
+            mbedtls_asn1_get_mpi(p, end, &ctx->MBEDTLS_CONTEXT_MEMBER(DP)) != 0 ||
+            mbedtls_asn1_get_mpi(p, end, &ctx->MBEDTLS_CONTEXT_MEMBER(DQ)) != 0 ||
              /* q ^ (-1) mod p */
-            mbedtls_asn1_get_mpi(p, end, &ctx->QP) != 0) {
+            mbedtls_asn1_get_mpi(p, end, &ctx->MBEDTLS_CONTEXT_MEMBER(QP)) != 0) {
 
             return -4;
         }
     } else {
-        if (mbedtls_rsa_deduce_crt(&ctx->P, &ctx->Q, &ctx->D,
-                    &ctx->DP, &ctx->DQ, &ctx->QP) != 0) {
+        if (mbedtls_rsa_deduce_crt(&ctx->MBEDTLS_CONTEXT_MEMBER(P),
+                                   &ctx->MBEDTLS_CONTEXT_MEMBER(Q),
+                                   &ctx->MBEDTLS_CONTEXT_MEMBER(D),
+                                   &ctx->MBEDTLS_CONTEXT_MEMBER(DP),
+                                   &ctx->MBEDTLS_CONTEXT_MEMBER(DQ),
+                                   &ctx->MBEDTLS_CONTEXT_MEMBER(QP)) != 0) {
             return -5;
         }
     }
 #endif
 
-    ctx->len = mbedtls_mpi_size(&ctx->N);
+    ctx->MBEDTLS_CONTEXT_MEMBER(len) = mbedtls_mpi_size(&ctx->MBEDTLS_CONTEXT_MEMBER(N));
 
     if (mbedtls_rsa_check_privkey(ctx) != 0) {
         return -6;
@@ -190,12 +195,12 @@
         return -5;
     }
 
-    if (alg.len != sizeof(ec_pubkey_oid) - 1 ||
-        memcmp(alg.p, ec_pubkey_oid, sizeof(ec_pubkey_oid) - 1)) {
+    if (alg.MBEDTLS_CONTEXT_MEMBER(len) != sizeof(ec_pubkey_oid) - 1 ||
+        memcmp(alg.MBEDTLS_CONTEXT_MEMBER(p), ec_pubkey_oid, sizeof(ec_pubkey_oid) - 1)) {
         return -6;
     }
-    if (param.len != sizeof(ec_secp256r1_oid) - 1 ||
-        memcmp(param.p, ec_secp256r1_oid, sizeof(ec_secp256r1_oid) - 1)) {
+    if (param.MBEDTLS_CONTEXT_MEMBER(len) != sizeof(ec_secp256r1_oid) - 1 ||
+        memcmp(param.MBEDTLS_CONTEXT_MEMBER(p), ec_secp256r1_oid, sizeof(ec_secp256r1_oid) - 1)) {
         return -7;
     }
 
@@ -267,8 +272,8 @@
         return -4;
     }
 
-    if (alg.len != sizeof(ec_pubkey_oid) - 1 ||
-        memcmp(alg.p, ec_pubkey_oid, sizeof(ec_pubkey_oid) - 1)) {
+    if (alg.MBEDTLS_CONTEXT_MEMBER(len) != sizeof(ec_pubkey_oid) - 1 ||
+        memcmp(alg.MBEDTLS_CONTEXT_MEMBER(p), ec_pubkey_oid, sizeof(ec_pubkey_oid) - 1)) {
         return -5;
     }
 
@@ -433,25 +438,22 @@
 
 #if defined(MCUBOOT_ENCRYPT_RSA)
 #    define EXPECTED_ENC_TLV        IMAGE_TLV_ENC_RSA2048
-#    define EXPECTED_ENC_EXT_LEN    EXPECTED_ENC_TLV
 #elif defined(MCUBOOT_ENCRYPT_KW)
-#    define EXPECTED_ENC_TLV        IMAGE_TLV_ENC_KW128
-#    define EXPECTED_ENC_EXT_LEN    EXPECTED_ENC_TLV
+#    define EXPECTED_ENC_TLV    IMAGE_TLV_ENC_KW
 #elif defined(MCUBOOT_ENCRYPT_EC256)
 #    define EXPECTED_ENC_TLV     IMAGE_TLV_ENC_EC256
 #    define EXPECTED_ENC_EXT_LEN (EXPECTED_ENC_LEN + BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE)
 #    define EC_PUBK_INDEX       (0)
 #    define EC_TAG_INDEX        (65)
 #    define EC_CIPHERKEY_INDEX  (65 + 32)
-_Static_assert(EC_CIPHERKEY_INDEX + 16 == EXPECTED_ENC_LEN,
+_Static_assert(EC_CIPHERKEY_INDEX + BOOT_ENC_KEY_SIZE == EXPECTED_ENC_LEN,
         "Please fix ECIES-P256 component indexes");
 #elif defined(MCUBOOT_ENCRYPT_X25519)
 #    define EXPECTED_ENC_TLV        IMAGE_TLV_ENC_X25519
-#    define EXPECTED_ENC_EXT_LEN    EXPECTED_ENC_TLV
 #    define EC_PUBK_INDEX       (0)
 #    define EC_TAG_INDEX        (32)
 #    define EC_CIPHERKEY_INDEX  (32 + 32)
-_Static_assert(EC_CIPHERKEY_INDEX + 16 == EXPECTED_ENC_LEN,
+_Static_assert(EC_CIPHERKEY_INDEX + BOOT_ENC_KEY_SIZE == EXPECTED_ENC_LEN,
         "Please fix ECIES-X25519 component indexes");
 #endif
 
@@ -459,12 +461,30 @@
 #define EXPECTED_ENC_EXT_LEN        EXPECTED_ENC_LEN
 #endif
 
+#if defined(MCUBOOT_ENCRYPT_RSA) || \
+    (defined(MCUBOOT_ENCRYPT_EC256) && defined(MCUBOOT_USE_MBED_TLS))
+#if MBEDTLS_VERSION_NUMBER >= 0x03000000
+static int fake_rng(void *p_rng, unsigned char *output, size_t len)
+{
+    size_t i;
+
+    (void)p_rng;
+    for (i = 0; i < len; i++) {
+        output[i] = (char)i;
+    }
+
+    return 0;
+}
+#endif
+#endif /* defined(MCUBOOT_ENCRYPT_RSA) ||
+          defined(MCUBOOT_ENCRYPT_EC256) && defined(MCUBOOT_USE_MBED_TLS) */
+
 /*
  * Decrypt an encryption key TLV.
  *
- * @param buf An encryption TLV buffer red from flash
+ * @param buf An encryption TLV read from flash (build time fixed length)
  * @param sz An encryption TLV buffer data size
- * @param enckey An AES-128 key sized buffer to store to plain key.
+ * @param enckey An AES-128 or AES-256 key sized buffer to store to plain key.
  */
 int
 boot_enc_decrypt(const uint8_t *buf, uint8_t *enckey, uint32_t sz, uint8_t *enciv)
@@ -505,8 +525,12 @@
 
 #if defined(MCUBOOT_ENCRYPT_RSA)
 
+#if MBEDTLS_VERSION_NUMBER >= 0x03000000
+    mbedtls_rsa_init(&rsa);
+    mbedtls_rsa_set_padding(&rsa, MBEDTLS_RSA_PKCS_V21, MBEDTLS_MD_SHA256);
+#else
     mbedtls_rsa_init(&rsa, MBEDTLS_RSA_PKCS_V21, MBEDTLS_MD_SHA256);
-
+#endif
     cp = (uint8_t *)bootutil_enc_key.key;
     cpend = cp + *bootutil_enc_key.len;
 
@@ -515,16 +539,20 @@
         mbedtls_rsa_free(&rsa);
         return rc;
     }
-
+#if MBEDTLS_VERSION_NUMBER >= 0x03000000
+    rc = mbedtls_rsa_rsaes_oaep_decrypt(&rsa, fake_rng, NULL,
+            NULL, 0, &olen, buf, enckey, BOOT_ENC_KEY_SIZE);
+#else
     rc = mbedtls_rsa_rsaes_oaep_decrypt(&rsa, NULL, NULL, MBEDTLS_RSA_PRIVATE,
             NULL, 0, &olen, buf, enckey, BOOT_ENC_KEY_SIZE);
+#endif
     mbedtls_rsa_free(&rsa);
 
 #endif /* defined(MCUBOOT_ENCRYPT_RSA) */
 
 #if defined(MCUBOOT_ENCRYPT_KW)
 
-    assert(*bootutil_enc_key.len == 16);
+    assert(*bootutil_enc_key.len == BOOT_ENC_KEY_SIZE);
     rc = key_unwrap(buf, enckey);
 
 #endif /* defined(MCUBOOT_ENCRYPT_KW) */
@@ -592,8 +620,8 @@
      */
 
     /* Prepare for default encryption scheme with zero salt and AES IVs */
-    memset(counter, 0, BOOTUTIL_CRYPTO_AES_CTR_BLOCK_SIZE);
-    memset(salt, 0, BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE);
+    (void)memset(counter, 0, BOOTUTIL_CRYPTO_AES_CTR_BLOCK_SIZE);
+    (void)memset(salt, 0, BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE);
 
     len = BOOTUTIL_CRYPTO_AES_CTR_KEY_SIZE + BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE;
 
@@ -625,13 +653,13 @@
 
     bootutil_hmac_sha256_init(&hmac);
 
-    rc = bootutil_hmac_sha256_set_key(&hmac, &derived_key[BOOTUTIL_CRYPTO_AES_CTR_BLOCK_SIZE], BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE);
+    rc = bootutil_hmac_sha256_set_key(&hmac, &derived_key[BOOT_ENC_KEY_SIZE], BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE);
     if (rc != 0) {
         (void)bootutil_hmac_sha256_drop(&hmac);
         return -1;
     }
 
-    rc = bootutil_hmac_sha256_update(&hmac, &buf[EC_CIPHERKEY_INDEX], BOOTUTIL_CRYPTO_AES_CTR_BLOCK_SIZE);
+    rc = bootutil_hmac_sha256_update(&hmac, &buf[EC_CIPHERKEY_INDEX], BOOT_ENC_KEY_SIZE);
     if (rc != 0) {
         (void)bootutil_hmac_sha256_drop(&hmac);
         return -1;
@@ -699,16 +727,18 @@
     uint8_t slot;
     int rc;
 
-    rc = flash_area_id_to_multi_image_slot(image_index, fap->fa_id);
+    rc = flash_area_id_to_multi_image_slot(image_index, flash_area_get_id(fap));
     if (rc < 0) {
         return rc;
     }
     slot = rc;
 
+#ifndef MCUBOOT_ENC_IMAGES_XIP
     /* Already loaded... */
     if (enc_state[slot].valid) {
         return 1;
     }
+#endif
 
     /* Initialize the AES context */
     boot_enc_init(enc_state, slot);
@@ -729,11 +759,11 @@
 
 #if MCUBOOT_SWAP_SAVE_ENCTLV
     buf = bs->enctlv[slot];
-    memset(buf, 0xff, BOOT_ENC_TLV_ALIGN_SIZE);
+    (void)memset(buf, BOOT_UNINITIALIZED_TLV_FILL, BOOT_ENC_TLV_ALIGN_SIZE);
 #endif
 
     rc = flash_area_read(fap, off, buf, len);
-    if (rc) {
+    if (rc != 0) {
         return -1;
     }
 
@@ -746,7 +776,7 @@
 {
     int rc;
 
-    rc = flash_area_id_to_multi_image_slot(image_index, fap->fa_id);
+    rc = flash_area_id_to_multi_image_slot(image_index, flash_area_get_id(fap));
     if (rc < 0) {
         /* can't get proper slot number - skip encryption, */
         /* postpone the error for a upper layer */
@@ -756,7 +786,7 @@
     return enc_state[rc].valid;
 }
 
-void
+int
 boot_encrypt(struct enc_key_data *enc_state, int image_index,
         const struct flash_area *fap, uint32_t off, uint32_t sz,
         uint32_t blk_off, uint8_t *buf)
@@ -768,13 +798,23 @@
     /* boot_copy_region will call boot_encrypt with sz = 0 when skipping over
        the TLVs. */
     if (sz == 0) {
-       return;
+        return 0;
     }
 
-    rc = flash_area_id_to_multi_image_slot(image_index, fap->fa_id);
-    if (rc < 0) {
-        assert(0);
-        return;
+#if MCUBOOT_SWAP_USING_SCRATCH
+/* in this case scratch area contains encrypted source block that was copied
+from secondary slot */
+    if (fap->fa_id == FLASH_AREA_IMAGE_SCRATCH) {
+        rc = 1;
+    }
+    else
+#endif
+    {
+        rc = flash_area_id_to_multi_image_slot(image_index, fap->fa_id);
+        if (rc < 0) {
+            assert(0);
+            return rc;
+        }
     }
 
     enc = &enc_state[rc];
@@ -788,7 +828,7 @@
     nonce[14] = (uint8_t)(off >> 8);
     nonce[15] = (uint8_t)off;
 
-    bootutil_aes_ctr_encrypt(&enc->aes_ctr, nonce, buf, sz, blk_off, buf);
+    return bootutil_aes_ctr_encrypt(&enc->aes_ctr, nonce, buf, sz, blk_off, buf);
 }
 
 /**
@@ -801,7 +841,7 @@
     for (slot = 0; slot < BOOT_NUM_SLOTS; slot++) {
         (void)boot_enc_drop(enc_state, slot);
     }
-    memset(enc_state, 0, sizeof(struct enc_key_data) * BOOT_NUM_SLOTS);
+    (void)memset(enc_state, 0, sizeof(struct enc_key_data) * BOOT_NUM_SLOTS);
 }
 
 #endif /* MCUBOOT_ENC_IMAGES */
diff --git a/boot/bootutil/src/fault_injection_hardening.c b/boot/bootutil/src/fault_injection_hardening.c
index 5ee109c..5e818c6 100644
--- a/boot/bootutil/src/fault_injection_hardening.c
+++ b/boot/bootutil/src/fault_injection_hardening.c
@@ -10,9 +10,9 @@
 /* Variable that could be (but isn't) changed at runtime to force the compiler
  * not to optimize the double check. Value doesn't matter.
  */
-volatile int _fih_mask = _FIH_MASK_VALUE;
-fih_int FIH_SUCCESS = {FIH_POSITIVE_VALUE, _FIH_MASK_VALUE ^ FIH_POSITIVE_VALUE};
-fih_int FIH_FAILURE = {FIH_NEGATIVE_VALUE, _FIH_MASK_VALUE ^ FIH_NEGATIVE_VALUE};
+volatile int _fih_mask = FIH_MASK_VALUE;
+fih_int FIH_SUCCESS = {FIH_POSITIVE_VALUE, FIH_MASK_VALUE ^ FIH_POSITIVE_VALUE};
+fih_int FIH_FAILURE = {FIH_NEGATIVE_VALUE, FIH_MASK_VALUE ^ FIH_NEGATIVE_VALUE};
 #else
 fih_int FIH_SUCCESS = {FIH_POSITIVE_VALUE};
 fih_int FIH_FAILURE = {FIH_NEGATIVE_VALUE};
@@ -21,7 +21,7 @@
 #ifdef FIH_ENABLE_CFI
 
 #ifdef FIH_ENABLE_DOUBLE_VARS
-fih_int _fih_cfi_ctr = {0, 0 ^ _FIH_MASK_VALUE};
+fih_int _fih_cfi_ctr = {0, 0 ^ FIH_MASK_VALUE};
 #else
 fih_int _fih_cfi_ctr = {0};
 #endif /* FIH_ENABLE_DOUBLE_VARS */
diff --git a/boot/bootutil/src/image_ec.c b/boot/bootutil/src/image_ec.c
index a127b74..2d92afb 100644
--- a/boot/bootutil/src/image_ec.c
+++ b/boot/bootutil/src/image_ec.c
@@ -2,6 +2,7 @@
  * SPDX-License-Identifier: Apache-2.0
  *
  * Copyright (c) 2016-2018 JUUL Labs
+ * Copyright (C) 2021 Arm Limited
  *
  * Original license:
  *
@@ -34,6 +35,7 @@
 #include "mbedtls/oid.h"
 #include "mbedtls/asn1.h"
 
+#include "bootutil/crypto/common.h"
 #include "bootutil_priv.h"
 
 /*
@@ -70,7 +72,7 @@
         return -4;
     }
 
-    if (mbedtls_ecp_group_load(&ctx->grp, MBEDTLS_ECP_DP_SECP224R1)) {
+    if (mbedtls_ecp_group_load(&ctx->MBEDTLS_CONTEXT_MEMBER(grp), MBEDTLS_ECP_DP_SECP224R1)) {
         return -5;
     }
 
@@ -81,11 +83,11 @@
         return -7;
     }
 
-    if (mbedtls_ecp_point_read_binary(&ctx->grp, &ctx->Q, *p, end - *p)) {
+    if (mbedtls_ecp_point_read_binary(&ctx->MBEDTLS_CONTEXT_MEMBER(grp), &ctx->MBEDTLS_CONTEXT_MEMBER(Q), *p, end - *p)) {
         return -8;
     }
 
-    if (mbedtls_ecp_check_pubkey(&ctx->grp, &ctx->Q)) {
+    if (mbedtls_ecp_check_pubkey(&ctx->MBEDTLS_CONTEXT_MEMBER(grp), &ctx->MBEDTLS_CONTEXT_MEMBER(Q))) {
         return -9;
     }
     return 0;
diff --git a/boot/bootutil/src/image_ec256.c b/boot/bootutil/src/image_ec256.c
index bdd72c6..9f0ed3d 100644
--- a/boot/bootutil/src/image_ec256.c
+++ b/boot/bootutil/src/image_ec256.c
@@ -3,6 +3,7 @@
  *
  * Copyright (c) 2016-2019 JUUL Labs
  * Copyright (c) 2017 Linaro LTD
+ * Copyright (C) 2021 Arm Limited
  *
  * Original license:
  *
@@ -40,6 +41,7 @@
 #include "mbedtls/oid.h"
 #include "mbedtls/asn1.h"
 #include "bootutil/crypto/ecdsa_p256.h"
+#include "bootutil/crypto/common.h"
 #include "bootutil_priv.h"
 
 /*
@@ -68,12 +70,12 @@
     if (mbedtls_asn1_get_alg(p, end, &alg, &param)) {
         return -2;
     }
-    if (alg.len != sizeof(ec_pubkey_oid) - 1 ||
-      memcmp(alg.p, ec_pubkey_oid, sizeof(ec_pubkey_oid) - 1)) {
+    if (alg.MBEDTLS_CONTEXT_MEMBER(len) != sizeof(ec_pubkey_oid) - 1 ||
+      memcmp(alg.MBEDTLS_CONTEXT_MEMBER(p), ec_pubkey_oid, sizeof(ec_pubkey_oid) - 1)) {
         return -3;
     }
-    if (param.len != sizeof(ec_secp256r1_oid) - 1||
-      memcmp(param.p, ec_secp256r1_oid, sizeof(ec_secp256r1_oid) - 1)) {
+    if (param.MBEDTLS_CONTEXT_MEMBER(len) != sizeof(ec_secp256r1_oid) - 1||
+      memcmp(param.MBEDTLS_CONTEXT_MEMBER(p), ec_secp256r1_oid, sizeof(ec_secp256r1_oid) - 1)) {
         return -4;
     }
 
@@ -116,13 +118,13 @@
         return -2;
     }
     /* id-ecPublicKey (RFC5480) */
-    if (alg.len != sizeof(ec_pubkey_oid) - 1 ||
-        memcmp(alg.p, ec_pubkey_oid, sizeof(ec_pubkey_oid) - 1)) {
+    if (alg.MBEDTLS_CONTEXT_MEMBER(len) != sizeof(ec_pubkey_oid) - 1 ||
+        memcmp(alg.MBEDTLS_CONTEXT_MEMBER(p), ec_pubkey_oid, sizeof(ec_pubkey_oid) - 1)) {
         return -3;
     }
     /* namedCurve (RFC5480) */
-    if (param.len != sizeof(ec_secp256r1_oid) - 1 ||
-        memcmp(param.p, ec_secp256r1_oid, sizeof(ec_secp256r1_oid) - 1)) {
+    if (param.MBEDTLS_CONTEXT_MEMBER(len) != sizeof(ec_secp256r1_oid) - 1 ||
+        memcmp(param.MBEDTLS_CONTEXT_MEMBER(p), ec_secp256r1_oid, sizeof(ec_secp256r1_oid) - 1)) {
         return -4;
     }
     /* ECPoint (RFC5480) */
diff --git a/boot/bootutil/src/image_ed25519.c b/boot/bootutil/src/image_ed25519.c
index 940c18d..b5838c4 100644
--- a/boot/bootutil/src/image_ed25519.c
+++ b/boot/bootutil/src/image_ed25519.c
@@ -2,6 +2,7 @@
  * SPDX-License-Identifier: Apache-2.0
  *
  * Copyright (c) 2019 JUUL Labs
+ * Copyright (c) 2021 Arm Limited
  */
 
 #include <string.h>
@@ -15,6 +16,7 @@
 #include "mbedtls/asn1.h"
 
 #include "bootutil_priv.h"
+#include "bootutil/crypto/common.h"
 
 static const uint8_t ed25519_pubkey_oid[] = MBEDTLS_OID_ISO_IDENTIFIED_ORG "\x65\x70";
 #define NUM_ED25519_BYTES 32
@@ -43,8 +45,8 @@
         return -2;
     }
 
-    if (alg.len != sizeof(ed25519_pubkey_oid) - 1 ||
-        memcmp(alg.p, ed25519_pubkey_oid, sizeof(ed25519_pubkey_oid) - 1)) {
+    if (alg.MBEDTLS_CONTEXT_MEMBER(len) != sizeof(ed25519_pubkey_oid) - 1 ||
+        memcmp(alg.MBEDTLS_CONTEXT_MEMBER(p), ed25519_pubkey_oid, sizeof(ed25519_pubkey_oid) - 1)) {
         return -3;
     }
 
diff --git a/boot/bootutil/src/image_rsa.c b/boot/bootutil/src/image_rsa.c
index 1a1727e..42d2db7 100644
--- a/boot/bootutil/src/image_rsa.c
+++ b/boot/bootutil/src/image_rsa.c
@@ -3,7 +3,7 @@
  *
  * Copyright (c) 2017-2018 Linaro LTD
  * Copyright (c) 2017-2019 JUUL Labs
- * Copyright (c) 2020 Arm Limited
+ * Copyright (c) 2020-2021 Arm Limited
  *
  * Original license:
  *
@@ -32,6 +32,7 @@
 #ifdef MCUBOOT_SIGN_RSA
 #include "bootutil/sign_key.h"
 #include "bootutil/crypto/sha256.h"
+#include "bootutil/crypto/common.h"
 
 #include "mbedtls/rsa.h"
 #include "mbedtls/asn1.h"
@@ -88,12 +89,12 @@
         return -2;
     }
 
-    if ((rc = mbedtls_asn1_get_mpi(p, end, &ctx->N)) != 0 ||
-      (rc = mbedtls_asn1_get_mpi(p, end, &ctx->E)) != 0) {
+    if ((rc = mbedtls_asn1_get_mpi(p, end, &ctx->MBEDTLS_CONTEXT_MEMBER(N))) != 0 ||
+        (rc = mbedtls_asn1_get_mpi(p, end, &ctx->MBEDTLS_CONTEXT_MEMBER(E))) != 0) {
         return -3;
     }
 
-    ctx->len = mbedtls_mpi_size(&ctx->N);
+    ctx->MBEDTLS_CONTEXT_MEMBER(len) = mbedtls_mpi_size(&ctx->MBEDTLS_CONTEXT_MEMBER(N));
 
     if (*p != end) {
         return -4;
@@ -101,7 +102,8 @@
 
     /* The mbedtls version is more than 2.6.1 */
 #if MBEDTLS_VERSION_NUMBER > 0x02060100
-    rc = mbedtls_rsa_import(ctx, &ctx->N, NULL, NULL, NULL, &ctx->E);
+    rc = mbedtls_rsa_import(ctx, &ctx->MBEDTLS_CONTEXT_MEMBER(N), NULL,
+                            NULL, NULL, &ctx->MBEDTLS_CONTEXT_MEMBER(E));
     if (rc != 0) {
         return -5;
     }
@@ -112,7 +114,7 @@
         return -6;
     }
 
-    ctx->len = mbedtls_mpi_size(&ctx->N);
+    ctx->MBEDTLS_CONTEXT_MEMBER(len) = mbedtls_mpi_size(&ctx->MBEDTLS_CONTEXT_MEMBER(N));
 
     return 0;
 }
@@ -171,7 +173,8 @@
     int rc = 0;
     fih_int fih_rc = FIH_FAILURE;
 
-    if (ctx->len != PSS_EMLEN || PSS_EMLEN > MBEDTLS_MPI_MAX_SIZE) {
+    if (ctx->MBEDTLS_CONTEXT_MEMBER(len) != PSS_EMLEN ||
+        PSS_EMLEN > MBEDTLS_MPI_MAX_SIZE) {
         rc = -1;
         goto out;
     }
@@ -296,13 +299,17 @@
     uint8_t *cp;
     uint8_t *end;
 
+#if MBEDTLS_VERSION_NUMBER >= 0x03000000
+    mbedtls_rsa_init(&ctx);
+#else
     mbedtls_rsa_init(&ctx, 0, 0);
+#endif
 
     cp = (uint8_t *)bootutil_keys[key_id].key;
     end = cp + *bootutil_keys[key_id].len;
 
     rc = bootutil_parse_rsakey(&ctx, &cp, end);
-    if (rc || slen != ctx.len) {
+    if (rc || slen != ctx.MBEDTLS_CONTEXT_MEMBER(len)) {
         mbedtls_rsa_free(&ctx);
         goto out;
     }
diff --git a/boot/bootutil/src/image_validate.c b/boot/bootutil/src/image_validate.c
index 18967c5..e7cff90 100644
--- a/boot/bootutil/src/image_validate.c
+++ b/boot/bootutil/src/image_validate.c
@@ -4,6 +4,7 @@
  * Copyright (c) 2017-2019 Linaro LTD
  * Copyright (c) 2016-2019 JUUL Labs
  * Copyright (c) 2019-2020 Arm Limited
+ * Copyright (c) 2021 Infineon Technologies AG
  *
  * Original license:
  *
@@ -38,6 +39,8 @@
 #include "bootutil/security_cnt.h"
 #include "bootutil/fault_injection_hardening.h"
 
+#include "bootutil/bootutil_log.h"
+
 #include "mcuboot_config/mcuboot_config.h"
 
 #ifdef MCUBOOT_ENC_IMAGES
@@ -85,6 +88,9 @@
     (void)blk_sz;
     (void)off;
     (void)rc;
+    (void)fap;
+    (void)tmp_buf;
+    (void)tmp_buf_sz;
 #endif
 #endif
 
@@ -113,7 +119,9 @@
     size += hdr->ih_protect_tlv_size;
 
 #ifdef MCUBOOT_RAM_LOAD
-    bootutil_sha256_update(&sha256_ctx,(void*)(hdr->ih_load_addr), size);
+    bootutil_sha256_update(&sha256_ctx,
+                           (void*)(IMAGE_RAM_BASE + hdr->ih_load_addr),
+                           size);
 #else
     for (off = 0; off < size; off += blk_sz) {
         blk_sz = size - off;
@@ -143,8 +151,16 @@
             /* Only payload is encrypted (area between header and TLVs) */
             if (off >= hdr_size && off < tlv_off) {
                 blk_off = (off - hdr_size) & 0xf;
-                boot_encrypt(enc_state, image_index, fap, off - hdr_size,
+#ifdef MCUBOOT_ENC_IMAGES_XIP
+                rc = bootutil_img_encrypt(enc_state, image_index, hdr, fap, off,
                         blk_sz, blk_off, tmp_buf);
+#else
+                rc = boot_encrypt(enc_state, image_index, fap, off - hdr_size,
+                        blk_sz, blk_off, tmp_buf);
+#endif
+                if (rc) {
+                    return rc;
+                }
             }
         }
 #endif
@@ -204,9 +220,9 @@
     bootutil_sha256_context sha256_ctx;
     int i;
     const struct bootutil_key *key;
-    uint8_t hash[32];
+    uint8_t hash[BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE];
 
-    if (keyhash_len > 32) {
+    if (keyhash_len != BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE) {
         return -1;
     }
 
@@ -229,8 +245,8 @@
 bootutil_find_key(uint8_t image_index, uint8_t *key, uint16_t key_len)
 {
     bootutil_sha256_context sha256_ctx;
-    uint8_t hash[32];
-    uint8_t key_hash[32];
+    uint8_t hash[BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE];
+    uint8_t key_hash[BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE];
     size_t key_hash_size = sizeof(key_hash);
     int rc;
     fih_int fih_rc;
@@ -323,6 +339,67 @@
 
     return 0;
 }
+#ifdef CYW20829
+/**
+ * Reads the content of an image's reprovisioning packet.
+ *
+ * @param hdr            Pointer to the image header structure.
+ * @param fap            Pointer to a description structure of the image's
+ *                       flash area.
+ * @param reprov_packet  Pointer to store the reprovisioning packet.
+ *
+ * @return               0 on success; nonzero on failure.
+ */
+int32_t
+bootutil_get_img_reprov_packet(struct image_header *hdr,
+                              const struct flash_area *fap,
+                              uint8_t *reprov_packet)
+{
+    struct image_tlv_iter it;
+    uint32_t off;
+    uint16_t len;
+    int32_t rc;
+
+    if ((hdr == NULL) ||
+        (fap == NULL) ||
+        (reprov_packet == NULL)) {
+        /* Invalid parameter. */
+        return BOOT_EBADARGS;
+    }
+
+    /* The reprovisioning packet TLV is in the protected part of the TLV area. */
+    if (hdr->ih_protect_tlv_size == 0) {
+        return BOOT_EBADIMAGE;
+    }
+
+    rc = bootutil_tlv_iter_begin(&it, hdr, fap, IMAGE_TLV_PROV_PACK, true);
+    if (rc) {
+        return rc;
+    }
+
+    /* Traverse through the protected TLV area to find
+     * the reprovisioning apcket TLV.
+     */
+
+    rc = bootutil_tlv_iter_next(&it, &off, &len, NULL);
+    if (rc != 0) {
+        /* Reprovisioning packet TLV has not been found. */
+        return -1;
+    }
+
+    if (len != REPROV_PACK_SIZE) {
+        /* Reprovisioning packet is not valid. */
+        return BOOT_EBADIMAGE;
+    }
+
+    rc = LOAD_IMAGE_DATA(hdr, fap, off, reprov_packet, len);
+    if (rc != 0) {
+        return BOOT_EFLASH;
+    }
+
+    return 0;
+}
+#endif /* CYW20289 */
 #endif /* MCUBOOT_HW_ROLLBACK_PROT */
 
 /*
@@ -349,12 +426,13 @@
 #endif /* EXPECTED_SIG_TLV */
     struct image_tlv_iter it;
     uint8_t buf[SIG_BUF_SIZE];
-    uint8_t hash[32];
+    uint8_t hash[BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE];
     int rc = 0;
     fih_int fih_rc = FIH_FAILURE;
 #ifdef MCUBOOT_HW_ROLLBACK_PROT
-    fih_int security_cnt = fih_int_encode(INT_MAX);
+    fih_uint security_cnt = fih_uint_encode(UINT_MAX);
     uint32_t img_security_cnt = 0;
+    uint8_t reprov_packet[REPROV_PACK_SIZE];
     fih_int security_counter_valid = FIH_FAILURE;
 #endif
 
@@ -365,7 +443,7 @@
     }
 
     if (out_hash) {
-        memcpy(out_hash, hash, 32);
+        memcpy(out_hash, hash, BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE);
     }
 
     rc = bootutil_tlv_iter_begin(&it, hdr, fap, IMAGE_TLV_ANY, false);
@@ -411,7 +489,7 @@
             /*
              * Determine which key we should be checking.
              */
-            if (len > 32) {
+            if (len != BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE) {
                 rc = -1;
                 goto out;
             }
@@ -484,18 +562,52 @@
                 goto out;
             }
 
+            BOOT_LOG_DBG("NV Counter read from efuse = %u", fih_uint_decode(security_cnt));
+
+            BOOT_LOG_DBG("NV Counter read from image = %" PRIu32, img_security_cnt);
+
             /* Compare the new image's security counter value against the
              * stored security counter value.
              */
             fih_rc = fih_int_encode_zero_equality(img_security_cnt <
-                                   fih_int_decode(security_cnt));
+                                   (uint32_t)fih_uint_decode(security_cnt));
             if (fih_not_eq(fih_rc, FIH_SUCCESS)) {
                 /* The image's security counter is not accepted. */
                 goto out;
             }
-
+#ifndef CYW20829
             /* The image's security counter has been successfully verified. */
             security_counter_valid = fih_rc;
+        }
+#else
+            /* The image's security counter has been successfully verified. */
+            security_counter_valid = fih_int_encode(HW_ROLLBACK_CNT_VALID);
+        } else if (type == IMAGE_TLV_PROV_PACK) {
+
+            if (fih_eq(security_counter_valid, fih_int_encode(HW_ROLLBACK_CNT_VALID))) {
+                /*
+                * Verify the image reprovisioning packet.
+                * This must always be present.
+                */
+                BOOT_LOG_INF("Prov packet length 0x51 TLV = %" PRIu16, len);
+
+                if (len != sizeof(reprov_packet)) {
+                    /* Re-provisioning packet is not valid. */
+                    rc = -1;
+                    goto out;
+                }
+
+                rc = LOAD_IMAGE_DATA(hdr, fap, off, reprov_packet, len);
+                if (rc) {
+                    goto out;
+                }
+
+                security_counter_valid = fih_int_encode(fih_int_decode(security_counter_valid) | REPROV_PACK_VALID);
+            }
+            else{
+                goto out;
+            }
+#endif /* CYW20829 */
 #endif /* MCUBOOT_HW_ROLLBACK_PROT */
         }
     }
@@ -509,7 +621,12 @@
                                                      FIH_SUCCESS));
 #endif
 #ifdef MCUBOOT_HW_ROLLBACK_PROT
+#ifdef CYW20829
+    if (fih_not_eq(security_counter_valid, fih_int_encode(REPROV_PACK_VALID | HW_ROLLBACK_CNT_VALID))) {
+        BOOT_LOG_DBG("Reprovisioning packet TLV 0x51 is not present image = %d", image_index);
+#else
     if (fih_not_eq(security_counter_valid, FIH_SUCCESS)) {
+#endif /* CYW20829 */
         rc = -1;
         goto out;
     }
diff --git a/boot/bootutil/src/loader.c b/boot/bootutil/src/loader.c
index e4c0a3f..7794b5a 100644
--- a/boot/bootutil/src/loader.c
+++ b/boot/bootutil/src/loader.c
@@ -3,7 +3,7 @@
  *
  * Copyright (c) 2016-2020 Linaro LTD
  * Copyright (c) 2016-2019 JUUL Labs
- * Copyright (c) 2019-2020 Arm Limited
+ * Copyright (c) 2019-2021 Arm Limited
  *
  * Original license:
  *
@@ -35,7 +35,6 @@
 #include <inttypes.h>
 #include <stdlib.h>
 #include <string.h>
-#include <os/os_malloc.h>
 #include "bootutil/bootutil.h"
 #include "bootutil/image.h"
 #include "bootutil_priv.h"
@@ -44,14 +43,20 @@
 #include "bootutil/security_cnt.h"
 #include "bootutil/boot_record.h"
 #include "bootutil/fault_injection_hardening.h"
+#include "bootutil/ramload.h"
+#include "bootutil/boot_hooks.h"
 
 #ifdef MCUBOOT_ENC_IMAGES
 #include "bootutil/enc_key.h"
 #endif
 
+#if !defined(MCUBOOT_DIRECT_XIP) && !defined(MCUBOOT_RAM_LOAD)
+#include <os/os_malloc.h>
+#endif
+
 #include "mcuboot_config/mcuboot_config.h"
 
-MCUBOOT_LOG_MODULE_DECLARE(mcuboot);
+BOOT_LOG_MODULE_DECLARE(mcuboot);
 
 static struct boot_loader_state boot_data;
 
@@ -61,6 +66,22 @@
 #define IMAGES_ITER(x)
 #endif
 
+#if defined(MCUBOOT_DIRECT_XIP) || defined(MCUBOOT_RAM_LOAD)
+struct slot_usage_t {
+    /* Index of the slot chosen to be loaded */
+    uint32_t active_slot;
+    bool slot_available[BOOT_NUM_SLOTS];
+#if defined(MCUBOOT_RAM_LOAD)
+    /* Image destination and size for the active slot */
+    uint32_t img_dst;
+    uint32_t img_sz;
+#elif defined(MCUBOOT_DIRECT_XIP_REVERT)
+    /* Swap status for the active slot */
+    struct boot_swap_state swap_state;
+#endif
+};
+#endif /* MCUBOOT_DIRECT_XIP || MCUBOOT_RAM_LOAD */
+
 /*
  * This macro allows some control on the allocation of local variables.
  * When running natively on a target, we don't want to allocated huge
@@ -82,7 +103,12 @@
     int i;
 
     for (i = 0; i < BOOT_NUM_SLOTS; i++) {
-        rc = boot_read_image_header(state, i, boot_img_hdr(state, i), bs);
+        rc = BOOT_HOOK_CALL(boot_read_image_header_hook, BOOT_HOOK_REGULAR,
+                            BOOT_CURR_IMG(state), i, boot_img_hdr(state, i));
+        if (rc == BOOT_HOOK_REGULAR)
+        {
+            rc = boot_read_image_header(state, i, boot_img_hdr(state, i), bs);
+        }
         if (rc != 0) {
             /* If `require_all` is set, fail on any single fail, otherwise
              * if at least the first slot's header was read successfully,
@@ -101,6 +127,101 @@
     return 0;
 }
 
+/**
+ * Saves boot status and shared data for current image.
+ *
+ * @param  state        Boot loader status information.
+ * @param  active_slot  Index of the slot will be loaded for current image.
+ *
+ * @return              0 on success; nonzero on failure.
+ */
+static int
+boot_add_shared_data(struct boot_loader_state *state,
+                     uint32_t active_slot)
+{
+#if defined(MCUBOOT_MEASURED_BOOT) || defined(MCUBOOT_DATA_SHARING)
+    int rc;
+
+#ifdef MCUBOOT_MEASURED_BOOT
+    rc = boot_save_boot_status(BOOT_CURR_IMG(state),
+                                boot_img_hdr(state, active_slot),
+                                BOOT_IMG_AREA(state, active_slot));
+    if (rc != 0) {
+        BOOT_LOG_ERR("Failed to add image data to shared area");
+        return rc;
+    }
+#endif /* MCUBOOT_MEASURED_BOOT */
+
+#ifdef MCUBOOT_DATA_SHARING
+    rc = boot_save_shared_data(boot_img_hdr(state, active_slot),
+                                BOOT_IMG_AREA(state, active_slot));
+    if (rc != 0) {
+        BOOT_LOG_ERR("Failed to add data to shared memory area.");
+        return rc;
+    }
+#endif /* MCUBOOT_DATA_SHARING */
+
+    return 0;
+
+#else /* MCUBOOT_MEASURED_BOOT || MCUBOOT_DATA_SHARING */
+    (void) (state);
+    (void) (active_slot);
+
+    return 0;
+#endif
+}
+
+/**
+ * Fills rsp to indicate how booting should occur.
+ *
+ * @param  state        Boot loader status information.
+ * @param  slot_usage   Information about the active and available slots.
+ *                      Only used in MCUBOOT_DIRECT_XIP and MCUBOOT_RAM_LOAD
+ * @param  rsp          boot_rsp struct to fill.
+ */
+static void
+fill_rsp(struct boot_loader_state *state, void *slot_usage,
+         struct boot_rsp *rsp)
+{
+    uint32_t active_slot;
+
+#if (BOOT_IMAGE_NUMBER > 1)
+    /* Always boot from Image 0. */
+    BOOT_CURR_IMG(state) = 0;
+#endif
+
+#if defined(MCUBOOT_DIRECT_XIP) || defined(MCUBOOT_RAM_LOAD)
+    active_slot = ((struct slot_usage_t *)slot_usage)[BOOT_CURR_IMG(state)].active_slot;
+#else
+    (void) (slot_usage);
+    active_slot = BOOT_PRIMARY_SLOT;
+#endif
+
+    rsp->br_flash_dev_id = flash_area_get_device_id(BOOT_IMG_AREA(state, active_slot));
+    rsp->br_image_off = boot_img_slot_off(state, active_slot);
+    rsp->br_hdr = boot_img_hdr(state, active_slot);
+}
+
+/**
+ * Closes all flash areas.
+ *
+ * @param  state    Boot loader status information.
+ */
+static void
+close_all_flash_areas(struct boot_loader_state *state)
+{
+    uint32_t slot;
+
+    IMAGES_ITER(BOOT_CURR_IMG(state)) {
+#if MCUBOOT_SWAP_USING_SCRATCH
+        flash_area_close(BOOT_SCRATCH_AREA(state));
+#endif
+        for (slot = 0; slot < BOOT_NUM_SLOTS; slot++) {
+            flash_area_close(BOOT_IMG_AREA(state, BOOT_NUM_SLOTS - 1 - slot));
+        }
+    }
+}
+
 #if !defined(MCUBOOT_DIRECT_XIP)
 /*
  * Compute the total size of the given image.  Includes the size of
@@ -110,7 +231,7 @@
 static int
 boot_read_image_size(struct boot_loader_state *state, int slot, uint32_t *size)
 {
-    const struct flash_area *fap;
+    const struct flash_area *fap = NULL;
     struct image_tlv_info info;
     uint32_t off;
     uint32_t protect_tlv_size;
@@ -170,18 +291,22 @@
 static uint32_t
 boot_write_sz(struct boot_loader_state *state)
 {
-    uint32_t elem_sz;
+    size_t elem_sz;
 #if MCUBOOT_SWAP_USING_SCRATCH
-    uint32_t align;
+    size_t align;
 #endif
 
     /* Figure out what size to write update status update as.  The size depends
      * on what the minimum write size is for scratch area, active image slot.
      * We need to use the bigger of those 2 values.
      */
+
     elem_sz = flash_area_align(BOOT_IMG_AREA(state, BOOT_PRIMARY_SLOT));
+    assert(elem_sz != 0u);
+
 #if MCUBOOT_SWAP_USING_SCRATCH
     align = flash_area_align(BOOT_SCRATCH_AREA(state));
+    assert(align != 0u);
     if (align > elem_sz) {
         elem_sz = align;
     }
@@ -190,51 +315,20 @@
     return elem_sz;
 }
 
-#ifndef MCUBOOT_USE_FLASH_AREA_GET_SECTORS
 static int
 boot_initialize_area(struct boot_loader_state *state, int flash_area)
 {
-    int num_sectors = BOOT_MAX_IMG_SECTORS;
-    int rc;
-
-    if (flash_area == FLASH_AREA_IMAGE_PRIMARY(BOOT_CURR_IMG(state))) {
-        rc = flash_area_to_sectors(flash_area, &num_sectors,
-                                   BOOT_IMG(state, BOOT_PRIMARY_SLOT).sectors);
-        BOOT_IMG(state, BOOT_PRIMARY_SLOT).num_sectors = (size_t)num_sectors;
-
-    } else if (flash_area == FLASH_AREA_IMAGE_SECONDARY(BOOT_CURR_IMG(state))) {
-        rc = flash_area_to_sectors(flash_area, &num_sectors,
-                                 BOOT_IMG(state, BOOT_SECONDARY_SLOT).sectors);
-        BOOT_IMG(state, BOOT_SECONDARY_SLOT).num_sectors = (size_t)num_sectors;
-
-#if MCUBOOT_SWAP_USING_SCRATCH
-    } else if (flash_area == FLASH_AREA_IMAGE_SCRATCH) {
-        rc = flash_area_to_sectors(flash_area, &num_sectors,
-                                   state->scratch.sectors);
-        state->scratch.num_sectors = (size_t)num_sectors;
-#endif
-
-    } else {
-        return BOOT_EFLASH;
-    }
-
-    return rc;
-}
-#else  /* defined(MCUBOOT_USE_FLASH_AREA_GET_SECTORS) */
-static int
-boot_initialize_area(struct boot_loader_state *state, int flash_area)
-{
-    uint32_t num_sectors;
-    struct flash_sector *out_sectors;
+    uint32_t num_sectors = BOOT_MAX_IMG_SECTORS;
+    boot_sector_t *out_sectors;
     size_t *out_num_sectors;
     int rc;
 
     num_sectors = BOOT_MAX_IMG_SECTORS;
 
-    if (flash_area == FLASH_AREA_IMAGE_PRIMARY(BOOT_CURR_IMG(state))) {
+    if (flash_area == (int) FLASH_AREA_IMAGE_PRIMARY(BOOT_CURR_IMG(state))) {
         out_sectors = BOOT_IMG(state, BOOT_PRIMARY_SLOT).sectors;
         out_num_sectors = &BOOT_IMG(state, BOOT_PRIMARY_SLOT).num_sectors;
-    } else if (flash_area == FLASH_AREA_IMAGE_SECONDARY(BOOT_CURR_IMG(state))) {
+    } else if (flash_area == (int) FLASH_AREA_IMAGE_SECONDARY(BOOT_CURR_IMG(state))) {
         out_sectors = BOOT_IMG(state, BOOT_SECONDARY_SLOT).sectors;
         out_num_sectors = &BOOT_IMG(state, BOOT_SECONDARY_SLOT).num_sectors;
 #if MCUBOOT_SWAP_USING_SCRATCH
@@ -251,14 +345,18 @@
         return BOOT_EFLASH;
     }
 
+#ifdef MCUBOOT_USE_FLASH_AREA_GET_SECTORS
     rc = flash_area_get_sectors(flash_area, &num_sectors, out_sectors);
+#else
+    _Static_assert(sizeof(int) <= sizeof(uint32_t), "Fix needed");
+    rc = flash_area_to_sectors(flash_area, (int *)&num_sectors, out_sectors);
+#endif /* defined(MCUBOOT_USE_FLASH_AREA_GET_SECTORS) */
     if (rc != 0) {
         return rc;
     }
     *out_num_sectors = num_sectors;
     return 0;
 }
-#endif  /* !defined(MCUBOOT_USE_FLASH_AREA_GET_SECTORS) */
 
 /**
  * Determines the sector layout of both image slots and the scratch area.
@@ -281,7 +379,8 @@
 
     rc = boot_initialize_area(state, FLASH_AREA_IMAGE_SECONDARY(image_index));
     if (rc != 0) {
-        return BOOT_EFLASH;
+        /* We need to differentiate from the primary image issue */
+        return BOOT_EFLASH_SEC;
     }
 
 #if MCUBOOT_SWAP_USING_STATUS
@@ -307,9 +406,11 @@
 boot_status_reset(struct boot_status *bs)
 {
 #ifdef MCUBOOT_ENC_IMAGES
-    memset(&bs->enckey, 0xff, BOOT_NUM_SLOTS * BOOT_ENC_KEY_SIZE);
+    (void)memset(&bs->enckey, BOOT_UNINITIALIZED_KEY_FILL,
+                 BOOT_NUM_SLOTS * BOOT_ENC_KEY_SIZE);
 #if MCUBOOT_SWAP_SAVE_ENCTLV
-    memset(&bs->enctlv, 0xff, BOOT_NUM_SLOTS * BOOT_ENC_TLV_ALIGN_SIZE);
+    (void)memset(&bs->enctlv, BOOT_UNINITIALIZED_TLV_FILL,
+                 BOOT_NUM_SLOTS * BOOT_ENC_TLV_ALIGN_SIZE);
 #endif
 #endif /* MCUBOOT_ENC_IMAGES */
 
@@ -343,12 +444,12 @@
 int
 boot_write_status(const struct boot_loader_state *state, struct boot_status *bs)
 {
-    const struct flash_area *fap;
+    const struct flash_area *fap = NULL;
     uint32_t off;
     int area_id;
     int rc;
     uint8_t buf[BOOT_MAX_ALIGN];
-    uint8_t align;
+    size_t align;
     uint8_t erased_val;
 
     /* NOTE: The first sector copied (that is the last sector on slot) contains
@@ -378,8 +479,13 @@
     off = boot_status_off(fap) +
           boot_status_internal_off(bs, BOOT_WRITE_SZ(state));
     align = flash_area_align(fap);
+    if (align == 0u) {
+        rc = BOOT_EFLASH;
+        goto done;
+    }
+
     erased_val = flash_area_erased_val(fap);
-    memset(buf, erased_val, BOOT_MAX_ALIGN);
+    (void)memset(buf, erased_val, BOOT_MAX_ALIGN);
     buf[0] = bs->state;
 
     rc = flash_area_write(fap, off, buf, align);
@@ -421,14 +527,24 @@
 
     image_index = BOOT_CURR_IMG(state);
 
-#ifdef MCUBOOT_ENC_IMAGES
+/* In the case of ram loading the image has already been decrypted as it is
+ * decrypted when copied in ram */
+#if defined(MCUBOOT_ENC_IMAGES) && !defined(MCUBOOT_RAM_LOAD)
     if (MUST_DECRYPT(fap, image_index, hdr)) {
-        rc = boot_enc_load(BOOT_CURR_ENC(state), image_index, hdr, fap, bs);
+        rc = flash_area_id_to_multi_image_slot(image_index, fap->fa_id);
         if (rc < 0) {
             FIH_RET(fih_rc);
         }
-        if (rc == 0 && boot_enc_set_key(BOOT_CURR_ENC(state), 1, bs)) {
-            FIH_RET(fih_rc);
+        else {
+            uint8_t slot = (uint8_t)rc;
+
+            rc = boot_enc_load(BOOT_CURR_ENC(state), image_index, hdr, fap, bs);
+            if (rc < 0) {
+                FIH_RET(fih_rc);
+            }
+            if (0 == rc && boot_enc_set_key(BOOT_CURR_ENC(state), slot, bs)) {
+                FIH_RET(fih_rc);
+            }
         }
     }
 #endif
@@ -490,7 +606,7 @@
         return false;
     }
 
-    if (size >= fap->fa_size) {
+    if (size >= flash_area_get_size(fap)) {
         return false;
     }
 
@@ -516,7 +632,7 @@
 static int
 boot_check_header_erased(struct boot_loader_state *state, int slot)
 {
-    const struct flash_area *fap;
+    const struct flash_area *fap = NULL;
     struct image_header *hdr;
     uint8_t erased_val;
     int area_id;
@@ -542,8 +658,6 @@
 #if (BOOT_IMAGE_NUMBER > 1) || \
     defined(MCUBOOT_DIRECT_XIP) || \
     defined(MCUBOOT_RAM_LOAD) || \
-    defined(MCUBOOT_SWAP_USING_SCRATCH) || \
-    defined(MCUBOOT_SWAP_USING_MOVE) || \
     (defined(MCUBOOT_OVERWRITE_ONLY) && defined(MCUBOOT_DOWNGRADE_PREVENTION))
 /**
  * Compare image version numbers not including the build number
@@ -597,13 +711,21 @@
  *          does not match the slot address.
  */
 static bool
-boot_rom_address_check(struct boot_loader_state *state, size_t slot)
+boot_rom_address_check(struct boot_loader_state *state,
+                       struct slot_usage_t slot_usage[])
 {
-    const struct image_header *hdr = boot_img_hdr(state, slot);
-    uint32_t f_off = boot_img_slot_off(state, slot);
+    uint32_t active_slot;
+    const struct image_header *hdr;
+    uint32_t f_off;
+
+    active_slot = slot_usage[BOOT_CURR_IMG(state)].active_slot;
+    hdr = boot_img_hdr(state, active_slot);
+    f_off = boot_img_slot_off(state, active_slot);
+
     if (hdr->ih_flags & IMAGE_F_ROM_FIXED && hdr->ih_load_addr != f_off) {
-        BOOT_LOG_WRN("Image in %s slot at 0x%x has been built for offset 0x%x"\
-                     ", skipping", slot == 0 ? "primary" : "secondary", f_off,
+        BOOT_LOG_WRN("Image in %s slot at 0x%" PRIx32
+                     " has been built for offset 0x%" PRIx32 ", skipping",
+                     active_slot == 0 ? "primary" : "secondary", f_off,
                      hdr->ih_load_addr);
 
         /* If there is address mismatch, the image is not bootable from this
@@ -627,7 +749,7 @@
 boot_validate_slot(struct boot_loader_state *state, int slot,
                    struct boot_status *bs)
 {
-    const struct flash_area *fap;
+    const struct flash_area *fap = NULL;
     struct image_header *hdr;
     int area_id;
     fih_int fih_rc = FIH_FAILURE;
@@ -639,7 +761,7 @@
         FIH_RET(fih_rc);
     }
 
-    BOOT_LOG_DBG("> boot_validate_slot: fa_id = %d", fap->fa_id);
+    BOOT_LOG_DBG("> boot_validate_slot: fa_id = %u", (unsigned)fap->fa_id);
 
     hdr = boot_img_hdr(state, slot);
     if (boot_check_header_erased(state, slot) == 0 ||
@@ -677,7 +799,7 @@
                 &boot_img_hdr(state, BOOT_PRIMARY_SLOT)->ih_ver);
         if (rc < 0 && boot_check_header_erased(state, BOOT_PRIMARY_SLOT)) {
             BOOT_LOG_ERR("insufficient version in secondary slot");
-            flash_area_erase(fap, 0, fap->fa_size);
+            flash_area_erase(fap, 0, flash_area_get_size(fap));
             /* Image in the secondary slot does not satisfy version requirement.
              * Erase the image and continue booting from the primary slot.
              */
@@ -686,12 +808,16 @@
         }
     }
 #endif
-
-    FIH_CALL(boot_image_check, fih_rc, state, hdr, fap, bs);
+    BOOT_HOOK_CALL_FIH(boot_image_check_hook, fih_int_encode(BOOT_HOOK_REGULAR),
+                       fih_rc, BOOT_CURR_IMG(state), slot);
+    if (fih_eq(fih_rc, fih_int_encode(BOOT_HOOK_REGULAR)))
+    {
+        FIH_CALL(boot_image_check, fih_rc, state, hdr, fap, bs);
+    }
     if (!boot_is_header_valid(hdr, fap) || fih_not_eq(fih_rc, FIH_SUCCESS)) {
         if ((slot != BOOT_PRIMARY_SLOT) || ARE_SLOTS_EQUIVALENT()) {
             BOOT_LOG_DBG(" * Image in the secondary slot is invalid. Erase the image");
-            flash_area_erase(fap, 0, fap->fa_size);
+            flash_area_erase(fap, 0, flash_area_get_size(fap));
             /* Image is invalid, erase it to prevent further unnecessary
              * attempts to validate and boot it.
              */
@@ -732,7 +858,11 @@
 {
     const struct flash_area *fap = NULL;
     uint32_t img_security_cnt;
+    void * custom_data = NULL;
     int rc;
+#ifdef CYW20829
+    uint8_t buff[REPROV_PACK_SIZE];
+#endif /* CYW20829 */
 
     rc = flash_area_open(flash_area_id_from_multi_image_slot(image_index, slot),
                          &fap);
@@ -746,10 +876,13 @@
         goto done;
     }
 
-    rc = boot_nv_security_counter_update(image_index, img_security_cnt);
-    if (rc != 0) {
-        goto done;
+#ifdef CYW20829
+    rc = bootutil_get_img_reprov_packet(hdr, fap, buff);
+    if (rc == 0) {
+        custom_data = (void *)buff;
     }
+#endif /* CYW20829 */
+    rc = boot_nv_security_counter_update(image_index, img_security_cnt, custom_data);
 
 done:
     flash_area_close(fap);
@@ -790,6 +923,7 @@
 
     return swap_type;
 }
+#endif
 
 /**
  * Erases a region of flash.
@@ -807,6 +941,7 @@
     return flash_area_erase(fap, off, sz);
 }
 
+#if !defined(MCUBOOT_DIRECT_XIP) && !defined(MCUBOOT_RAM_LOAD)
 /**
  * Copies the contents of one flash region to another.  You must erase the
  * destination region prior to calling this function.
@@ -830,7 +965,7 @@
     uint32_t bytes_copied;
     int chunk_sz;
     int rc;
-#ifdef MCUBOOT_ENC_IMAGES
+#if defined (MCUBOOT_ENC_IMAGES) && !defined(MCUBOOT_ENC_IMAGES_XIP)
     uint32_t off;
     uint32_t tlv_off;
     size_t blk_off;
@@ -840,9 +975,20 @@
     uint8_t image_index;
 #endif
 
-    TARGET_STATIC uint8_t buf[1024];
+/* NOTE:
+ * Default value 1024 is not suitable for platforms with larger erase size.
+ * Give user ability to define platform tolerant chunk size. In most cases
+ * it would be flash erase alignment.
+ */
+#ifdef MCUBOOT_PLATFORM_CHUNK_SIZE
+    #define MCUBOOT_CHUNK_SIZE MCUBOOT_PLATFORM_CHUNK_SIZE
+#else
+    #define MCUBOOT_CHUNK_SIZE 1024
+#endif
 
-#if !defined(MCUBOOT_ENC_IMAGES)
+    TARGET_STATIC uint8_t buf[MCUBOOT_CHUNK_SIZE] __attribute__((aligned(4)));
+
+#if !defined(MCUBOOT_ENC_IMAGES) || defined(MCUBOOT_ENC_IMAGES_XIP)
     (void)state;
 #endif
 
@@ -859,28 +1005,28 @@
             return BOOT_EFLASH;
         }
 
-#ifdef MCUBOOT_ENC_IMAGES
+#if defined(MCUBOOT_ENC_IMAGES) && !defined(MCUBOOT_ENC_IMAGES_XIP)
         image_index = BOOT_CURR_IMG(state);
-        if ((fap_src->fa_id == FLASH_AREA_IMAGE_PRIMARY(image_index) ||
-             fap_dst->fa_id == FLASH_AREA_IMAGE_PRIMARY(image_index)) &&
+        if ((flash_area_get_id(fap_src) == FLASH_AREA_IMAGE_PRIMARY(image_index) ||
+             flash_area_get_id(fap_dst) == FLASH_AREA_IMAGE_PRIMARY(image_index)) &&
 
-            !(fap_src->fa_id == FLASH_AREA_IMAGE_PRIMARY(image_index) &&
-              fap_dst->fa_id == FLASH_AREA_IMAGE_PRIMARY(image_index)) &&
-            !(fap_src->fa_id == FLASH_AREA_IMAGE_SECONDARY(image_index) &&
-              fap_dst->fa_id == FLASH_AREA_IMAGE_SECONDARY(image_index)))
+            !(flash_area_get_id(fap_src) == FLASH_AREA_IMAGE_PRIMARY(image_index) &&
+              flash_area_get_id(fap_dst) == FLASH_AREA_IMAGE_PRIMARY(image_index)) &&
+            !(flash_area_get_id(fap_src) == FLASH_AREA_IMAGE_SECONDARY(image_index) &&
+              flash_area_get_id(fap_dst) == FLASH_AREA_IMAGE_SECONDARY(image_index)))
         {
             /* assume the primary slot as src, needs encryption */
             hdr = boot_img_hdr(state, BOOT_PRIMARY_SLOT);
 #if !defined(MCUBOOT_SWAP_USING_MOVE)
             off = off_src;
-            if (fap_dst->fa_id == FLASH_AREA_IMAGE_PRIMARY(image_index)) {
+            if (flash_area_get_id(fap_dst) == FLASH_AREA_IMAGE_PRIMARY(image_index)) {
                 /* might need decryption (metadata from the secondary slot) */
                 hdr = boot_img_hdr(state, BOOT_SECONDARY_SLOT);
                 off = off_dst;
             }
 #else
             off = off_dst;
-            if (fap_dst->fa_id == FLASH_AREA_IMAGE_PRIMARY(image_index)) {
+            if (flash_area_get_id(fap_dst) == FLASH_AREA_IMAGE_PRIMARY(image_index)) {
                 hdr = boot_img_hdr(state, BOOT_SECONDARY_SLOT);
             }
 #endif
@@ -909,10 +1055,11 @@
                         blk_sz = tlv_off - (off + bytes_copied);
                     }
                 }
-                if(0 != blk_sz) {
-                    boot_encrypt(BOOT_CURR_ENC(state), image_index, fap_src,
-                        (off + bytes_copied + idx) - hdr->ih_hdr_size, blk_sz,
-                        blk_off, &buf[idx]);
+                rc = boot_encrypt(BOOT_CURR_ENC(state), image_index, fap_src,
+                    (off + bytes_copied + idx) - hdr->ih_hdr_size, blk_sz,
+                    blk_off, &buf[idx]);
+                if (rc != 0) {
+                    return rc;
                 }
             }
         }
@@ -954,8 +1101,8 @@
     size_t size;
     size_t this_size;
     size_t last_sector;
-    const struct flash_area *fap_primary_slot;
-    const struct flash_area *fap_secondary_slot;
+    const struct flash_area *fap_primary_slot = NULL;
+    const struct flash_area *fap_secondary_slot = NULL;
     uint8_t image_index;
 
 #if defined(MCUBOOT_OVERWRITE_ONLY_FAST)
@@ -987,12 +1134,9 @@
     assert (rc == 0);
 
     sect_count = boot_img_num_sectors(state, BOOT_PRIMARY_SLOT);
-    BOOT_LOG_DBG(" * primary slot sectors: %d", sect_count);
+    BOOT_LOG_DBG(" * primary slot sectors: %lu", (unsigned long)sect_count);
     for (sect = 0, size = 0; sect < sect_count; sect++) {
         this_size = boot_img_sector_size(state, BOOT_PRIMARY_SLOT, sect);
-        /* BOOT_LOG_DBG(" * primary slot erase region (0x%0lx / 0x%0lx): sector = %d, off = %0x, size = %d",
-                     fap_primary_slot->fa_off, fap_primary_slot->fa_size,
-                     sect, size, this_size); */
         rc = boot_erase_region(fap_primary_slot, size, this_size);
         assert(rc == 0);
 
@@ -1036,8 +1180,7 @@
     }
 #endif
 
-    BOOT_LOG_INF("Copying the secondary slot to the primary slot: 0x%x bytes",
-                 size);
+    BOOT_LOG_INF("Copying the secondary slot to the primary slot: 0x%lx bytes", (unsigned long)size);
     rc = boot_copy_region(state, fap_secondary_slot, fap_primary_slot, 0, 0, size);
     if (rc != 0) {
         return rc;
@@ -1050,6 +1193,12 @@
     }
 #endif
 
+    rc = BOOT_HOOK_CALL(boot_copy_region_post_hook, 0, BOOT_CURR_IMG(state),
+                        BOOT_IMG_AREA(state, BOOT_PRIMARY_SLOT), size);
+    if (rc != 0) {
+        return rc;
+    }
+
 #ifdef MCUBOOT_HW_ROLLBACK_PROT
     /* Update the stored security counter with the new image's security counter
      * value. Both slots hold the new image at this point, but the secondary
@@ -1112,12 +1261,11 @@
 #ifdef MCUBOOT_ENC_IMAGES
     const struct flash_area *fap;
     uint8_t slot;
-    uint8_t i;
 #endif
     uint32_t size;
     uint32_t copy_size;
     uint8_t image_index;
-    int rc;
+    int rc = -1;
 
     /* FIXME: just do this if asked by user? */
 
@@ -1148,7 +1296,8 @@
                 rc = 0;
             }
         } else {
-            memset(bs->enckey[0], 0xff, BOOT_ENC_KEY_SIZE);
+            (void)memset(bs->enckey[0], BOOT_UNINITIALIZED_KEY_FILL,
+                         BOOT_ENC_KEY_SIZE);
         }
 #endif
 
@@ -1172,7 +1321,8 @@
                 rc = 0;
             }
         } else {
-            memset(bs->enckey[1], 0xff, BOOT_ENC_KEY_SIZE);
+            (void)memset(bs->enckey[1], BOOT_UNINITIALIZED_KEY_FILL,
+                         BOOT_ENC_KEY_SIZE);
         }
 #endif
 
@@ -1194,16 +1344,14 @@
 #ifdef MCUBOOT_ENC_IMAGES
         for (slot = 0; slot < BOOT_NUM_SLOTS; slot++) {
             rc = boot_read_enc_key(image_index, slot, bs);
-            assert(rc == 0);
+            assert(0 == rc);
 
-            for (i = 0; i < BOOT_ENC_KEY_SIZE; i++) {
-                if (bs->enckey[slot][i] != 0xff) {
-                    break;
-                }
-            }
-
-            if (i != BOOT_ENC_KEY_SIZE) {
-                boot_enc_set_key(BOOT_CURR_ENC(state), slot, bs);
+            /* Set only an initialized key */
+            if (!bootutil_buffer_is_filled(bs->enckey[slot],
+                                           BOOT_UNINITIALIZED_KEY_FILL,
+                                           BOOT_ENC_KEY_SIZE)) {
+                rc = boot_enc_set_key(BOOT_CURR_ENC(state), slot, bs);
+                assert(rc == 0);
             }
         }
 #endif
@@ -1219,7 +1367,7 @@
     }
 #endif
 
-    return 0;
+    return rc;
 }
 #endif
 
@@ -1368,7 +1516,12 @@
         if (rc == 0) {
             /* All dependencies've been satisfied, continue with next image. */
             BOOT_CURR_IMG(state)++;
-        } else if (rc == BOOT_EBADVERSION) {
+        } else {
+#if (USE_SHARED_SLOT == 1)
+            /* Disable an upgrading of this image.*/
+            BOOT_SWAP_TYPE(state) = BOOT_SWAP_TYPE_NONE;
+            BOOT_CURR_IMG(state)++;
+#else
             /* Cannot upgrade due to non-met dependencies, so disable all
              * image upgrades.
              */
@@ -1377,9 +1530,7 @@
                 BOOT_SWAP_TYPE(state) = BOOT_SWAP_TYPE_NONE;
             }
             break;
-        } else {
-            /* Other error happened, images are inconsistent */
-            return rc;
+#endif /* (USE_SHARED_SLOT == 1) */
         }
     }
     return rc;
@@ -1401,7 +1552,7 @@
     uint8_t swap_type;
 #endif
 
-    BOOT_LOG_DBG("> boot_perform_update: bs->idx = %d", bs->idx);
+    BOOT_LOG_DBG("> boot_perform_update: bs->idx = %" PRIu32, bs->idx);
 
     /* At this point there are no aborted swaps. */
 #if defined(MCUBOOT_OVERWRITE_ONLY)
@@ -1415,6 +1566,21 @@
     rc = boot_check_header_erased(state, BOOT_PRIMARY_SLOT);
     FIH_CALL(boot_validate_slot, fih_rc, state, BOOT_PRIMARY_SLOT, bs);
     if (rc == 0 || fih_not_eq(fih_rc, FIH_SUCCESS)) {
+        /* Initialize swap status partition for primary slot, because
+         * in swap mode it is needed to properly complete copying the image
+         * to the primary slot.
+         */
+        const struct flash_area *fap_primary_slot = NULL;
+
+        rc = flash_area_open(FLASH_AREA_IMAGE_PRIMARY(BOOT_CURR_IMG(state)),
+                             &fap_primary_slot);
+        assert(rc == 0);
+
+        rc = swap_status_init(state, fap_primary_slot, bs);
+        assert(rc == 0);
+
+        flash_area_close(fap_primary_slot);
+
         rc = boot_copy_image(state, bs);
     } else {
         rc = boot_swap_image(state, bs);
@@ -1598,27 +1764,33 @@
     int rc;
     fih_int fih_rc = FIH_FAILURE;
 
-    BOOT_LOG_DBG("> boot_prepare_image_for_update: image = %d", BOOT_CURR_IMG(state));
+    BOOT_LOG_DBG("> boot_prepare_image_for_update: image = %u",
+                 (unsigned)BOOT_CURR_IMG(state));
 
     /* Determine the sector layout of the image slots and scratch area. */
     rc = boot_read_sectors(state);
     if (rc != 0) {
-        BOOT_LOG_WRN("Failed reading sectors; BOOT_MAX_IMG_SECTORS=%d"
-                     " - too small?", BOOT_MAX_IMG_SECTORS);
+        BOOT_LOG_WRN("Failed reading sectors; BOOT_MAX_IMG_SECTORS=%u"
+                     " - too small?", (unsigned int) BOOT_MAX_IMG_SECTORS);
         /* Unable to determine sector layout, continue with next image
          * if there is one.
          */
         BOOT_SWAP_TYPE(state) = BOOT_SWAP_TYPE_NONE;
-        return;
+        if (rc == BOOT_EFLASH)
+        {
+            /* Only return on error from the primary image flash */
+            return;
+        }
     }
 
     /* Attempt to read an image header from each slot. */
     rc = boot_read_image_headers(state, false, NULL);
-    BOOT_LOG_DBG(" * Read an image (%d) header from each slot: rc = %d", BOOT_CURR_IMG(state), rc);
+    BOOT_LOG_DBG(" * Read an image (%u) header from each slot: rc = %d",
+                 (unsigned)BOOT_CURR_IMG(state), rc);
     if (rc != 0) {
         /* Continue with next image if there is one. */
         BOOT_LOG_WRN("Failed reading image headers; Image=%u",
-                BOOT_CURR_IMG(state));
+                     (unsigned)BOOT_CURR_IMG(state));
         BOOT_SWAP_TYPE(state) = BOOT_SWAP_TYPE_NONE;
         return;
     }
@@ -1633,7 +1805,7 @@
         rc = swap_read_status(state, bs);
         if (rc != 0) {
             BOOT_LOG_WRN("Failed reading boot status; Image=%u",
-                    BOOT_CURR_IMG(state));
+                         (unsigned)BOOT_CURR_IMG(state));
             /* Continue with next image if there is one. */
             BOOT_SWAP_TYPE(state) = BOOT_SWAP_TYPE_NONE;
             return;
@@ -1646,7 +1818,8 @@
          * have been updated in the previous function call.
          */
         rc = boot_read_image_headers(state, !boot_status_is_reset(bs), bs);
-        BOOT_LOG_DBG(" * re-read image(%d) headers: rc = %d.", BOOT_CURR_IMG(state), rc);
+        BOOT_LOG_DBG(" * re-read image(%u) headers: rc = %d.",
+                     (unsigned)BOOT_CURR_IMG(state), rc);
 #ifdef MCUBOOT_BOOTSTRAP
         /* When bootstrapping it's OK to not have image magic in the primary slot */
         if (rc != 0 && (BOOT_CURR_IMG(state) != BOOT_PRIMARY_SLOT ||
@@ -1748,6 +1921,50 @@
     BOOT_LOG_DBG("< boot_prepare_image_for_update");
 }
 
+/**
+ * Updates the security counter for the current image.
+ *
+ * @param  state        Boot loader status information.
+ *
+ * @return              0 on success; nonzero on failure.
+ */
+static int
+boot_update_hw_rollback_protection(struct boot_loader_state *state)
+{
+#ifdef MCUBOOT_HW_ROLLBACK_PROT
+    int rc;
+
+    /* Update the stored security counter with the active image's security
+    * counter value. It will only be updated if the new security counter is
+    * greater than the stored value.
+    *
+    * In case of a successful image swapping when the swap type is TEST the
+    * security counter can be increased only after a reset, when the swap
+    * type is NONE and the image has marked itself "OK" (the image_ok flag
+    * has been set). This way a "revert" can be performed when it's
+    * necessary.
+    */
+    if (BOOT_SWAP_TYPE(state) == BOOT_SWAP_TYPE_NONE) {
+        rc = boot_update_security_counter(
+                                BOOT_CURR_IMG(state),
+                                BOOT_PRIMARY_SLOT,
+                                boot_img_hdr(state, BOOT_PRIMARY_SLOT));
+        if (rc != 0) {
+            BOOT_LOG_ERR("Security counter update failed after image "
+                            "validation.");
+            return rc;
+        }
+    }
+
+    return 0;
+
+#else /* MCUBOOT_HW_ROLLBACK_PROT */
+    (void) (state);
+
+    return 0;
+#endif
+}
+
 fih_int
 context_boot_go(struct boot_loader_state *state, struct boot_rsp *rsp)
 {
@@ -1773,7 +1990,8 @@
     TARGET_STATIC boot_sector_t status_sectors[BOOT_MAX_SWAP_STATUS_SECTORS];
 #endif
 
-    memset(state, 0, sizeof(struct boot_loader_state));
+    (void)memset(&bs, 0, sizeof(bs));
+    (void)memset(state, 0, sizeof(struct boot_loader_state));
     has_upgrade = false;
 
 #if (BOOT_IMAGE_NUMBER == 1)
@@ -1870,7 +2088,7 @@
         /* Set the previously determined swap type */
         bs.swap_type = BOOT_SWAP_TYPE(state);
 
-        BOOT_LOG_DBG(" * process swap_type = %d", bs.swap_type);
+        BOOT_LOG_DBG(" * process swap_type = %u", (unsigned)bs.swap_type);
 
         switch (BOOT_SWAP_TYPE(state)) {
         case BOOT_SWAP_TYPE_NONE:
@@ -1879,8 +2097,14 @@
         case BOOT_SWAP_TYPE_TEST:          /* fallthrough */
         case BOOT_SWAP_TYPE_PERM:          /* fallthrough */
         case BOOT_SWAP_TYPE_REVERT:
-            BOOT_LOG_DBG(" * perform update...", bs.swap_type);
-            rc = boot_perform_update(state, &bs);
+             BOOT_LOG_DBG(" * perform update, mode %u...", (unsigned)bs.swap_type);
+            rc = BOOT_HOOK_CALL(boot_perform_update_hook, BOOT_HOOK_REGULAR,
+                                BOOT_CURR_IMG(state), &(BOOT_IMG(state, 1).hdr),
+                                BOOT_IMG_AREA(state, BOOT_SECONDARY_SLOT));
+            if (rc == BOOT_HOOK_REGULAR)
+            {
+                rc = boot_perform_update(state, &bs);
+            }
             assert(rc == 0);
             break;
 
@@ -1890,7 +2114,8 @@
              * pretending we just reverted back to primary slot.
              */
 #ifndef MCUBOOT_OVERWRITE_ONLY
-            BOOT_LOG_DBG(" * update failed! Set image_ok manually for image(%d)", BOOT_CURR_IMG(state));
+            BOOT_LOG_DBG(" * update failed! Set image_ok manually for image(%u)",
+                         (unsigned)BOOT_CURR_IMG(state));
             /* image_ok needs to be explicitly set to avoid a new revert. */
             rc = swap_set_image_ok(BOOT_CURR_IMG(state));
             if (rc != 0) {
@@ -1933,7 +2158,7 @@
         }
 
 #ifdef MCUBOOT_VALIDATE_PRIMARY_SLOT
-        FIH_CALL(boot_validate_slot, fih_rc, state, BOOT_PRIMARY_SLOT, NULL);
+        FIH_CALL(boot_validate_slot, fih_rc, state, BOOT_PRIMARY_SLOT, &bs);
         if (fih_not_eq(fih_rc, FIH_SUCCESS)) {
             goto out;
         }
@@ -1943,57 +2168,33 @@
          * the magic number on the image is OK.
          */
         if (BOOT_IMG(state, BOOT_PRIMARY_SLOT).hdr.ih_magic != IMAGE_MAGIC) {
-            BOOT_LOG_ERR("bad image magic 0x%lx; Image=%u", (unsigned long)
-                         &boot_img_hdr(state,BOOT_PRIMARY_SLOT)->ih_magic,
-                         BOOT_CURR_IMG(state));
+            BOOT_LOG_ERR("bad image magic 0x%" PRIx32 "; Image=%u",
+                         BOOT_IMG(state, BOOT_PRIMARY_SLOT).hdr.ih_magic,
+                         (unsigned)BOOT_CURR_IMG(state));
             rc = BOOT_EBADIMAGE;
             goto out;
         }
 #endif /* MCUBOOT_VALIDATE_PRIMARY_SLOT */
 
-#ifdef MCUBOOT_HW_ROLLBACK_PROT
-        /* Update the stored security counter with the active image's security
-         * counter value. It will only be updated if the new security counter is
-         * greater than the stored value.
-         *
-         * In case of a successful image swapping when the swap type is TEST the
-         * security counter can be increased only after a reset, when the swap
-         * type is NONE and the image has marked itself "OK" (the image_ok flag
-         * has been set). This way a "revert" can be performed when it's
-         * necessary.
-         */
-        if (BOOT_SWAP_TYPE(state) == BOOT_SWAP_TYPE_NONE) {
-            rc = boot_update_security_counter(
-                                    BOOT_CURR_IMG(state),
-                                    BOOT_PRIMARY_SLOT,
-                                    boot_img_hdr(state, BOOT_PRIMARY_SLOT));
-            if (rc != 0) {
-                BOOT_LOG_ERR("Security counter update failed after image "
-                             "validation.");
-                goto out;
+#ifdef MCUBOOT_ENC_IMAGES_XIP
+        if (0 == BOOT_CURR_IMG(state)) {
+            if (IS_ENCRYPTED(boot_img_hdr(state, BOOT_PRIMARY_SLOT)))
+            {
+                (void)memcpy((uint8_t*)rsp->xip_iv, BOOT_CURR_ENC(state)->aes_iv, BOOTUTIL_CRYPTO_AES_CTR_BLOCK_SIZE);
+                (void)memcpy((uint8_t*)rsp->xip_key, bs.enckey[BOOT_CURR_IMG(state)], BOOTUTIL_CRYPTO_AES_CTR_KEY_SIZE);
             }
         }
-#endif /* MCUBOOT_HW_ROLLBACK_PROT */
+#endif /* MCUBOOT_ENC_IMAGES_XIP */
 
-#ifdef MCUBOOT_MEASURED_BOOT
-        rc = boot_save_boot_status(BOOT_CURR_IMG(state),
-                                   boot_img_hdr(state, BOOT_PRIMARY_SLOT),
-                                   BOOT_IMG_AREA(state, BOOT_PRIMARY_SLOT));
+        rc = boot_update_hw_rollback_protection(state);
         if (rc != 0) {
-            BOOT_LOG_ERR("Failed to add Image %u data to shared memory area",
-                         BOOT_CURR_IMG(state));
             goto out;
         }
-#endif /* MCUBOOT_MEASURED_BOOT */
 
-#ifdef MCUBOOT_DATA_SHARING
-        rc = boot_save_shared_data(boot_img_hdr(state, BOOT_PRIMARY_SLOT),
-                                   BOOT_IMG_AREA(state, BOOT_PRIMARY_SLOT));
+        rc = boot_add_shared_data(state, BOOT_PRIMARY_SLOT);
         if (rc != 0) {
-            BOOT_LOG_ERR("Failed to add data to shared memory area.");
             goto out;
         }
-#endif /* MCUBOOT_DATA_SHARING */
     }
 
 #if (BOOT_IMAGE_NUMBER > 1)
@@ -2001,27 +2202,21 @@
     BOOT_CURR_IMG(state) = 0;
 #endif
 
+    rsp->br_flash_dev_id = BOOT_IMG_AREA(state, BOOT_PRIMARY_SLOT)->fa_device_id;
+    rsp->br_image_off = boot_img_slot_off(state, BOOT_PRIMARY_SLOT);
+    rsp->br_hdr = boot_img_hdr(state, BOOT_PRIMARY_SLOT);
     /*
      * Since the boot_status struct stores plaintext encryption keys, reset
      * them here to avoid the possibility of jumping into an image that could
      * easily recover them.
      */
-    memset(&bs, 0, sizeof(struct boot_status));
+    (void)memset(&bs, 0, sizeof(struct boot_status));
 
-    rsp->br_flash_dev_id = BOOT_IMG_AREA(state, BOOT_PRIMARY_SLOT)->fa_device_id;
-    rsp->br_image_off = boot_img_slot_off(state, BOOT_PRIMARY_SLOT);
-    rsp->br_hdr = boot_img_hdr(state, BOOT_PRIMARY_SLOT);
+    fill_rsp(state, NULL, rsp);
 
     fih_rc = FIH_SUCCESS;
 out:
-    IMAGES_ITER(BOOT_CURR_IMG(state)) {
-#if MCUBOOT_SWAP_USING_SCRATCH
-        flash_area_close(BOOT_SCRATCH_AREA(state));
-#endif
-        for (slot = 0; slot < BOOT_NUM_SLOTS; slot++) {
-            flash_area_close(BOOT_IMG_AREA(state, BOOT_NUM_SLOTS - 1 - slot));
-        }
-    }
+    close_all_flash_areas(state);
 
     if (rc) {
         fih_rc = fih_int_encode(rc);
@@ -2040,6 +2235,10 @@
     int rc;
     fih_int fih_rc = FIH_FAILURE;
 
+    if ((loader_slot < 0) || (split_slot < 0)) {
+        FIH_RET(FIH_FAILURE);
+    }
+
     sectors = malloc(BOOT_MAX_IMG_SECTORS * 2 * sizeof *sectors);
     if (sectors == NULL) {
         FIH_RET(FIH_FAILURE);
@@ -2100,92 +2299,180 @@
 
 #else /* MCUBOOT_DIRECT_XIP || MCUBOOT_RAM_LOAD */
 
+#define NO_ACTIVE_SLOT UINT32_MAX
+
 /**
- * Iterates over all slots and determines which contain a firmware image.
+ * Opens all flash areas and checks which contain an image with a valid header.
  *
- * @param state          Boot loader status information.
- * @param slot_usage     Pointer to an array, which aim is to carry information
- *                       about slots that contain an image. After return the
- *                       corresponding array elements are set to a non-zero
- *                       value if the given slots are in use (contain a firmware
- *                       image), otherwise they are set to zero.
- * @param slot_cnt       The number of slots, which can contain firmware images.
- *                       (Equal to or smaller than the size of the
- *                       slot_usage array.)
+ * @param  state        Boot loader status information.
+ * @param  slot_usage   Structure to fill with information about the available
+ *                      slots.
  *
- * @return               The number of found images.
+ * @return              0 on success; nonzero on failure.
+ */
+static int
+boot_get_slot_usage(struct boot_loader_state *state,
+                    struct slot_usage_t slot_usage[])
+{
+    uint32_t slot;
+    int fa_id;
+    int rc;
+    struct image_header *hdr = NULL;
+
+    IMAGES_ITER(BOOT_CURR_IMG(state)) {
+        /* Open all the slots */
+        for (slot = 0; slot < BOOT_NUM_SLOTS; slot++) {
+            fa_id = flash_area_id_from_multi_image_slot(
+                                                BOOT_CURR_IMG(state), slot);
+            rc = flash_area_open(fa_id, &BOOT_IMG_AREA(state, slot));
+            assert(rc == 0);
+        }
+
+        /* Attempt to read an image header from each slot. */
+        rc = boot_read_image_headers(state, false, NULL);
+        if (rc != 0) {
+            BOOT_LOG_WRN("Failed reading image headers.");
+            return rc;
+        }
+
+        /* Check headers in all slots */
+        for (slot = 0; slot < BOOT_NUM_SLOTS; slot++) {
+            hdr = boot_img_hdr(state, slot);
+
+            if (boot_is_header_valid(hdr, BOOT_IMG_AREA(state, slot))) {
+                slot_usage[BOOT_CURR_IMG(state)].slot_available[slot] = true;
+                BOOT_LOG_IMAGE_INFO(slot, hdr);
+            } else {
+                slot_usage[BOOT_CURR_IMG(state)].slot_available[slot] = false;
+                BOOT_LOG_INF("Image %u %s slot: Image not found",
+                             (unsigned)BOOT_CURR_IMG(state),
+                             (slot == BOOT_PRIMARY_SLOT)
+                             ? "Primary" : "Secondary");
+            }
+        }
+
+        slot_usage[BOOT_CURR_IMG(state)].active_slot = NO_ACTIVE_SLOT;
+    }
+
+    return 0;
+}
+
+/**
+ * Finds the slot containing the image with the highest version number for the
+ * current image.
+ *
+ * @param  state        Boot loader status information.
+ * @param  slot_usage   Information about the active and available slots.
+ *
+ * @return              NO_ACTIVE_SLOT if no available slot found, number of
+ *                      the found slot otherwise.
  */
 static uint32_t
-boot_get_slot_usage(struct boot_loader_state *state, uint8_t slot_usage[],
-                    uint32_t slot_cnt)
+find_slot_with_highest_version(struct boot_loader_state *state,
+                               struct slot_usage_t slot_usage[])
 {
-    struct image_header *hdr = NULL;
-    uint32_t image_cnt = 0;
     uint32_t slot;
+    uint32_t candidate_slot = NO_ACTIVE_SLOT;
+    int rc;
 
-    memset(slot_usage, 0, slot_cnt);
-
-    for (slot = 0; slot < slot_cnt; slot++) {
-        hdr = boot_img_hdr(state, slot);
-
-        if (boot_is_header_valid(hdr, BOOT_IMG_AREA(state, slot))) {
-            slot_usage[slot] = 1;
-            image_cnt++;
-            BOOT_LOG_IMAGE_INFO(slot, hdr);
-        } else {
-            BOOT_LOG_INF("%s slot: Image not found", (slot == BOOT_PRIMARY_SLOT)
-                         ? "Primary" : "Secondary");
+    for (slot = 0; slot < BOOT_NUM_SLOTS; slot++) {
+        if (slot_usage[BOOT_CURR_IMG(state)].slot_available[slot]) {
+            if (candidate_slot == NO_ACTIVE_SLOT) {
+                candidate_slot = slot;
+            } else {
+                rc = boot_version_cmp(
+                            &boot_img_hdr(state, slot)->ih_ver,
+                            &boot_img_hdr(state, candidate_slot)->ih_ver);
+                if (rc == 1) {
+                    /* The version of the image being examined is greater than
+                     * the version of the current candidate.
+                     */
+                    candidate_slot = slot;
+                }
+            }
         }
     }
 
-    return image_cnt;
+    return candidate_slot;
 }
 
-#ifdef MCUBOOT_DIRECT_XIP_REVERT
+#ifdef MCUBOOT_HAVE_LOGGING
 /**
- * Checks whether the image in the given slot was previously selected to run.
- * Erases the image if it was selected but its execution failed, otherwise marks
- * it as selected if it has not been before.
+ * Prints the state of the loaded images.
  *
- * @param state     Image metadata from the image trailer. This function fills
- *                  this struct with the data read from the image trailer.
- * @param slot      Image slot number.
+ * @param  state        Boot loader status information.
+ * @param  slot_usage   Information about the active and available slots.
+ */
+static void
+print_loaded_images(struct boot_loader_state *state,
+                    struct slot_usage_t slot_usage[])
+{
+    uint32_t active_slot;
+
+    (void)state;
+
+    IMAGES_ITER(BOOT_CURR_IMG(state)) {
+        active_slot = slot_usage[BOOT_CURR_IMG(state)].active_slot;
+
+        BOOT_LOG_INF("Image %u loaded from the %s slot",
+                     (unsigned)BOOT_CURR_IMG(state),
+                     (active_slot == BOOT_PRIMARY_SLOT) ?
+                     "primary" : "secondary");
+    }
+}
+#endif
+
+#if defined(MCUBOOT_DIRECT_XIP) && defined(MCUBOOT_DIRECT_XIP_REVERT)
+/**
+ * Checks whether the active slot of the current image was previously selected
+ * to run. Erases the image if it was selected but its execution failed,
+ * otherwise marks it as selected if it has not been before.
  *
- * @return          0 on success; nonzero on failure.
+ * @param  state        Boot loader status information.
+ * @param  slot_usage   Information about the active and available slots.
+ *
+ * @return              0 on success; nonzero on failure.
  */
 static int
-boot_select_or_erase(struct boot_swap_state *state, uint32_t slot)
+boot_select_or_erase(struct boot_loader_state *state,
+                     struct slot_usage_t slot_usage[])
 {
     const struct flash_area *fap;
     int fa_id;
     int rc;
+    uint32_t active_slot;
+    struct boot_swap_state* active_swap_state;
 
-    fa_id = flash_area_id_from_image_slot(slot);
+    active_slot = slot_usage[BOOT_CURR_IMG(state)].active_slot;
+
+    fa_id = flash_area_id_from_multi_image_slot(BOOT_CURR_IMG(state), active_slot);
     rc = flash_area_open(fa_id, &fap);
     assert(rc == 0);
 
-    memset(state, 0, sizeof(struct boot_swap_state));
-    rc = boot_read_swap_state(fap, state);
+    active_swap_state = &(slot_usage[BOOT_CURR_IMG(state)].swap_state);
+
+    (void)memset(active_swap_state, 0, sizeof(struct boot_swap_state));
+    rc = boot_read_swap_state(fap, active_swap_state);
     assert(rc == 0);
 
-    if (state->magic != BOOT_MAGIC_GOOD ||
-        (state->copy_done == BOOT_FLAG_SET &&
-         state->image_ok  != BOOT_FLAG_SET)) {
+    if (active_swap_state->magic != BOOT_MAGIC_GOOD ||
+        (active_swap_state->copy_done == BOOT_FLAG_SET &&
+         active_swap_state->image_ok  != BOOT_FLAG_SET)) {
         /*
          * A reboot happened without the image being confirmed at
          * runtime or its trailer is corrupted/invalid. Erase the image
          * to prevent it from being selected again on the next reboot.
          */
         BOOT_LOG_DBG("Erasing faulty image in the %s slot.",
-                     (slot == BOOT_PRIMARY_SLOT) ? "primary" : "secondary");
-        rc = flash_area_erase(fap, 0, fap->fa_size);
+                     (active_slot == BOOT_PRIMARY_SLOT) ? "primary" : "secondary");
+        rc = flash_area_erase(fap, 0, flash_area_get_size(fap));
         assert(rc == 0);
 
         flash_area_close(fap);
         rc = -1;
     } else {
-        if (state->copy_done != BOOT_FLAG_SET) {
-            if (state->copy_done == BOOT_FLAG_BAD) {
+        if (active_swap_state->copy_done != BOOT_FLAG_SET) {
+            if (active_swap_state->copy_done == BOOT_FLAG_BAD) {
                 BOOT_LOG_DBG("The copy_done flag had an unexpected value. Its "
                              "value was neither 'set' nor 'unset', but 'bad'.");
             }
@@ -2198,7 +2485,7 @@
             rc = boot_write_copy_done(fap);
             if (rc != 0) {
                 BOOT_LOG_WRN("Failed to set copy_done flag of the image in "
-                             "the %s slot.", (slot == BOOT_PRIMARY_SLOT) ?
+                             "the %s slot.", (active_slot == BOOT_PRIMARY_SLOT) ?
                              "primary" : "secondary");
                 rc = 0;
             }
@@ -2208,29 +2495,54 @@
 
     return rc;
 }
-#endif /* MCUBOOT_DIRECT_XIP_REVERT */
+#endif /* MCUBOOT_DIRECT_XIP && MCUBOOT_DIRECT_XIP_REVERT */
 
 #ifdef MCUBOOT_RAM_LOAD
 
+#ifndef MULTIPLE_EXECUTABLE_RAM_REGIONS
 #if !defined(IMAGE_EXECUTABLE_RAM_START) || !defined(IMAGE_EXECUTABLE_RAM_SIZE)
 #error "Platform MUST define executable RAM bounds in case of RAM_LOAD"
 #endif
+#endif
 
 /**
- * Verifies that the image in a slot will be loaded within the predefined bounds
- * that are allowed to be used by executable images.
+ * Verifies that the active slot of the current image can be loaded within the
+ * predefined bounds that are allowed to be used by executable images.
  *
- * @param img_dst         The address to which the image is going to be copied.
- * @param img_sz          The size of the image.
+ * @param  state        Boot loader status information.
+ * @param  slot_usage   Information about the active and available slots.
  *
- * @return                0 on success; nonzero on failure.
+ * @return              0 on success; nonzero on failure.
  */
 static int
-boot_verify_ram_load_address(uint32_t img_dst, uint32_t img_sz)
+boot_verify_ram_load_address(struct boot_loader_state *state,
+                             struct slot_usage_t slot_usage[])
 {
+    uint32_t img_dst;
+    uint32_t img_sz;
     uint32_t img_end_addr;
+    uint32_t exec_ram_start;
+    uint32_t exec_ram_size;
 
-    if (img_dst < IMAGE_EXECUTABLE_RAM_START) {
+    (void)state;
+
+#ifdef MULTIPLE_EXECUTABLE_RAM_REGIONS
+    int      rc;
+
+    rc = boot_get_image_exec_ram_info(BOOT_CURR_IMG(state), &exec_ram_start,
+                                      &exec_ram_size);
+    if (rc != 0) {
+        return BOOT_EBADSTATUS;
+    }
+#else
+    exec_ram_start = IMAGE_EXECUTABLE_RAM_START;
+    exec_ram_size = IMAGE_EXECUTABLE_RAM_SIZE;
+#endif
+
+    img_dst = slot_usage[BOOT_CURR_IMG(state)].img_dst;
+    img_sz = slot_usage[BOOT_CURR_IMG(state)].img_sz;
+
+    if (img_dst < exec_ram_start) {
         return BOOT_EBADIMAGE;
     }
 
@@ -2238,17 +2550,121 @@
         return BOOT_EBADIMAGE;
     }
 
-    if (img_end_addr > (IMAGE_EXECUTABLE_RAM_START +
-                        IMAGE_EXECUTABLE_RAM_SIZE)) {
+    if (img_end_addr > (exec_ram_start + exec_ram_size)) {
         return BOOT_EBADIMAGE;
     }
 
     return 0;
 }
 
+#ifdef MCUBOOT_ENC_IMAGES
+
 /**
- * Copies an image from a slot in the flash to an SRAM address.
+ * Copies and decrypts an image from a slot in the flash to an SRAM address.
  *
+ * @param  state    Boot loader status information.
+ * @param  slot     The flash slot of the image to be copied to SRAM.
+ * @param  hdr      The image header.
+ * @param  src_sz   Size of the image.
+ * @param  img_dst  Pointer to the address at which the image needs to be
+ *                  copied to SRAM.
+ *
+ * @return          0 on success; nonzero on failure.
+ */
+static int
+boot_decrypt_and_copy_image_to_sram(struct boot_loader_state *state,
+                                    uint32_t slot, struct image_header *hdr,
+                                    uint32_t src_sz, uint32_t img_dst)
+{
+    /* The flow for the decryption and copy of the image is as follows :
+     * 1. The whole image is copied to the RAM (header + payload + TLV).
+     * 2. The encryption key is loaded from the TLV in flash.
+     * 3. The image is then decrypted chunk by chunk in RAM (1 chunk
+     * is 1024 bytes). Only the payload section is decrypted.
+     * 4. The image is authenticated in RAM.
+     */
+    const struct flash_area *fap_src = NULL;
+    struct boot_status bs;
+    uint32_t blk_off;
+    uint32_t tlv_off;
+    uint32_t blk_sz;
+    uint32_t bytes_copied = hdr->ih_hdr_size;
+    uint32_t chunk_sz;
+    uint32_t max_sz = 1024;
+    uint16_t idx;
+    uint8_t image_index;
+    uint8_t * cur_dst;
+    int area_id;
+    int rc;
+    uint8_t * ram_dst = (void *)(IMAGE_RAM_BASE + img_dst);
+
+    image_index = BOOT_CURR_IMG(state);
+    area_id = flash_area_id_from_multi_image_slot(BOOT_CURR_IMG(state), slot);
+    rc = flash_area_open(area_id, &fap_src);
+    if (rc != 0){
+        return BOOT_EFLASH;
+    }
+
+    tlv_off = BOOT_TLV_OFF(hdr);
+
+    /* Copying the whole image in RAM */
+    rc = flash_area_read(fap_src, 0, ram_dst, src_sz);
+    if (rc != 0) {
+        goto done;
+    }
+
+    rc = boot_enc_load(BOOT_CURR_ENC(state), image_index, hdr, fap_src, &bs);
+    if (rc < 0) {
+        goto done;
+    }
+
+    /* if rc > 0 then the key has already been loaded */
+    if (rc == 0 && boot_enc_set_key(BOOT_CURR_ENC(state), slot, &bs)) {
+        goto done;
+    }
+
+    /* Starting at the end of the header as the header section is not encrypted */
+    while (bytes_copied < tlv_off) { /* TLV section copied previously */
+        if (src_sz - bytes_copied > max_sz) {
+            chunk_sz = max_sz;
+        } else {
+            chunk_sz = src_sz - bytes_copied;
+        }
+
+        cur_dst = ram_dst + bytes_copied;
+        blk_sz = chunk_sz;
+        idx = 0;
+        if (bytes_copied + chunk_sz > tlv_off) {
+            /* Going over TLV section
+             * Part of the chunk is encrypted payload */
+            blk_off = ((bytes_copied) - hdr->ih_hdr_size) & 0xf;
+            blk_sz = tlv_off - (bytes_copied);
+            boot_encrypt(BOOT_CURR_ENC(state), image_index, fap_src,
+                (bytes_copied + idx) - hdr->ih_hdr_size, blk_sz,
+                blk_off, cur_dst);
+        } else {
+            /* Image encrypted payload section */
+            blk_off = ((bytes_copied) - hdr->ih_hdr_size) & 0xf;
+            boot_encrypt(BOOT_CURR_ENC(state), image_index, fap_src,
+                    (bytes_copied + idx) - hdr->ih_hdr_size, blk_sz,
+                    blk_off, cur_dst);
+        }
+
+        bytes_copied += chunk_sz;
+    }
+    rc = 0;
+
+done:
+    flash_area_close(fap_src);
+
+    return rc;
+}
+
+#endif /* MCUBOOT_ENC_IMAGES */
+/**
+ * Copies a slot of the current image into SRAM.
+ *
+ * @param  state    Boot loader status information.
  * @param  slot     The flash slot of the image to be copied to SRAM.
  * @param  img_dst  The address at which the image needs to be copied to
  *                  SRAM.
@@ -2257,18 +2673,26 @@
  * @return          0 on success; nonzero on failure.
  */
 static int
-boot_copy_image_to_sram(int slot, uint32_t img_dst, uint32_t img_sz)
+boot_copy_image_to_sram(struct boot_loader_state *state, int slot,
+                        uint32_t img_dst, uint32_t img_sz)
 {
     int rc;
     const struct flash_area *fap_src = NULL;
+    int area_id;
 
-    rc = flash_area_open(flash_area_id_from_image_slot(slot), &fap_src);
+#if (BOOT_IMAGE_NUMBER == 1)
+    (void)state;
+#endif
+
+    area_id = flash_area_id_from_multi_image_slot(BOOT_CURR_IMG(state), slot);
+
+    rc = flash_area_open(area_id, &fap_src);
     if (rc != 0) {
         return BOOT_EFLASH;
     }
 
     /* Direct copy from flash to its new location in SRAM. */
-    rc = flash_area_read(fap_src, 0, (void *)img_dst, img_sz);
+    rc = flash_area_read(fap_src, 0, (void *)(IMAGE_RAM_BASE + img_dst), img_sz);
     if (rc != 0) {
         BOOT_LOG_INF("Error whilst copying image from Flash to SRAM: %d", rc);
     }
@@ -2278,50 +2702,142 @@
     return rc;
 }
 
+#if (BOOT_IMAGE_NUMBER > 1)
 /**
- * Copies an image from a slot in the flash to an SRAM address. The load
- * address and image size is extracted from the image header.
+ * Checks if two memory regions (A and B) are overlap or not.
  *
- * @param  state    Boot loader status information.
- * @param  slot     The flash slot of the image to be copied to SRAM.
- * @param  hdr      Pointer to the image header structure of the image
- * @param  img_dst  Pointer to the address at which the image needs to be
- *                  copied to SRAM.
- * @param  img_sz   Pointer to the size of the image that needs to be
- *                  copied to SRAM.
+ * @param  start_a  Start of the A region.
+ * @param  end_a    End of the A region.
+ * @param  start_b  Start of the B region.
+ * @param  end_b    End of the B region.
  *
- * @return          0 on success; nonzero on failure.
+ * @return          true if there is overlap; false otherwise.
+ */
+static bool
+do_regions_overlap(uint32_t start_a, uint32_t end_a,
+                   uint32_t start_b, uint32_t end_b)
+{
+    if (start_b > end_a) {
+        return false;
+    } else if (start_b >= start_a) {
+        return true;
+    } else if (end_b > start_a) {
+        return true;
+    }
+
+    return false;
+}
+
+/**
+ * Checks if the image we want to load to memory overlap with an already
+ * ramloaded image.
+ *
+ * @param  slot_usage         Information about the active and available slots.
+ * @param  image_id_to_check  The ID of the image we would like to load.
+ *
+ * @return                    0 if there is no overlap; nonzero otherwise.
  */
 static int
-boot_load_image_to_sram(struct boot_loader_state *state, uint32_t slot,
-                        struct image_header *hdr, uint32_t *img_dst,
-                        uint32_t *img_sz)
+boot_check_ram_load_overlapping(struct slot_usage_t slot_usage[],
+                                uint32_t image_id_to_check)
 {
+    uint32_t i;
+
+    uint32_t start_a;
+    uint32_t end_a;
+    uint32_t start_b;
+    uint32_t end_b;
+
+    start_a = slot_usage[image_id_to_check].img_dst;
+    /* Safe to add here, values are already verified in
+     * boot_verify_ram_load_address() */
+    end_a = start_a + slot_usage[image_id_to_check].img_sz;
+
+    for (i = 0; i < BOOT_IMAGE_NUMBER; i++) {
+        if (slot_usage[i].active_slot == NO_ACTIVE_SLOT
+            || i == image_id_to_check) {
+            continue;
+        }
+
+        start_b = slot_usage[i].img_dst;
+        /* Safe to add here, values are already verified in
+         * boot_verify_ram_load_address() */
+        end_b = start_b + slot_usage[i].img_sz;
+
+        if (do_regions_overlap(start_a, end_a, start_b, end_b)) {
+            return -1;
+        }
+    }
+
+    return 0;
+}
+#endif
+
+/**
+ * Loads the active slot of the current image into SRAM. The load address and
+ * image size is extracted from the image header.
+ *
+ * @param  state        Boot loader status information.
+ * @param  slot_usage   Information about the active and available slots.
+ *
+ * @return              0 on success; nonzero on failure.
+ */
+static int
+boot_load_image_to_sram(struct boot_loader_state *state,
+                        struct slot_usage_t slot_usage[])
+{
+    uint32_t active_slot;
+    struct image_header *hdr = NULL;
+    uint32_t img_dst;
+    uint32_t img_sz;
     int rc;
 
+    active_slot = slot_usage[BOOT_CURR_IMG(state)].active_slot;
+    hdr = boot_img_hdr(state, active_slot);
+
     if (hdr->ih_flags & IMAGE_F_RAM_LOAD) {
 
-        *img_dst = hdr->ih_load_addr;
+        img_dst = hdr->ih_load_addr;
 
-        rc = boot_read_image_size(state, slot, img_sz);
+        rc = boot_read_image_size(state, active_slot, &img_sz);
         if (rc != 0) {
             return rc;
         }
 
-        rc = boot_verify_ram_load_address(*img_dst, *img_sz);
+        slot_usage[BOOT_CURR_IMG(state)].img_dst = img_dst;
+        slot_usage[BOOT_CURR_IMG(state)].img_sz = img_sz;
+
+        rc = boot_verify_ram_load_address(state, slot_usage);
         if (rc != 0) {
-            BOOT_LOG_INF("Image RAM load address 0x%x is invalid.", *img_dst);
+            BOOT_LOG_INF("Image RAM load address 0x%" PRIx32 " is invalid.", img_dst);
             return rc;
         }
 
+#if (BOOT_IMAGE_NUMBER > 1)
+        rc = boot_check_ram_load_overlapping(slot_usage, BOOT_CURR_IMG(state));
+        if (rc != 0) {
+            BOOT_LOG_INF("Image RAM loading to address 0x%" PRIx32
+                         " would overlap with another image.", img_dst);
+            return rc;
+        }
+#endif
+#ifdef MCUBOOT_ENC_IMAGES
+        /* decrypt image if encrypted and copy it to RAM */
+        if (IS_ENCRYPTED(hdr)) {
+            rc = boot_decrypt_and_copy_image_to_sram(state, active_slot, hdr, img_sz, img_dst);
+        } else {
+            rc = boot_copy_image_to_sram(state, active_slot, img_dst, img_sz);
+        }
+#else
         /* Copy image to the load address from where it currently resides in
          * flash.
          */
-        rc = boot_copy_image_to_sram(slot, *img_dst, *img_sz);
+        rc = boot_copy_image_to_sram(state, active_slot, img_dst, img_sz);
+#endif
         if (rc != 0) {
-            BOOT_LOG_INF("RAM loading to 0x%x is failed.", *img_dst);
+            BOOT_LOG_INF("RAM loading to 0x%" PRIx32 " is failed.", img_dst);
         } else {
-            BOOT_LOG_INF("RAM loading to 0x%x is succeeded.", *img_dst);
+            BOOT_LOG_INF("RAM loading to 0x%" PRIx32 " is succeeded.", img_dst);
         }
     } else {
         /* Only images that support IMAGE_F_RAM_LOAD are allowed if
@@ -2330,114 +2846,277 @@
         rc = BOOT_EBADIMAGE;
     }
 
+    if (rc != 0) {
+        slot_usage[BOOT_CURR_IMG(state)].img_dst = 0;
+        slot_usage[BOOT_CURR_IMG(state)].img_sz = 0;
+    }
+
     return rc;
 }
 
 /**
  * Removes an image from SRAM, by overwriting it with zeros.
  *
- * @param  img_dst  The address of the image that needs to be removed from
- *                  SRAM.
- * @param  img_sz   The size of the image that needs to be removed from
- *                  SRAM.
+ * @param  state        Boot loader status information.
+ * @param  slot_usage   Information about the active and available slots.
+ *
+ * @return              0 on success; nonzero on failure.
+ */
+static inline int
+boot_remove_image_from_sram(struct boot_loader_state *state,
+                            struct slot_usage_t slot_usage[])
+{
+    (void)state;
+
+    BOOT_LOG_INF("Removing image from SRAM at address 0x%" PRIx32,
+                 slot_usage[BOOT_CURR_IMG(state)].img_dst);
+
+    memset((void*)(IMAGE_RAM_BASE + slot_usage[BOOT_CURR_IMG(state)].img_dst),
+           0, slot_usage[BOOT_CURR_IMG(state)].img_sz);
+
+    slot_usage[BOOT_CURR_IMG(state)].img_dst = 0;
+    slot_usage[BOOT_CURR_IMG(state)].img_sz = 0;
+
+    return 0;
+}
+
+/**
+ * Removes an image from flash by erasing the corresponding flash area
+ *
+ * @param  state    Boot loader status information.
+ * @param  slot     The flash slot of the image to be erased.
  *
  * @return          0 on success; nonzero on failure.
  */
 static inline int
-boot_remove_image_from_sram(uint32_t img_dst, uint32_t img_sz)
+boot_remove_image_from_flash(struct boot_loader_state *state, uint32_t slot)
 {
-    BOOT_LOG_INF("Removing image from SRAM at address 0x%x", img_dst);
-    memset((void*)img_dst, 0, img_sz);
+    int area_id;
+    int rc;
+    const struct flash_area *fap;
 
-    return 0;
+    (void)state;
+
+    BOOT_LOG_INF("Removing image %u slot %" PRIu32 " from flash",
+                 (unsigned)BOOT_CURR_IMG(state), slot);
+    area_id = flash_area_id_from_multi_image_slot(BOOT_CURR_IMG(state), slot);
+    rc = flash_area_open(area_id, &fap);
+    if (rc == 0) {
+        flash_area_erase(fap, 0, flash_area_get_size(fap));
+    }
+
+    return rc;
 }
 #endif /* MCUBOOT_RAM_LOAD */
 
-fih_int
-context_boot_go(struct boot_loader_state *state, struct boot_rsp *rsp)
+#if (BOOT_IMAGE_NUMBER > 1)
+/**
+ * Checks the image dependency whether it is satisfied.
+ *
+ * @param  state        Boot loader status information.
+ * @param  slot_usage   Information about the active and available slots.
+ * @param  dep          Image dependency which has to be verified.
+ *
+ * @return              0 if dependencies are met; nonzero otherwise.
+ */
+static int
+boot_verify_slot_dependency(struct boot_loader_state *state,
+                            struct slot_usage_t  slot_usage[],
+                            struct image_dependency *dep)
 {
-    struct image_header *hdr = NULL;
-    struct image_header *selected_image_header = NULL;
-    uint8_t slot_usage[BOOT_NUM_SLOTS];
-    uint32_t selected_slot;
-    uint32_t slot;
-    uint32_t img_cnt;
-    uint32_t i;
-    int fa_id;
+    struct image_version *dep_version;
+    uint32_t dep_slot;
     int rc;
-#ifdef MCUBOOT_RAM_LOAD
-    uint32_t img_dst;
-    uint32_t img_sz;
-    uint32_t img_loaded = 0;
-#endif /* MCUBOOT_RAM_LOAD */
-#ifdef MCUBOOT_DIRECT_XIP_REVERT
-    struct boot_swap_state slot_state;
-#endif /* MCUBOOT_DIRECT_XIP_REVERT */
-    fih_int fih_rc = FIH_FAILURE;
 
-    memset(state, 0, sizeof(struct boot_loader_state));
-
-    /* Open primary and secondary image areas for the duration
-     * of this call.
+    /* Determine the source of the image which is the subject of
+     * the dependency and get it's version.
      */
-    for (slot = 0; slot < BOOT_NUM_SLOTS; slot++) {
-        fa_id = flash_area_id_from_image_slot(slot);
-        rc = flash_area_open(fa_id, &BOOT_IMG_AREA(state, slot));
-        assert(rc == 0);
+    dep_slot = slot_usage[dep->image_id].active_slot;
+    dep_version = &state->imgs[dep->image_id][dep_slot].hdr.ih_ver;
+
+    rc = boot_version_cmp(dep_version, &dep->image_min_version);
+    if (rc >= 0) {
+        /* Dependency satisfied. */
+        rc = 0;
     }
 
-    /* Attempt to read an image header from each slot. */
-    rc = boot_read_image_headers(state, false, NULL);
+    return rc;
+}
+
+/**
+ * Reads all dependency TLVs of an image and verifies one after another to see
+ * if they are all satisfied.
+ *
+ * @param  state        Boot loader status information.
+ * @param  slot_usage   Information about the active and available slots.
+ *
+ * @return              0 if dependencies are met; nonzero otherwise.
+ */
+static int
+boot_verify_slot_dependencies(struct boot_loader_state *state,
+                              struct slot_usage_t slot_usage[])
+{
+    uint32_t active_slot;
+    const struct flash_area *fap;
+    struct image_tlv_iter it;
+    struct image_dependency dep;
+    uint32_t off;
+    uint16_t len;
+    int area_id;
+    int rc;
+
+    active_slot = slot_usage[BOOT_CURR_IMG(state)].active_slot;
+
+    area_id = flash_area_id_from_multi_image_slot(BOOT_CURR_IMG(state),
+                                                                active_slot);
+    rc = flash_area_open(area_id, &fap);
     if (rc != 0) {
-        BOOT_LOG_WRN("Failed reading image headers.");
-        goto out;
+        rc = BOOT_EFLASH;
+        goto done;
     }
 
-    img_cnt = boot_get_slot_usage(state, slot_usage,
-                                  sizeof(slot_usage)/sizeof(slot_usage[0]));
+    rc = bootutil_tlv_iter_begin(&it, boot_img_hdr(state, active_slot), fap,
+            IMAGE_TLV_DEPENDENCY, true);
+    if (rc != 0) {
+        goto done;
+    }
 
-    if (img_cnt) {
-        /* Select the newest and valid image. */
-        for (i = 0; i < img_cnt; i++) {
-            selected_slot = 0;
+    while (true) {
+        rc = bootutil_tlv_iter_next(&it, &off, &len, NULL);
+        if (rc < 0) {
+            return -1;
+        } else if (rc > 0) {
+            rc = 0;
+            break;
+        }
 
-            /* Iterate over all the slots that are in use (contain an image)
-             * and select the one that holds the newest image.
-             */
-            for (slot = 0; slot < BOOT_NUM_SLOTS; slot++) {
-                if (slot_usage[slot]) {
-                    hdr = boot_img_hdr(state, slot);
-                    if (selected_image_header != NULL) {
-                        rc = boot_version_cmp(&hdr->ih_ver,
-                                              &selected_image_header->ih_ver);
-                        if (rc < 1) {
-                            /* The version of the image being examined wasn't
-                             * greater than the currently selected image's
-                             * version.
-                             */
-                            continue;
-                        }
-                    }
-                    /* Check if image has IMAGE_F_ROM_FIXED flag set and
-                     * is in proper slot.
-                     */
-                    if (boot_rom_address_check(state, slot) != 0) {
-                        continue;
-                    }
-                    selected_slot = slot;
-                    selected_image_header = hdr;
-                }
+        if (len != sizeof(dep)) {
+            rc = BOOT_EBADIMAGE;
+            goto done;
+        }
+
+        rc = LOAD_IMAGE_DATA(boot_img_hdr(state, active_slot),
+                             fap, off, &dep, len);
+        if (rc != 0) {
+            rc = BOOT_EFLASH;
+            goto done;
+        }
+
+        if (dep.image_id >= BOOT_IMAGE_NUMBER) {
+            rc = BOOT_EBADARGS;
+            goto done;
+        }
+
+        rc = boot_verify_slot_dependency(state, slot_usage, &dep);
+        if (rc != 0) {
+            /* Dependency not satisfied. */
+            goto done;
+        }
+    }
+
+done:
+    flash_area_close(fap);
+    return rc;
+}
+
+/**
+ * Checks the dependency of all the active slots. If an image found with
+ * invalid or not satisfied dependencies the image is removed from SRAM (in
+ * case of MCUBOOT_RAM_LOAD strategy) and its slot is set to unavailable.
+ *
+ * @param  state        Boot loader status information.
+ * @param  slot_usage   Information about the active and available slots.
+ *
+ * @return              0 if dependencies are met; nonzero otherwise.
+ */
+static int
+boot_verify_dependencies(struct boot_loader_state *state,
+                         struct slot_usage_t slot_usage[])
+{
+    int rc = -1;
+    uint32_t active_slot;
+
+    IMAGES_ITER(BOOT_CURR_IMG(state)) {
+        rc = boot_verify_slot_dependencies(state, slot_usage);
+        if (rc != 0) {
+            /* Dependencies not met or invalid dependencies. */
+
+#ifdef MCUBOOT_RAM_LOAD
+            boot_remove_image_from_sram(state, slot_usage);
+#endif /* MCUBOOT_RAM_LOAD */
+
+            active_slot = slot_usage[BOOT_CURR_IMG(state)].active_slot;
+            slot_usage[BOOT_CURR_IMG(state)].slot_available[active_slot] = false;
+            slot_usage[BOOT_CURR_IMG(state)].active_slot = NO_ACTIVE_SLOT;
+
+            return rc;
+        }
+    }
+
+    return rc;
+}
+#endif /* (BOOT_IMAGE_NUMBER > 1) */
+
+/**
+ * Tries to load a slot for all the images with validation.
+ *
+ * @param  state        Boot loader status information.
+ * @param  slot_usage   Information about the active and available slots.
+ *
+ * @return              0 on success; nonzero on failure.
+ */
+fih_int
+boot_load_and_validate_images(struct boot_loader_state *state,
+                              struct slot_usage_t slot_usage[])
+{
+    uint32_t active_slot;
+    int rc;
+    fih_int fih_rc;
+
+    /* Go over all the images and try to load one */
+    IMAGES_ITER(BOOT_CURR_IMG(state)) {
+        /* All slots tried until a valid image found. Breaking from this loop
+         * means that a valid image found or already loaded. If no slot is
+         * found the function returns with error code. */
+        while (true) {
+
+            /* Go over all the slots and try to load one */
+            active_slot = slot_usage[BOOT_CURR_IMG(state)].active_slot;
+            if (active_slot != NO_ACTIVE_SLOT){
+                /* A slot is already active, go to next image. */
+                break;
+            }
+
+            active_slot = find_slot_with_highest_version(state,
+                                                         slot_usage);
+            if (active_slot == NO_ACTIVE_SLOT) {
+                BOOT_LOG_INF("No slot to load for image %u",
+                             (unsigned)BOOT_CURR_IMG(state));
+                FIH_RET(FIH_FAILURE);
+            }
+
+            /* Save the number of the active slot. */
+            slot_usage[BOOT_CURR_IMG(state)].active_slot = active_slot;
+
+#ifdef MCUBOOT_DIRECT_XIP
+            rc = boot_rom_address_check(state, slot_usage);
+            if (rc != 0) {
+                /* The image is placed in an unsuitable slot. */
+                slot_usage[BOOT_CURR_IMG(state)].slot_available[active_slot] = false;
+                slot_usage[BOOT_CURR_IMG(state)].active_slot = NO_ACTIVE_SLOT;
+                continue;
             }
 
 #ifdef MCUBOOT_DIRECT_XIP_REVERT
-            rc = boot_select_or_erase(&slot_state, selected_slot);
+            rc = boot_select_or_erase(state, slot_usage);
             if (rc != 0) {
                 /* The selected image slot has been erased. */
-                slot_usage[selected_slot] = 0;
-                selected_image_header = NULL;
+                slot_usage[BOOT_CURR_IMG(state)].slot_available[active_slot] = false;
+                slot_usage[BOOT_CURR_IMG(state)].active_slot = NO_ACTIVE_SLOT;
                 continue;
             }
 #endif /* MCUBOOT_DIRECT_XIP_REVERT */
+#endif /* MCUBOOT_DIRECT_XIP */
 
 #ifdef MCUBOOT_RAM_LOAD
             /* Image is first loaded to RAM and authenticated there in order to
@@ -2445,111 +3124,146 @@
              * when loading images from external (untrusted) flash to internal
              * (trusted) RAM and image is authenticated before copying.
              */
-            rc = boot_load_image_to_sram(state, selected_slot,
-                                         selected_image_header, &img_dst,
-                                         &img_sz);
+            rc = boot_load_image_to_sram(state, slot_usage);
             if (rc != 0 ) {
-                /* Image loading failed try the next one. */
-                slot_usage[selected_slot] = 0;
-                selected_image_header = NULL;
+                /* Image cannot be ramloaded. */
+                boot_remove_image_from_flash(state, active_slot);
+                slot_usage[BOOT_CURR_IMG(state)].slot_available[active_slot] = false;
+                slot_usage[BOOT_CURR_IMG(state)].active_slot = NO_ACTIVE_SLOT;
                 continue;
-            } else {
-                img_loaded = 1;
             }
 #endif /* MCUBOOT_RAM_LOAD */
-            FIH_CALL(boot_validate_slot, fih_rc, state, selected_slot, NULL);
-            if (fih_eq(fih_rc, FIH_SUCCESS)) {
-                /* If a valid image is found then there is no reason to check
-                 * the rest of the images, as each of them has a smaller version
-                 * number.
-                 */
-                break;
-            }
+
+            FIH_CALL(boot_validate_slot, fih_rc, state, active_slot, NULL);
+            if (fih_not_eq(fih_rc, FIH_SUCCESS)) {
+                /* Image is invalid. */
 #ifdef MCUBOOT_RAM_LOAD
-            else if (img_loaded) {
-                /* If an image is found to be invalid then it is removed from
-                 * RAM to prevent it being a shellcode vector.
-                 */
-                boot_remove_image_from_sram(img_dst, img_sz);
-                img_loaded = 0;
-            }
+                boot_remove_image_from_sram(state, slot_usage);
 #endif /* MCUBOOT_RAM_LOAD */
-            /* The selected image is invalid, mark its slot as "unused"
-             * and start over.
-             */
-            slot_usage[selected_slot] = 0;
-            selected_image_header = NULL;
-        }
-
-        if (fih_not_eq(fih_rc, FIH_SUCCESS) || (selected_image_header == NULL)) {
-            /* If there was no valid image at all */
-            goto out;
-        }
-
-#ifdef MCUBOOT_HW_ROLLBACK_PROT
-        /* Update the stored security counter with the newer (active) image's
-         * security counter value.
-         */
-#ifdef MCUBOOT_DIRECT_XIP_REVERT
-        /* When the 'revert' mechanism is enabled in direct-xip mode, the
-         * security counter can be increased only after reboot, if the image
-         * has been confirmed at runtime (the image_ok flag has been set).
-         * This way a 'revert' can be performed when it's necessary.
-         */
-        if (slot_state.image_ok == BOOT_FLAG_SET) {
-#endif
-            rc = boot_update_security_counter(0, selected_slot,
-                                              selected_image_header);
-            if (rc != 0) {
-                BOOT_LOG_ERR("Security counter update failed after image "
-                             "validation.");
-                goto out;
+                slot_usage[BOOT_CURR_IMG(state)].slot_available[active_slot] = false;
+                slot_usage[BOOT_CURR_IMG(state)].active_slot = NO_ACTIVE_SLOT;
+                continue;
             }
-#ifdef MCUBOOT_DIRECT_XIP_REVERT
+
+            /* Valid image loaded from a slot, go to next image. */
+            break;
         }
+    }
+
+    FIH_RET(FIH_SUCCESS);
+}
+
+/**
+ * Updates the security counter for the current image.
+ *
+ * @param  state        Boot loader status information.
+ * @param  slot_usage   Information about the active and available slots.
+ *
+ * @return              0 on success; nonzero on failure.
+ */
+static int
+boot_update_hw_rollback_protection(struct boot_loader_state *state,
+                                   const struct slot_usage_t slot_usage[])
+{
+#ifdef MCUBOOT_HW_ROLLBACK_PROT
+    int rc;
+
+    /* Update the stored security counter with the newer (active) image's
+     * security counter value.
+     */
+#if defined(MCUBOOT_DIRECT_XIP) && defined(MCUBOOT_DIRECT_XIP_REVERT)
+    /* When the 'revert' mechanism is enabled in direct-xip mode, the
+     * security counter can be increased only after reboot, if the image
+     * has been confirmed at runtime (the image_ok flag has been set).
+     * This way a 'revert' can be performed when it's necessary.
+     */
+    if (slot_usage[BOOT_CURR_IMG(state)].swap_state.image_ok == BOOT_FLAG_SET) {
 #endif
-#endif /* MCUBOOT_HW_ROLLBACK_PROT */
-
-#ifdef MCUBOOT_MEASURED_BOOT
-        rc = boot_save_boot_status(0, selected_image_header,
-                                   BOOT_IMG_AREA(state, selected_slot));
+        rc = boot_update_security_counter(BOOT_CURR_IMG(state),
+                                          slot_usage[BOOT_CURR_IMG(state)].active_slot,
+                                          boot_img_hdr(state, slot_usage[BOOT_CURR_IMG(state)].active_slot));
         if (rc != 0) {
-            BOOT_LOG_ERR("Failed to add image data to shared area");
+            BOOT_LOG_ERR("Security counter update failed after image "
+                            "validation.");
+            return rc;
         }
-#endif /* MCUBOOT_MEASURED_BOOT */
+#if defined(MCUBOOT_DIRECT_XIP) && defined(MCUBOOT_DIRECT_XIP_REVERT)
+    }
+#endif
 
-#ifdef MCUBOOT_DATA_SHARING
-        rc = boot_save_shared_data(selected_image_header,
-                                   BOOT_IMG_AREA(state, selected_slot));
-        if (rc != 0) {
-            BOOT_LOG_ERR("Failed to add data to shared memory area.");
-        }
-#endif /* MCUBOOT_DATA_SHARING */
+    return 0;
 
-        BOOT_LOG_INF("Booting image from the %s slot",
-                     (selected_slot == BOOT_PRIMARY_SLOT) ?
-                     "primary" : "secondary");
+#else /* MCUBOOT_HW_ROLLBACK_PROT */
+    (void) (state);
+    (void) (slot_usage);
+    return 0;
+#endif
+}
 
-        rsp->br_flash_dev_id =
-            BOOT_IMG_AREA(state, selected_slot)->fa_device_id;
-        rsp->br_image_off = boot_img_slot_off(state, selected_slot);
-        rsp->br_hdr = selected_image_header;
-    } else {
-        /* No candidate image available */
-        rc = BOOT_EBADIMAGE;
+fih_int
+context_boot_go(struct boot_loader_state *state, struct boot_rsp *rsp)
+{
+    struct slot_usage_t slot_usage[BOOT_IMAGE_NUMBER];
+    int rc;
+    fih_int fih_rc = fih_int_encode(0);
+
+    memset(state, 0, sizeof(struct boot_loader_state));
+    memset(slot_usage, 0, sizeof(struct slot_usage_t) * BOOT_IMAGE_NUMBER);
+
+    rc = boot_get_slot_usage(state, slot_usage);
+    if (rc != 0) {
         goto out;
     }
 
+#if (BOOT_IMAGE_NUMBER > 1)
+    while (true) {
+#endif
+        FIH_CALL(boot_load_and_validate_images, fih_rc, state, slot_usage);
+        if (fih_not_eq(fih_rc, FIH_SUCCESS)) {
+            goto out;
+        }
+
+#if (BOOT_IMAGE_NUMBER > 1)
+        rc = boot_verify_dependencies(state, slot_usage);
+        if (rc != 0) {
+            /* Dependency check failed for an image, it has been removed from
+             * SRAM in case of MCUBOOT_RAM_LOAD strategy, and set to
+             * unavailable. Try to load an image from another slot.
+             */
+            continue;
+        }
+        /* Dependency check was successful. */
+        break;
+    }
+#endif
+
+    IMAGES_ITER(BOOT_CURR_IMG(state)) {
+        rc = boot_update_hw_rollback_protection(state, slot_usage);
+        if (rc != 0) {
+            goto out;
+        }
+
+        rc = boot_add_shared_data(state, slot_usage[BOOT_CURR_IMG(state)].active_slot);
+        if (rc != 0) {
+            goto out;
+        }
+    }
+
+    /* All image loaded successfully. */
+#ifdef MCUBOOT_HAVE_LOGGING
+    print_loaded_images(state, slot_usage);
+#endif
+
+    fill_rsp(state, slot_usage, rsp);
+
 out:
-   for (slot = 0; slot < BOOT_NUM_SLOTS; slot++) {
-       flash_area_close(BOOT_IMG_AREA(state, BOOT_NUM_SLOTS - 1 - slot));
-   }
+    close_all_flash_areas(state);
 
-   if (rc) {
-       fih_rc = fih_int_encode(rc);
-   }
+    if (fih_eq(fih_rc, FIH_SUCCESS)) {
+        fih_rc = fih_int_encode(rc);
+    }
 
-   FIH_RET(fih_rc);
+    FIH_RET(fih_rc);
 }
 #endif /* MCUBOOT_DIRECT_XIP || MCUBOOT_RAM_LOAD */
 
diff --git a/boot/bootutil/src/swap_misc.c b/boot/bootutil/src/swap_misc.c
index 42ae7a4..13af238 100644
--- a/boot/bootutil/src/swap_misc.c
+++ b/boot/bootutil/src/swap_misc.c
@@ -28,7 +28,7 @@
 
 #include "mcuboot_config/mcuboot_config.h"
 
-MCUBOOT_LOG_MODULE_DECLARE(mcuboot);
+BOOT_LOG_MODULE_DECLARE(mcuboot);
 
 #if defined(MCUBOOT_SWAP_USING_SCRATCH) || defined(MCUBOOT_SWAP_USING_MOVE)
 
@@ -49,7 +49,7 @@
     uint8_t image_index;
     int rc;
 
-    BOOT_LOG_DBG("erasing trailer; fa_id=%d", fap->fa_id);
+    BOOT_LOG_DBG("erasing trailer; fa_id=%u", (unsigned)flash_area_get_id(fap));
 
     image_index = BOOT_CURR_IMG(state);
     fa_id_primary = flash_area_id_from_multi_image_slot(image_index,
@@ -57,9 +57,9 @@
     fa_id_secondary = flash_area_id_from_multi_image_slot(image_index,
             BOOT_SECONDARY_SLOT);
 
-    if (fap->fa_id == fa_id_primary) {
+    if (flash_area_get_id(fap) == fa_id_primary) {
         slot = BOOT_PRIMARY_SLOT;
-    } else if (fap->fa_id == fa_id_secondary) {
+    } else if (flash_area_get_id(fap) == fa_id_secondary) {
         slot = BOOT_SECONDARY_SLOT;
     } else {
         return BOOT_EFLASH;
@@ -97,7 +97,7 @@
 
     image_index = BOOT_CURR_IMG(state);
 
-    BOOT_LOG_DBG("initializing status; fa_id=%d", fap->fa_id);
+    BOOT_LOG_DBG("initializing status; fa_id=%u", (unsigned)flash_area_get_id(fap));
 
     rc = boot_read_swap_state_by_id(FLASH_AREA_IMAGE_SECONDARY(image_index),
             &swap_state);
@@ -167,12 +167,12 @@
     rc = swap_read_status_bytes(fap, state, bs);
     if (rc == 0) {
         off = boot_swap_info_off(fap);
-                rc = flash_area_read(fap, off, &swap_info, sizeof swap_info);
+        rc = flash_area_read(fap, off, &swap_info, sizeof swap_info);
         if (rc != 0) {
             return BOOT_EFLASH;
         }
 
-        if (bootutil_buffer_is_erased(fap, &swap_info, sizeof swap_info)) {
+        if (swap_info == flash_area_erased_val(fap)) {
             BOOT_SET_SWAP_INFO(swap_info, 0, BOOT_SWAP_TYPE_NONE);
             rc = 0;
         }
@@ -190,11 +190,10 @@
 int
 swap_set_copy_done(uint8_t image_index)
 {
-    const struct flash_area *fap;
+    const struct flash_area *fap = NULL;
     int rc;
 
-    rc = flash_area_open(FLASH_AREA_IMAGE_PRIMARY(image_index),
-            &fap);
+    rc = flash_area_open(FLASH_AREA_IMAGE_PRIMARY(image_index), &fap);
     if (rc != 0) {
         return BOOT_EFLASH;
     }
@@ -207,12 +206,11 @@
 int
 swap_set_image_ok(uint8_t image_index)
 {
-    const struct flash_area *fap;
-    struct boot_swap_state state;
+    const struct flash_area *fap = NULL;
+    struct boot_swap_state state = {0};
     int rc;
 
-    rc = flash_area_open(FLASH_AREA_IMAGE_PRIMARY(image_index),
-            &fap);
+    rc = flash_area_open(FLASH_AREA_IMAGE_PRIMARY(image_index), &fap);
     if (rc != 0) {
         return BOOT_EFLASH;
     }
diff --git a/boot/bootutil/src/swap_move.c b/boot/bootutil/src/swap_move.c
index 8fe0b44..08c21da 100644
--- a/boot/bootutil/src/swap_move.c
+++ b/boot/bootutil/src/swap_move.c
@@ -31,7 +31,7 @@
 
 #include "mcuboot_config/mcuboot_config.h"
 
-MCUBOOT_LOG_MODULE_DECLARE(mcuboot);
+BOOT_LOG_MODULE_DECLARE(mcuboot);
 
 #ifdef MCUBOOT_SWAP_USING_MOVE
 
@@ -128,7 +128,7 @@
     uint8_t status;
     int max_entries;
     int found_idx;
-    uint8_t write_sz;
+    uint32_t write_sz;
     int move_entries;
     int rc;
     int last_rc;
@@ -152,7 +152,7 @@
             return BOOT_EFLASH;
         }
 
-        if (bootutil_buffer_is_erased(fap, &status, 1)) {
+        if (flash_area_erased_val(fap) == status) {
             if (rc != last_rc) {
                 erased_sections++;
             }
@@ -197,7 +197,7 @@
 }
 
 uint32_t
-boot_status_internal_off(const struct boot_status *bs, int elem_sz)
+boot_status_internal_off(const struct boot_status *bs, uint32_t elem_sz)
 {
     uint32_t off;
     int idx_sz;
@@ -228,7 +228,7 @@
     num_sectors_sec = boot_img_num_sectors(state, BOOT_SECONDARY_SLOT);
 
     if (num_sectors_sec == 0) {
-        BOOT_LOG_WRN("Upgrade disabled for image %d", BOOT_CURR_IMG(state));
+        BOOT_LOG_WRN("Upgrade disabled for image %u", (unsigned)BOOT_CURR_IMG(state));
         return 0;
     }
 
@@ -262,16 +262,15 @@
     return 1;
 }
 
-#define BOOT_LOG_SWAP_STATE(area, state)                            \
-    BOOT_LOG_INF("%s: magic=%s, swap_type=0x%x, copy_done=0x%x, "   \
-                 "image_ok=0x%x",                                   \
-                 (area),                                            \
-                 ((state)->magic == BOOT_MAGIC_GOOD ? "good" :      \
-                  (state)->magic == BOOT_MAGIC_UNSET ? "unset" :    \
-                  "bad"),                                           \
-                 (state)->swap_type,                                \
-                 (state)->copy_done,                                \
-                 (state)->image_ok)
+#define BOOT_LOG_SWAP_STATE(area, state)                                        \
+    BOOT_LOG_INF("%s: magic=%s, swap_type=0x%x, copy_done=0x%x, image_ok=0x%x", \
+                 (area),                                                        \
+                 ((state)->magic == BOOT_MAGIC_GOOD  ? "good" :                 \
+                  (state)->magic == BOOT_MAGIC_UNSET ? "unset" :                \
+                                                       "bad"),                  \
+                 (unsigned)(state)->swap_type,                                  \
+                 (unsigned)(state)->copy_done,                                  \
+                 (unsigned)(state)->image_ok)
 
 int
 swap_status_source(struct boot_loader_state *state)
@@ -412,7 +411,7 @@
  */
 void
 fixup_revert(const struct boot_loader_state *state, struct boot_status *bs,
-        const struct flash_area *fap_sec, uint8_t sec_id)
+        const struct flash_area *fap_sec)
 {
     struct boot_swap_state swap_state;
     int rc;
@@ -428,7 +427,7 @@
         return;
     }
 
-    rc = boot_read_swap_state_by_id(sec_id, &swap_state);
+    rc = boot_read_swap_state(fap_sec, &swap_state);
     assert(rc == 0);
 
     BOOT_LOG_SWAP_STATE("Secondary image", &swap_state);
@@ -509,7 +508,7 @@
     rc = flash_area_open(FLASH_AREA_IMAGE_SECONDARY(image_index), &fap_sec);
     assert (rc == 0);
 
-    fixup_revert(state, bs, fap_sec, FLASH_AREA_IMAGE_SECONDARY(image_index));
+    fixup_revert(state, bs, fap_sec);
 
     if (bs->op == BOOT_STATUS_OP_MOVE) {
         idx = g_last_idx;
diff --git a/boot/bootutil/src/swap_priv.h b/boot/bootutil/src/swap_priv.h
index 9cf5777..86d0b72 100644
--- a/boot/bootutil/src/swap_priv.h
+++ b/boot/bootutil/src/swap_priv.h
@@ -95,7 +95,7 @@
 
 static inline size_t boot_scratch_area_size(const struct boot_loader_state *state)
 {
-    return BOOT_SCRATCH_AREA(state)->fa_size;
+    return flash_area_get_size(BOOT_SCRATCH_AREA(state));
 }
 #endif
 
diff --git a/boot/bootutil/src/swap_scratch.c b/boot/bootutil/src/swap_scratch.c
index 9b9228a..fea6ce1 100644
--- a/boot/bootutil/src/swap_scratch.c
+++ b/boot/bootutil/src/swap_scratch.c
@@ -31,7 +31,7 @@
 
 #include "mcuboot_config/mcuboot_config.h"
 
-MCUBOOT_LOG_MODULE_DECLARE(mcuboot);
+BOOT_LOG_MODULE_DECLARE(mcuboot);
 
 #ifndef MCUBOOT_SWAP_USING_MOVE
 
@@ -54,7 +54,7 @@
 boot_read_image_header(struct boot_loader_state *state, int slot,
                        struct image_header *out_hdr, struct boot_status *bs)
 {
-    const struct flash_area *fap;
+    const struct flash_area *fap = NULL;
     int area_id;
     int rc;
 
@@ -74,9 +74,21 @@
         }
         else if (bs->state == BOOT_STATUS_STATE_2) {
             if (slot == 0) {
+#if MCUBOOT_SWAP_USING_SCRATCH
+                /* encrypted scratch partition needs area wrapper */
+                uint32_t image_proc_size = boot_scratch_area_size(state) * bs->idx;
+                uint32_t primary_img_size = boot_img_hdr(state, BOOT_PRIMARY_SLOT)->ih_img_size;
+                uint32_t secondary_img_size = boot_img_hdr(state, BOOT_SECONDARY_SLOT)->ih_img_size;
+                slot = 2;
+
+                if (secondary_img_size >= primary_img_size &&
+                    secondary_img_size - primary_img_size < image_proc_size) {
+                    slot = 1;
+                }
+#else
                 slot = 1;
-            }
-            else {
+#endif
+            } else {
                 slot = 2;
             }
         }
@@ -89,16 +101,12 @@
         goto done;
     }
 
-    rc = flash_area_read_is_empty(fap, 0, out_hdr, sizeof *out_hdr);
+    rc = flash_area_read(fap, 0, out_hdr, sizeof *out_hdr);
     if (rc < 0) {
         rc = BOOT_EFLASH;
         goto done;
     }
 
-    if (rc == 1) {
-        memset(out_hdr, 0, sizeof(*out_hdr));
-    }
-
     /* We only know where the headers are located when bs is valid */
     if (bs != NULL && out_hdr->ih_magic != IMAGE_MAGIC) {
 
@@ -170,7 +178,7 @@
             return BOOT_EFLASH;
         }
 
-        if (bootutil_buffer_is_erased(fap, &status, 1)) {
+        if (status == flash_area_erased_val(fap)) {
             if (found && !found_idx) {
                 found_idx = i;
             }
@@ -210,7 +218,7 @@
 }
 
 uint32_t
-boot_status_internal_off(const struct boot_status *bs, int elem_sz)
+boot_status_internal_off(const struct boot_status *bs, uint32_t elem_sz)
 {
     int idx_sz;
 
@@ -244,15 +252,17 @@
     num_sectors_secondary = boot_img_num_sectors(state, BOOT_SECONDARY_SLOT);
 
     if (num_sectors_secondary == 0) {
-        BOOT_LOG_WRN("Upgrade disabled for image %d", BOOT_CURR_IMG(state));
+        BOOT_LOG_WRN("Upgrade disabled for image %u", (unsigned)BOOT_CURR_IMG(state));
         return 0;
     }
 
     if ((num_sectors_primary > BOOT_MAX_IMG_SECTORS) ||
         (num_sectors_secondary > BOOT_MAX_IMG_SECTORS)) {
         BOOT_LOG_WRN("Cannot upgrade: more sectors than allowed");
-        BOOT_LOG_DBG("sectors_primary (%d) or sectors_secondary (%d) > BOOT_MAX_IMG_SECTORS (%d)",
-                     num_sectors_primary, num_sectors_secondary, BOOT_MAX_IMG_SECTORS);
+        BOOT_LOG_DBG("sectors_primary (%lu) or sectors_secondary (%lu) > BOOT_MAX_IMG_SECTORS (%lu)",
+                     (unsigned long)num_sectors_primary,
+                     (unsigned long)num_sectors_secondary,
+                     (unsigned long)BOOT_MAX_IMG_SECTORS);
         return 0;
     }
 
@@ -325,16 +335,15 @@
     return 1;
 }
 
-#define BOOT_LOG_SWAP_STATE(area, state)                            \
-    BOOT_LOG_INF("%s: magic=%s, swap_type=0x%x, copy_done=0x%x, "   \
-                 "image_ok=0x%x",                                   \
-                 (area),                                            \
-                 ((state)->magic == BOOT_MAGIC_GOOD ? "good" :      \
-                  (state)->magic == BOOT_MAGIC_UNSET ? "unset" :    \
-                  "bad"),                                           \
-                 (state)->swap_type,                                \
-                 (state)->copy_done,                                \
-                 (state)->image_ok)
+#define BOOT_LOG_SWAP_STATE(area, state)                                        \
+    BOOT_LOG_INF("%s: magic=%s, swap_type=0x%x, copy_done=0x%x, image_ok=0x%x", \
+                 (area),                                                        \
+                 ((state)->magic == BOOT_MAGIC_GOOD  ? "good" :                 \
+                  (state)->magic == BOOT_MAGIC_UNSET ? "unset" :                \
+                                                       "bad"),                  \
+                 (unsigned)(state)->swap_type,                                  \
+                 (unsigned)(state)->copy_done,                                  \
+                 (unsigned)(state)->image_ok)
 
 struct boot_status_table {
     uint8_t bst_magic_primary_slot;
@@ -427,7 +436,9 @@
 swap_status_source(struct boot_loader_state *state)
 {
     const struct boot_status_table *table;
+#if MCUBOOT_SWAP_USING_SCRATCH
     struct boot_swap_state state_scratch;
+#endif
     struct boot_swap_state state_primary_slot;
     int rc;
     size_t i;
@@ -443,26 +454,33 @@
             &state_primary_slot);
     assert(rc == 0);
 
+#if MCUBOOT_SWAP_USING_SCRATCH
     rc = boot_read_swap_state_by_id(FLASH_AREA_IMAGE_SCRATCH, &state_scratch);
     assert(rc == 0);
+#endif
+
+    (void)rc;
 
     BOOT_LOG_SWAP_STATE("Primary image", &state_primary_slot);
+#if MCUBOOT_SWAP_USING_SCRATCH
     BOOT_LOG_SWAP_STATE("Scratch", &state_scratch);
-
+#endif
     for (i = 0; i < BOOT_STATUS_TABLES_COUNT; i++) {
         table = &boot_status_tables[i];
 
         if (boot_magic_compatible_check(table->bst_magic_primary_slot,
                           state_primary_slot.magic) &&
+#if MCUBOOT_SWAP_USING_SCRATCH
             boot_magic_compatible_check(table->bst_magic_scratch,
                           state_scratch.magic) &&
+#endif
             (table->bst_copy_done_primary_slot == BOOT_FLAG_ANY ||
              table->bst_copy_done_primary_slot == state_primary_slot.copy_done))
         {
             source = table->bst_status_source;
 
-#if (BOOT_IMAGE_NUMBER > 1)
-            /* In case of multi image boot it can happen that if boot status
+#if (BOOT_IMAGE_NUMBER > 1) && MCUBOOT_SWAP_USING_SCRATCH
+            /* In case of multi-image boot it can happen that if boot status
              * info is found on scratch area then it does not belong to the
              * currently examined image.
              */
@@ -546,14 +564,14 @@
 boot_swap_sectors(int idx, uint32_t sz, struct boot_loader_state *state,
         struct boot_status *bs)
 {
-    const struct flash_area *fap_primary_slot;
-    const struct flash_area *fap_secondary_slot;
-    const struct flash_area *fap_scratch;
+    const struct flash_area *fap_primary_slot = NULL;
+    const struct flash_area *fap_secondary_slot = NULL;
+    const struct flash_area *fap_scratch = NULL;
     uint32_t copy_sz;
     uint32_t trailer_sz;
     uint32_t img_off;
     uint32_t scratch_trailer_off;
-    struct boot_swap_state swap_state;
+    struct boot_swap_state swap_state = {0};
     size_t last_sector;
     bool erase_scratch;
     uint8_t image_index;
@@ -563,8 +581,12 @@
     img_off = boot_img_sector_off(state, BOOT_PRIMARY_SLOT, idx);
 
     copy_sz = sz;
-    // trailer_sz = boot_trailer_sz(BOOT_WRITE_SZ(state)); // TODO: fixme for status use case
-    trailer_sz = BOOT_WRITE_SZ(state);
+
+#ifdef MCUBOOT_SWAP_USING_STATUS
+    trailer_sz = BOOT_WRITE_SZ(state); // TODO: deep investigation in swap_status use case
+#else
+    trailer_sz = boot_trailer_sz(BOOT_WRITE_SZ(state));
+#endif
 
     /* sz in this function is always sized on a multiple of the sector size.
      * The check against the start offset of the last sector
@@ -598,7 +620,7 @@
 
     if (bs->state == BOOT_STATUS_STATE_0) {
         BOOT_LOG_DBG("erasing scratch area");
-        rc = boot_erase_region(fap_scratch, 0, fap_scratch->fa_size);
+        rc = boot_erase_region(fap_scratch, 0, flash_area_get_size(fap_scratch));
         assert(rc == 0);
 
         if (bs->idx == BOOT_STATUS_IDX_0) {
@@ -622,7 +644,8 @@
 
                 /* Erase the temporary trailer from the scratch area. */
 #ifndef MCUBOOT_SWAP_USING_STATUS
-                rc = boot_erase_region(fap_scratch, 0, fap_scratch->fa_size);
+                rc = boot_erase_region(fap_scratch, 0,
+                        flash_area_get_size(fap_scratch));
                 assert(rc == 0);
 #else
                 rc = swap_erase_trailer_sectors(state, fap_scratch);
@@ -681,8 +704,7 @@
                         (BOOT_STATUS_STATE_COUNT - 1) * BOOT_WRITE_SZ(state));
             BOOT_STATUS_ASSERT(rc == 0);
 
-            rc = boot_read_swap_state_by_id(FLASH_AREA_IMAGE_SCRATCH,
-                                            &swap_state);
+            rc = boot_read_swap_state(fap_scratch, &swap_state);
             assert(rc == 0);
 
             if (swap_state.image_ok == BOOT_FLAG_SET) {
diff --git a/boot/bootutil/src/swap_status.c b/boot/bootutil/src/swap_status.c
index 6b9d33b..7f7e065 100644
--- a/boot/bootutil/src/swap_status.c
+++ b/boot/bootutil/src/swap_status.c
@@ -37,6 +37,7 @@
 #include "swap_priv.h"
 #include "swap_status.h"
 #include "bootutil/bootutil_log.h"
+#include "bootutil/fault_injection_hardening.h"
 
 #include "mcuboot_config/mcuboot_config.h"
 
@@ -49,18 +50,25 @@
 swap_read_status_bytes(const struct flash_area *fap,
         struct boot_loader_state *state, struct boot_status *bs)
 {
+    const struct flash_area *fap_stat = NULL;
     uint32_t off;
     uint8_t status = 0;
-    uint8_t last_status = 0xff;
-    int max_entries;
+    uint8_t erased_val;
+    uint8_t last_status;
+    uint32_t max_entries;
     int32_t found_idx;
     bool found;
     bool invalid;
     int rc;
-    int i;
+    uint32_t i;
     (void)state;
 
-    BOOT_LOG_DBG("> STATUS: swap_read_status_bytes: fa_id = %d", fap->fa_id);
+    BOOT_LOG_DBG("> STATUS: swap_read_status_bytes: fa_id = %u", (unsigned)fap->fa_id);
+
+    rc = flash_area_open(FLASH_AREA_IMAGE_SWAP_STATUS, &fap_stat);
+    if (rc != 0) {
+        return -1;
+    }
 
     if (fap->fa_id == FLASH_AREA_IMAGE_SCRATCH) {
         max_entries = 1;
@@ -68,22 +76,25 @@
         max_entries = BOOT_STATUS_MAX_ENTRIES;
     }
 
+    erased_val = flash_area_erased_val(fap_stat);
+
     off = boot_status_off(fap);
 
     found = false;
     found_idx = -1;
     invalid = false;
+    last_status = erased_val;
 
     for (i = 0; i < max_entries; i++) {
         rc = swap_status_retrieve(fap->fa_id, off + i, &status, 1);
         if (rc < 0) {
-            return BOOT_EFLASH;
+            flash_area_close(fap_stat);
+            return -1;
         }
 
-        // if (status != flash_area_erased_val(fap)) { // TODO: fixup for external memory fap's
-        if (status == 0) {
+        if (status == erased_val) {
             if (found && (found_idx == -1)) {
-                found_idx = i;
+                found_idx = (int)i;
             }
         } else {
             last_status = status;
@@ -93,6 +104,8 @@
             } else if (found_idx > 0) {
                 invalid = true;
                 break;
+            } else {
+                /* No action required */
             }
         }
     }
@@ -109,7 +122,7 @@
         /* With validation of the primary slot disabled, there is no way
          * to be sure the swapped primary slot is OK, so abort!
          */
-        assert(0);
+        FIH_PANIC;
 #endif
     }
 
@@ -120,7 +133,8 @@
         uint8_t image_index = BOOT_CURR_IMG(state);
         rc = boot_read_swap_size((int32_t)image_index, &bs->swap_size);
         if (rc < 0) {
-            return BOOT_EFLASH;
+            flash_area_close(fap_stat);
+            return -1;
         }
 
 #ifdef MCUBOOT_SWAP_USING_MOVE
@@ -137,7 +151,7 @@
         {
             /* resume swap sectors operation */
             last_status++;
-            if (last_status > BOOT_STATUS_STATE_COUNT) {
+            if (last_status > (uint8_t)BOOT_STATUS_STATE_COUNT) {
                 last_status = BOOT_STATUS_STATE_0;
                 found_idx++;
             }
@@ -148,14 +162,16 @@
         }
     }
 
+    flash_area_close(fap_stat);
+
     return 0;
 }
 
 /* this is internal offset in swap status area */
 uint32_t
-boot_status_internal_off(const struct boot_status *bs, int elem_sz)
+boot_status_internal_off(const struct boot_status *bs, uint32_t elem_sz)
 {
-    uint32_t off = (bs->idx - BOOT_STATUS_IDX_0) * (uint32_t)elem_sz;
+    uint32_t off = (bs->idx - BOOT_STATUS_IDX_0) * elem_sz;
 
     return off;
 }
diff --git a/boot/bootutil/src/swap_status.h b/boot/bootutil/src/swap_status.h
index dc27d64..cd96856 100644
--- a/boot/bootutil/src/swap_status.h
+++ b/boot/bootutil/src/swap_status.h
@@ -37,30 +37,29 @@
 
 #ifdef MCUBOOT_SWAP_USING_STATUS
 
-#define BOOT_LOG_SWAP_STATE_M(area, state)                            \
-    BOOT_LOG_DBG("%s: magic=%s, swap_type=0x%x, copy_done=0x%x, "   \
-                 "image_ok=0x%x",                                   \
-                 (area),                                            \
-                 ((state)->magic == (uint8_t)BOOT_MAGIC_GOOD ? "good" :\
-                  (state)->magic == (uint8_t)BOOT_MAGIC_UNSET ? "unset" :\
-                  "bad"),                                           \
-                 (state)->swap_type,                                \
-                 (state)->copy_done,                                \
-                 (state)->image_ok)
+#define BOOT_LOG_SWAP_STATE_M(area, state)                                      \
+    BOOT_LOG_DBG("%s: magic=%s, swap_type=0x%x, copy_done=0x%x, image_ok=0x%x", \
+                 (area),                                                        \
+                 ((state)->magic == (uint8_t)BOOT_MAGIC_GOOD  ? "good" :        \
+                  (state)->magic == (uint8_t)BOOT_MAGIC_UNSET ? "unset" :       \
+                                                                "bad"),         \
+                 (unsigned)(state)->swap_type,                                  \
+                 (unsigned)(state)->copy_done,                                  \
+                 (unsigned)(state)->image_ok)
 
-#define BOOT_SET_SWAP_INFO_M(swap_info, image, type)  {                          \
-                                                    assert((int)((image) < 0xFu));     \
-                                                    assert((int)((type)  < 0xFu));     \
-                                                    (swap_info) = (image) << 4u \
-                                                                | (type);      \
-                                                    }
+#define BOOT_SET_SWAP_INFO_M(swap_info, image, type) do {                            \
+                                                         assert((image) < 0xFu);     \
+                                                         assert((type)  < 0xFu);     \
+                                                         (swap_info) = (image) << 4u \
+                                                                     | (type);       \
+                                                     } while (false)
 
-#define BOOT_GET_SWAP_TYPE_M(swap_info)    ((swap_info) & 0x0Fu)
-#define BOOT_GET_IMAGE_NUM_M(swap_info)    ((swap_info) >> 4u)
+#define BOOT_GET_SWAP_TYPE_M(swap_info) ((swap_info) & 0x0Fu)
+#define BOOT_GET_IMAGE_NUM_M(swap_info) ((swap_info) >> 4u)
 
 extern const uint32_t stat_part_magic[1];
 
-#define BOOT_SWAP_STATUS_MAGIC       (0xDEADBEAFu)
+#define BOOT_SWAP_STATUS_MAGIC          (0xDEADBEEFu)
 
 #define BOOT_SWAP_STATUS_ENCK1_SZ       16UL
 #define BOOT_SWAP_STATUS_ENCK2_SZ       16UL
@@ -90,9 +89,9 @@
 
 /* agreed to name it "a record" */
 #define BOOT_SWAP_STATUS_PAYLD_SZ       (BOOT_SWAP_STATUS_ROW_SZ -\
-                                            BOOT_SWAP_STATUS_MGCREC_SZ - \
-                                            BOOT_SWAP_STATUS_CNT_SZ - \
-                                            BOOT_SWAP_STATUS_CRC_SZ)
+                                         BOOT_SWAP_STATUS_MGCREC_SZ - \
+                                         BOOT_SWAP_STATUS_CNT_SZ - \
+                                         BOOT_SWAP_STATUS_CRC_SZ)
 #define BOOT_SWAP_STATUS_ROW_SZ_MIN     16UL
 
 /* INFO: defining record structure for better understanding */
@@ -133,13 +132,13 @@
 
 /* the size of one copy of status area */
 #define BOOT_SWAP_STATUS_D_SIZE     (BOOT_SWAP_STATUS_ROW_SZ * \
-                                    (BOOT_SWAP_STATUS_SECT_ROWS_NUM + \
-                                    BOOT_SWAP_STATUS_TRAIL_ROWS_NUM))
+                                     (BOOT_SWAP_STATUS_SECT_ROWS_NUM + \
+                                      BOOT_SWAP_STATUS_TRAIL_ROWS_NUM))
 
 /* the size of one copy of status area without cnt and crc fields */
 #define BOOT_SWAP_STATUS_D_SIZE_RAW (BOOT_SWAP_STATUS_PAYLD_SZ * \
-                                    (BOOT_SWAP_STATUS_SECT_ROWS_NUM + \
-                                    BOOT_SWAP_STATUS_TRAIL_ROWS_NUM))
+                                     (BOOT_SWAP_STATUS_SECT_ROWS_NUM + \
+                                      BOOT_SWAP_STATUS_TRAIL_ROWS_NUM))
 
 /* multiplier which defines how many blocks will be used to reduce Flash wear
  * 1 is for single write wear, 2 - twice less wear, 3 - three times less wear, etc */
@@ -153,14 +152,15 @@
 
 #define BOOT_SWAP_STATUS_OFFS_PRIM  0UL
 #define BOOT_SWAP_STATUS_OFFS_SEC   (BOOT_SWAP_STATUS_OFFS_PRIM + \
-                                    BOOT_SWAP_STATUS_SZ_PRIM)
+                                     BOOT_SWAP_STATUS_SZ_PRIM)
 
-int32_t swap_status_init_offset(uint32_t area_id);
-int swap_status_update(uint32_t target_area_id, uint32_t offs, const void *data, uint32_t len);
-int swap_status_retrieve(uint32_t target_area_id, uint32_t offs, void *data, uint32_t len);
+/* size Limit for primary slot trailer buffer */
+#define MAX_TRAILER_BUF_SIZE        CY_FLASH_ALIGN
 
-int boot_write_trailer(const struct flash_area *fap, uint32_t off,
-                        const uint8_t *inbuf, uint8_t inlen);
+int32_t swap_status_init_offset(uint8_t area_id);
+int swap_status_update(uint8_t target_area_id, uint32_t offs, const void *data, uint32_t len);
+int swap_status_retrieve(uint8_t target_area_id, uint32_t offs, void *data, uint32_t len);
+int swap_status_to_image_trailer(const struct flash_area *fap);
 
 #endif /* MCUBOOT_SWAP_USING_STATUS */
 
diff --git a/boot/bootutil/src/swap_status_misc.c b/boot/bootutil/src/swap_status_misc.c
index c8f8f95..8512b02 100644
--- a/boot/bootutil/src/swap_status_misc.c
+++ b/boot/bootutil/src/swap_status_misc.c
@@ -69,50 +69,36 @@
     return BOOT_FLAG_SET;
 }
 
-static inline size_t
-boot_status_sector_size(const struct boot_loader_state *state, size_t sector)
-{
-    return state->status.sectors[sector].fs_size;
-}
-
-static inline uint32_t
-boot_status_sector_off(const struct boot_loader_state *state,
-                    size_t sector)
-{
-    return state->status.sectors[sector].fs_off -
-           state->status.sectors[0].fs_off;
-}
-
 /* Offset Section */
 static inline uint32_t
 boot_magic_off(const struct flash_area *fap)
 {
     (void)fap;
-    return ((uint32_t)BOOT_SWAP_STATUS_D_SIZE_RAW - (uint32_t)BOOT_MAGIC_SZ);
+    return (uint32_t)BOOT_SWAP_STATUS_D_SIZE_RAW - (uint32_t)BOOT_MAGIC_SZ;
 }
 
 uint32_t
 boot_image_ok_off(const struct flash_area *fap)
 {
-    return (uint32_t)(boot_magic_off(fap) - 1u);
+    return boot_magic_off(fap) - BOOT_SWAP_STATUS_IMG_OK_SZ;
 }
 
 uint32_t
 boot_copy_done_off(const struct flash_area *fap)
 {
-    return (uint32_t)(boot_image_ok_off(fap) - 1u);
+    return boot_image_ok_off(fap) - BOOT_SWAP_STATUS_COPY_DONE_SZ;
 }
 
 uint32_t
 boot_swap_info_off(const struct flash_area *fap)
 {
-    return (uint32_t)(boot_copy_done_off(fap) - 1u);
+    return boot_copy_done_off(fap) - BOOT_SWAP_STATUS_SWAPINF_SZ;
 }
 
 uint32_t
 boot_swap_size_off(const struct flash_area *fap)
 {
-    return (uint32_t)(boot_swap_info_off(fap) - 4u);
+    return boot_swap_info_off(fap) - BOOT_SWAP_STATUS_SWAPSZ_SZ;
 }
 
 uint32_t
@@ -130,9 +116,9 @@
 {
 #ifdef MCUBOOT_SWAP_SAVE_ENCTLV
     /* suggest encryption key is also stored in status partition */
-    return (uint32_t)(boot_swap_size_off(fap) - (uint32_t)((slot + 1u) * (uint32_t)BOOT_ENC_TLV_SIZE));
+    return boot_swap_size_off(fap) - (((uint32_t)slot + 1UL) * (uint32_t)BOOT_ENC_TLV_SIZE);
 #else
-    return (uint32_t)(boot_swap_size_off(fap) - (uint32_t)((slot + 1u) * (uint32_t)BOOT_ENC_KEY_SIZE));
+    return boot_swap_size_off(fap) - (((uint32_t)slot + 1UL) * (uint32_t)BOOT_ENC_KEY_SIZE);
 #endif
 }
 #endif
@@ -140,7 +126,7 @@
 /**
  * Write trailer data; status bytes, swap_size, etc
  *
- * @returns 0 on success, != 0 on error.
+ * @returns 0 on success, -1 on error.
  */
 int
 boot_write_trailer(const struct flash_area *fap, uint32_t off,
@@ -148,10 +134,23 @@
 {
     int rc;
 
-    rc = swap_status_update(fap->fa_id, off, (uint8_t *)inbuf, inlen);
+    /* copy status part trailer to primary image before set copy_done flag */
+    if (boot_copy_done_off(fap) == off &&
+        fap->fa_id == FLASH_AREA_IMAGE_PRIMARY(0u) &&
+        BOOT_SWAP_STATUS_COPY_DONE_SZ == inlen) {
+
+        BOOT_LOG_DBG("copy status part trailer to primary image slot");
+        rc = swap_status_to_image_trailer(fap);
+        if (rc != 0) {
+            BOOT_LOG_ERR("trailer copy failed");
+            return -1;
+        }
+    }
+
+    rc = swap_status_update(fap->fa_id, off, inbuf, inlen);
 
     if (rc != 0) {
-        return BOOT_EFLASH;
+        return -1;
     }
     return rc;
 }
@@ -167,14 +166,14 @@
     off = boot_enc_key_off(fap, slot);
 #ifdef MCUBOOT_SWAP_SAVE_ENCTLV
     rc = swap_status_update(fap->fa_id, off,
-                            (uint8_t *) bs->enctlv[slot], BOOT_ENC_TLV_ALIGN_SIZE);
+                            bs->enctlv[slot], BOOT_ENC_TLV_ALIGN_SIZE);
 #else
     rc = swap_status_update(fap->fa_id, off,
-                            (uint8_t *) bs->enckey[slot], BOOT_ENC_KEY_SIZE);
+                            bs->enckey[slot], BOOT_ENC_KEY_SIZE);
 #endif
-   if (rc != 0) {
-       return BOOT_EFLASH;
-   }
+    if (rc != 0) {
+        return -1;
+    }
 
     return 0;
 }
@@ -184,25 +183,21 @@
 {
     uint32_t off;
     const struct flash_area *fap;
-#ifdef MCUBOOT_SWAP_SAVE_ENCTLV
-    int i;
-#endif
     int rc;
 
     rc = boot_find_status(image_index, &fap);
-    if (rc == 0) {
+    if (0 == rc) {
         off = boot_enc_key_off(fap, slot);
 #ifdef MCUBOOT_SWAP_SAVE_ENCTLV
         rc = swap_status_retrieve(fap->fa_id, off, bs->enctlv[slot], BOOT_ENC_TLV_ALIGN_SIZE);
-        if (rc == 0) {
-            for (i = 0; i < BOOT_ENC_TLV_ALIGN_SIZE; i++) {
-                if (bs->enctlv[slot][i] != 0xff) {
-                    break;
-                }
-            }
-            /* Only try to decrypt non-erased TLV metadata */
-            if (i != BOOT_ENC_TLV_ALIGN_SIZE) {
-                rc = boot_enc_decrypt(bs->enctlv[slot], bs->enckey[slot]);
+        if (0 == rc) {
+            uint8_t aes_iv[BOOTUTIL_CRYPTO_AES_CTR_BLOCK_SIZE];
+
+            /* Only try to decrypt initialized TLV metadata */
+            if (!bootutil_buffer_is_filled(bs->enctlv[slot],
+                                           BOOT_UNINITIALIZED_TLV_FILL,
+                                           BOOT_ENC_TLV_ALIGN_SIZE)) {
+                rc = boot_enc_decrypt(bs->enctlv[slot], bs->enckey[slot], 0, aes_iv);
             }
         }
 #else
@@ -225,19 +220,14 @@
     off = boot_magic_off(fap);
 
     rc = swap_status_update(fap->fa_id, off,
-                            (uint8_t *) boot_img_magic, BOOT_MAGIC_SZ);
+                            boot_img_magic, BOOT_MAGIC_SZ);
 
     if (rc != 0) {
-        return BOOT_EFLASH;
+        return -1;
     }
     return 0;
 }
 
-int boot_status_num_sectors(const struct boot_loader_state *state)
-{
-    return (int)(BOOT_SWAP_STATUS_SIZE / boot_status_sector_size(state, 0));
-}
-
 /**
  * Writes the supplied boot status to the flash file system.  The boot status
  * contains the current state of an in-progress image copy operation.
@@ -251,10 +241,13 @@
 {
     const struct flash_area *fap = NULL;
     uint32_t off;
-    int area_id;
+    uint8_t area_id;
     int rc;
     (void)state;
 
+    if (bs->idx < BOOT_STATUS_IDX_0) {
+        return BOOT_EFLASH;
+    }
     /* NOTE: The first sector copied (that is the last sector on slot) contains
      *       the trailer. Since in the last step the primary slot is erased, the
      *       first two status writes go to the scratch which will be copied to
@@ -262,17 +255,17 @@
      */
 
 #ifdef MCUBOOT_SWAP_USING_SCRATCH
-    if (bs->use_scratch) {
+    if (bs->use_scratch != 0U) {
         /* Write to scratch status. */
-        area_id = FLASH_AREA_IMAGE_SCRATCH;
+        area_id = (uint8_t)FLASH_AREA_IMAGE_SCRATCH;
     } else
 #endif
     {
         /* Write to the primary slot. */
-        area_id = (int)FLASH_AREA_IMAGE_PRIMARY(BOOT_CURR_IMG(state));
+        area_id = FLASH_AREA_IMAGE_PRIMARY(BOOT_CURR_IMG(state));
     }
 
-    rc = flash_area_open((uint8_t)area_id, &fap);
+    rc = flash_area_open(area_id, &fap);
     if (rc != 0) {
         rc = BOOT_EFLASH;
         goto done;
@@ -294,20 +287,6 @@
 }
 
 int
-boot_read_data_empty(const struct flash_area *fap, void *data, uint32_t len)
-{
-    uint8_t *buf;
-
-    buf = (uint8_t *)data;
-    for (uint32_t i = 0; i < len; i++) {
-        if (buf[i] != flash_area_erased_val(fap)) {
-            return 0;
-        }
-    }
-    return 1;
-}
-
-int
 boot_read_swap_state(const struct flash_area *fap,
                      struct boot_swap_state *state)
 {
@@ -317,54 +296,64 @@
     uint8_t swap_info = 0U;
     int rc;
     uint32_t erase_trailer = 0;
+    bool buf_is_clean = false;
+    bool is_primary = false;
+    bool is_secondary = false;
+    uint32_t i;
 
     const struct flash_area *fap_stat = NULL;
 
     rc = flash_area_open(FLASH_AREA_IMAGE_SWAP_STATUS, &fap_stat);
     if (rc != 0) {
-        return BOOT_EFLASH;
+        return -1;
     }
 
     off = boot_magic_off(fap);
     /* retrieve value for magic field from status partition area */
     rc = swap_status_retrieve(fap->fa_id, off, magic, BOOT_MAGIC_SZ);
     if (rc < 0) {
-        return BOOT_EFLASH;
+        return -1;
     }
-    rc = boot_read_data_empty(fap_stat, magic, BOOT_MAGIC_SZ);
-    if (rc < 0) {
-        return BOOT_EFLASH;
-    }
-    /* fill magic number value if equal to expected */
-    if (rc == 1) {
 
+    for (i = 0u; i < (uint32_t)BOOT_IMAGE_NUMBER; i++) {
+        if (fap->fa_id == FLASH_AREA_IMAGE_PRIMARY(i)) {
+            is_primary = true;
+            break;
+        }
+        if (fap->fa_id == FLASH_AREA_IMAGE_SECONDARY(i)) {
+            is_secondary = true;
+            break;
+        }
+    }
+
+    /* fill magic number value if equal to expected */
+    if (bootutil_buffer_is_erased(fap_stat, magic, BOOT_MAGIC_SZ)) {
         state->magic = BOOT_MAGIC_UNSET;
 
         /* attempt to find magic in upgrade img slot trailer */
-        if (fap->fa_id == FLASH_AREA_IMAGE_1 ||
-            fap->fa_id == FLASH_AREA_IMAGE_3) {
+        if (is_secondary) {
+            trailer_off = fap->fa_size - BOOT_MAGIC_SZ;
 
-                trailer_off = fap->fa_size - BOOT_MAGIC_SZ;
-
-                rc = flash_area_read_is_empty(fap, trailer_off, magic, BOOT_MAGIC_SZ);
+            rc = flash_area_read(fap, trailer_off, magic, BOOT_MAGIC_SZ);
+            if (rc != 0) {
+                return -1;
+            }
+            buf_is_clean = bootutil_buffer_is_erased(fap, magic, BOOT_MAGIC_SZ);
+            if (buf_is_clean) {
+                state->magic = BOOT_MAGIC_UNSET;
+            } else {
+                state->magic = (uint8_t)boot_magic_decode(magic);
+                /* put magic to status partition for upgrade slot*/
+                if ((uint32_t)BOOT_MAGIC_GOOD == state->magic) {
+                    rc = swap_status_update(fap->fa_id, off,
+                                    (uint8_t *) magic, BOOT_MAGIC_SZ);
+                }
                 if (rc < 0) {
-                    return BOOT_EFLASH;
-                }
-                if (rc == 1) {
-                    state->magic = BOOT_MAGIC_UNSET;
+                    return -1;
                 } else {
-                    state->magic = (uint8_t)boot_magic_decode(magic);
-                    /* put magic to status partition for upgrade slot*/
-                    if (state->magic == (uint32_t)BOOT_MAGIC_GOOD) {
-                        rc = swap_status_update(fap->fa_id, off,
-                                        (uint8_t *) magic, BOOT_MAGIC_SZ);
-                    }
-                    if (rc < 0) {
-                        return BOOT_EFLASH;
-                    } else {
-                        erase_trailer = 1;
-                    }
+                    erase_trailer = 1;
                 }
+            }
         }
     } else {
         state->magic = (uint8_t)boot_magic_decode(magic);
@@ -373,13 +362,10 @@
     off = boot_swap_info_off(fap);
     rc = swap_status_retrieve(fap->fa_id, off, &swap_info, sizeof swap_info);
     if (rc < 0) {
-        return BOOT_EFLASH;
+        return -1;
     }
-    rc = boot_read_data_empty(fap_stat, &swap_info, sizeof swap_info);
-    if (rc < 0) {
-        return BOOT_EFLASH;
-    }
-    if (rc == 1 || state->swap_type > (uint8_t)BOOT_SWAP_TYPE_REVERT) {
+    if (bootutil_buffer_is_erased(fap_stat, &swap_info, sizeof swap_info) ||
+        state->swap_type >= (uint8_t)BOOT_SWAP_TYPE_FAIL) {
         state->swap_type = (uint8_t)BOOT_SWAP_TYPE_NONE;
         state->image_num = 0;
     }
@@ -392,14 +378,10 @@
     off = boot_copy_done_off(fap);
     rc = swap_status_retrieve(fap->fa_id, off, &state->copy_done, sizeof state->copy_done);
     if (rc < 0) {
-        return BOOT_EFLASH;
+        return -1;
     }
-    rc = boot_read_data_empty(fap_stat, &state->copy_done, sizeof state->copy_done);
     /* need to check swap_info was empty */
-    if (rc < 0) {
-       return BOOT_EFLASH;
-    }
-    if (rc == 1) {
+    if (bootutil_buffer_is_erased(fap_stat, &state->copy_done, sizeof state->copy_done)) {
        state->copy_done = BOOT_FLAG_UNSET;
     } else {
        state->copy_done = (uint8_t)boot_flag_decode(state->copy_done);
@@ -408,58 +390,40 @@
     off = boot_image_ok_off(fap);
     rc = swap_status_retrieve(fap->fa_id, off, &state->image_ok, sizeof state->image_ok);
     if (rc < 0) {
-       return BOOT_EFLASH;
+       return -1;
     }
-    rc = boot_read_data_empty(fap_stat, &state->image_ok, sizeof state->image_ok);
     /* need to check swap_info was empty */
-    if (rc < 0) {
-       return BOOT_EFLASH;
-    }
-    if (rc == 1) {
+    if (bootutil_buffer_is_erased(fap_stat, &state->image_ok, sizeof state->image_ok)) {
         /* assign img_ok unset */
         state->image_ok = BOOT_FLAG_UNSET;
 
         /* attempt to read img_ok value in upgrade img slots trailer area
          * it is set when image in slot for upgrade is signed for swap_type permanent
         */
-        uint32_t process_image_ok = 0;
-        switch (fap->fa_id) {
-            case FLASH_AREA_IMAGE_0:
-            case FLASH_AREA_IMAGE_2:
-            {
-                if (state->copy_done == (uint8_t)BOOT_FLAG_SET)
-                    process_image_ok = 1;
-            }
-            break;
-            case FLASH_AREA_IMAGE_1:
-            case FLASH_AREA_IMAGE_3:
-            {
-                process_image_ok = 1;
-            }
-            break;
-            case FLASH_AREA_IMAGE_SCRATCH:
-            {
-                BOOT_LOG_DBG(" * selected SCRATCH area, copy_done = %d", state->copy_done);
-                {
-                    if (state->copy_done == (uint8_t)BOOT_FLAG_SET)
-                        process_image_ok = 1;
-                }
-            }
-            break;
-            default:
-            {
-                return BOOT_EFLASH;
-            }
-            break;
+        bool process_image_ok = (uint8_t)BOOT_FLAG_SET == state->copy_done;
+        if (fap->fa_id == FLASH_AREA_IMAGE_SCRATCH) {
+            BOOT_LOG_DBG(" * selected SCRATCH area, copy_done = %u", (unsigned)state->copy_done);
         }
-        if (process_image_ok != 0u) {
+        else if (is_secondary) {
+            process_image_ok = true;
+        }
+        else if (!is_primary) {
+            process_image_ok = false;
+            rc = -1;
+        }
+        else {
+            /* Fix MISRA Rule 15.7 */
+        }
+        if (process_image_ok) {
             trailer_off = fap->fa_size - (uint8_t)BOOT_MAGIC_SZ - (uint8_t)BOOT_MAX_ALIGN;
 
-            rc = flash_area_read_is_empty(fap, trailer_off, &state->image_ok, sizeof state->image_ok);
-            if (rc < 0) {
-                return BOOT_EFLASH;
+            rc = flash_area_read(fap, trailer_off, &state->image_ok, sizeof state->image_ok);
+            if (rc != 0) {
+                return -1;
             }
-            if (rc == 1) {
+
+            buf_is_clean = bootutil_buffer_is_erased(fap, &state->image_ok, sizeof state->image_ok);
+            if (buf_is_clean) {
                 state->image_ok = BOOT_FLAG_UNSET;
             } else {
                 state->image_ok = (uint8_t)boot_flag_decode(state->image_ok);
@@ -470,24 +434,25 @@
                                 &state->image_ok, sizeof state->image_ok);
                 }
                 if (rc < 0) {
-                    return BOOT_EFLASH;
+                    return -1;
                 }
 
-                /* mark img trailer needs to be erased */
-                erase_trailer = 1;
+                /* don't erase trailer, just move img_ok to status part */
+                erase_trailer = 0;
             }
         }
     } else {
        state->image_ok = (uint8_t)boot_flag_decode(state->image_ok);
     }
 
-    if ((erase_trailer != 0u) && (fap->fa_id != FLASH_AREA_IMAGE_SCRATCH)) {
+    if ((erase_trailer != 0u) && (fap->fa_id != FLASH_AREA_IMAGE_SCRATCH) && (0 == rc)) {
         /* erase magic from upgrade img trailer */
         rc = flash_area_erase(fap, trailer_off, BOOT_MAGIC_SZ);
-        if (rc != 0)
+        if (rc != 0) {
             return rc;
+        }
     }
-    return 0;
+    return rc;
 }
 
 /**
@@ -504,10 +469,15 @@
 {
     uint32_t magic[BOOT_MAGIC_ARR_SZ] = {0};
     uint32_t off;
-
-    /* the status is always in status partition */
-    uint8_t area = FLASH_AREA_IMAGE_PRIMARY((uint32_t)image_index);
     int rc = -1;
+    uint8_t area = FLASH_AREA_ERROR;
+
+    if ((image_index < 0) || (image_index >= MCUBOOT_IMAGE_NUMBER)) {
+        return rc;
+    }
+    /* the status is always in status partition */
+    area = FLASH_AREA_IMAGE_PRIMARY((uint32_t)image_index);
+
 
     /*
      * In the middle a swap, tries to locate the area that is currently
@@ -523,7 +493,7 @@
     off = boot_magic_off(*fap);
     rc = swap_status_retrieve(area, off, magic, BOOT_MAGIC_SZ);
 
-    if (rc == 0) {
+    if (0 == rc) {
         rc = memcmp(magic, boot_img_magic, BOOT_MAGIC_SZ);
     }
 
@@ -539,7 +509,7 @@
     int rc;
 
     rc = boot_find_status(image_index, &fap);
-    if (rc == 0) {
+    if (0 == rc) {
         off = boot_swap_size_off(fap);
 
         rc = swap_status_retrieve(fap->fa_id, off, swap_size, sizeof *swap_size);
@@ -551,48 +521,58 @@
 swap_erase_trailer_sectors(const struct boot_loader_state *state,
                            const struct flash_area *fap)
 {
-    uint32_t sub_offs, trailer_offs;
-    uint32_t sz;
+    int32_t sub_offs;
+    uint32_t trailer_offs;
     uint8_t fa_id_primary;
     uint8_t fa_id_secondary;
     uint8_t image_index;
     int rc;
     (void)state;
 
-    BOOT_LOG_INF("Erasing trailer; fa_id=%d", fap->fa_id);
+    BOOT_LOG_INF("Erasing trailer; fa_id=%u", (unsigned)fap->fa_id);
     /* trailer is located in status-partition */
     const struct flash_area *fap_stat = NULL;
 
     rc = flash_area_open(FLASH_AREA_IMAGE_SWAP_STATUS, &fap_stat);
     if (rc != 0) {
-        return BOOT_EFLASH;
+        return -1;
     }
 
-    if (fap->fa_id != FLASH_AREA_IMAGE_SCRATCH)
-    {
+    if (fap->fa_id != FLASH_AREA_IMAGE_SCRATCH) {
         image_index = BOOT_CURR_IMG(state);
-        fa_id_primary = (uint8_t)flash_area_id_from_multi_image_slot((int32_t)image_index,
+        rc = flash_area_id_from_multi_image_slot((int32_t)image_index,
                 BOOT_PRIMARY_SLOT);
-        fa_id_secondary = (uint8_t)flash_area_id_from_multi_image_slot((int32_t)image_index,
+        if (rc < 0) {
+            return -1;
+        }
+        fa_id_primary = (uint8_t)rc;
+
+        rc = flash_area_id_from_multi_image_slot((int32_t)image_index,
                 BOOT_SECONDARY_SLOT);
+        if (rc < 0) {
+            return -1;
+        }
+        fa_id_secondary = (uint8_t)rc;
 
         /* skip if Flash Area is not recognizable */
         if ((fap->fa_id != fa_id_primary) && (fap->fa_id != fa_id_secondary)) {
-            return BOOT_EFLASH;
+            return -1;
         }
     }
 
-    sub_offs = (uint32_t)swap_status_init_offset(fap->fa_id);
+    sub_offs = swap_status_init_offset(fap->fa_id);
+    if (sub_offs < 0) {
+        return -1;
+    }
 
     /* delete starting from last sector and moving to beginning */
     /* calculate last sector of status sub-area */
-    sz = (uint32_t)BOOT_SWAP_STATUS_SIZE;
+    rc = flash_area_erase(fap_stat, (uint32_t)sub_offs, (uint32_t)BOOT_SWAP_STATUS_SIZE);
+    if (rc != 0) {
+        return -1;
+    }
 
-    rc = flash_area_erase(fap_stat, sub_offs, sz);
-    assert((int)(rc == 0));
-
-    if (fap->fa_id != FLASH_AREA_IMAGE_SCRATCH)
-    {
+    if (fap->fa_id != FLASH_AREA_IMAGE_SCRATCH) {
         /*
          * it is also needed to erase trailer area in slots since they may contain
          * data, which is already cleared in corresponding status partition
@@ -621,37 +601,37 @@
 
     image_index = BOOT_CURR_IMG(state);
 
-    BOOT_LOG_DBG("initializing status; fa_id=%d", fap->fa_id);
+    BOOT_LOG_DBG("initializing status; fa_id=%u", (unsigned)fap->fa_id);
 
     rc = boot_read_swap_state_by_id((int32_t)FLASH_AREA_IMAGE_SECONDARY(image_index),
             &swap_state);
-    assert((int)(rc == 0));
+    assert(0 == rc);
 
     if (bs->swap_type != (uint8_t)BOOT_SWAP_TYPE_NONE) {
         rc = boot_write_swap_info(fap, bs->swap_type, image_index);
-        assert((int)(rc == 0));
+        assert(0 == rc);
     }
 
-    if (swap_state.image_ok == (uint8_t)BOOT_FLAG_SET) {
+    if ((uint8_t)BOOT_FLAG_SET == swap_state.image_ok) {
         rc = boot_write_image_ok(fap);
-        assert((int)(rc == 0));
+        assert(0 == rc);
     }
 
     rc = boot_write_swap_size(fap, bs->swap_size);
-    assert((int)(rc == 0));
+    assert(0 == rc);
 
 #ifdef MCUBOOT_ENC_IMAGES
     rc = boot_write_enc_key(fap, 0, bs);
-    assert((int)(rc == 0));
+    assert(0 == rc);
 
     rc = boot_write_enc_key(fap, 1, bs);
-    assert((int)(rc == 0));
+    assert(0 == rc);
 #endif
 
     rc = boot_write_magic(fap);
-    assert((int)(rc == 0));
+    assert(0 == rc);
 
-    return 0;
+    return rc;
 }
 
 int
@@ -661,46 +641,42 @@
     const struct flash_area *fap_stat = NULL;
     uint32_t off;
     uint8_t swap_info = 0;
-    int area_id;
+    uint8_t area_id;
     int rc = 0;
 
     bs->source = swap_status_source(state);
 
-    if (bs->source == BOOT_STATUS_SOURCE_NONE) {
+    if (BOOT_STATUS_SOURCE_NONE == bs->source) {
         return 0;
     }
-
-    if (bs->source ==  BOOT_STATUS_SOURCE_PRIMARY_SLOT) {
-        area_id = (int32_t)FLASH_AREA_IMAGE_PRIMARY(BOOT_CURR_IMG(state));
-    } else if (bs->source ==  BOOT_STATUS_SOURCE_SCRATCH) {
+    else if (BOOT_STATUS_SOURCE_PRIMARY_SLOT == bs->source) {
+        area_id = FLASH_AREA_IMAGE_PRIMARY(BOOT_CURR_IMG(state));
+    }
+    else if (BOOT_STATUS_SOURCE_SCRATCH == bs->source) {
         area_id = FLASH_AREA_IMAGE_SCRATCH;
-    } else {
-        return BOOT_EBADARGS;
+    }
+    else {
+        return -1;
     }
 
-    rc = flash_area_open((uint8_t)area_id, &fap);
+    rc = flash_area_open(area_id, &fap);
     if (rc != 0) {
-        return BOOT_EFLASH;
+        return -1;
     }
 
     rc = flash_area_open(FLASH_AREA_IMAGE_SWAP_STATUS, &fap_stat);
     if (rc != 0) {
-        return BOOT_EFLASH;
+        return -1;
     }
 
     rc = swap_read_status_bytes(fap, state, bs);
-    if (rc == 0) {
+    if (0 == rc) {
         off = boot_swap_info_off(fap);
         rc = swap_status_retrieve((uint8_t)area_id, off, &swap_info, sizeof swap_info);
         if (rc < 0) {
-            return BOOT_EFLASH;
+            return -1;
         }
-        rc = boot_read_data_empty(fap_stat, &swap_info, sizeof swap_info);
-        if (rc < 0) {
-            return BOOT_EFLASH;
-        }
-
-        if (rc == 1) {
+        if (bootutil_buffer_is_erased(fap_stat, &swap_info, sizeof swap_info)) {
             BOOT_SET_SWAP_INFO_M(swap_info, 0u, (uint8_t)BOOT_SWAP_TYPE_NONE);
             rc = 0;
         }
diff --git a/boot/bootutil/src/swap_status_part.c b/boot/bootutil/src/swap_status_part.c
index a2899a9..f6be047 100644
--- a/boot/bootutil/src/swap_status_part.c
+++ b/boot/bootutil/src/swap_status_part.c
@@ -26,97 +26,83 @@
  * under the License.
  */
 
-#include <assert.h>
 #include "crc32c.h"
 #include <string.h>
+#include <stdlib.h>
 #include "swap_status.h"
 
 #ifdef MCUBOOT_SWAP_USING_STATUS
 
-#define IMAGE_0_STATUS_OFFS        0
-#define IMAGE_0_STATUS_SIZE        (BOOT_SWAP_STATUS_SIZE)
-
-#define IMAGE_1_STATUS_OFFS        (IMAGE_0_STATUS_OFFS + IMAGE_0_STATUS_SIZE)
-#define IMAGE_1_STATUS_SIZE        (BOOT_SWAP_STATUS_SIZE)
-
-#define SCRATCH_STATUS_OFFS        (IMAGE_1_STATUS_OFFS + BOOT_SWAP_STATUS_SIZE)
-#ifdef MCUBOOT_SWAP_USING_SCRATCH
-#define SCRATCH_STATUS_SIZE        (BOOT_SWAP_STATUS_SIZE)
-#else
-#define SCRATCH_STATUS_SIZE        0
-#endif
-
-#if (MCUBOOT_IMAGE_NUMBER == 2)
-#define IMAGE_2_STATUS_OFFS        (SCRATCH_STATUS_OFFS + SCRATCH_STATUS_SIZE)
-#define IMAGE_2_STATUS_SIZE        (BOOT_SWAP_STATUS_SIZE)
-
-#define IMAGE_3_STATUS_OFFS        (IMAGE_2_STATUS_OFFS + IMAGE_2_STATUS_SIZE)
-#define IMAGE_3_STATUS_SIZE        (BOOT_SWAP_STATUS_SIZE)
-#endif
+static uint8_t record_buff[BOOT_SWAP_STATUS_ROW_SZ];
+static uint8_t status_buff[BOOT_SWAP_STATUS_PAYLD_SZ];
 
 const uint32_t stat_part_magic[] = {
     BOOT_SWAP_STATUS_MAGIC
 };
 
-uint32_t calc_rec_idx(uint32_t value)
+static inline uint32_t calc_rec_idx(uint32_t value)
 {
-    uint32_t rec_idx;
-
-    rec_idx = value/BOOT_SWAP_STATUS_PAYLD_SZ;
-
-    return rec_idx;
+    return value / BOOT_SWAP_STATUS_PAYLD_SZ;
 }
 
-uint32_t calc_record_offs(uint32_t offs)
+static inline uint32_t calc_record_offs(uint32_t offs)
 {
-    uint32_t rec_offs;
-
-    rec_offs = BOOT_SWAP_STATUS_ROW_SZ*calc_rec_idx(offs);
-
-    return rec_offs;
+    return BOOT_SWAP_STATUS_ROW_SZ * calc_rec_idx(offs);
 }
 
-uint32_t calc_record_crc(const uint8_t *data, uint32_t length)
+static inline uint32_t calc_record_crc(const uint8_t *data, uint32_t length)
 {
-    uint32_t crc;
-
-    crc = crc32c_checksum(data, length);
-
-    return crc;
+    return crc32c_checksum(data, length);
 }
 
-int32_t swap_status_init_offset(uint32_t area_id)
+static inline uint32_t pack_bytes_u32(const uint8_t *data)
 {
-    int32_t offset = -1;
-    /* calculate an offset caused by area type: primary_x/secondary_x */
-    switch (area_id) {
-    case FLASH_AREA_IMAGE_0:
-        offset = (int)IMAGE_0_STATUS_OFFS;
-        break;
-    case FLASH_AREA_IMAGE_1:
-        offset = (int)IMAGE_1_STATUS_OFFS;
-        break;
+    uint32_t result = 0U;
+
+    result = ((uint32_t)data[3U] << 24U) | ((uint32_t)data[2U] << 16U) |
+             ((uint32_t)data[1U] << 8U) | (uint32_t)data[0U];
+
+    return result;
+}
+
+int32_t swap_status_init_offset(uint8_t area_id)
+{
+    uint8_t order[] = {
+        FLASH_AREA_IMAGE_PRIMARY(0U)
+      , FLASH_AREA_IMAGE_SECONDARY(0U)
 #ifdef MCUBOOT_SWAP_USING_SCRATCH
-    case FLASH_AREA_IMAGE_SCRATCH:
-        offset = (int)SCRATCH_STATUS_OFFS;
-        break;
-#endif
-#if (MCUBOOT_IMAGE_NUMBER == 2)
-    case FLASH_AREA_IMAGE_2:
-        offset = (int)IMAGE_2_STATUS_OFFS;
-        break;
-    case FLASH_AREA_IMAGE_3:
-        offset = (int)IMAGE_3_STATUS_OFFS;
-        break;
-#endif
-    default:
-        offset = -1;
-        break;
+      , FLASH_AREA_IMAGE_SCRATCH
+#endif /* MCUBOOT_SWAP_USING_SCRATCH */
+#if BOOT_IMAGE_NUMBER >= 2
+      , FLASH_AREA_IMAGE_PRIMARY(1U)
+      , FLASH_AREA_IMAGE_SECONDARY(1U)
+#endif /* BOOT_IMAGE_NUMBER >= 2 */
+#if BOOT_IMAGE_NUMBER >= 3
+      , FLASH_AREA_IMAGE_PRIMARY(2U)
+      , FLASH_AREA_IMAGE_SECONDARY(2U)
+#endif /* BOOT_IMAGE_NUMBER >= 3 */
+#if BOOT_IMAGE_NUMBER == 4
+      , FLASH_AREA_IMAGE_PRIMARY(3U)
+      , FLASH_AREA_IMAGE_SECONDARY(3U)
+#endif /* BOOT_IMAGE_NUMBER == 4 */
+    };
+
+    int32_t result = -1;
+    int32_t offset = 0;
+    uint32_t i;
+
+    for (i = 0U; i < sizeof(order) / sizeof(order[0U]); i++) {
+        if (order[i] == area_id) {
+            result = offset;
+            break;
+        }
+        offset += BOOT_SWAP_STATUS_SIZE;
     }
-    return offset;
+
+    return result;
 }
 
-int swap_status_read_record(uint32_t rec_offset, uint8_t *data, uint32_t *copy_counter)
+static int swap_status_read_record(uint32_t rec_offset, uint8_t *data, uint32_t *copy_counter, uint32_t *max_idx)
 { /* returns BOOT_SWAP_STATUS_PAYLD_SZ of data */
     int rc = -1;
 
@@ -127,60 +113,56 @@
     uint32_t magic_fail = 0;
     uint32_t max_cnt = 0;
 
-    int32_t max_idx = 0;
-
-    uint8_t buff[BOOT_SWAP_STATUS_ROW_SZ];
-
     const struct flash_area *fap_stat = NULL;
 
     rc = flash_area_open(FLASH_AREA_IMAGE_SWAP_STATUS, &fap_stat);
     if (rc != 0) {
-        return BOOT_EFLASH;
+        return -1;
     }
+    else {
+        /* loop over copies/duplicates */
+        for (uint32_t i = 0; i < BOOT_SWAP_STATUS_MULT; i++) {
+            /* calculate final duplicate offset */
+            fin_offset = rec_offset + i * BOOT_SWAP_STATUS_D_SIZE;
 
-    /* loop over copies/duplicates */
-    for(uint32_t i = 0; i<BOOT_SWAP_STATUS_MULT; i++) {
-        /* calculate final duplicate offset */
-        fin_offset = rec_offset + i*BOOT_SWAP_STATUS_D_SIZE;
-
-        rc = flash_area_read(fap_stat, fin_offset, buff, sizeof(buff));
-        if (rc != 0) {
-            return BOOT_EFLASH;
-        }
-        /* read magic value to know if area was pre-erased */
-        magic = *((uint32_t *)&buff[BOOT_SWAP_STATUS_ROW_SZ -\
-                                  BOOT_SWAP_STATUS_MGCREC_SZ -\
-                                  BOOT_SWAP_STATUS_CNT_SZ-\
-                                  BOOT_SWAP_STATUS_CRC_SZ]);
-        if (magic == BOOT_SWAP_STATUS_MAGIC) {   /* read CRC */
-            crc = *((uint32_t *)&buff[BOOT_SWAP_STATUS_ROW_SZ -\
-                                      BOOT_SWAP_STATUS_CRC_SZ]);
-            /* check record data integrity first */
-            if (crc == calc_record_crc(buff, BOOT_SWAP_STATUS_ROW_SZ-BOOT_SWAP_STATUS_CRC_SZ)) {
-                /* look for counter */
-                counter = *((uint32_t *)&buff[BOOT_SWAP_STATUS_ROW_SZ -\
-                                              BOOT_SWAP_STATUS_CNT_SZ - \
-                                              BOOT_SWAP_STATUS_CRC_SZ]);
-                /* find out counter max */
-                if (counter >= max_cnt) {
-                    max_cnt = counter;
-                    max_idx = (int32_t)i;
-                    data_offset = fin_offset;
+            rc = flash_area_read(fap_stat, fin_offset, record_buff, sizeof(record_buff));
+            if (rc != 0) {
+                return -1;
+            }
+            else {
+                /* read magic value to know if area was pre-erased */
+                magic = pack_bytes_u32(&record_buff[BOOT_SWAP_STATUS_PAYLD_SZ]);
+                if (magic == BOOT_SWAP_STATUS_MAGIC) {   /* read CRC */
+                    crc = pack_bytes_u32(&record_buff[BOOT_SWAP_STATUS_ROW_SZ -
+                                            BOOT_SWAP_STATUS_CRC_SZ]);
+                    /* check record data integrity first */
+                    if (crc == calc_record_crc(record_buff, BOOT_SWAP_STATUS_ROW_SZ-BOOT_SWAP_STATUS_CRC_SZ)) {
+                        /* look for counter */
+                        counter = pack_bytes_u32(&record_buff[BOOT_SWAP_STATUS_ROW_SZ -
+                                                    BOOT_SWAP_STATUS_CNT_SZ -
+                                                    BOOT_SWAP_STATUS_CRC_SZ]);
+                        /* find out counter max */
+                        if (counter >= max_cnt) {
+                            max_cnt = counter;
+                            *max_idx = i;
+                            data_offset = fin_offset;
+                        }
+                    }
+                    /* if crc != calculated() */
+                    else {
+                        crc_fail++;
+                    }
+                }
+                else {
+                    magic_fail++;
                 }
             }
-            /* if crc != calculated() */
-            else {
-                crc_fail++;
-            }
-        }
-        else {
-            magic_fail++;
         }
     }
     /* no magic found - status area is pre-erased, start from scratch */
     if (magic_fail == BOOT_SWAP_STATUS_MULT) {
         /* emulate last index was received, so next will start from beginning */
-        max_idx = (int32_t)(BOOT_SWAP_STATUS_MULT-1U);
+        *max_idx = BOOT_SWAP_STATUS_MULT - 1U;
         *copy_counter = 0;
         /* return all erased values */
         (void)memset(data, (int32_t)flash_area_erased_val(fap_stat), BOOT_SWAP_STATUS_PAYLD_SZ);
@@ -188,21 +170,21 @@
     else {
         /* no valid CRC found - status pre-read failure */
         if (crc_fail == BOOT_SWAP_STATUS_MULT) {
-            max_idx = -1;
+            rc = -1;
         }
         else {
             *copy_counter = max_cnt;
             /* read payload data */
             rc = flash_area_read(fap_stat, data_offset, data, BOOT_SWAP_STATUS_PAYLD_SZ);
-	        if (rc != 0) {
-	            return BOOT_EFLASH;
-	        }
+            if (rc != 0) {
+                rc = -1;
+            }
         }
     }
     flash_area_close(fap_stat);
 
     /* return back duplicate index */
-    return max_idx;
+    return rc;
 }
 
 static int swap_status_write_record(uint32_t rec_offset, uint32_t copy_num, uint32_t copy_counter, const uint8_t *data)
@@ -214,35 +196,28 @@
     uint32_t next_counter = copy_counter + 1U;
     uint32_t next_crc;
 
-    uint8_t buff[BOOT_SWAP_STATUS_ROW_SZ];
-
     const struct flash_area *fap_stat = NULL;
 
     rc = flash_area_open(FLASH_AREA_IMAGE_SWAP_STATUS, &fap_stat);
     if (rc != 0) {
-        return BOOT_EFLASH;
+        return -1;
     }
 
     /* copy data into buffer */
-    (void)memcpy(buff, data, BOOT_SWAP_STATUS_PAYLD_SZ);
+    (void)memcpy(record_buff, data, BOOT_SWAP_STATUS_PAYLD_SZ);
     /* append next counter to whole record row */
-    (void)memcpy(&buff[BOOT_SWAP_STATUS_ROW_SZ-BOOT_SWAP_STATUS_CNT_SZ-BOOT_SWAP_STATUS_CRC_SZ], \
-            &next_counter, \
+    (void)memcpy(&record_buff[BOOT_SWAP_STATUS_ROW_SZ-BOOT_SWAP_STATUS_CNT_SZ-BOOT_SWAP_STATUS_CRC_SZ],
+            (uint8_t *)&next_counter,
             BOOT_SWAP_STATUS_CNT_SZ);
     /* append record magic */
-    (void)memcpy(&buff[BOOT_SWAP_STATUS_ROW_SZ-\
-                    BOOT_SWAP_STATUS_MGCREC_SZ-\
-                    BOOT_SWAP_STATUS_CNT_SZ-\
-                    BOOT_SWAP_STATUS_CRC_SZ], \
-                    stat_part_magic, \
-                    BOOT_SWAP_STATUS_MGCREC_SZ);
+    (void)memcpy(&record_buff[BOOT_SWAP_STATUS_PAYLD_SZ], (const uint8_t *)stat_part_magic, BOOT_SWAP_STATUS_MGCREC_SZ);
 
     /* calculate CRC field*/
-    next_crc = calc_record_crc(buff, BOOT_SWAP_STATUS_ROW_SZ-BOOT_SWAP_STATUS_CRC_SZ);
+    next_crc = calc_record_crc(record_buff, BOOT_SWAP_STATUS_ROW_SZ-BOOT_SWAP_STATUS_CRC_SZ);
 
     /* append new CRC to whole record row */
-    (void)memcpy(&buff[BOOT_SWAP_STATUS_ROW_SZ-BOOT_SWAP_STATUS_CRC_SZ], \
-            &next_crc, \
+    (void)memcpy(&record_buff[BOOT_SWAP_STATUS_ROW_SZ-BOOT_SWAP_STATUS_CRC_SZ],
+            (uint8_t *)&next_crc,
             BOOT_SWAP_STATUS_CRC_SZ);
 
     /* we already know what copy number was last and correct */
@@ -256,80 +231,108 @@
     }
     fin_offset = rec_offset + copy_num*BOOT_SWAP_STATUS_D_SIZE;
 
+    /* erase obsolete status record before write */
+    rc = flash_area_erase(fap_stat, fin_offset, sizeof(record_buff));
+    if (rc != 0) {
+        return -1;
+    }
+
     /* write prepared record into flash */
-    rc = flash_area_write(fap_stat, fin_offset, buff, sizeof(buff));
+    rc = flash_area_write(fap_stat, fin_offset, record_buff, sizeof(record_buff));
 
     flash_area_close(fap_stat);
 
     return rc;
 }
 
+static int boot_magic_decode(uint8_t *magic)
+{
+    if (memcmp(magic, (const uint8_t *)boot_img_magic, BOOT_MAGIC_SZ) == 0) {
+        return BOOT_MAGIC_GOOD;
+    }
+    return BOOT_MAGIC_BAD;
+}
+
 /**
  * Updates len bytes of status partition with values from *data-pointer.
  *
- * @param targ_area_id  Target area id for which status is being written.
- *                      Not a status-partition area id.
- * @param offset        Status byte offset inside status table. Should not include CRC and CNT.
- * @param data          Pointer to data status table to needs to be updated with.
- * @param len           Number of bytes to be written
+ * @param target_area_id Target area id for which status is being written.
+ *                       Not a status-partition area id.
+ * @param offset         Status byte offset inside status table. Should not include CRC and CNT.
+ * @param data           Pointer to data status table to needs to be updated with.
+ * @param len            Number of bytes to be written
  *
- * @return              0 on success; nonzero on failure.
+ * @return               0 on success; -1 on failure.
  */
-int swap_status_update(uint32_t targ_area_id, uint32_t offs, const void *data, uint32_t len)
+int swap_status_update(uint8_t target_area_id, uint32_t offs, const void *data, uint32_t len)
 {
     int rc = -1;
 
     int32_t init_offs;
-    int32_t length = (int32_t)len;
-    int32_t copy_num;
+    uint32_t copy_num = 0;
 
     uint32_t rec_offs;
     uint32_t copy_sz;
-    uint32_t copy_counter;
+    uint32_t copy_counter = 0;
     uint32_t data_idx = 0;
-    uint32_t buff_idx = offs%BOOT_SWAP_STATUS_PAYLD_SZ;
+    uint32_t buff_idx = offs % BOOT_SWAP_STATUS_PAYLD_SZ;
 
-    uint8_t buff[BOOT_SWAP_STATUS_PAYLD_SZ];
+    if ((UINT32_MAX - offs) < len) {
+        return -1;
+    }
 
     /* check if end of data is still inside writable area */
-    assert ((int)((offs + len) <= BOOT_SWAP_STATUS_D_SIZE_RAW));
+    if ((offs + len) > BOOT_SWAP_STATUS_D_SIZE_RAW) {
+        return -1;
+    }
 
     /* pre-calculate sub-area offset */
-    init_offs = swap_status_init_offset(targ_area_id);
-    assert ((int)(init_offs >= 0));
+    init_offs = swap_status_init_offset(target_area_id);
+    if (init_offs < 0) {
+        return -1;
+    }
 
     /* will start from it
      * this will be write-aligned */
     rec_offs = (uint32_t)init_offs + calc_record_offs(offs);
 
     /* go over all records to be updated */
-    while (length > 0) {
+    while (len != 0U) {
         /* preserve record */
-        copy_num = swap_status_read_record(rec_offs, buff, &copy_counter);
+        rc = swap_status_read_record(rec_offs, status_buff, &copy_counter, &copy_num);
         /* it returns copy number */
-        if (copy_num < 0)
+        if (rc < 0)
         {   /* something went wrong while read, exit */
             rc = -1;
             break;
         }
         /* update record data */
-        if (length > (int)BOOT_SWAP_STATUS_PAYLD_SZ) {
+        if (len > BOOT_SWAP_STATUS_PAYLD_SZ) {
             copy_sz = BOOT_SWAP_STATUS_PAYLD_SZ - buff_idx;
         }
         else {
-            copy_sz = (uint32_t)length;
+            copy_sz = len;
         }
-        (void)memcpy((void *)&buff[buff_idx], &((uint8_t *)data)[data_idx], copy_sz);
+
+        (void)memcpy(status_buff + buff_idx, (const uint8_t *)data + data_idx, copy_sz);
         buff_idx = 0;
 
         /* write record back */
-        rc = swap_status_write_record(rec_offs, (uint32_t)copy_num, copy_counter, buff);
-        assert ((int)(rc == 0));
+        rc = swap_status_write_record(rec_offs, copy_num, copy_counter, status_buff);
+        if (rc != 0) {
+            break;
+        }
 
         /* proceed to next record */
-        length -= (int32_t)BOOT_SWAP_STATUS_PAYLD_SZ;
-        rec_offs += BOOT_SWAP_STATUS_ROW_SZ;
-        data_idx += BOOT_SWAP_STATUS_PAYLD_SZ;
+        if (len < BOOT_SWAP_STATUS_PAYLD_SZ) {
+            len = 0;
+        }
+        else {
+            len -= BOOT_SWAP_STATUS_PAYLD_SZ;
+            rec_offs += BOOT_SWAP_STATUS_ROW_SZ;
+            data_idx += BOOT_SWAP_STATUS_PAYLD_SZ;
+        }
+
     }
     return rc;
 }
@@ -337,68 +340,206 @@
 /**
  * Reads len bytes of status partition with values from *data-pointer.
  *
- * @param targ_area_id  Target area id for which status is being read.
- *                      Not a status-partition area id.
- * @param offset        Status byte offset inside status table. Should not include CRC and CNT.
- * @param data          Pointer to data where status table values will be written.
- * @param len           Number of bytes to be read from status table.
+ * @param target_area_id Target area id for which status is being read.
+ *                       Not a status-partition area id.
+ * @param offset         Status byte offset inside status table. Should not include CRC and CNT.
+ * @param data           Pointer to data where status table values will be written.
+ * @param len            Number of bytes to be read from status table.
  *
- * @return              0 on success; nonzero on failure.
+ * @return               0 on success; -1 on failure.
  */
-int swap_status_retrieve(uint32_t target_area_id, uint32_t offs, void *data, uint32_t len)
+int swap_status_retrieve(uint8_t target_area_id, uint32_t offs, void *data, uint32_t len)
 {
     int rc = 0;
 
     int32_t init_offs;
-    int32_t length = (int32_t)len;
-    int32_t copy_num;
+    uint32_t copy_num = 0;
 
     uint32_t rec_offs;
     uint32_t copy_sz;
-    uint32_t copy_counter;
+    uint32_t copy_counter = 0;
     uint32_t data_idx = 0;
     uint32_t buff_idx = offs % BOOT_SWAP_STATUS_PAYLD_SZ;
 
-    uint8_t buff[BOOT_SWAP_STATUS_PAYLD_SZ];
+    if ((UINT32_MAX - offs) < len) {
+        return -1;
+    }
 
     /* check if end of data is still inside writable area */
-    // TODO: update for multi image
-    assert ((int)((offs + len) <= BOOT_SWAP_STATUS_D_SIZE_RAW));
+    if ((offs + len) > BOOT_SWAP_STATUS_D_SIZE_RAW) {
+        return -1;
+    }
 
     /* pre-calculate sub-area offset */
     init_offs = swap_status_init_offset(target_area_id);
-    assert ((int)(init_offs >= 0));
+    if (init_offs < 0) {
+        return -1;
+    }
 
     /* will start from it
      * this will be write-aligned */
     rec_offs = (uint32_t)init_offs + calc_record_offs(offs);
 
     /* go over all records to be updated */
-    while (length > 0) {
+    while (len != 0U) {
         /* preserve record */
-        copy_num = swap_status_read_record(rec_offs, buff, &copy_counter);
+        rc = swap_status_read_record(rec_offs, status_buff, &copy_counter, &copy_num);
         /* it returns copy number */
-        if (copy_num < 0) {
+        if (rc < 0) {
             /* something went wrong while read, exit */
             rc = -1;
             break;
         }
         /* update record data */
-        if (length > (int)BOOT_SWAP_STATUS_PAYLD_SZ) {
+        if (len > BOOT_SWAP_STATUS_PAYLD_SZ) {
             copy_sz = BOOT_SWAP_STATUS_PAYLD_SZ - buff_idx;
         }
         else {
-            copy_sz = (uint32_t)length;
+            copy_sz = len;
         }
-        (void)memcpy(&((uint8_t *)data)[data_idx], &buff[buff_idx], copy_sz);
+
+        (void)memcpy((uint8_t *)data + data_idx, status_buff + buff_idx, copy_sz);
         buff_idx = 0;
 
         /* proceed to next record */
-        length -= (int32_t)BOOT_SWAP_STATUS_PAYLD_SZ;
-        rec_offs += BOOT_SWAP_STATUS_ROW_SZ;
-        data_idx += BOOT_SWAP_STATUS_PAYLD_SZ;
+        if (len < BOOT_SWAP_STATUS_PAYLD_SZ) {
+            len = 0;
+        }
+        else {
+            len -= BOOT_SWAP_STATUS_PAYLD_SZ;
+            rec_offs += BOOT_SWAP_STATUS_ROW_SZ;
+            data_idx += BOOT_SWAP_STATUS_PAYLD_SZ;
+        }
     }
     return rc;
 }
 
+/**
+ * Copy trailer from status partition to primary image and set copy_done flag.
+ * This function calls only once, before set copy_done flag in status trailer
+ *
+ * @param fap           Target area id for which status is being read.
+ *
+ * @return              0 on success; -1 on failure.
+ */
+int swap_status_to_image_trailer(const struct flash_area *fap)
+{
+    uint8_t erased_val;
+    uint8_t stat_erased_val;
+    const uint8_t *copy_src;
+    uint32_t cur_trailer_pos;
+    uint32_t primary_trailer_sz;
+    uint32_t primary_trailer_buf_sz;
+    uint32_t align = CY_FLASH_ALIGN;
+    int rc = 0;
+    const struct flash_area *fap_stat = NULL;
+    uint8_t primary_trailer_buf[MAX_TRAILER_BUF_SIZE];
+
+    /* get the erased flash byte for status partition */
+    rc = flash_area_open(FLASH_AREA_IMAGE_SWAP_STATUS, &fap_stat);
+    if (rc != 0) {
+        return rc;
+    }
+    stat_erased_val = flash_area_erased_val(fap_stat);
+    flash_area_close(fap_stat);
+
+    /* get status partition trailer size and copy it to buffer */
+    const uint32_t status_trailer_buf_sz = BOOT_SWAP_STATUS_SWAPSZ_SZ + BOOT_SWAP_STATUS_SWAPINF_SZ +
+                BOOT_SWAP_STATUS_COPY_DONE_SZ + BOOT_SWAP_STATUS_IMG_OK_SZ + BOOT_SWAP_STATUS_MAGIC_SZ;
+    uint8_t status_trailer_buf[status_trailer_buf_sz];
+    (void)memset(&status_trailer_buf, 0, status_trailer_buf_sz);
+    rc = swap_status_retrieve(fap->fa_id, boot_swap_size_off(fap), (uint8_t *)status_trailer_buf, status_trailer_buf_sz);
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* check trailer magic in status partition */
+    if (boot_magic_decode(&status_trailer_buf[status_trailer_buf_sz - BOOT_SWAP_STATUS_MAGIC_SZ]) != BOOT_MAGIC_GOOD) {
+        return -1;
+    }
+
+    /* get primary slot trailer size without status data fields */
+    primary_trailer_sz = boot_trailer_sz(0);
+
+    /* align image trailer buffer size to minimal write size */
+#if !defined(__BOOTSIM__)
+    align = flash_area_align(fap);
+#else
+    align = CY_FLASH_ALIGN;
+#endif
+
+    if ((align > MAX_TRAILER_BUF_SIZE) || (align == 0U)) {
+        return -1;
+    }
+
+    primary_trailer_buf_sz = align;
+    while (primary_trailer_buf_sz < primary_trailer_sz) {
+        primary_trailer_buf_sz += align;
+    }
+    if (primary_trailer_buf_sz > MAX_TRAILER_BUF_SIZE) {
+        return -1;
+    }
+
+    /* erase primary slot trailer */
+    rc= flash_area_erase(fap, fap->fa_size - primary_trailer_buf_sz, primary_trailer_buf_sz);
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* erase trailer area */
+    erased_val = flash_area_erased_val(fap); /* stat_erased_val and erased_val may differ! */
+    (void)memset(&primary_trailer_buf[primary_trailer_buf_sz-primary_trailer_sz], (int) erased_val, primary_trailer_sz);
+
+    /* copy and align flags and data from status_trailer_buf to primary_trailer_buf
+                                         Status part trailer --> Pimary image trailer */
+
+    /* copy trailer magic */
+    cur_trailer_pos = primary_trailer_buf_sz - BOOT_SWAP_STATUS_MAGIC_SZ;
+    copy_src = &status_trailer_buf[status_trailer_buf_sz - BOOT_SWAP_STATUS_MAGIC_SZ];
+    if (stat_erased_val != erased_val &&
+        bootutil_buffer_is_filled(copy_src, stat_erased_val, BOOT_SWAP_STATUS_MAGIC_SZ)) {
+
+        (void)memset(&primary_trailer_buf[cur_trailer_pos], (int)erased_val, BOOT_SWAP_STATUS_MAGIC_SZ);
+    }
+    else {
+        (void)memcpy(&primary_trailer_buf[cur_trailer_pos], copy_src, BOOT_SWAP_STATUS_MAGIC_SZ);
+    }
+
+    /* copy image_ok flag */
+    cur_trailer_pos -= BOOT_MAX_ALIGN;
+    copy_src = &status_trailer_buf[BOOT_SWAP_STATUS_SWAPSZ_SZ + BOOT_SWAP_STATUS_SWAPINF_SZ + BOOT_SWAP_STATUS_COPY_DONE_SZ];
+    if (stat_erased_val != erased_val && stat_erased_val == *copy_src) {
+        copy_src = &erased_val;
+    }
+    primary_trailer_buf[cur_trailer_pos] = *copy_src;
+
+    /* set copy_done flag */
+    cur_trailer_pos -= BOOT_MAX_ALIGN;
+    primary_trailer_buf[cur_trailer_pos] = BOOT_FLAG_SET;
+
+    /* copy swap_type flag */
+    cur_trailer_pos -= BOOT_MAX_ALIGN;
+    copy_src = &status_trailer_buf[BOOT_SWAP_STATUS_SWAPSZ_SZ];
+    if (stat_erased_val != erased_val && stat_erased_val == *copy_src) {
+        copy_src = &erased_val;
+    }
+    primary_trailer_buf[cur_trailer_pos] = *copy_src;
+
+    /* copy swap_size field */
+    cur_trailer_pos -= BOOT_MAX_ALIGN;
+    if (stat_erased_val != erased_val &&
+        bootutil_buffer_is_filled(status_trailer_buf, stat_erased_val, BOOT_SWAP_STATUS_SWAPSZ_SZ)) {
+
+        (void)memset(&primary_trailer_buf[cur_trailer_pos], (int)erased_val, BOOT_SWAP_STATUS_SWAPSZ_SZ);
+    }
+    else {
+        (void)memcpy(&primary_trailer_buf[cur_trailer_pos], status_trailer_buf, BOOT_SWAP_STATUS_SWAPSZ_SZ);
+    }
+
+    /* write primary image trailer with copy_done flag set */
+    rc = flash_area_write(fap, fap->fa_size - primary_trailer_buf_sz, primary_trailer_buf, primary_trailer_buf_sz);
+
+    return rc;
+}
+
 #endif /* MCUBOOT_SWAP_USING_STATUS */
diff --git a/boot/bootutil/src/tlv.c b/boot/bootutil/src/tlv.c
index 37d12a3..00f5a04 100644
--- a/boot/bootutil/src/tlv.c
+++ b/boot/bootutil/src/tlv.c
@@ -98,7 +98,9 @@
     struct image_tlv tlv;
     int rc;
 
-    if (it == NULL || it->hdr == NULL || it->fap == NULL) {
+    if (it == NULL || it->hdr == NULL || it->fap == NULL ||
+        off == NULL || len == NULL) {
+
         return -1;
     }