diff --git a/boot/bootutil/include/bootutil/boot_status.h b/boot/bootutil/include/bootutil/boot_status.h
index d1be1e4..674ce4b 100644
--- a/boot/bootutil/include/bootutil/boot_status.h
+++ b/boot/bootutil/include/bootutil/boot_status.h
@@ -65,18 +65,18 @@
  */
 
 /* General macros to handle TLV type */
-#define MAJOR_MASK 0xFu     /* 4  bit */
-#define MAJOR_POS  12u      /* 12 bit */
-#define MINOR_MASK 0xFFFu   /* 12 bit */
+#define MAJOR_MASK (0xFu)     /* 4  bit */
+#define MAJOR_POS  (12u)      /* 12 bit */
+#define MINOR_MASK (0xFFFu)   /* 12 bit */
 
 #define SET_TLV_TYPE(major, minor) \
-        (((uint16_t)((major) & MAJOR_MASK) << MAJOR_POS) \
+        (((uint16_t)((uint16_t)(major) & MAJOR_MASK) << MAJOR_POS) \
         | ((minor) & MINOR_MASK))
 #define GET_MAJOR(tlv_type) ((uint16_t)(tlv_type) >> MAJOR_POS)
 #define GET_MINOR(tlv_type) ((tlv_type) & MINOR_MASK)
 
 /* Magic value which marks the beginning of shared data area in memory */
-#define SHARED_DATA_TLV_INFO_MAGIC 0x2016u
+#define SHARED_DATA_TLV_INFO_MAGIC (0x2016u)
 
 /* Initial attestation specific macros */
 
@@ -84,24 +84,24 @@
  * Major numbers (4 bit) to identify the
  * consumer of shared data in runtime SW.
  */
-#define TLV_MAJOR_IAS    0x1u
-#define TLV_MAJOR_FWU    0x2u
+#define TLV_MAJOR_IAS    (0x1u)
+#define TLV_MAJOR_FWU    (0x2u)
 
 /* Initial attestation: Claim per SW components / SW modules */
 /* Bits: 0-2 */
-#define SW_VERSION       0x00u
-#define SW_SIGNER_ID     0x01u
+#define SW_VERSION       (0x00u)
+#define SW_SIGNER_ID     (0x01u)
 /* Reserved              0x02u */
-#define SW_TYPE          0x03u
+#define SW_TYPE          (0x03u)
 /* Bits: 3-5 */
-#define SW_MEASURE_VALUE 0x08u
-#define SW_MEASURE_TYPE  0x09u
-#define SW_BOOT_RECORD   0x3Fu
+#define SW_MEASURE_VALUE (0x08u)
+#define SW_MEASURE_TYPE  (0x09u)
+#define SW_BOOT_RECORD   (0x3Fu)
 
-#define MODULE_POS            6u    /* 6 bit */
-#define MODULE_MASK           0x3Fu /* 6 bit */
-#define CLAIM_MASK            0x3Fu /* 6 bit */
-#define MEASUREMENT_CLAIM_POS 3u    /* 3 bit */
+#define MODULE_POS            (6u)    /* 6 bit */
+#define MODULE_MASK           (0x3Fu) /* 6 bit */
+#define CLAIM_MASK            (0x3Fu) /* 6 bit */
+#define MEASUREMENT_CLAIM_POS (3u)    /* 3 bit */
 
 #define GET_IAS_MODULE(tlv_type) ((uint16_t)GET_MINOR(tlv_type) >> MODULE_POS)
 #define GET_IAS_CLAIM(tlv_type)  (GET_MINOR(tlv_type) & CLAIM_MASK)
@@ -109,8 +109,8 @@
         (((uint16_t)(sw_module) << MODULE_POS) | (claim))
 
 #define SET_FWU_MINOR(sw_module, claim)                    \
-    ((uint16_t)((sw_module & MODULE_MASK) << MODULE_POS) | \
-     (uint16_t)(claim & CLAIM_MASK))
+    ((uint16_t)(((uint16_t)(sw_module) & MODULE_MASK) << MODULE_POS) | \
+     (uint16_t)((uint16_t)(claim) & CLAIM_MASK))
 
 /**
  * Shared data TLV header.  All fields in little endian.
@@ -124,7 +124,7 @@
     uint16_t tlv_tot_len; /* size of whole TLV area (including this header) */
 };
 
-#define SHARED_DATA_HEADER_SIZE sizeof(struct shared_data_tlv_header)
+#define SHARED_DATA_HEADER_SIZE (sizeof(struct shared_data_tlv_header))
 
 /**
  * Shared data TLV entry header format. All fields in little endian.
@@ -140,8 +140,8 @@
     uint16_t tlv_len; /* TLV data length (not including this header). */
 };
 
-#define SHARED_DATA_ENTRY_HEADER_SIZE sizeof(struct shared_data_tlv_entry)
-#define SHARED_DATA_ENTRY_SIZE(size) (size + SHARED_DATA_ENTRY_HEADER_SIZE)
+#define SHARED_DATA_ENTRY_HEADER_SIZE (sizeof(struct shared_data_tlv_entry))
+#define SHARED_DATA_ENTRY_SIZE(size)  ((size) + SHARED_DATA_ENTRY_HEADER_SIZE)
 
 /* Structure to store the boot data for the runtime SW. */
 struct shared_boot_data {
diff --git a/boot/bootutil/include/bootutil/bootutil.h b/boot/bootutil/include/bootutil/bootutil.h
index 6ea66ff..4e9fc38 100644
--- a/boot/bootutil/include/bootutil/bootutil.h
+++ b/boot/bootutil/include/bootutil/bootutil.h
@@ -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
  *
  * Original license:
  *
@@ -32,10 +32,22 @@
 #include "bootutil/fault_injection_hardening.h"
 #include "bootutil/bootutil_public.h"
 
+#ifdef MCUBOOT_ENC_IMAGES_XIP
+#include "bootutil/enc_key.h"
+#endif
+
 #ifdef __cplusplus
 extern "C" {
 #endif
 
+#ifdef MCUBOOT_IMAGE_NUMBER
+#define BOOT_IMAGE_NUMBER          MCUBOOT_IMAGE_NUMBER
+#else
+#define BOOT_IMAGE_NUMBER          1
+#endif
+
+_Static_assert(BOOT_IMAGE_NUMBER > 0, "Invalid value for BOOT_IMAGE_NUMBER");
+
 struct image_header;
 /**
  * A response object provided by the boot loader code; indicates where to jump
@@ -68,14 +80,25 @@
     uint8_t pad2[BOOT_MAX_ALIGN - 1];
     uint8_t image_ok;
     uint8_t pad3[BOOT_MAX_ALIGN - 1];
-    uint8_t magic[16];
+#if BOOT_MAX_ALIGN > BOOT_MAGIC_SZ
+    uint8_t pad4[BOOT_MAGIC_ALIGN_SIZE - BOOT_MAGIC_SZ];
+#endif
+    uint8_t magic[BOOT_MAGIC_SZ];
 };
 
 /* you must have pre-allocated all the entries within this structure */
 fih_int boot_go(struct boot_rsp *rsp);
+fih_int boot_go_for_image_id(struct boot_rsp *rsp, uint32_t image_id);
 
 struct boot_loader_state;
-fih_int context_boot_go(struct boot_loader_state *state, struct boot_rsp *rsp);
+void boot_state_clear(struct boot_loader_state *state);
+fih_int context_boot_go_flash(struct boot_loader_state *state, struct boot_rsp *rsp);
+
+#if defined(MCUBOOT_RAM_LOAD)
+fih_int context_boot_go_ram(struct boot_loader_state *state, struct boot_rsp *rsp);
+fih_int boot_go_for_image_id_ram(struct boot_rsp *rsp, uint32_t image_id);
+#endif
+
 
 #define SPLIT_GO_OK                 (0)
 #define SPLIT_GO_NON_MATCHING       (-1)
@@ -87,4 +110,4 @@
 }
 #endif
 
-#endif
\ No newline at end of file
+#endif
diff --git a/boot/bootutil/include/bootutil/bootutil_public.h b/boot/bootutil/include/bootutil/bootutil_public.h
index adde43c..2c303d8 100644
--- a/boot/bootutil/include/bootutil/bootutil_public.h
+++ b/boot/bootutil/include/bootutil/bootutil_public.h
@@ -47,31 +47,51 @@
 extern "C" {
 #endif
 
+#ifndef ALIGN_UP
+#define ALIGN_UP(num, align)    (((num) + ((align) - 1U)) & ~((align) - 1U))
+#endif
+
+#ifndef ALIGN_DOWN
+#define ALIGN_DOWN(num, align)  ((num) & ~((align) - 1U))
+#endif
+
 /** Attempt to boot the contents of the primary slot. */
-#define BOOT_SWAP_TYPE_NONE     1
+#define BOOT_SWAP_TYPE_NONE     1U
 
 /**
  * Swap to the secondary slot.
  * Absent a confirm command, revert back on next boot.
  */
-#define BOOT_SWAP_TYPE_TEST     2
+#define BOOT_SWAP_TYPE_TEST     2U
 
 /**
  * Swap to the secondary slot,
  * and permanently switch to booting its contents.
  */
-#define BOOT_SWAP_TYPE_PERM     3
+#define BOOT_SWAP_TYPE_PERM     3U
 
 /** Swap back to alternate slot.  A confirm changes this state to NONE. */
-#define BOOT_SWAP_TYPE_REVERT   4
+#define BOOT_SWAP_TYPE_REVERT   4U
 
 /** Swap failed because image to be run is not valid */
-#define BOOT_SWAP_TYPE_FAIL     5
+#define BOOT_SWAP_TYPE_FAIL     5U
 
 /** Swapping encountered an unrecoverable error */
-#define BOOT_SWAP_TYPE_PANIC    0xff
+#define BOOT_SWAP_TYPE_PANIC    0xFFU
 
+#define BOOT_MAGIC_SZ           16U
+
+#ifdef MCUBOOT_BOOT_MAX_ALIGN
+
+_Static_assert(MCUBOOT_BOOT_MAX_ALIGN >= 8 && MCUBOOT_BOOT_MAX_ALIGN <= 32,
+               "Unsupported value for MCUBOOT_BOOT_MAX_ALIGN");
+
+#define BOOT_MAX_ALIGN          MCUBOOT_BOOT_MAX_ALIGN
+#define BOOT_MAGIC_ALIGN_SIZE   ALIGN_UP(BOOT_MAGIC_SZ, BOOT_MAX_ALIGN)
+#else
 #define BOOT_MAX_ALIGN          8U
+#define BOOT_MAGIC_ALIGN_SIZE   BOOT_MAGIC_SZ
+#endif
 
 #define BOOT_MAGIC_GOOD     1
 #define BOOT_MAGIC_BAD      2
@@ -87,8 +107,6 @@
 #define BOOT_FLAG_UNSET     3
 #define BOOT_FLAG_ANY       4  /* NOTE: control only, not dependent on sector */
 
-#define BOOT_MAGIC_SZ (sizeof boot_img_magic)
-
 #define BOOT_EFLASH      1
 #define BOOT_EFILE       2
 #define BOOT_EBADIMAGE   3
@@ -104,15 +122,15 @@
  * Extract the swap type and image number from image trailers's swap_info
  * filed.
  */
-#define BOOT_GET_SWAP_TYPE(swap_info)    ((swap_info) & 0x0F)
-#define BOOT_GET_IMAGE_NUM(swap_info)    ((swap_info) >> 4)
+#define BOOT_GET_SWAP_TYPE(swap_info)    ((swap_info) & 0x0FU)
+#define BOOT_GET_IMAGE_NUM(swap_info)    ((swap_info) >> 4U)
 
 /* Construct the swap_info field from swap type and image number */
-#define BOOT_SET_SWAP_INFO(swap_info, image, type)  {                          \
-                                                    assert((image) < 0xF);     \
-                                                    assert((type)  < 0xF);     \
-                                                    (swap_info) = (image) << 4 \
-                                                                | (type);      \
+#define BOOT_SET_SWAP_INFO(swap_info, image, type)  {                           \
+                                                    assert((image) < 0xFU);     \
+                                                    assert((type)  < 0xFU);     \
+                                                    (swap_info) = (image) << 4U;\
+                                                    (swap_info) |= (type);      \
                                                     }
 #ifdef MCUBOOT_HAVE_ASSERT_H
 #include "mcuboot_config/mcuboot_assert.h"
diff --git a/boot/bootutil/include/bootutil/crypto/ecdh_p256.h b/boot/bootutil/include/bootutil/crypto/ecdh_p256.h
index 962535c..3811037 100644
--- a/boot/bootutil/include/bootutil/crypto/ecdh_p256.h
+++ b/boot/bootutil/include/bootutil/crypto/ecdh_p256.h
@@ -124,7 +124,7 @@
         return -1;
     }
 
-    mbedtls_mpi_read_binary(&ctx->d, sk, NUM_ECC_BYTES);
+    (void)mbedtls_mpi_read_binary(&ctx->d, sk, NUM_ECC_BYTES);
 
 #if MBEDTLS_VERSION_NUMBER >= 0x03000000
     rc = mbedtls_ecdh_compute_shared(&ctx->grp,
@@ -141,7 +141,7 @@
                                      NULL,
                                      NULL);
 #endif
-    mbedtls_mpi_write_binary(&ctx->z, z, NUM_ECC_BYTES);
+    (void)mbedtls_mpi_write_binary(&ctx->z, z, NUM_ECC_BYTES);
 
     return rc;
 }
diff --git a/boot/bootutil/include/bootutil/enc_key.h b/boot/bootutil/include/bootutil/enc_key.h
index 449d9ec..bb04c0e 100644
--- a/boot/bootutil/include/bootutil/enc_key.h
+++ b/boot/bootutil/include/bootutil/enc_key.h
@@ -38,10 +38,7 @@
 extern "C" {
 #endif
 
-#define BOOT_ENC_KEY_SIZE_BITS  (BOOT_ENC_KEY_SIZE * 8)
-
-#define BOOT_ENC_TLV_ALIGN_SIZE \
-    ((((BOOT_ENC_TLV_SIZE - 1) / BOOT_MAX_ALIGN) + 1) * BOOT_MAX_ALIGN)
+#define BOOT_ENC_TLV_ALIGN_SIZE ALIGN_UP(BOOT_ENC_TLV_SIZE, BOOT_MAX_ALIGN)
 
 struct enc_key_data {
     uint8_t valid;
@@ -65,6 +62,11 @@
 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);
+#ifdef MCUBOOT_ENC_IMAGES_XIP
+int bootutil_img_encrypt(struct enc_key_data *enc_state, int image_index,
+        struct image_header *hdr, const struct flash_area *fap, uint32_t off, uint32_t sz,
+        uint32_t blk_off, uint8_t *buf);
+#endif /* MCUBOOT_ENC_IMAGES_XIP */
 void boot_enc_zeroize(struct enc_key_data *enc_state);
 
 #ifdef __cplusplus
diff --git a/boot/bootutil/include/bootutil/enc_key_public.h b/boot/bootutil/include/bootutil/enc_key_public.h
index 9ca16ac..9c77ff9 100644
--- a/boot/bootutil/include/bootutil/enc_key_public.h
+++ b/boot/bootutil/include/bootutil/enc_key_public.h
@@ -32,14 +32,20 @@
 extern "C" {
 #endif
 
+#ifndef ALIGN_UP
+#define ALIGN_UP(num, align)    (((num) + ((align) - 1U)) & ~((align) - 1U))
+#endif
+
 #ifdef MCUBOOT_AES_256
 #define BOOT_ENC_KEY_SIZE       32U
 #else
 #define BOOT_ENC_KEY_SIZE       16U
 #endif
 
+#define BOOT_ENC_KEY_ALIGN_SIZE ALIGN_UP(BOOT_ENC_KEY_SIZE, BOOT_MAX_ALIGN)
+
 #define TLV_ENC_RSA_SZ    256U
-#define TLV_ENC_KW_SZ     BOOT_ENC_KEY_SIZE + 8U
+#define TLV_ENC_KW_SZ     (BOOT_ENC_KEY_SIZE + 8U)
 #define TLV_ENC_EC256_SZ  (65U + 32U + BOOT_ENC_KEY_SIZE)
 #define TLV_ENC_X25519_SZ (32U + 32U + BOOT_ENC_KEY_SIZE)
 
diff --git a/boot/bootutil/include/bootutil/fault_injection_hardening.h b/boot/bootutil/include/bootutil/fault_injection_hardening.h
index bccdbc2..d067e7f 100644
--- a/boot/bootutil/include/bootutil/fault_injection_hardening.h
+++ b/boot/bootutil/include/bootutil/fault_injection_hardening.h
@@ -1,25 +1,30 @@
 /*
- * SPDX-License-Identifier: Apache-2.0
+ * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
  *
- * Copyright (c) 2020 Arm Limited
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
  */
 
 #ifndef FAULT_INJECTION_HARDENING_H
 #define FAULT_INJECTION_HARDENING_H
 
-/* Fault injection mitigation library.
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+
+/*
+ * Fault injection mitigation library.
  *
  * Has support for different measures, which can either be enabled/disabled
  * separately or by defining one of the MCUBOOT_FIH_PROFILEs.
  *
- * NOTE: These constructs against fault injection attacks are not guaranteed to
- *       be secure for all compilers, but execution is going to be correct and
- *       including them will certainly help to harden the code.
+ * NOTE: It is not guaranteed that these constructs against fault injection
+ *       attacks can be preserved in all compilers.
  *
  * FIH_ENABLE_DOUBLE_VARS makes critical variables into a tuple (x, x ^ msk).
  * Then the correctness of x can be checked by XORing the two tuple values
  * together. This also means that comparisons between fih_ints can be verified
- * by doing x == y && x_msk == y_msk.
+ * by doing x == y && x.msk == y_msk.
  *
  * FIH_ENABLE_GLOBAL_FAIL makes all while(1) failure loops redirect to a global
  * failure loop. This loop has mitigations against loop escapes / unlooping.
@@ -32,7 +37,7 @@
  * the counter has the same value as before this process. This can be used to
  * verify that the function has actually been called. This protection is
  * intended to discover that important functions are called in an expected
- * sequence and neither of them is missed due to an instruction skip which could
+ * sequence and none of them is missed due to an instruction skip which could
  * be a result of glitching attack. It does not provide protection against ROP
  * or JOP attacks.
  *
@@ -46,315 +51,1223 @@
  * fih_int fih_rc = FIH_FAILURE;
  * FIH_CALL(vulnerable_function, fih_rc, arg1, arg2);
  * if (fih_not_eq(fih_rc, FIH_SUCCESS)) {
- *     FIH_PANIC;
+ *     error_handling();
  * }
  *
- * Note that any function called by FIH_CALL must only return using FIH_RETURN,
+ * If a fault injection is detected, call FIH_PANIC to trap the execution.
+ *
+ * Note that any function called by FIH_CALL must only return using FIH_RET,
  * as otherwise the CFI counter will not be decremented and the CFI check will
  * fail causing a panic.
  */
 
-#include <stdbool.h>
-#include "mcuboot_config/mcuboot_config.h"
-
-#if defined(MCUBOOT_FIH_PROFILE_HIGH)
-
-#define FIH_ENABLE_DELAY         /* Requires an entropy source */
-#define FIH_ENABLE_DOUBLE_VARS
-#define FIH_ENABLE_GLOBAL_FAIL
-#define FIH_ENABLE_CFI
-
-#elif defined(MCUBOOT_FIH_PROFILE_MEDIUM)
-
-#define FIH_ENABLE_DOUBLE_VARS
-#define FIH_ENABLE_GLOBAL_FAIL
-#define FIH_ENABLE_CFI
-
-#elif defined(MCUBOOT_FIH_PROFILE_LOW)
-
-#define FIH_ENABLE_GLOBAL_FAIL
-#define FIH_ENABLE_CFI
-
-#elif !defined(MCUBOOT_FIH_PROFILE_OFF)
-#define MCUBOOT_FIH_PROFILE_OFF
-#endif /* MCUBOOT_FIH_PROFILE */
-
-#ifdef FIH_ENABLE_DELAY
-#include "fault_injection_hardening_delay_rng.h"
-#endif /* FIH_ENABLE_DELAY */
-
-
 #ifdef __cplusplus
 extern "C" {
 #endif /* __cplusplus */
 
-#define FIH_TRUE   ((int)1)
-#define FIH_FALSE  ((int)0)
+#include "mcuboot_config/mcuboot_config.h"
+#undef FIH_ENABLE_GLOBAL_FAIL
+#undef FIH_ENABLE_CFI
+#undef FIH_ENABLE_DOUBLE_VARS
+#undef FIH_ENABLE_DELAY
 
-/* Non-zero success value to defend against register resets. Zero is the most
- * common value for a corrupted register so complex bit-patterns are used
- */
-#ifndef MCUBOOT_FIH_PROFILE_OFF
-#define FIH_POSITIVE_VALUE 0x1AAAAAAA
-#define FIH_NEGATIVE_VALUE 0x15555555
+#ifdef CY_COVERITY_2012_CHECK
+#define CY_COVERITY_PRAGMA_STR(a) #a
+#define FIH_MISRA_DEVIATE_BLOCK_START(MISRA, COUNT, MESSAGE) \
+_Pragma(CY_COVERITY_PRAGMA_STR(coverity compliance block (deviate:COUNT MISRA MESSAGE)))
+
+#define FIH_MISRA_BLOCK_END(MISRA) \
+_Pragma(CY_COVERITY_PRAGMA_STR(coverity compliance end_block MISRA))
 #else
-#define FIH_POSITIVE_VALUE 0
-#define FIH_NEGATIVE_VALUE -1
-#endif
+#define FIH_MISRA_DEVIATE_BLOCK_START(MISRA, COUNT, MESSAGE)
+#define FIH_MISRA_BLOCK_END(MISRA)
+#endif /* CY_COVERITY_2012_CHECK */
 
-/* A volatile mask is used to prevent compiler optimization - the mask is xored
- * with the variable to create the backup and the integrity can be checked with
- * another xor. The mask value doesn't _really_ matter that much, as long as
- * it has reasonably high hamming weight.
- */
-#define FIH_MASK_VALUE 0xBEEF
+FIH_MISRA_DEVIATE_BLOCK_START('MISRA C-2012 Rule 10.1', 10, 'Signed integer bitwise operations');
+FIH_MISRA_DEVIATE_BLOCK_START('MISRA C-2012 Rule 10.4', 10, 'Signed integer bitwise operations');
+
+#ifdef MCUBOOT_FIH_PROFILE_ON
+#if defined(MCUBOOT_FIH_PROFILE_LOW)
+#define FIH_ENABLE_GLOBAL_FAIL
+#define FIH_ENABLE_CFI
+
+#elif defined(MCUBOOT_FIH_PROFILE_MEDIUM)
+#define FIH_ENABLE_DOUBLE_VARS
+#define FIH_ENABLE_GLOBAL_FAIL
+#define FIH_ENABLE_CFI
+
+#elif defined(MCUBOOT_FIH_PROFILE_HIGH)
+#define FIH_ENABLE_DELAY         /* Requires an entropy source */
+#define FIH_ENABLE_DOUBLE_VARS
+#define FIH_ENABLE_GLOBAL_FAIL
+#define FIH_ENABLE_CFI
+
+#else
+#error "Invalid FIH Profile configuration"
+#endif /* MCUBOOT_FIH_PROFILE */
+
+/* Where possible, glue the FIH_TRUE from two components. */
+#define FIH_TRUE_1              ((int32_t)0x300AUL)
+#define FIH_TRUE_2              ((int32_t)0x0C50UL)
+#define FIH_TRUE                ((int32_t)0x3C5AUL) /* i.e., FIH_TRUE_1 | FIH_TRUE_2 */
+#define FIH_FALSE               ((int32_t)0xA5C3UL)
+
+#define FIH_POSITIVE_VALUE      ((int32_t) 0x5555AAAAUL)
+#define FIH_NEGATIVE_VALUE      ((int32_t)-0x5555AAABL)
 
 #ifdef FIH_ENABLE_DOUBLE_VARS
+/*
+ * A volatile mask is used to prevent compiler optimization - the mask is XORed
+ * with the variable to create the backup and the integrity can be checked with
+ * another xor. The mask value doesn't _really_ matter that much, as long as
+ * it has reasonably high Hamming weight.
+ */
+#define FIH_MASK_VALUE          0xA5C35A3CU
+#define FIH_UINT_MASK_VALUE     0xB779A31CU
 
-/* All ints are replaced with two int - the normal one and a backup which is
+#define FIH_INT_VAL_MASK(val) ((int32_t)((val) ^ FIH_MASK_VALUE))
+#define FIH_UINT_VAL_MASK(val) ((val) ^ FIH_UINT_MASK_VALUE)
+
+/*
+ * All ints are replaced with two int - the normal one and a backup which is
  * XORed with the mask.
  */
-extern volatile int _fih_mask;
-
-typedef volatile struct {
-    volatile int val;
-    volatile int msk;
+typedef struct {
+    volatile int32_t val;
+    volatile int32_t msk;
 } fih_int;
 
-typedef volatile struct {
-    volatile unsigned int val;
-    volatile unsigned int msk;
+#define FIH_INT_INIT(x)         {(x), FIH_INT_VAL_MASK(x)}
+
+typedef struct {
+    volatile uint32_t val;
+    volatile uint32_t msk;
 } fih_uint;
 
-#else
+#define FIH_UINT_INIT(x)        {(x), FIH_UINT_VAL_MASK(x)}
 
-typedef int fih_int;
-typedef unsigned int fih_uint;
+#else /* FIH_ENABLE_DOUBLE_VARS */
+/*
+ * GCC issues a warning: "type qualifiers ignored on function return type"
+ * when return type is volatile. This is NOT an issue, and as a workaround
+ * separate types for return values introduced
+ */
+typedef struct {
+    volatile int32_t val;
+} fih_int;
 
+typedef struct {
+    volatile uint32_t val;
+} fih_uint;
+
+#define FIH_INT_INIT(x)         {(x)}
+#define FIH_UINT_INIT(x)        {(x)}
 #endif /* FIH_ENABLE_DOUBLE_VARS */
 
-extern fih_int FIH_SUCCESS;
-extern fih_int FIH_FAILURE;
+#define FIH_SUCCESS     (fih_int_encode(FIH_POSITIVE_VALUE))
+#define FIH_FAILURE     (fih_int_encode(FIH_NEGATIVE_VALUE))
+#define FIH_UINT_ZERO   (fih_uint_encode(0U))
+#define FIH_UINT_MAX    (fih_uint_encode(0xFFFFFFFFU))
 
 #ifdef FIH_ENABLE_GLOBAL_FAIL
-/* Global failure handler - more resistant to unlooping. noinline and used are
- * used to prevent optimization
+/**
+ * Global failure handler - more resistant to unlooping. "noinline" and "used"
+ * are used to prevent optimization.
+ * NOTE: this failure handler shall be used as FIH specific error handling to
+ * capture FI attacks.
+ * Error handling in SPM and SP should be enhanced respectively.
  */
-__attribute__((noinline)) __attribute__((used))
+__attribute__((noinline))
+__attribute__((noreturn))
 void fih_panic_loop(void);
 #define FIH_PANIC fih_panic_loop()
-#else
-#define FIH_PANIC while (true) {}
+#else /* FIH_ENABLE_GLOBAL_FAIL */
+#define FIH_PANIC  \
+        do { \
+            FIH_LABEL("FAILURE_LOOP"); \
+            while (true) {} \
+        } while (false)
 #endif  /* FIH_ENABLE_GLOBAL_FAIL */
 
-/* NOTE: For functions to be inlined outside their compilation unit they have to
+/*
+ * NOTE: for functions to be inlined outside their compilation unit they have to
  * have the body in the header file. This is required as function calls are easy
  * to skip.
  */
+
 #ifdef FIH_ENABLE_DELAY
+/**
+ * @brief Set up the RNG for use with random delays. Called once at startup.
+ */
+void fih_delay_init(void);
 
-/* Delaying logic, with randomness from a CSPRNG */
+/**
+ * Get a random uint8_t value from an RNG seeded with an entropy source.
+ * NOTE: do not directly call this function!
+ *
+ * @return   random value.
+ */
+uint8_t fih_delay_random(void);
+
+/**
+ * Delaying logic, with randomness from a CSPRNG.
+ */
 __attribute__((always_inline)) static inline
-int fih_delay(void)
+void fih_delay(void)
 {
-    unsigned char delay;
-    int foo = 0;
-    volatile int rc;
+    uint32_t i = 0;
+    volatile uint32_t delay = 10u; /* TODO: REMOVE */
+    volatile uint32_t counter = 0;
 
-    delay = fih_delay_random_uchar();
+#if 0
+    delay = fih_delay_random();
 
-    for (volatile int i = 0; i < delay; i++) {
-        foo++;
+    if (delay == FIH_NEGATIVE_VALUE) {
+        FIH_PANIC;
     }
 
-    rc = 1;
+    delay &= 0xFF;
+#endif
 
-    /* rc is volatile so if it is the return value then the function cannot be
-     * optimized
-     */
-    return rc;
+    for (i = 0; i < delay; i++) {
+        counter++;
+    }
+
+    if (counter != delay) {
+        FIH_PANIC;
+    }
 }
+#else /* FIH_ENABLE_DELAY */
+/* NOOP */
+#define fih_delay_init()
 
-#else
-
-__attribute__((always_inline)) static inline
-int fih_delay_init(void)
-{
-    return 1;
-}
-
-__attribute__((always_inline)) static inline
-int fih_delay(void)
-{
-    return 1;
-}
+/* NOOP */
+#define fih_delay()
 #endif /* FIH_ENABLE_DELAY */
 
 #ifdef FIH_ENABLE_DOUBLE_VARS
-
-/* Validate fih_int for tampering. */
-__attribute__((always_inline)) static inline
-void fih_int_validate(fih_int x)
-{
-    if (x.val != (x.msk ^ _fih_mask)) {
-        FIH_PANIC;
-    }
-}
-
-/* Convert a fih_int to an int. Validate for tampering. */
-__attribute__((always_inline)) static inline
-int fih_int_decode(fih_int x)
-{
-    fih_int_validate(x);
-    return x.val;
-}
-
-/* Convert an int to a fih_int, can be used to encode specific error codes. */
-__attribute__((always_inline)) static inline
-fih_int fih_int_encode(int x)
-{
-    fih_int ret = {x, x ^ _fih_mask};
-    return ret;
-}
-
-/* Standard equality for fih_int type. If A == B then 1, else 0 */
-__attribute__((always_inline)) static inline
-int fih_eq(fih_int x, fih_int y)
-{
-    fih_int_validate(x);
-    fih_int_validate(y);
-    return (x.val == y.val) && fih_delay() && (x.msk == y.msk);
-}
-
-/* Standard non equality for fih_int type. If A != B then 1, else 0 */
-__attribute__((always_inline)) static inline
-int fih_not_eq(fih_int x, fih_int y)
-{
-    fih_int_validate(x);
-    fih_int_validate(y);
-    return (x.val != y.val) && fih_delay() && (x.msk != y.msk);
-}
-
-/* Validate fih_uint for tampering. */
-__attribute__((always_inline)) static inline
-void fih_uint_validate(fih_uint x)
-{
-    if (x.val != (x.msk ^ _fih_mask)) {
-        FIH_PANIC;
-    }
-}
-
-/* Convert a fih_uint to an unsigned int. Validate for tampering. */
-__attribute__((always_inline)) static inline
-unsigned int fih_uint_decode(fih_uint x)
-{
-    fih_uint_validate(x);
-    return x.val;
-}
-
-/* Convert an unsigned int to a fih_uint, can be used to encode specific error codes. */
-__attribute__((always_inline)) static inline
-fih_uint fih_uint_encode(unsigned int x)
-{
-    fih_uint ret = {x, x ^ _fih_mask};
-    return ret;
-}
-
-/* Standard equality for fih_uint type. If A == B then 1, else 0 */
-__attribute__((always_inline)) static inline
-bool fih_uint_eq(fih_uint x, fih_uint y)
-{
-    fih_uint_validate(x);
-    fih_uint_validate(y);
-    return (x.val == y.val) && fih_delay() && (x.msk == y.msk);
-}
-
-/* Standard non equality for fih_uint type. If A != B then 1, else 0 */
-__attribute__((always_inline)) static inline
-bool fih_uint_not_eq(fih_uint x, fih_uint y)
-{
-    fih_uint_validate(x);
-    fih_uint_validate(y);
-    return (x.val != y.val) && fih_delay() && (x.msk != y.msk);
-}
-
-#else
-
-/* NOOP */
-__attribute__((always_inline)) static inline
-void fih_int_validate(fih_int x)
-{
-    (void) x;
-    return;
-}
-
-/* NOOP */
-__attribute__((always_inline)) static inline
-int fih_int_decode(fih_int x)
-{
-    return x;
-}
-
-/* NOOP */
-__attribute__((always_inline)) static inline
-fih_int fih_int_encode(int x)
-{
-    return x;
-}
-
-__attribute__((always_inline)) static inline
-int fih_eq(fih_int x, fih_int y)
-{
-    return (int) (x == y);
-}
-
-__attribute__((always_inline)) static inline
-int fih_not_eq(fih_int x, fih_int y)
-{
-    return (int) (x != y);
-}
-
-/* NOOP */
-__attribute__((always_inline)) static inline
-void fih_uint_validate(fih_uint x)
-{
-    (void) x;
-    return;
-}
-
-/* NOOP */
-__attribute__((always_inline)) static inline
-unsigned int fih_uint_decode(fih_uint x)
-{
-    return x;
-}
-
-/* NOOP */
-__attribute__((always_inline)) static inline
-fih_uint fih_uint_encode(unsigned int x)
-{
-    return x;
-}
-
-__attribute__((always_inline)) static inline
-bool fih_uint_eq(fih_uint x, fih_uint y)
-{
-    return x == y;
-}
-
-__attribute__((always_inline)) static inline
-bool fih_uint_not_eq(fih_uint x, fih_uint y)
-{
-    return x != y;
-}
-#endif /* FIH_ENABLE_DOUBLE_VARS */
-
-/* C has a common return pattern where 0 is a correct value and all others are
- * errors. This function converts 0 to FIH_SUCCESS and any other number to a
- * value that is not FIH_SUCCESS
+/**
+ * Validate fih_int for tampering.
+ *
+ * @param x  fih_int value to be validated.
  */
 __attribute__((always_inline)) static inline
-fih_int fih_int_encode_zero_equality(int x)
+void fih_int_validate(fih_int x)
+{
+    int32_t x_msk = x.msk;
+
+    if (x.val != FIH_INT_VAL_MASK(x_msk)) {
+        FIH_PANIC;
+    }
+}
+
+/**
+ * Validate fih_uint for tampering.
+ *
+ * @param x  fih_uint value to be validated.
+ */
+__attribute__((always_inline)) static inline
+void fih_uint_validate(fih_uint x)
+{
+    uint32_t x_msk = x.msk;
+
+    if (x.val != FIH_UINT_VAL_MASK(x_msk)) {
+        FIH_PANIC;
+    }
+}
+
+/**
+ * Convert a fih_int to an int. Validate for tampering.
+ *
+ * @param x  fih_int value to be converted.
+ *
+ * @return   Value converted to int.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_int_decode(fih_int x)
+{
+    fih_int_validate(x);
+    return x.val;
+}
+
+/**
+ * Convert a fih_uint to an unsigned int. Validate for tampering.
+ *
+ * @param x  fih_uint value to be converted.
+ *
+ * @return   Value converted to unsigned int.
+ */
+__attribute__((always_inline)) static inline
+uint32_t fih_uint_decode(fih_uint x)
+{
+    fih_uint_validate(x);
+    return x.val;
+}
+
+/**
+ * Convert an int to a fih_int, can be used to encode specific error codes.
+ *
+ * @param x  Integer value to be converted.
+ *
+ * @return   Value converted to fih_int.
+ */
+__attribute__((always_inline)) static inline
+fih_int fih_int_encode(int32_t x)
+{
+    fih_int ret = FIH_INT_INIT(x);
+    return ret;
+}
+
+/**
+ * Convert an unsigned int to a fih_uint, can be used to encode specific error
+ * codes.
+ *
+ * @param x  Unsigned integer value to be converted.
+ *
+ * @return   Value converted to fih_uint.
+ */
+__attribute__((always_inline)) static inline
+fih_uint fih_uint_encode(uint32_t x)
+{
+    fih_uint ret = FIH_UINT_INIT(x);
+    return ret;
+}
+
+/**
+ * Standard equality for fih_int values.
+ *
+ * @param x  1st fih_int value to be compared.
+ * @param y  2nd fih_int value to be compared.
+ *
+ * @return   FIH_TRUE if x == y, other otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_eq(fih_int x, fih_int y)
+{
+    int32_t y_val, y_msk;
+    volatile int32_t rc = FIH_FALSE;
+
+    fih_int_validate(x);
+    fih_int_validate(y);
+
+    y_val = y.val;
+    if (x.val == y_val) {
+        rc = FIH_TRUE_1;
+    }
+
+    fih_delay();
+
+    y_msk = y.msk;
+    if (x.msk == y_msk) {
+        rc |= FIH_TRUE_2;
+    }
+
+    fih_delay();
+
+    y_val = y.val;
+    if (x.val != y_val) {
+        if (rc == FIH_TRUE) {
+            FIH_PANIC;
+        }
+    }
+
+    return rc;
+}
+
+/**
+ * Standard equality for fih_uint values.
+ *
+ * @param x  1st fih_uint value to be compared.
+ * @param y  2nd fih_uint value to be compared.
+ *
+ * @return   FIH_TRUE if x == y, other otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_uint_eq(fih_uint x, fih_uint y)
+{
+    uint32_t y_val, y_msk;
+    volatile int32_t rc = FIH_FALSE;
+
+    fih_uint_validate(x);
+    fih_uint_validate(y);
+
+    y_val = y.val;
+    if (x.val == y_val) {
+        rc = FIH_TRUE_1;
+    }
+
+    fih_delay();
+
+    y_msk = y.msk;
+    if (x.msk == y_msk) {
+        rc |= FIH_TRUE_2;
+    }
+
+    fih_delay();
+
+    y_val = y.val;
+    if (x.val != y_val) {
+        if (rc == FIH_TRUE) {
+            FIH_PANIC;
+        }
+    }
+
+    return rc;
+}
+
+/**
+ * Standard non-equality for fih_int values.
+ *
+ * @param x  1st fih_int value to be compared.
+ * @param y  2nd fih_int value to be compared.
+ *
+ * @return   FIH_TRUE if x != y, FIH_FALSE otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_not_eq(fih_int x, fih_int y)
+{
+    int32_t y_val, y_msk;
+    volatile int32_t rc = FIH_FALSE;
+
+    fih_int_validate(x);
+    fih_int_validate(y);
+
+    y_val = y.val;
+    if (x.val != y_val) {
+        rc = FIH_TRUE_1;
+    }
+
+    fih_delay();
+
+    y_msk = y.msk;
+    if (x.msk != y_msk) {
+        rc |= FIH_TRUE_2;
+    }
+
+    fih_delay();
+
+    y_val = y.val;
+    if (x.val == y_val) {
+        if (rc == FIH_TRUE) {
+            FIH_PANIC;
+        }
+    }
+
+    return rc;
+}
+
+/**
+ * Standard non-equality for fih_uint values.
+ *
+ * @param x  1st fih_uint value to be compared.
+ * @param y  2nd fih_uint value to be compared.
+ *
+ * @return   FIH_TRUE if x != y, FIH_FALSE otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_uint_not_eq(fih_uint x, fih_uint y)
+{
+    uint32_t y_val, y_msk;
+    volatile int32_t rc = FIH_FALSE;
+
+    fih_uint_validate(x);
+    fih_uint_validate(y);
+
+    y_val = y.val;
+    if (x.val != y_val) {
+        rc = FIH_TRUE_1;
+    }
+
+    fih_delay();
+
+    y_msk = y.msk;
+    if (x.msk != y_msk) {
+        rc |= FIH_TRUE_2;
+    }
+
+    fih_delay();
+
+    y_val = y.val;
+    if (x.val == y_val) {
+        if (rc == FIH_TRUE) {
+            FIH_PANIC;
+        }
+    }
+
+    return rc;
+}
+
+/**
+ * Standard greater than comparison for fih_int values.
+ *
+ * @param x  1st fih_int value to be compared.
+ * @param y  2nd fih_int value to be compared.
+ *
+ * @return   FIH_TRUE if x > y, FIH_FALSE otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_gt(fih_int x, fih_int y)
+{
+    int32_t y_val, y_msk;
+    volatile int32_t rc = FIH_FALSE;
+
+    fih_int_validate(x);
+    fih_int_validate(y);
+
+    y_val = y.val;
+    if (x.val > y_val) {
+        rc = FIH_TRUE_1;
+    }
+
+    fih_delay();
+
+    y_msk = y.msk;
+    if (FIH_INT_VAL_MASK(x.msk) > FIH_INT_VAL_MASK(y_msk)) {
+        rc |= FIH_TRUE_2;
+    }
+
+    fih_delay();
+
+    y_val = y.val;
+    if (x.val <= y_val) {
+        if (rc == FIH_TRUE) {
+            FIH_PANIC;
+        }
+    }
+
+    return rc;
+}
+
+/**
+ * Standard greater than comparison for fih_uint values.
+ *
+ * @param x  1st fih_uint value to be compared.
+ * @param y  2nd fih_uint value to be compared.
+ *
+ * @return   FIH_TRUE if x > y, FIH_FALSE otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_uint_gt(fih_uint x, fih_uint y)
+{
+    uint32_t y_val, y_msk;
+    volatile int32_t rc = FIH_FALSE;
+
+    fih_uint_validate(x);
+    fih_uint_validate(y);
+
+    y_val = y.val;
+    if (x.val > y_val) {
+        rc = FIH_TRUE_1;
+    }
+
+    fih_delay();
+
+    y_msk = y.msk;
+    if (FIH_UINT_VAL_MASK(x.msk) > FIH_UINT_VAL_MASK(y_msk)) {
+        rc |= FIH_TRUE_2;
+    }
+
+    fih_delay();
+
+    y_val = y.val;
+    if (x.val <= y_val) {
+        if (rc == FIH_TRUE) {
+            FIH_PANIC;
+        }
+    }
+
+    return rc;
+}
+
+/**
+ * Standard greater than or equal comparison for fih_int values.
+ *
+ * @param x  1st fih_int value to be compared.
+ * @param y  2nd fih_int value to be compared.
+ *
+ * @return   FIH_TRUE if x >= y, FIH_FALSE otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_ge(fih_int x, fih_int y)
+{
+    int32_t y_val, y_msk;
+    volatile int32_t rc = FIH_FALSE;
+
+    fih_int_validate(x);
+    fih_int_validate(y);
+
+    y_val = y.val;
+    if (x.val >= y_val) {
+        rc = FIH_TRUE_1;
+    }
+
+    fih_delay();
+
+    y_msk = y.msk;
+    if (FIH_INT_VAL_MASK(x.msk) >= FIH_INT_VAL_MASK(y_msk)) {
+        rc |= FIH_TRUE_2;
+    }
+
+    fih_delay();
+
+    y_val = y.val;
+    if (x.val < y_val) {
+        if (rc == FIH_TRUE) {
+            FIH_PANIC;
+        }
+    }
+
+    return rc;
+}
+
+/**
+ * Standard greater than or equal comparison for fih_uint values.
+ *
+ * @param x  1st fih_uint value to be compared.
+ * @param y  2nd fih_uint value to be compared.
+ *
+ * @return   FIH_TRUE if x >= y, FIH_FALSE otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_uint_ge(fih_uint x, fih_uint y)
+{
+    uint32_t y_val, y_msk;
+    volatile int32_t rc = FIH_FALSE;
+
+    fih_uint_validate(x);
+    fih_uint_validate(y);
+
+    y_val = y.val;
+    if (x.val >= y_val) {
+        rc = FIH_TRUE_1;
+    }
+
+    fih_delay();
+
+    y_msk = y.msk;
+    if (FIH_UINT_VAL_MASK(x.msk) >= FIH_UINT_VAL_MASK(y_msk)) {
+        rc |= FIH_TRUE_2;
+    }
+
+    fih_delay();
+
+    y_val = y.val;
+    if (x.val < y_val) {
+        if (rc == FIH_TRUE) {
+            FIH_PANIC;
+        }
+    }
+
+    return rc;
+}
+
+/**
+ * Standard less than comparison for fih_int values.
+ *
+ * @param x  1st fih_int value to be compared.
+ * @param y  2nd fih_int value to be compared.
+ *
+ * @return   FIH_TRUE if x < y, FIH_FALSE otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_lt(fih_int x, fih_int y)
+{
+    int32_t y_val, y_msk;
+    volatile int32_t rc = FIH_FALSE;
+
+    fih_int_validate(x);
+    fih_int_validate(y);
+
+    y_val = y.val;
+    if (x.val < y_val) {
+        rc = FIH_TRUE_1;
+    }
+
+    fih_delay();
+
+    y_msk = y.msk;
+    if (FIH_INT_VAL_MASK(x.msk) < FIH_INT_VAL_MASK(y_msk)) {
+        rc |= FIH_TRUE_2;
+    }
+
+    fih_delay();
+
+    y_val = y.val;
+    if (x.val >= y_val) {
+        if (rc == FIH_TRUE) {
+            FIH_PANIC;
+        }
+    }
+
+    return rc;
+}
+
+/**
+ * Standard less than comparison for fih_uint values.
+ *
+ * @param x  1st fih_uint value to be compared.
+ * @param y  2nd fih_uint value to be compared.
+ *
+ * @return   FIH_TRUE if x < y, FIH_FALSE otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_uint_lt(fih_uint x, fih_uint y)
+{
+    uint32_t y_val, y_msk;
+    volatile int32_t rc = FIH_FALSE;
+
+    fih_uint_validate(x);
+    fih_uint_validate(y);
+
+    y_val = y.val;
+    if (x.val < y_val) {
+        rc = FIH_TRUE_1;
+    }
+
+    fih_delay();
+
+    y_msk = y.msk;
+    if (FIH_UINT_VAL_MASK(x.msk) < FIH_UINT_VAL_MASK(y_msk)) {
+        rc |= FIH_TRUE_2;
+    }
+
+    fih_delay();
+
+    y_val = y.val;
+    if (x.val >= y_val) {
+        if (rc == FIH_TRUE) {
+            FIH_PANIC;
+        }
+    }
+
+    return rc;
+}
+
+/**
+ * Standard less than or equal comparison for fih_int values.
+ *
+ * @param x  1st fih_int value to be compared.
+ * @param y  2nd fih_int value to be compared.
+ *
+ * @return   FIH_TRUE if x <= y, FIH_FALSE otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_le(fih_int x, fih_int y)
+{
+    int32_t y_val, y_msk;
+    volatile int32_t rc = FIH_FALSE;
+
+    fih_int_validate(x);
+    fih_int_validate(y);
+
+    y_val = y.val;
+    if (x.val <= y_val) {
+        rc = FIH_TRUE_1;
+    }
+
+    fih_delay();
+
+    y_msk = y.msk;
+    if (FIH_INT_VAL_MASK(x.msk) <= FIH_INT_VAL_MASK(y_msk)) {
+        rc |= FIH_TRUE_2;
+    }
+
+    fih_delay();
+
+    y_val = y.val;
+    if (x.val > y_val) {
+        if (rc == FIH_TRUE) {
+            FIH_PANIC;
+        }
+    }
+
+    return rc;
+}
+
+/**
+ * Standard less than or equal comparison for fih_uint values.
+ *
+ * @param x  1st fih_uint value to be compared.
+ * @param y  2nd fih_uint value to be compared.
+ *
+ * @return   FIH_TRUE if x <= y, FIH_FALSE otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_uint_le(fih_uint x, fih_uint y)
+{
+    uint32_t y_val, y_msk;
+    volatile int32_t rc = FIH_FALSE;
+
+    fih_uint_validate(x);
+    fih_uint_validate(y);
+
+    y_val = y.val;
+    if (x.val <= y_val) {
+        rc = FIH_TRUE_1;
+    }
+
+    fih_delay();
+
+    y_msk = y.msk;
+    if (FIH_UINT_VAL_MASK(x.msk) <= FIH_UINT_VAL_MASK(y_msk)) {
+        rc |= FIH_TRUE_2;
+    }
+
+    fih_delay();
+
+    y_val = y.val;
+    if (x.val > y_val) {
+        if (rc == FIH_TRUE) {
+            FIH_PANIC;
+        }
+    }
+
+    return rc;
+}
+
+/**
+ * Standard logical OR for fih_uint values.
+ *
+ * @param x  1st fih_uint value to be ORed.
+ * @param y  2nd fih_uint value to be ORed.
+ *
+ * @return   ORed value
+ */
+__attribute__((always_inline)) static inline
+fih_uint fih_uint_or(fih_uint x, fih_uint y)
+{
+    uint32_t y_val, y_msk;
+    volatile fih_uint rc = {0};
+
+    fih_uint_validate(x);
+    fih_uint_validate(y);
+
+    y_val = y.val;
+    rc.val = x.val | y_val;
+
+    fih_delay();
+
+    y_msk = y.msk;
+    rc.msk = FIH_UINT_VAL_MASK(FIH_UINT_VAL_MASK(x.msk) | FIH_UINT_VAL_MASK(y_msk));
+
+    fih_uint_validate(rc);
+
+    return rc;
+}
+
+/**
+ * Standard logical AND for fih_uint values.
+ *
+ * @param x  1st fih_uint value to be ORed.
+ * @param y  2nd fih_uint value to be ORed.
+ *
+ * @return   ANDed value
+ */
+__attribute__((always_inline)) static inline
+fih_uint fih_uint_and(fih_uint x, fih_uint y)
+{
+    uint32_t y_val, y_msk;
+    volatile fih_uint rc = {0};
+
+    fih_uint_validate(x);
+    fih_uint_validate(y);
+
+    y_val = y.val;
+    rc.val = x.val & y_val;
+
+    fih_delay();
+
+    y_msk = y.msk;
+    rc.msk = FIH_UINT_VAL_MASK(FIH_UINT_VAL_MASK(x.msk) & FIH_UINT_VAL_MASK(y_msk));
+
+    fih_uint_validate(rc);
+
+    return rc;
+}
+
+#else /* FIH_ENABLE_DOUBLE_VARS */
+
+/* NOOP */
+#define fih_int_validate(x)
+#define fih_uint_validate(x)
+
+/* NOOP */
+#define fih_int_decode(x)       ((x).val)
+#define fih_uint_decode(x)      ((x).val)
+
+/* NOOP */
+#define fih_int_encode(x)       ((fih_int)FIH_INT_INIT(x))
+#define fih_uint_encode(x)      ((fih_uint)FIH_UINT_INIT(x))
+
+/**
+ * Standard equality for fih_int values.
+ *
+ * @param x  1st fih_int value to be compared.
+ * @param y  2nd fih_int value to be compared.
+ *
+ * @return   FIH_TRUE if x == y, FIH_FALSE otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_eq(fih_int x, fih_int y)
+{
+    volatile int32_t rc = FIH_FALSE;
+
+    if (x.val == y.val) {
+        rc = FIH_TRUE;
+    }
+
+    fih_delay();
+
+    if (x.val != y.val) {
+        rc = FIH_FALSE;
+    }
+
+    return rc;
+}
+
+/**
+ * Standard equality for fih_uint values.
+ *
+ * @param x  1st fih_uint value to be compared.
+ * @param y  2nd fih_uint value to be compared.
+ *
+ * @return   FIH_TRUE if x == y, FIH_FALSE otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_uint_eq(fih_uint x, fih_uint y)
+{
+    volatile int32_t rc = FIH_FALSE;
+
+    if (x.val == y.val) {
+        rc = FIH_TRUE;
+    }
+
+    fih_delay();
+
+    if (x.val != y.val) {
+        rc = FIH_FALSE;
+    }
+
+    return rc;
+}
+
+/**
+ * Standard non-equality for fih_int values.
+ *
+ * @param x  1st fih_int value to be compared.
+ * @param y  2nd fih_int value to be compared.
+ *
+ * @return   FIH_TRUE if x != y, FIH_FALSE otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_not_eq(fih_int x, fih_int y)
+{
+    volatile int32_t rc = FIH_FALSE;
+
+    if (x.val != y.val) {
+        rc = FIH_TRUE;
+    }
+
+    fih_delay();
+
+    if (x.val == y.val) {
+        rc = FIH_FALSE;
+    }
+
+    return rc;
+}
+
+/**
+ * Standard non-equality for fih_uint values.
+ *
+ * @param x  1st fih_uint value to be compared.
+ * @param y  2nd fih_uint value to be compared.
+ *
+ * @return   FIH_TRUE if x != y, FIH_FALSE otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_uint_not_eq(fih_uint x, fih_uint y)
+{
+    volatile int32_t rc = FIH_FALSE;
+
+    if (x.val != y.val) {
+        rc = FIH_TRUE;
+    }
+
+    fih_delay();
+
+    if (x.val == y.val) {
+        rc = FIH_FALSE;
+    }
+
+    return rc;
+}
+
+/**
+ * Standard greater than comparison for fih_int values.
+ *
+ * @param x  1st fih_int value to be compared.
+ * @param y  2nd fih_int value to be compared.
+ *
+ * @return   FIH_TRUE if x > y, FIH_FALSE otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_gt(fih_int x, fih_int y)
+{
+    volatile int32_t rc = FIH_FALSE;
+
+    if (x.val > y.val) {
+        rc = FIH_TRUE;
+    }
+
+    fih_delay();
+
+    if (x.val <= y.val) {
+        rc = FIH_FALSE;
+    }
+
+    return rc;
+}
+
+/**
+ * Standard greater than comparison for fih_uint values.
+ *
+ * @param x  1st fih_uint value to be compared.
+ * @param y  2nd fih_uint value to be compared.
+ *
+ * @return   FIH_TRUE if x > y, FIH_FALSE otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_uint_gt(fih_uint x, fih_uint y)
+{
+    volatile int32_t rc = FIH_FALSE;
+
+    if (x.val > y.val) {
+        rc = FIH_TRUE;
+    }
+
+    fih_delay();
+
+    if (x.val <= y.val) {
+        rc = FIH_FALSE;
+    }
+
+    return rc;
+}
+
+/**
+ * Standard greater than or equal comparison for fih_int values.
+ *
+ * @param x  1st fih_int value to be compared.
+ * @param y  2nd fih_int value to be compared.
+ *
+ * @return   FIH_TRUE if x >= y, FIH_FALSE otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_ge(fih_int x, fih_int y)
+{
+    volatile int32_t rc = FIH_FALSE;
+
+    if (x.val >= y.val) {
+        rc = FIH_TRUE;
+    }
+
+    fih_delay();
+
+    if (x.val < y.val) {
+        rc = FIH_FALSE;
+    }
+
+    return rc;
+}
+
+/**
+ * Standard greater than or equal comparison for fih_uint values.
+ *
+ * @param x  1st fih_uint value to be compared.
+ * @param y  2nd fih_uint value to be compared.
+ *
+ * @return   FIH_TRUE if x >= y, FIH_FALSE otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_uint_ge(fih_uint x, fih_uint y)
+{
+    volatile int32_t rc = FIH_FALSE;
+
+    if (x.val >= y.val) {
+        rc = FIH_TRUE;
+    }
+
+    fih_delay();
+
+    if (x.val < y.val) {
+        rc = FIH_FALSE;
+    }
+
+    return rc;
+}
+
+/**
+ * Standard less than comparison for fih_int values.
+ *
+ * @param x  1st fih_int value to be compared.
+ * @param y  2nd fih_int value to be compared.
+ *
+ * @return   FIH_TRUE if x < y, FIH_FALSE otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_lt(fih_int x, fih_int y)
+{
+    volatile int32_t rc = FIH_FALSE;
+
+    if (x.val < y.val) {
+        rc = FIH_TRUE;
+    }
+
+    fih_delay();
+
+    if (x.val >= y.val) {
+        rc = FIH_FALSE;
+    }
+
+    return rc;
+}
+
+/**
+ * Standard less than comparison for fih_uint values.
+ *
+ * @param x  1st fih_uint value to be compared.
+ * @param y  2nd fih_uint value to be compared.
+ *
+ * @return   FIH_TRUE if x < y, FIH_FALSE otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_uint_lt(fih_uint x, fih_uint y)
+{
+    volatile int32_t rc = FIH_FALSE;
+
+    if (x.val < y.val) {
+        rc = FIH_TRUE;
+    }
+
+    fih_delay();
+
+    if (x.val >= y.val) {
+        rc = FIH_FALSE;
+    }
+
+    return rc;
+}
+
+/**
+ * Standard less than or equal comparison for fih_int values.
+ *
+ * @param x  1st fih_int value to be compared.
+ * @param y  2nd fih_int value to be compared.
+ *
+ * @return   FIH_TRUE if x <= y, FIH_FALSE otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_le(fih_int x, fih_int y)
+{
+    volatile int32_t rc = FIH_FALSE;
+
+    if (x.val <= y.val) {
+        rc = FIH_TRUE;
+    }
+
+    fih_delay();
+
+    if (x.val > y.val) {
+        rc = FIH_FALSE;
+    }
+
+    return rc;
+}
+
+/**
+ * Standard less than or equal comparison for fih_uint values.
+ *
+ * @param x  1st fih_uint value to be compared.
+ * @param y  2nd fih_uint value to be compared.
+ *
+ * @return   FIH_TRUE if x <= y, FIH_FALSE otherwise.
+ */
+__attribute__((always_inline)) static inline
+int32_t fih_uint_le(fih_uint x, fih_uint y)
+{
+    volatile int32_t rc = FIH_FALSE;
+
+    if (x.val <= y.val) {
+        rc = FIH_TRUE;
+    }
+
+    fih_delay();
+
+    if (x.val > y.val) {
+        rc = FIH_FALSE;
+    }
+
+    return rc;
+}
+
+/**
+ * Standard logical OR for fih_uint values.
+ *
+ * @param x  1st fih_uint value to be ORed.
+ * @param y  2nd fih_uint value to be ORed.
+ *
+ * @return   ORed value
+ */
+__attribute__((always_inline)) static inline
+fih_uint fih_uint_or(fih_uint x, fih_uint y)
+{
+    fih_uint rc = {x.val | y.val};
+
+    fih_delay();
+
+    if (rc.val != (x.val | y.val)) {
+        FIH_PANIC;
+    }
+
+    return rc;
+}
+
+/**
+ * Standard logical AND for fih_uint values.
+ *
+ * @param x  1st fih_uint value to be ORed.
+ * @param y  2nd fih_uint value to be ORed.
+ *
+ * @return   ANDed value
+ */
+__attribute__((always_inline)) static inline
+fih_uint fih_uint_and(fih_uint x, fih_uint y)
+{
+    fih_uint rc = {x.val & y.val};
+
+    fih_delay();
+
+    if (rc.val != (x.val & y.val)) {
+        FIH_PANIC;
+    }
+
+    return rc;
+}
+
+#endif /* FIH_ENABLE_DOUBLE_VARS */
+
+/**
+ * C has a common return pattern where 0 is a correct value and all others are
+ * errors.  This function converts 0 to FIH_SUCCESS, and any other number to a
+ * value that is not FIH_SUCCESS.
+ *
+ * @param x  Return code to be checked.
+ *
+ * @return   FIH_SUCCESS if x == 0, FIH_FAILURE otherwise.
+ */
+__attribute__((always_inline)) static inline
+fih_int fih_int_encode_zero_equality(int32_t x)
 {
     if (x != 0) {
         return FIH_FAILURE;
@@ -364,80 +1277,39 @@
 }
 
 #ifdef FIH_ENABLE_CFI
-extern fih_int _fih_cfi_ctr;
-#endif /* FIH_ENABLE_CFI */
+/* Global Control Flow Integrity counter */
+extern fih_uint fih_cfi_ctr;
 
-fih_int fih_cfi_get_and_increment(void);
-void fih_cfi_validate(fih_int saved);
+/**
+ * Increment the CFI counter by input counter and return the value before the
+ * increment.
+ * NOTE: this function shall not be called directly.
+ *
+ * @param x  Increment value.
+ *
+ * @return   Previous value of the CFI counter.
+ */
+fih_uint fih_cfi_get_and_increment(uint8_t cnt);
+
+/**
+ * Validate that the saved precall value is the same as the value of the global
+ * counter. For this to be the case, a fih_ret must have been called between
+ * these functions being executed. If the values aren't the same then panic.
+ * NOTE: this function shall not be called directly.
+ *
+ * @param saved  Saved value.
+ */
+void fih_cfi_validate(fih_uint saved);
+
+/**
+ * Decrement the global CFI counter by one, so that it has the same value as
+ * before the cfi_precall.
+ * NOTE: this function shall not be called directly.
+ */
 void fih_cfi_decrement(void);
 
-/* Label for interacting with FIH testing tool. Can be parsed from the elf file
- * after compilation. Does not require debug symbols.
- */
-#if defined(__ICCARM__)
-#define FIH_LABEL(str, lin, cnt) __asm volatile ("FIH_LABEL_" str "_" #lin "_" #cnt "::" ::);
-#else
-#define FIH_LABEL(str) __asm volatile ("FIH_LABEL_" str "_%=:" ::);
-#endif
-
-/* Main FIH calling macro. return variable is second argument. Does some setup
- * before and validation afterwards. Inserts labels for use with testing script.
- *
- * First perform the precall step - this gets the current value of the CFI
- * counter and saves it to a local variable, and then increments the counter.
- *
- * Then set the return variable to FIH_FAILURE as a base case.
- *
- * Then perform the function call. As part of the funtion FIH_RET must be called
- * which will decrement the counter.
- *
- * The postcall step gets the value of the counter and compares it to the
- * previously saved value. If this is equal then the function call and all child
- * function calls were performed.
- */
-#if defined(__ICCARM__)
-#define FIH_CALL(f, ret, ...) FIH_CALL2(f, ret, __LINE__, __COUNTER__, __VA_ARGS__)
-
-#define FIH_CALL2(f, ret, l, c, ...) \
-    do { \
-        FIH_LABEL("FIH_CALL_START", l, c);        \
-        FIH_CFI_PRECALL_BLOCK; \
-        ret = FIH_FAILURE; \
-        if (fih_delay() != 0) { \
-            ret = f(__VA_ARGS__); \
-        } \
-        FIH_CFI_POSTCALL_BLOCK; \
-        FIH_LABEL("FIH_CALL_END", l, c);          \
-    } while (false)
-
-#else
-
-#define FIH_CALL(f, ret, ...) \
-    do { \
-        FIH_LABEL("FIH_CALL_START"); \
-        FIH_CFI_PRECALL_BLOCK; \
-        (ret) = FIH_FAILURE; \
-        if (fih_delay() != 0) { \
-            (ret) = (f)(__VA_ARGS__); \
-        } \
-        FIH_CFI_POSTCALL_BLOCK; \
-        FIH_LABEL("FIH_CALL_END"); \
-    } while (false)
-#endif
-
-/* FIH return changes the state of the internal state machine. If you do a
- * FIH_CALL then you need to do a FIH_RET else the state machine will detect
- * tampering and panic.
- */
-#define FIH_RET(ret) \
-    do { \
-        FIH_CFI_PRERET; \
-        return ret; \
-    } while (false)
-
-
-#ifdef FIH_ENABLE_CFI
-/* Macro wrappers for functions - Even when the functions have zero body this
+/*
+ * Macro wrappers for functions - Even when the functions have zero body this
  * saves a few bytes on noop functions as it doesn't generate the call/ret
  *
  * CFI precall function saves the CFI counter and then increments it - the
@@ -447,21 +1319,227 @@
  * called.
  */
 #define FIH_CFI_PRECALL_BLOCK \
-    fih_int _fih_cfi_saved_value = fih_cfi_get_and_increment()
+        fih_uint fih_cfi_precall_saved_value = fih_cfi_get_and_increment(1u)
 
 #define FIH_CFI_POSTCALL_BLOCK \
-        fih_cfi_validate(_fih_cfi_saved_value)
+        fih_cfi_validate(fih_cfi_precall_saved_value)
 
 #define FIH_CFI_PRERET \
         fih_cfi_decrement()
-#else
+
+/*
+ * Marcos to support protect the control flow integrity inside a function.
+ *
+ * The FIH_CFI_PRECALL_BLOCK/FIH_CFI_POSTCALL_BLOCK pair mainly protect function
+ * calls from fault injection. Fault injection may attack a function to skip its
+ * critical steps which are not function calls. It is difficult for the caller
+ * to dectect the injection as long as the function successfully returns.
+ *
+ * The following macros can be called in a function to track the critical steps,
+ * especially those which are not function calls.
+ */
+
+/*
+ * FIH_CFI_STEP_INIT() saves the CFI counter and increase the CFI counter by the
+ * number of the critical steps. It should be called before execution starts.
+ */
+#define FIH_CFI_STEP_INIT(x) \
+        fih_int fih_cfi_step_saved_value = fih_cfi_get_and_increment(x)
+
+/*
+ * FIH_CFI_STEP_DECREMENT() decrease the CFI counter by one. It can be called
+ * after each critical step execution completes.
+ */
+#define FIH_CFI_STEP_DECREMENT() \
+        fih_cfi_decrement()
+
+/*
+ * FIH_CFI_STEP_ERR_RESET() resets the CFI counter to the previous value saved
+ * by FIH_CFI_STEP_INIT(). It shall be called only when a functionality error
+ * occurs and forces the function to exit. It can enable the caller to capture
+ * the functionality error other than being trapped in fault injection error
+ * handling.
+ */
+#define FIH_CFI_STEP_ERR_RESET() \
+        do { \
+            fih_cfi_ctr = fih_cfi_step_saved_value; \
+            fih_int_validate(fih_cfi_ctr); \
+        } while(0)
+
+#else /* FIH_ENABLE_CFI */
 #define FIH_CFI_PRECALL_BLOCK
 #define FIH_CFI_POSTCALL_BLOCK
 #define FIH_CFI_PRERET
-#endif  /* FIH_ENABLE_CFI */
+
+#define FIH_CFI_STEP_INIT(x)
+#define FIH_CFI_STEP_DECREMENT()
+#define FIH_CFI_STEP_ERR_RESET()
+#endif /* FIH_ENABLE_CFI */
+
+/*
+ * Label for interacting with FIH testing tool. Can be parsed from the elf file
+ * after compilation. Does not require debug symbols.
+ */
+#define FIH_LABEL(str) __asm volatile ("FIH_LABEL_" str "_0_%=:" ::)
+#define FIH_LABEL_CRITICAL_POINT() FIH_LABEL("FIH_CRITICAL_POINT")
+
+/*
+ * Main FIH calling macro. return variable is second argument. Does some setup
+ * before and validation afterwards. Inserts labels for use with testing script.
+ *
+ * First perform the precall step - this gets the current value of the CFI
+ * counter and saves it to a local variable, and then increments the counter.
+ *
+ * Then set the return variable to FIH_FAILURE as a base case.
+ *
+ * Then perform the function call. As part of the function FIH_RET must be
+ * called which will decrement the counter.
+ *
+ * The postcall step gets the value of the counter and compares it to the
+ * previously saved value. If this is equal then the function call and all child
+ * function calls were performed.
+ */
+#define FIH_CALL(f, ret, ...) \
+    do { \
+        FIH_LABEL("FIH_CALL_START_" # f); \
+        FIH_CFI_PRECALL_BLOCK; \
+        (ret) = FIH_FAILURE; \
+        fih_delay(); \
+        (ret) = (f)(__VA_ARGS__); \
+        FIH_CFI_POSTCALL_BLOCK; \
+        fih_int_validate(ret); \
+        FIH_LABEL("FIH_CALL_END"); \
+    } while (false)
+
+/*
+ * Similar to FIH_CALL, but return value is ignored, like (void)f(...)
+ */
+#define FIH_VOID(f, ...) \
+    do { \
+        FIH_CFI_PRECALL_BLOCK; \
+        fih_delay(); \
+        (void)(f)(__VA_ARGS__); \
+        FIH_CFI_POSTCALL_BLOCK; \
+        FIH_LABEL("FIH_CALL_END"); \
+    } while (false)
+
+/*
+ * Similar to FIH_CALL, but ret is fih_uint instead of fih_int.
+ * NOTE: intended use is bit masks, so initialized by 0 instead of FIH_FAILURE!
+ */
+#define FIH_UCALL(f, ret, ...) \
+    do { \
+        FIH_LABEL("FIH_CALL_START_" # f); \
+        FIH_CFI_PRECALL_BLOCK; \
+        (ret) = FIH_UINT_ZERO; \
+        fih_delay(); \
+        (ret) = (f)(__VA_ARGS__); \
+        FIH_CFI_POSTCALL_BLOCK; \
+        fih_uint_validate(ret); \
+        FIH_LABEL("FIH_CALL_END"); \
+    } while (false)
+
+/*
+ * FIH return changes the state of the internal state machine. If you do a
+ * FIH_CALL then you need to do a FIH_RET else the state machine will detect
+ * tampering and panic.
+ */
+#define FIH_RET(ret) \
+    do { \
+        FIH_CFI_PRERET; \
+        return ret; \
+    } while (false)
+
+#else /* MCUBOOT_FIH_PROFILE_ON */
+typedef int32_t fih_int;
+typedef uint32_t fih_uint;
+
+typedef fih_int fih_int;
+typedef fih_uint fih_uint;
+
+#define FIH_INT_INIT(x)         (x)
+#define FIH_UINT_INIT(x)        (x)
+
+#define FIH_SUCCESS             (0)
+#define FIH_FAILURE            (-1)
+#define FIH_UINT_ZERO           (0UL)
+#define FIH_UINT_MAX            (0xFFFFFFFFUL)
+
+#define FIH_TRUE                (1)
+#define FIH_FALSE               (0)
+
+#define fih_int_validate(x)
+#define fih_uint_validate(x)
+
+#define fih_int_decode(x)       (x)
+#define fih_uint_decode(x)      (x)
+
+#define fih_int_encode(x)       (x)
+#define fih_uint_encode(x)      (x)
+
+#define fih_int_encode_zero_equality(x) ((x) == 0 ? 0 : 1)
+
+#define fih_eq(x, y)            ((x) == (y))
+#define fih_uint_eq(x, y)       ((x) == (y))
+
+#define fih_not_eq(x, y)        ((x) != (y))
+#define fih_uint_not_eq(x, y)   ((x) != (y))
+
+#define fih_gt(x, y)            ((x) > (y))
+#define fih_uint_gt(x, y)       ((x) > (y))
+
+#define fih_ge(x, y)            ((x) >= (y))
+#define fih_uint_ge(x, y)       ((x) >= (y))
+
+#define fih_lt(x, y)            ((x) < (y))
+#define fih_uint_lt(x, y)       ((x) < (y))
+
+#define fih_le(x, y)            ((x) <= (y))
+#define fih_uint_le(x, y)       ((x) <= (y))
+
+#define fih_uint_or(x, y)       ((x) | (y))
+#define fih_uint_and(x, y)      ((x) & (y))
+
+#define fih_delay_init()        (0)
+#define fih_delay()
+
+#define FIH_CALL(f, ret, ...) \
+    do { \
+        (ret) = (f)(__VA_ARGS__); \
+    } while (false)
+
+#define FIH_VOID(f, ...) \
+    do { \
+        (void)(f)(__VA_ARGS__); \
+    } while (false)
+
+#define FIH_UCALL(f, ret, ...) \
+    do { \
+        (ret) = (f)(__VA_ARGS__); \
+    } while (false)
+
+#define FIH_RET(ret) \
+    do { \
+        return ret; \
+    } while (false)
+
+#define FIH_PANIC do { \
+        while (true) {} \
+    } while (false)
+
+#define FIH_CFI_STEP_INIT(x)
+#define FIH_CFI_STEP_DECREMENT()
+#define FIH_CFI_STEP_ERR_RESET()
+
+#define FIH_LABEL_CRITICAL_POINT()
+
+#endif /* MCUBOOT_FIH_PROFILE_ON */
 
 #ifdef __cplusplus
 }
 #endif /* __cplusplus */
 
+FIH_MISRA_BLOCK_END('MISRA C-2012 Rule 10.1');
+FIH_MISRA_BLOCK_END('MISRA C-2012 Rule 10.4');
+
 #endif /* FAULT_INJECTION_HARDENING_H */
diff --git a/boot/bootutil/include/bootutil/image.h b/boot/bootutil/include/bootutil/image.h
index 103e6b7..7e19452 100644
--- a/boot/bootutil/include/bootutil/image.h
+++ b/boot/bootutil/include/bootutil/image.h
@@ -81,22 +81,22 @@
  *   1st on identifies the public key which should be used to verify it.
  *   2nd one is the actual signature.
  */
-#define IMAGE_TLV_KEYHASH           0x01   /* hash of the public key */
-#define IMAGE_TLV_PUBKEY            0x02   /* public key */
-#define IMAGE_TLV_SHA256            0x10   /* SHA256 of image hdr and body */
-#define IMAGE_TLV_RSA2048_PSS       0x20   /* RSA2048 of hash output */
-#define IMAGE_TLV_ECDSA224          0x21   /* ECDSA of hash output */
-#define IMAGE_TLV_ECDSA256          0x22   /* ECDSA of hash output */
-#define IMAGE_TLV_RSA3072_PSS       0x23   /* RSA3072 of hash output */
-#define IMAGE_TLV_ED25519           0x24   /* ed25519 of hash output */
-#define IMAGE_TLV_ENC_RSA2048       0x30   /* Key encrypted with RSA-OAEP-2048 */
-#define IMAGE_TLV_ENC_KW            0x31   /* Key encrypted with AES-KW 128 or 256*/
-#define IMAGE_TLV_ENC_EC256         0x32   /* Key encrypted with ECIES-EC256 */
-#define IMAGE_TLV_ENC_X25519        0x33   /* Key encrypted with ECIES-X25519 */
-#define IMAGE_TLV_DEPENDENCY        0x40   /* Image depends on other image */
-#define IMAGE_TLV_SEC_CNT           0x50   /* security counter */
-#define IMAGE_TLV_PROV_PACK         0x51   /* Reprovisioning packet */
-#define IMAGE_TLV_BOOT_RECORD       0x60   /* measured boot record */
+#define IMAGE_TLV_KEYHASH           (0x01)   /* hash of the public key */
+#define IMAGE_TLV_PUBKEY            (0x02)   /* public key */
+#define IMAGE_TLV_SHA256            (0x10)   /* SHA256 of image hdr and body */
+#define IMAGE_TLV_RSA2048_PSS       (0x20)   /* RSA2048 of hash output */
+#define IMAGE_TLV_ECDSA224          (0x21)   /* ECDSA of hash output */
+#define IMAGE_TLV_ECDSA256          (0x22)   /* ECDSA of hash output */
+#define IMAGE_TLV_RSA3072_PSS       (0x23)   /* RSA3072 of hash output */
+#define IMAGE_TLV_ED25519           (0x24)   /* ed25519 of hash output */
+#define IMAGE_TLV_ENC_RSA2048       (0x30)   /* Key encrypted with RSA-OAEP-2048 */
+#define IMAGE_TLV_ENC_KW            (0x31)   /* Key encrypted with AES-KW 128 or 256*/
+#define IMAGE_TLV_ENC_EC256         (0x32)   /* Key encrypted with ECIES-EC256 */
+#define IMAGE_TLV_ENC_X25519        (0x33)   /* Key encrypted with ECIES-X25519 */
+#define IMAGE_TLV_DEPENDENCY        (0x40)   /* Image depends on other image */
+#define IMAGE_TLV_SEC_CNT           (0x50)   /* security counter */
+#define IMAGE_TLV_PROV_PACK         (0x51)   /* Reprovisioning packet */
+#define IMAGE_TLV_BOOT_RECORD       (0x60)   /* measured boot record */
 					   /*
 					    * vendor reserved TLVs at xxA0-xxFF,
 					    * where xx denotes the upper byte
@@ -164,15 +164,23 @@
     (flash_area_get_id(fap) == FLASH_AREA_IMAGE_SECONDARY(idx) && IS_ENCRYPTED(hdr))
 #endif
 
+#if defined(MCUBOOT_RAM_LOAD)
+#define IS_RAM_BOOTABLE(hdr)                                       \
+    ((((hdr)->ih_flags & IMAGE_F_RAM_LOAD) == IMAGE_F_RAM_LOAD) && \
+     ((hdr)->ih_load_addr != 0U) && ((hdr)->ih_load_addr != (uintptr_t)(-1)))
+#else
+#define IS_RAM_BOOTABLE(hdr) (false)
+#endif
+
 _Static_assert(sizeof(struct image_header) == IMAGE_HEADER_SIZE,
                "struct image_header not required size");
 
 struct enc_key_data;
 fih_int bootutil_img_validate(struct enc_key_data *enc_state, int image_index,
-                              struct image_header *hdr,
-                              const struct flash_area *fap,
-                              uint8_t *tmp_buf, uint32_t tmp_buf_sz,
-                              uint8_t *seed, int seed_len, uint8_t *out_hash);
+                                  struct image_header *hdr,
+                                  const struct flash_area *fap,
+                                  uint8_t *tmp_buf, uint32_t tmp_buf_sz,
+                                  uint8_t *seed, int seed_len, uint8_t *out_hash);
 
 struct image_tlv_iter {
     const struct image_header *hdr;
@@ -191,9 +199,9 @@
 int bootutil_tlv_iter_next(struct image_tlv_iter *it, uint32_t *off,
                            uint16_t *len, uint16_t *type);
 
-int32_t bootutil_get_img_security_cnt(struct image_header *hdr,
-                                      const struct flash_area *fap,
-                                      uint32_t *security_cnt);
+fih_int bootutil_get_img_security_cnt(struct image_header *hdr,
+                                          const struct flash_area *fap,
+                                          fih_uint *security_cnt);
 #ifdef CYW20829
 int32_t bootutil_get_img_reprov_packet(struct image_header *hdr,
                               const struct flash_area *fap,
diff --git a/boot/bootutil/include/bootutil/security_cnt.h b/boot/bootutil/include/bootutil/security_cnt.h
index 9c12ed3..515041e 100644
--- a/boot/bootutil/include/bootutil/security_cnt.h
+++ b/boot/bootutil/include/bootutil/security_cnt.h
@@ -64,7 +64,7 @@
  * @return                  0 on success; nonzero on failure.
  */
 int32_t boot_nv_security_counter_update(uint32_t image_id,
-                                        uint32_t img_security_cnt,
+                                        fih_uint img_security_cnt,
                                         void * custom_data);
 
 #ifdef __cplusplus
diff --git a/boot/bootutil/src/boot_record.c b/boot/bootutil/src/boot_record.c
index b6b0e50..5b7ec08 100644
--- a/boot/bootutil/src/boot_record.c
+++ b/boot/bootutil/src/boot_record.c
@@ -71,16 +71,16 @@
      * shared data area.
      */
     if (!shared_memory_init_done) {
-        memset((void *)MCUBOOT_SHARED_DATA_BASE, 0, MCUBOOT_SHARED_DATA_SIZE);
+        (void)memset((void *)MCUBOOT_SHARED_DATA_BASE, 0, MCUBOOT_SHARED_DATA_SIZE);
         boot_data->header.tlv_magic   = SHARED_DATA_TLV_INFO_MAGIC;
-        boot_data->header.tlv_tot_len = SHARED_DATA_HEADER_SIZE;
+        boot_data->header.tlv_tot_len = (uint16_t)SHARED_DATA_HEADER_SIZE;
         shared_memory_init_done = true;
     }
 
     /* Check whether TLV entry is already added.
      * Get the boundaries of TLV section
      */
-    tlv_end = MCUBOOT_SHARED_DATA_BASE + boot_data->header.tlv_tot_len;
+    tlv_end = MCUBOOT_SHARED_DATA_BASE + (uint32_t)(boot_data->header.tlv_tot_len);
     offset  = MCUBOOT_SHARED_DATA_BASE + SHARED_DATA_HEADER_SIZE;
 
     /* Iterates over the TLV section looks for the same entry if found then
@@ -88,7 +88,7 @@
      */
     while (offset < tlv_end) {
         /* Create local copy to avoid unaligned access */
-        memcpy(&tlv_entry, (const void *)offset, SHARED_DATA_ENTRY_HEADER_SIZE);
+        (void)memcpy((void*)&tlv_entry, (const void *)offset, SHARED_DATA_ENTRY_HEADER_SIZE);
         if (tlv_entry.tlv_type == type) {
             return SHARED_MEMORY_OVERWRITE;
         }
@@ -98,10 +98,14 @@
 
     /* Add TLV entry */
     tlv_entry.tlv_type = type;
-    tlv_entry.tlv_len  = size;
 
+    if (size > (unsigned)UINT16_MAX - SHARED_DATA_ENTRY_HEADER_SIZE) {
+        return SHARED_MEMORY_GEN_ERROR;
+    }
+
+    tlv_entry.tlv_len = (uint16_t)size;
     if (!boot_u16_safe_add(&boot_data_size, boot_data->header.tlv_tot_len,
-                           SHARED_DATA_ENTRY_SIZE(size))) {
+                            (uint16_t)SHARED_DATA_ENTRY_SIZE(size))) {
         return SHARED_MEMORY_GEN_ERROR;
     }
 
@@ -111,10 +115,10 @@
     }
 
     offset = tlv_end;
-    (void)memcpy((void *)offset, &tlv_entry, SHARED_DATA_ENTRY_HEADER_SIZE);
+    (void)memcpy((void *)offset, (const void *)&tlv_entry, SHARED_DATA_ENTRY_HEADER_SIZE);
 
     offset += SHARED_DATA_ENTRY_HEADER_SIZE;
-    (void)memcpy((void *)offset, data, size);
+    (void)memcpy((void *)offset, (const void *)data, size);
 
     boot_data->header.tlv_tot_len = boot_data_size;
 
@@ -151,7 +155,7 @@
      */
 
     rc = bootutil_tlv_iter_begin(&it, hdr, fap, IMAGE_TLV_ANY, false);
-    if (rc) {
+    if (rc != 0) {
         return -1;
     }
 
@@ -165,26 +169,29 @@
         } else if (rc > 0) {
             break;
         }
+        else {
+            /* No action required - for MISRA C-2012 Rule 15.7 rule */
+        }
 
-        if (type == IMAGE_TLV_BOOT_RECORD) {
+        if ((uint16_t)IMAGE_TLV_BOOT_RECORD == type) {
             if (len > sizeof(buf)) {
                 return -1;
             }
             rc = flash_area_read(fap, offset, buf, len);
-            if (rc) {
+            if (rc != 0) {
                 return -1;
             }
 
             record_len = len;
             boot_record_found = true;
 
-        } else if (type == IMAGE_TLV_SHA256) {
+        } else if ((uint16_t)IMAGE_TLV_SHA256 == type) {
             /* Get the image's hash value from the manifest section. */
-            if (len != BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE) {
+            if (len != (uint16_t)BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE) {
                 return -1;
             }
             rc = flash_area_read(fap, offset, image_hash, len);
-            if (rc) {
+            if (rc != 0) {
                 return -1;
             }
 
@@ -197,6 +204,9 @@
              */
             break;
         }
+        else {
+            /* No action required - for MISRA C-2012 Rule 15.7 rule */
+        }
     }
 
 
@@ -251,7 +261,7 @@
         return -1;
     }
 
-    for (i = 0; i < MCUBOOT_IMAGE_NUMBER; i++) {
+    for (i = 0; i < (uint8_t)MCUBOOT_IMAGE_NUMBER; i++) {
         if (flash_area_open(FLASH_AREA_IMAGE_PRIMARY(i),
                             &temp_fap) != 0) {
             return -1;
@@ -263,7 +273,7 @@
         }
     }
 
-    if (MCUBOOT_IMAGE_NUMBER == i) {
+    if ((uint8_t)MCUBOOT_IMAGE_NUMBER == i) {
         return -1;
     }
 
diff --git a/boot/bootutil/src/bootutil_misc.c b/boot/bootutil/src/bootutil_misc.c
index b817640..f9059aa 100644
--- a/boot/bootutil/src/bootutil_misc.c
+++ b/boot/bootutil/src/bootutil_misc.c
@@ -52,11 +52,6 @@
 /* Currently only used by imgmgr */
 int boot_current_slot;
 
-extern const uint32_t boot_img_magic[];
-
-#define BOOT_MAGIC_ARR_SZ \
-    (sizeof boot_img_magic / sizeof boot_img_magic[0])
-
 /**
  * @brief Determine if the data at two memory addresses is equal
  *
@@ -82,8 +77,8 @@
 fih_int boot_fih_memequal(const void *s1, const void *s2, size_t n)
 {
     size_t i;
-    uint8_t *s1_p = (uint8_t*) s1;
-    uint8_t *s2_p = (uint8_t*) s2;
+    const volatile uint8_t *s1_p = (const uint8_t*) s1;
+    const volatile uint8_t *s2_p = (const uint8_t*) s2;
     fih_int ret = FIH_FAILURE;
 
     for (i = 0; i < n; i++) {
@@ -100,31 +95,65 @@
 }
 #endif
 
+/*
+ * Amount of space used to save information required when doing a swap,
+ * or while a swap is under progress, but not the status of sector swap
+ * progress itself.
+ */
+static inline uint32_t
+boot_trailer_info_sz(void)
+{
+    return (
+#ifdef MCUBOOT_ENC_IMAGES
+           /* encryption keys */
+    #ifdef MCUBOOT_SWAP_SAVE_ENCTLV
+           BOOT_ENC_TLV_ALIGN_SIZE * 2            +
+#  else
+           BOOT_ENC_KEY_ALIGN_SIZE * 2            +
+#  endif
+#endif
+           /* swap_type + copy_done + image_ok + swap_size */
+           BOOT_MAX_ALIGN * 4                     +
+           BOOT_MAGIC_ALIGN_SIZE
+           );
+}
+
+/*
+ * Amount of space used to maintain progress information for a single swap
+ * operation.
+ */
+static inline uint32_t
+boot_status_entry_sz(uint32_t min_write_sz)
+{
+    return BOOT_STATUS_STATE_COUNT * min_write_sz;
+}
+
 uint32_t
 boot_status_sz(uint32_t min_write_sz)
 {
-    return /* state for all sectors */
-           BOOT_STATUS_MAX_ENTRIES * BOOT_STATUS_STATE_COUNT * min_write_sz;
+    return BOOT_STATUS_MAX_ENTRIES * boot_status_entry_sz(min_write_sz);
 }
 
 uint32_t
 boot_trailer_sz(uint32_t min_write_sz)
 {
-    return /* state for all sectors */
-           boot_status_sz(min_write_sz)           +
-#ifdef MCUBOOT_ENC_IMAGES
-           /* encryption keys */
-#  if MCUBOOT_SWAP_SAVE_ENCTLV
-           BOOT_ENC_TLV_ALIGN_SIZE * 2            +
-#  else
-           BOOT_ENC_KEY_SIZE * 2                  +
-#  endif
-#endif
-           /* swap_type + copy_done + image_ok + swap_size */
-           BOOT_MAX_ALIGN * 4                     +
-           BOOT_MAGIC_SZ;
+    return boot_status_sz(min_write_sz) + boot_trailer_info_sz();
 }
 
+#if !defined(MCUBOOT_SWAP_USING_STATUS) && defined(MCUBOOT_SWAP_USING_SCRATCH)
+/*
+ * Similar to `boot_trailer_sz` but this function returns the space used to
+ * store status in the scratch partition. The scratch partition only stores
+ * status during the swap of the last sector from primary/secondary (which
+ * is the first swap operation) and thus only requires space for one swap.
+ */
+static uint32_t
+boot_scratch_trailer_sz(uint32_t min_write_sz)
+{
+    return boot_status_entry_sz(min_write_sz) + boot_trailer_info_sz();
+}
+#endif
+
 int
 boot_status_entries(int image_index, const struct flash_area *fap)
 {
@@ -145,19 +174,33 @@
 boot_status_off(const struct flash_area *fap)
 {
     uint32_t off_from_end;
-    size_t elem_sz;
+    uint32_t elem_sz;
 
     elem_sz = flash_area_align(fap);
     assert(elem_sz != 0u);
 
-    off_from_end = boot_trailer_sz(elem_sz);
+#if MCUBOOT_SWAP_USING_SCRATCH
+    if (fap->fa_id == FLASH_AREA_IMAGE_SCRATCH) {
+        off_from_end = boot_scratch_trailer_sz(elem_sz);
+    } else {
+#endif
+        off_from_end = boot_trailer_sz(elem_sz);
+#if MCUBOOT_SWAP_USING_SCRATCH
+    }
+#endif
 
     assert(off_from_end <= flash_area_get_size(fap));
     return flash_area_get_size(fap) - off_from_end;
 }
-#endif
 
-#ifndef MCUBOOT_SWAP_USING_STATUS
+static uint32_t
+boot_magic_decode(const uint8_t *magic)
+{
+    if (memcmp(magic, BOOT_IMG_MAGIC, BOOT_MAGIC_SZ) == 0) {
+        return BOOT_MAGIC_GOOD;
+    }
+    return BOOT_MAGIC_BAD;
+}
 
 static inline uint32_t
 boot_magic_off(const struct flash_area *fap)
@@ -169,7 +212,7 @@
 static inline uint32_t
 boot_image_ok_off(const struct flash_area *fap)
 {
-    return boot_magic_off(fap) - BOOT_MAX_ALIGN;
+    return ALIGN_DOWN(boot_magic_off(fap) - BOOT_MAX_ALIGN, BOOT_MAX_ALIGN);
 }
 
 static inline uint32_t
@@ -190,10 +233,9 @@
 boot_enc_key_off(const struct flash_area *fap, uint8_t slot)
 {
 #if MCUBOOT_SWAP_SAVE_ENCTLV
-    return boot_swap_size_off(fap) - ((slot + 1) *
-            ((((BOOT_ENC_TLV_SIZE - 1) / BOOT_MAX_ALIGN) + 1) * BOOT_MAX_ALIGN));
+    return boot_swap_size_off(fap) - (((uint32_t)slot + 1U) * BOOT_ENC_TLV_ALIGN_SIZE);
 #else
-    return boot_swap_size_off(fap) - ((slot + 1) * BOOT_ENC_KEY_SIZE);
+    return boot_swap_size_off(fap) - (((uint32_t)slot + 1U) * BOOT_ENC_KEY_ALIGN_SIZE);
 #endif
 }
 #endif
@@ -211,7 +253,7 @@
 static int
 boot_find_status(int image_index, const struct flash_area **fap)
 {
-    uint32_t magic[BOOT_MAGIC_ARR_SZ];
+    uint8_t magic[BOOT_MAGIC_SZ];
     uint32_t off;
     uint8_t areas[2] = {
 #if MCUBOOT_SWAP_USING_SCRATCH
@@ -238,16 +280,16 @@
 
         off = boot_magic_off(*fap);
         rc = flash_area_read(*fap, off, magic, BOOT_MAGIC_SZ);
+        flash_area_close(*fap);
+
         if (rc != 0) {
-            flash_area_close(*fap);
             return rc;
         }
 
-        if (memcmp(magic, boot_img_magic, BOOT_MAGIC_SZ) == 0) {
+        if (BOOT_MAGIC_GOOD == boot_magic_decode(magic)) {
             return 0;
         }
 
-        flash_area_close(*fap);
     }
 
     /* If we got here, no magic was found */
@@ -282,7 +324,7 @@
     rc = boot_find_status(image_index, &fap);
     if (0 == rc) {
         off = boot_enc_key_off(fap, slot);
-#if MCUBOOT_SWAP_SAVE_ENCTLV
+#ifdef MCUBOOT_SWAP_SAVE_ENCTLV
         uint8_t aes_iv[BOOTUTIL_CRYPTO_AES_CTR_BLOCK_SIZE];
 
         rc = flash_area_read(fap, off, bs->enctlv[slot], BOOT_ENC_TLV_ALIGN_SIZE);
@@ -295,7 +337,7 @@
             }
         }
 #else
-        rc = flash_area_read(fap, off, bs->enckey[slot], BOOT_ENC_KEY_SIZE);
+        rc = flash_area_read(fap, off, bs->enckey[slot], BOOT_ENC_KEY_ALIGN_SIZE);
 #endif
         flash_area_close(fap);
     }
@@ -344,10 +386,10 @@
     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
+#ifdef MCUBOOT_SWAP_SAVE_ENCTLV
     rc = flash_area_write(fap, off, bs->enctlv[slot], BOOT_ENC_TLV_ALIGN_SIZE);
 #else
-    rc = flash_area_write(fap, off, bs->enckey[slot], BOOT_ENC_KEY_SIZE);
+    rc = flash_area_write(fap, off, bs->enckey[slot], BOOT_ENC_KEY_ALIGN_SIZE);
 #endif
     if (rc != 0) {
         return BOOT_EFLASH;
diff --git a/boot/bootutil/src/bootutil_priv.h b/boot/bootutil/src/bootutil_priv.h
index ac40a6e..75367d6 100644
--- a/boot/bootutil/src/bootutil_priv.h
+++ b/boot/bootutil/src/bootutil_priv.h
@@ -55,17 +55,15 @@
 /** Number of image slots in flash; currently limited to two. */
 #define BOOT_NUM_SLOTS                  2
 
-#if (defined(MCUBOOT_OVERWRITE_ONLY) + \
-     defined(MCUBOOT_SWAP_USING_MOVE) + \
-     defined(MCUBOOT_DIRECT_XIP) + \
-     defined(MCUBOOT_RAM_LOAD)) > 1
-#error "Please enable only one of MCUBOOT_OVERWRITE_ONLY, MCUBOOT_SWAP_USING_MOVE, MCUBOOT_DIRECT_XIP or MCUBOOT_RAM_LOAD"
-#endif
-
 #if !defined(MCUBOOT_OVERWRITE_ONLY) && \
     !defined(MCUBOOT_SWAP_USING_MOVE) && \
     !defined(MCUBOOT_DIRECT_XIP) && \
-    !defined(MCUBOOT_RAM_LOAD)
+    !(defined(MCUBOOT_RAM_LOAD) && !defined(MCUBOOT_MULTI_MEMORY_LOAD))
+#define MCUBOOT_SWAP_USING_SCRATCH 1
+#endif
+
+#if !defined(MCUBOOT_OVERWRITE_ONLY) && \
+    !defined(MCUBOOT_SWAP_USING_MOVE)
 #define MCUBOOT_SWAP_USING_SCRATCH 1
 #endif
 
@@ -84,8 +82,8 @@
     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];
-#ifdef MCUBOOT_SWAP_SAVE_ENCTLV
+    uint8_t enckey[BOOT_NUM_SLOTS][BOOT_ENC_KEY_ALIGN_SIZE];
+#if MCUBOOT_SWAP_SAVE_ENCTLV
 #define BOOT_UNINITIALIZED_TLV_FILL 0xFF
     uint8_t enctlv[BOOT_NUM_SLOTS][BOOT_ENC_TLV_ALIGN_SIZE];
 #endif
@@ -112,16 +110,28 @@
  *  |                 Encryption key 0 (16 octets) [*]              |
  *  |                                                               |
  *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |                    0xff padding as needed                     |
+ *  |  (BOOT_MAX_ALIGN minus 16 octets from Encryption key 0) [*]   |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  *  |                 Encryption key 1 (16 octets) [*]              |
  *  |                                                               |
  *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |                    0xff padding as needed                     |
+ *  |  (BOOT_MAX_ALIGN minus 16 octets from Encryption key 1) [*]   |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  *  |                      Swap size (4 octets)                     |
  *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
- *  |   Swap info   |           0xff padding (7 octets)             |
+ *  |                    0xff padding as needed                     |
+ *  |        (BOOT_MAX_ALIGN minus 4 octets from Swap size)         |
  *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
- *  |   Copy done   |           0xff padding (7 octets)             |
+ *  |   Swap info   |  0xff padding (BOOT_MAX_ALIGN minus 1 octet)  |
  *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
- *  |   Image OK    |           0xff padding (7 octets)             |
+ *  |   Copy done   |  0xff padding (BOOT_MAX_ALIGN minus 1 octet)  |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |   Image OK    |  0xff padding (BOOT_MAX_ALIGN minus 1 octet)  |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |                    0xff padding as needed                     |
+ *  |         (BOOT_MAX_ALIGN minus 16 octets from MAGIC)           |
  *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
  *  |                       MAGIC (16 octets)                       |
  *  |                                                               |
@@ -131,15 +141,26 @@
  *      (`MCUBOOT_ENC_IMAGES`).
  */
 
-extern const uint32_t boot_img_magic[4];
+union boot_img_magic_t
+{
+    struct {
+        uint16_t align;
+        uint8_t magic[14];
+    };
+    uint8_t val[16];
+};
 
-#ifdef MCUBOOT_IMAGE_NUMBER
-#define BOOT_IMAGE_NUMBER          MCUBOOT_IMAGE_NUMBER
+extern const union boot_img_magic_t boot_img_magic;
+
+#define BOOT_IMG_MAGIC  (boot_img_magic.val)
+
+#if BOOT_MAX_ALIGN == 8
+#define BOOT_IMG_ALIGN  (BOOT_MAX_ALIGN)
 #else
-#define BOOT_IMAGE_NUMBER          1
+#define BOOT_IMG_ALIGN  (boot_img_magic.align)
 #endif
 
-_Static_assert(BOOT_IMAGE_NUMBER > 0, "Invalid value for BOOT_IMAGE_NUMBER");
+_Static_assert(sizeof(boot_img_magic) == BOOT_MAGIC_SZ, "Invalid size for image magic");
 
 #if !defined(MCUBOOT_DIRECT_XIP) && !defined(MCUBOOT_RAM_LOAD)
 #define ARE_SLOTS_EQUIVALENT()    0
@@ -162,18 +183,7 @@
                  (hdr)->ih_ver.iv_revision,                               \
                  (hdr)->ih_ver.iv_build_num)
 
-/*
- * The current flashmap API does not check the amount of space allocated when
- * loading sector data from the flash device, allowing for smaller counts here
- * would most surely incur in overruns.
- *
- * TODO: make flashmap API receive the current sector array size.
- */
-#if BOOT_MAX_IMG_SECTORS < 32
-#error "Too few sectors, please increase BOOT_MAX_IMG_SECTORS to at least 32"
-#endif
-
-#if defined(MCUBOOT_SWAP_USING_MOVE)
+#if 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)
@@ -184,6 +194,7 @@
 /** Maximum number of image sectors supported by the bootloader. */
 #define BOOT_STATUS_MAX_ENTRIES         BOOT_MAX_IMG_SECTORS
 
+#define NO_ACTIVE_SLOT                  UINT32_MAX
 #define BOOT_PRIMARY_SLOT               0
 #define BOOT_SECONDARY_SLOT             1
 
@@ -235,7 +246,24 @@
 
 #if (BOOT_IMAGE_NUMBER > 1)
     uint8_t curr_img_idx;
+    bool img_mask[BOOT_IMAGE_NUMBER];
 #endif
+
+#if defined(MCUBOOT_DIRECT_XIP) || defined(MCUBOOT_RAM_LOAD) || defined(MCUBOOT_MULTI_MEMORY_BOOT)
+    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
+    } slot_usage[BOOT_IMAGE_NUMBER];
+#endif /* MCUBOOT_DIRECT_XIP || MCUBOOT_RAM_LOAD */
 };
 
 fih_int bootutil_verify_sig(uint8_t *hash, uint32_t hlen, uint8_t *sig,
@@ -423,7 +451,7 @@
 
 #endif  /* !defined(MCUBOOT_USE_FLASH_AREA_GET_SECTORS) */
 
-#ifdef MCUBOOT_RAM_LOAD
+#if defined(MCUBOOT_RAM_LOAD)
 #   ifdef __BOOTSIM__
 
 /* Query for the layout of a RAM buffer appropriate for holding the
@@ -445,16 +473,37 @@
 #       define IMAGE_RAM_BASE ((uintptr_t)0)
 #   endif
 
-#define LOAD_IMAGE_DATA(hdr, fap, start, output, size)       \
-    (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 */
 
+#define LOAD_IMAGE_DATA_RAM(hdr, fap, start, output, size)                    \
+    (memcpy((output),(void*)(IMAGE_RAM_BASE + (hdr)->ih_load_addr + (start)), \
+    (size)), 0)
+
+#define LOAD_IMAGE_DATA_FLASH(hdr, fap, start, output, size)    \
+    (flash_area_read((fap), (start), (output), (size)))
+
+#if defined(MCUBOOT_MULTI_MEMORY_LOAD) && defined(MCUBOOT_RAM_LOAD)
+#define LOAD_IMAGE_DATA(hdr, fap, start, output, size)                         \
+    ({                                                                         \
+        int rc;                                                                \
+        if (IS_RAM_BOOTABLE(hdr)) {                                            \
+            rc = LOAD_IMAGE_DATA_RAM((hdr), (fap), (start), (output), (size)); \
+        } else {                                                               \
+            rc = LOAD_IMAGE_DATA_FLASH((hdr), (fap), (start), (output),        \
+                                       (size));                                \
+        }                                                                      \
+        rc;                                                                    \
+    })
+#elif defined(MCUBOOT_RAM_LOAD)
+#define LOAD_IMAGE_DATA(hdr, fap, start, output, size) \
+    LOAD_IMAGE_DATA_RAM((hdr), (fap), (start), (output), (size))
+#else /* !defined(MCUBOOT_RAM_LOAD)*/
+#define LOAD_IMAGE_DATA(hdr, fap, start, output, size) \
+    LOAD_IMAGE_DATA_FLASH((hdr), (fap), (start), (output), (size))
+#endif /* MCUBOOT_MULTI_MEMORY_LOAD */
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/boot/bootutil/src/bootutil_public.c b/boot/bootutil/src/bootutil_public.c
index b1b4f60..82763d2 100644
--- a/boot/bootutil/src/bootutil_public.c
+++ b/boot/bootutil/src/bootutil_public.c
@@ -52,6 +52,7 @@
 #endif
 
 #include "bootutil/boot_public_hooks.h"
+#include "bootutil_priv.h"
 
 #ifdef CONFIG_MCUBOOT
 BOOT_LOG_MODULE_DECLARE(mcuboot);
@@ -59,15 +60,26 @@
 BOOT_LOG_MODULE_REGISTER(mcuboot_util);
 #endif
 
-const uint32_t boot_img_magic[] = {
-    0xf395c277,
-    0x7fefd260,
-    0x0f505235,
-    0x8079b62c,
+#if BOOT_MAX_ALIGN == 8
+const union boot_img_magic_t boot_img_magic = {
+    .val = {
+        0x77, 0xc2, 0x95, 0xf3,
+        0x60, 0xd2, 0xef, 0x7f,
+        0x35, 0x52, 0x50, 0x0f,
+        0x2c, 0xb6, 0x79, 0x80
+    }
 };
-
-#define BOOT_MAGIC_ARR_SZ \
-    (sizeof boot_img_magic / sizeof boot_img_magic[0])
+#else
+const union boot_img_magic_t boot_img_magic = {
+    .align = BOOT_MAX_ALIGN,
+    .magic = {
+        0x2d, 0xe1,
+        0x5d, 0x29, 0x41, 0x0b,
+        0x8d, 0x77, 0x67, 0x9c,
+        0x11, 0x0f, 0x1f, 0x8a
+    }
+};
+#endif
 
 struct boot_swap_table {
     uint8_t magic_primary_slot;
@@ -122,9 +134,9 @@
 
 #ifndef MCUBOOT_SWAP_USING_STATUS
 static int
-boot_magic_decode(const uint32_t *magic)
+boot_magic_decode(const uint8_t *magic)
 {
-    if (memcmp(magic, boot_img_magic, BOOT_MAGIC_SZ) == 0) {
+    if (memcmp(magic, BOOT_IMG_MAGIC, BOOT_MAGIC_SZ) == 0) {
         return BOOT_MAGIC_GOOD;
     }
     return BOOT_MAGIC_BAD;
@@ -150,7 +162,7 @@
 static inline uint32_t
 boot_image_ok_off(const struct flash_area *fap)
 {
-    return boot_magic_off(fap) - BOOT_MAX_ALIGN;
+    return ALIGN_DOWN(boot_magic_off(fap) - BOOT_MAX_ALIGN, BOOT_MAX_ALIGN);
 }
 
 static inline uint32_t
@@ -253,7 +265,7 @@
 boot_read_swap_state(const struct flash_area *fap,
                      struct boot_swap_state *state)
 {
-    uint32_t magic[BOOT_MAGIC_ARR_SZ];
+    uint8_t magic[BOOT_MAGIC_SZ];
     uint32_t off;
     uint8_t swap_info;
     int rc;
@@ -317,14 +329,32 @@
 boot_write_magic(const struct flash_area *fap)
 {
     uint32_t off;
+    uint32_t pad_off;
     int rc;
+    uint8_t magic[BOOT_MAGIC_ALIGN_SIZE];
+    uint8_t erased_val;
 
     off = boot_magic_off(fap);
 
+    /* image_trailer structure was modified with additional padding such that
+     * the pad+magic ends up in a flash minimum write region. The address
+     * returned by boot_magic_off() is the start of magic which is not the
+     * start of the flash write boundary and thus writes to the magic will fail.
+     * To account for this change, write to magic is first padded with 0xFF
+     * before writing to the trailer.
+     */
+    pad_off = ALIGN_DOWN(off, BOOT_MAX_ALIGN);
+
+    erased_val = flash_area_erased_val(fap);
+
+    (void)memset(&magic[0], erased_val, sizeof(magic));
+    (void)memcpy(&magic[BOOT_MAGIC_ALIGN_SIZE - BOOT_MAGIC_SZ], BOOT_IMG_MAGIC, BOOT_MAGIC_SZ);
+
     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);
+    rc = flash_area_write(fap, pad_off, &magic[0], BOOT_MAGIC_ALIGN_SIZE);
+
     if (rc != 0) {
         return BOOT_EFLASH;
     }
@@ -342,22 +372,24 @@
         const uint8_t *inbuf, uint8_t inlen)
 {
     uint8_t buf[BOOT_MAX_ALIGN];
-    size_t align;
     uint8_t erased_val;
+    uint32_t align;
     int rc;
 
     align = flash_area_align(fap);
+
     if (align == 0u) {
         return BOOT_EFLASH;
     }
-    align = (inlen + align - 1) & ~(align - 1);
+    
+    align = ALIGN_UP(inlen, align);
     if (align > BOOT_MAX_ALIGN) {
         return -1;
     }
     erased_val = flash_area_erased_val(fap);
 
-    memcpy(buf, inbuf, inlen);
-    memset(&buf[inlen], erased_val, align - inlen);
+    (void)memcpy(buf, inbuf, inlen);
+    (void)memset(&buf[inlen], erased_val, align - inlen);
 
     rc = flash_area_write(fap, off, buf, align);
     if (rc != 0) {
diff --git a/boot/bootutil/src/encrypted.c b/boot/bootutil/src/encrypted.c
index 73e1f1f..152271f 100644
--- a/boot/bootutil/src/encrypted.c
+++ b/boot/bootutil/src/encrypted.c
@@ -230,7 +230,7 @@
         return -12;
     }
 
-    memcpy(private_key, *p, len);
+    (void)memcpy(private_key, *p, len);
 
     /* publicKey usually follows but is not parsed here */
 
@@ -289,7 +289,7 @@
         return -8;
     }
 
-    memcpy(private_key, *p, PRIV_KEY_LEN);
+    (void)memcpy(private_key, *p, PRIV_KEY_LEN);
     return 0;
 }
 #endif /* defined(MCUBOOT_ENCRYPT_X25519) */
@@ -385,10 +385,10 @@
         }
 
         if (len > BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE) {
-            memcpy(&okm[off], T, BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE);
+            (void)memcpy(&okm[off], T, BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE);
             len -= BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE;
         } else {
-            memcpy(&okm[off], T, len);
+            (void)memcpy(&okm[off], T, len);
             len = 0;
         }
     }
@@ -424,7 +424,7 @@
 
     rc = bootutil_aes_ctr_set_key(&enc_state[slot].aes_ctr, bs->enckey[slot]);
     if (rc != 0) {
-        boot_enc_drop(enc_state, slot);
+        (void)boot_enc_drop(enc_state, slot);
         enc_state[slot].valid = 0;
         return -1;
     }
@@ -640,7 +640,7 @@
     }
 
     out_len = len;
-    rc = hkdf(shared, SHARED_KEY_LEN, (uint8_t *)"MCUBoot_ECIES_v1", BOOTUTIL_CRYPTO_AES_CTR_BLOCK_SIZE,
+    rc = hkdf(shared, SHARED_KEY_LEN, (const uint8_t *)"MCUBoot_ECIES_v1", BOOTUTIL_CRYPTO_AES_CTR_BLOCK_SIZE,
               my_salt, BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE, derived_key, &out_len);
 
     if (rc != 0 || len != out_len) {
@@ -719,7 +719,7 @@
     uint32_t off;
     uint16_t len;
     struct image_tlv_iter it;
-#if MCUBOOT_SWAP_SAVE_ENCTLV
+#ifdef MCUBOOT_SWAP_SAVE_ENCTLV
     uint8_t *buf;
 #else
     uint8_t buf[EXPECTED_ENC_EXT_LEN];
@@ -741,7 +741,7 @@
 #endif
 
     /* Initialize the AES context */
-    boot_enc_init(enc_state, slot);
+    (void)boot_enc_init(enc_state, slot);
 
     rc = bootutil_tlv_iter_begin(&it, hdr, fap, EXPECTED_ENC_TLV, false);
     if (rc) {
@@ -757,7 +757,7 @@
         return -1;
     }
 
-#if MCUBOOT_SWAP_SAVE_ENCTLV
+#ifdef MCUBOOT_SWAP_SAVE_ENCTLV
     buf = bs->enctlv[slot];
     (void)memset(buf, BOOT_UNINITIALIZED_TLV_FILL, BOOT_ENC_TLV_ALIGN_SIZE);
 #endif
diff --git a/boot/bootutil/src/fault_injection_hardening.c b/boot/bootutil/src/fault_injection_hardening.c
index 5e818c6..4dcbdd9 100644
--- a/boot/bootutil/src/fault_injection_hardening.c
+++ b/boot/bootutil/src/fault_injection_hardening.c
@@ -1,59 +1,52 @@
 /*
- * SPDX-License-Identifier: Apache-2.0
+ * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
  *
- * Copyright (c) 2020 Arm Limited
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
  */
 
 #include "bootutil/fault_injection_hardening.h"
 
-#ifdef FIH_ENABLE_DOUBLE_VARS
-/* 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};
-#else
-fih_int FIH_SUCCESS = {FIH_POSITIVE_VALUE};
-fih_int FIH_FAILURE = {FIH_NEGATIVE_VALUE};
-#endif /* FIH_ENABLE_DOUBLE_VARS */
-
 #ifdef FIH_ENABLE_CFI
+fih_uint fih_cfi_ctr = FIH_UINT_INIT(0u);
 
-#ifdef FIH_ENABLE_DOUBLE_VARS
-fih_int _fih_cfi_ctr = {0, 0 ^ FIH_MASK_VALUE};
-#else
-fih_int _fih_cfi_ctr = {0};
-#endif /* FIH_ENABLE_DOUBLE_VARS */
-
-/* Increment the CFI counter by one, and return the value before the increment.
- */
-fih_int fih_cfi_get_and_increment(void)
+fih_uint fih_cfi_get_and_increment(uint8_t cnt)
 {
-    fih_int saved = _fih_cfi_ctr;
-    _fih_cfi_ctr = fih_int_encode(fih_int_decode(saved) + 1);
-    return saved;
+    fih_uint saved_ctr = fih_cfi_ctr;
+
+    if (fih_uint_decode(fih_cfi_ctr) > UINT32_MAX - cnt) {
+        /* Overflow */
+        FIH_PANIC;
+    }
+
+    fih_cfi_ctr = fih_uint_encode(fih_uint_decode(fih_cfi_ctr) + cnt);
+
+    fih_uint_validate(fih_cfi_ctr);
+    fih_uint_validate(saved_ctr);
+
+    return saved_ctr;
 }
 
-/* Validate that the saved precall value is the same as the value of the global
- * counter. For this to be the case, a fih_ret must have been called between
- * these functions being executed. If the values aren't the same then panic.
- */
-void fih_cfi_validate(fih_int saved)
+void fih_cfi_validate(fih_uint saved)
 {
-    if (fih_int_decode(saved) != fih_int_decode(_fih_cfi_ctr)) {
+    volatile int32_t rc = FIH_FALSE;
+
+    rc = fih_uint_eq(saved, fih_cfi_ctr);
+    if (rc != FIH_TRUE) {
         FIH_PANIC;
     }
 }
 
-/* Decrement the global CFI counter by one, so that it has the same value as
- * before the cfi_precall
- */
 void fih_cfi_decrement(void)
 {
-    _fih_cfi_ctr = fih_int_encode(fih_int_decode(_fih_cfi_ctr) - 1);
-}
+    if (fih_uint_decode(fih_cfi_ctr) < 1u) {
+        FIH_PANIC;
+    }
 
+    fih_cfi_ctr = fih_uint_encode(fih_uint_decode(fih_cfi_ctr) - 1u);
+
+    fih_uint_validate(fih_cfi_ctr);
+}
 #endif /* FIH_ENABLE_CFI */
 
 #ifdef FIH_ENABLE_GLOBAL_FAIL
@@ -61,10 +54,12 @@
  * compiler removing due to non-standard calling procedure. Multiple loop jumps
  * used to make unlooping difficult.
  */
-__attribute__((used))
 __attribute__((noinline))
+__attribute__((noreturn))
+__attribute__((weak))
 void fih_panic_loop(void)
 {
+    FIH_LABEL("FAILURE_LOOP");
     __asm volatile ("b fih_panic_loop");
     __asm volatile ("b fih_panic_loop");
     __asm volatile ("b fih_panic_loop");
@@ -74,5 +69,20 @@
     __asm volatile ("b fih_panic_loop");
     __asm volatile ("b fih_panic_loop");
     __asm volatile ("b fih_panic_loop");
+    while (true) {} /* Satisfy noreturn */
 }
 #endif /* FIH_ENABLE_GLOBAL_FAIL */
+
+#ifdef FIH_ENABLE_DELAY
+void fih_delay_init(void)
+{
+    /* Implement here */
+}
+
+uint8_t fih_delay_random(void)
+{
+    /* Implement here */
+
+    return 0xFF;
+}
+#endif /* FIH_ENABLE_DELAY */
diff --git a/boot/bootutil/src/fault_injection_hardening_delay_rng_mbedtls.c b/boot/bootutil/src/fault_injection_hardening_delay_rng_mbedtls.c
index 9c6f218..95547fb 100644
--- a/boot/bootutil/src/fault_injection_hardening_delay_rng_mbedtls.c
+++ b/boot/bootutil/src/fault_injection_hardening_delay_rng_mbedtls.c
@@ -6,7 +6,7 @@
 
 #include "bootutil/fault_injection_hardening.h"
 
-#ifdef FIH_ENABLE_DELAY
+#ifdef FIH_ENABLE_DELAY_
 
 #include "mcuboot-mbedtls-cfg.h"
 #include "mbedtls/ctr_drbg.h"
diff --git a/boot/bootutil/src/image_ec.c b/boot/bootutil/src/image_ec.c
index 2d92afb..de469af 100644
--- a/boot/bootutil/src/image_ec.c
+++ b/boot/bootutil/src/image_ec.c
@@ -100,7 +100,7 @@
     return mbedtls_ecdsa_read_signature(ctx, hash, hlen, sig, slen);
 }
 
-int
+fih_int
 bootutil_verify_sig(uint8_t *hash, uint32_t hlen, uint8_t *sig, size_t slen,
   uint8_t key_id)
 {
@@ -125,6 +125,6 @@
     rc = bootutil_cmp_sig(&ctx, hash, hlen, sig, slen);
     mbedtls_ecdsa_free(&ctx);
 
-    return rc;
+    FIH_RET(fih_int_encode_zero_equality(rc));
 }
 #endif /* MCUBOOT_SIGN_EC */
diff --git a/boot/bootutil/src/image_ec256.c b/boot/bootutil/src/image_ec256.c
index 69cd507..e5c0cd6 100644
--- a/boot/bootutil/src/image_ec256.c
+++ b/boot/bootutil/src/image_ec256.c
@@ -74,7 +74,7 @@
       memcmp(alg.MBEDTLS_CONTEXT_MEMBER(p), ec_pubkey_oid, sizeof(ec_pubkey_oid) - 1)) {
         return -3;
     }
-    if (param.MBEDTLS_CONTEXT_MEMBER(len) != 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;
     }
@@ -92,12 +92,12 @@
 
     if (mbedtls_ecp_point_read_binary(&ctx->MBEDTLS_CONTEXT_MEMBER(grp),
                                       &ctx->MBEDTLS_CONTEXT_MEMBER(Q),
-                                      *p, end - *p) != 0) {
+                                      *p, end - *p)) {
         return -8;
     }
 
     if (mbedtls_ecp_check_pubkey(&ctx->MBEDTLS_CONTEXT_MEMBER(grp),
-                                 &ctx->MBEDTLS_CONTEXT_MEMBER(Q)) != 0) {
+                                 &ctx->MBEDTLS_CONTEXT_MEMBER(Q))) {
         return -9;
     }
     return 0;
@@ -161,10 +161,10 @@
     }
 
     if (len >= NUM_ECC_BYTES) {
-        memcpy(i, *cp + len - NUM_ECC_BYTES, NUM_ECC_BYTES);
+        (void)memcpy(i, *cp + len - NUM_ECC_BYTES, NUM_ECC_BYTES);
     } else {
-        memset(i, 0, NUM_ECC_BYTES - len);
-        memcpy(i + NUM_ECC_BYTES - len, *cp, len);
+        (void)memset(i, 0, NUM_ECC_BYTES - len);
+        (void)memcpy(i + NUM_ECC_BYTES - len, *cp, len);
     }
     *cp += len;
     return 0;
@@ -200,7 +200,7 @@
 }
 #endif /* not MCUBOOT_ECDSA_NEED_ASN1_SIG */
 
-int
+fih_int
 bootutil_verify_sig(uint8_t *hash, uint32_t hlen, uint8_t *sig, size_t slen,
   uint8_t key_id)
 {
@@ -223,13 +223,13 @@
     rc = bootutil_import_key(&pubkey, end);
 #endif
     if (rc != 0) {
-        return -1;
+        return FIH_FAILURE;
     }
 
 #ifndef MCUBOOT_ECDSA_NEED_ASN1_SIG
     rc = bootutil_decode_sig(signature, sig, sig + slen);
-    if (rc) {
-        return -1;
+    if (rc != 0) {
+        return FIH_FAILURE;
     }
 #endif
 
@@ -242,7 +242,7 @@
 
 #else /* CY_MBEDTLS_HW_ACCELERATION */
     if (hlen != NUM_ECC_BYTES) {
-        return -1;
+        return FIH_FAILURE;
     }
 
     bootutil_ecdsa_p256_init(&ctx);
@@ -256,7 +256,7 @@
 
     bootutil_ecdsa_p256_drop(&ctx);
 
-    return rc;
+    FIH_RET(fih_int_encode_zero_equality(rc));
 }
 
 #endif /* MCUBOOT_USE_TINYCRYPT || defined MCUBOOT_USE_CC310 */
diff --git a/boot/bootutil/src/image_ed25519.c b/boot/bootutil/src/image_ed25519.c
index b5838c4..47fd5ba 100644
--- a/boot/bootutil/src/image_ed25519.c
+++ b/boot/bootutil/src/image_ed25519.c
@@ -64,32 +64,24 @@
     return 0;
 }
 
-int
+fih_int
 bootutil_verify_sig(uint8_t *hash, uint32_t hlen, uint8_t *sig, size_t slen,
   uint8_t key_id)
 {
-    int rc;
-    uint8_t *pubkey;
-    uint8_t *end;
+    fih_int fih_rc = FIH_FAILURE;
 
-    if (hlen != 32 || slen != 64) {
-        return -1;
+    if (hlen == 32 && slen == 64) {
+        uint8_t *pubkey = (uint8_t *)bootutil_keys[key_id].key;
+        uint8_t *end = pubkey + *bootutil_keys[key_id].len;
+
+        if (0 == bootutil_import_key(&pubkey, end) &&
+            ED25519_verify(hash, 32, sig, pubkey)) {
+
+            fih_rc = FIH_SUCCESS;
+        }
     }
 
-    pubkey = (uint8_t *)bootutil_keys[key_id].key;
-    end = pubkey + *bootutil_keys[key_id].len;
-
-    rc = bootutil_import_key(&pubkey, end);
-    if (rc) {
-        return -1;
-    }
-
-    rc = ED25519_verify(hash, 32, sig, pubkey);
-    if (rc == 0) {
-        return -2;
-    }
-
-    return 0;
+    FIH_RET(fih_rc);
 }
 
 #endif /* MCUBOOT_SIGN_ED25519 */
diff --git a/boot/bootutil/src/image_rsa.c b/boot/bootutil/src/image_rsa.c
index 42d2db7..327d94f 100644
--- a/boot/bootutil/src/image_rsa.c
+++ b/boot/bootutil/src/image_rsa.c
@@ -148,7 +148,7 @@
         if (bytes > count)
             bytes = count;
 
-        memcpy(mask, htmp, bytes);
+        (void)memcpy(mask, htmp, bytes);
         mask += bytes;
         count -= bytes;
     }
diff --git a/boot/bootutil/src/image_validate.c b/boot/bootutil/src/image_validate.c
index a53ccb4..4807b48 100644
--- a/boot/bootutil/src/image_validate.c
+++ b/boot/bootutil/src/image_validate.c
@@ -122,55 +122,66 @@
     /* If protected TLVs are present they are also hashed. */
     size += hdr->ih_protect_tlv_size;
 
-#ifdef MCUBOOT_RAM_LOAD
-    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;
-        if (blk_sz > tmp_buf_sz) {
-            blk_sz = tmp_buf_sz;
+    do
+    {      
+#if defined(MCUBOOT_RAM_LOAD)
+#if defined(MCUBOOT_MULTI_MEMORY_LOAD)
+        if (IS_RAM_BOOTABLE(hdr))
+#endif /* MCUBOOT_MULTI_MEMORY_LOAD */
+        {
+            bootutil_sha256_update(
+                &sha256_ctx, (void *)(IMAGE_RAM_BASE + hdr->ih_load_addr), size);
+                break;
         }
+#endif /* MCUBOOT_RAM_LOAD */
+#if !(defined(MCUBOOT_RAM_LOAD) && !defined(MCUBOOT_MULTI_MEMORY_LOAD))
+        {
+            for (off = 0; off < size; off += blk_sz) {
+                blk_sz = size - off;
+                if (blk_sz > tmp_buf_sz) {
+                    blk_sz = tmp_buf_sz;
+                }
 #ifdef MCUBOOT_ENC_IMAGES
-        /* The only data that is encrypted in an image is the payload;
-         * both header and TLVs (when protected) are not.
-         */
-        if ((off < hdr_size) && ((off + blk_sz) > hdr_size)) {
-            /* read only the header */
-            blk_sz = hdr_size - off;
-        }
-        if ((off < tlv_off) && ((off + blk_sz) > tlv_off)) {
-            /* read only up to the end of the image payload */
-            blk_sz = tlv_off - off;
-        }
-#endif
-        rc = flash_area_read(fap, off, tmp_buf, blk_sz);
-        if (rc) {
-            bootutil_sha256_drop(&sha256_ctx);
-            return rc;
-        }
-#ifdef MCUBOOT_ENC_IMAGES
-        if (MUST_DECRYPT(fap, image_index, hdr)) {
-            /* Only payload is encrypted (area between header and TLVs) */
-            if (off >= hdr_size && off < tlv_off) {
-                blk_off = (off - hdr_size) & 0xf;
-#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
+                /* The only data that is encrypted in an image is the payload;
+                * both header and TLVs (when protected) are not.
+                */
+                if ((off < hdr_size) && ((off + blk_sz) > hdr_size)) {
+                    /* read only the header */
+                    blk_sz = hdr_size - off;
+                }
+                if ((off < tlv_off) && ((off + blk_sz) > tlv_off)) {
+                    /* read only up to the end of the image payload */
+                    blk_sz = tlv_off - off;
+                }
+#endif /* MCUBOOT_ENC_IMAGES */
+                rc = flash_area_read(fap, off, tmp_buf, blk_sz);
                 if (rc) {
+                    bootutil_sha256_drop(&sha256_ctx);
                     return rc;
                 }
+#ifdef MCUBOOT_ENC_IMAGES
+                if (MUST_DECRYPT(fap, image_index, hdr)) {
+                    /* Only payload is encrypted (area between header and TLVs) */
+                    if (off >= hdr_size && off < tlv_off) {
+                        blk_off = (off - hdr_size) & 0xf;
+#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 /* MCUBOOT_ENC_IMAGES_XIP */
+                        if (rc) {
+                            return rc;
+                        }
+                    }
+                }
+#endif /* MCUBOOT_ENC_IMAGES */
+                bootutil_sha256_update(&sha256_ctx, tmp_buf, blk_sz);
             }
         }
-#endif
-        bootutil_sha256_update(&sha256_ctx, tmp_buf, blk_sz);
-    }
-#endif /* MCUBOOT_RAM_LOAD */
+#endif /* !MCUBOOT_RAM_LOAD || MCUBOOT_MULTI_MEMORY_LOAD */
+    } while (false);
     bootutil_sha256_finish(&sha256_ctx, hash_result);
     bootutil_sha256_drop(&sha256_ctx);
 
@@ -272,7 +283,7 @@
      *   HW) a fault is injected to accept the public key as valid one.
      */
     FIH_CALL(boot_fih_memequal, fih_rc, hash, key_hash, key_hash_size);
-    if (fih_eq(fih_rc, FIH_SUCCESS)) {
+    if (FIH_TRUE == fih_eq(fih_rc, FIH_SUCCESS)) {
         bootutil_keys[0].key = key;
         pub_key_len = key_len;
         return 0;
@@ -292,33 +303,36 @@
  *                      flash area.
  * @param security_cnt  Pointer to store the security counter value.
  *
- * @return              0 on success; nonzero on failure.
+ * @return              FIH_SUCCESS on success; FIH_FAILURE on failure.
  */
-int32_t
+fih_int
 bootutil_get_img_security_cnt(struct image_header *hdr,
                               const struct flash_area *fap,
-                              uint32_t *img_security_cnt)
+                              fih_uint *img_security_cnt)
 {
-    struct image_tlv_iter it;
-    uint32_t off;
-    uint16_t len;
-    int32_t rc;
+    uint32_t img_sec_cnt = 0u;
+    uint32_t img_chk_cnt = 0u;
+    struct image_tlv_iter it = {0};
+    uint32_t off = 0u;
+    uint16_t len = 0u;
+    int32_t rc = -1;
+    fih_int fih_rc = FIH_FAILURE;
 
-    if ((hdr == NULL) ||
-        (fap == NULL) ||
-        (img_security_cnt == NULL)) {
+    if ((NULL == hdr) ||
+        (NULL == fap) ||
+        (NULL == img_security_cnt)) {
         /* Invalid parameter. */
-        return BOOT_EBADARGS;
+        goto out;
     }
 
     /* The security counter TLV is in the protected part of the TLV area. */
-    if (hdr->ih_protect_tlv_size == 0) {
-        return BOOT_EBADIMAGE;
+    if (0u == hdr->ih_protect_tlv_size) {
+        goto out;
     }
 
     rc = bootutil_tlv_iter_begin(&it, hdr, fap, IMAGE_TLV_SEC_CNT, true);
-    if (rc) {
-        return rc;
+    if (rc != 0) {
+        goto out;
     }
 
     /* Traverse through the protected TLV area to find
@@ -328,20 +342,36 @@
     rc = bootutil_tlv_iter_next(&it, &off, &len, NULL);
     if (rc != 0) {
         /* Security counter TLV has not been found. */
-        return -1;
+        goto out;
     }
 
-    if (len != sizeof(*img_security_cnt)) {
+    if (len != sizeof(img_sec_cnt)) {
         /* Security counter is not valid. */
-        return BOOT_EBADIMAGE;
+        goto out;
     }
 
-    rc = LOAD_IMAGE_DATA(hdr, fap, off, img_security_cnt, len);
+    rc = LOAD_IMAGE_DATA(hdr, fap, off, &img_sec_cnt, len);
     if (rc != 0) {
-        return BOOT_EFLASH;
+        goto out;
     }
 
-    return 0;
+    *img_security_cnt = fih_uint_encode(img_sec_cnt);
+
+    rc = LOAD_IMAGE_DATA(hdr, fap, off, &img_chk_cnt, len);
+    if (rc != 0) {
+        goto out;
+    }
+
+    if (FIH_TRUE == fih_uint_eq(fih_uint_encode(img_chk_cnt),
+                                *img_security_cnt)) {
+
+        if (img_sec_cnt == img_chk_cnt) {
+            fih_rc = FIH_SUCCESS;
+        }
+    }
+
+out:
+    FIH_RET(fih_rc);
 }
 #ifdef CYW20829
 /**
@@ -434,12 +464,12 @@
     int rc = 0;
     fih_int fih_rc = FIH_FAILURE;
 #ifdef MCUBOOT_HW_ROLLBACK_PROT
-    fih_uint security_cnt = fih_uint_encode(UINT_MAX);
+    fih_uint security_cnt = FIH_UINT_MAX;
     uint32_t img_security_cnt = 0;
     uint8_t reprov_packet[REPROV_PACK_SIZE];
     fih_int security_counter_valid = FIH_FAILURE;
     #ifdef CYW20829
-        fih_uint extracted_img_cnt = fih_uint_encode(UINT_MAX);
+    fih_uint extracted_img_cnt = FIH_UINT_MAX;
     #endif /* CYW20829 */
 #endif /* MCUBOOT_HW_ROLLBACK_PROT */
 
@@ -450,7 +480,7 @@
     }
 
     if (out_hash) {
-        memcpy(out_hash, hash, BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE);
+        (void)memcpy(out_hash, hash, BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE);
     }
 
     rc = bootutil_tlv_iter_begin(&it, hdr, fap, IMAGE_TLV_ANY, false);
@@ -485,7 +515,7 @@
             }
 
             FIH_CALL(boot_fih_memequal, fih_rc, hash, buf, sizeof(hash));
-            if (fih_not_eq(fih_rc, FIH_SUCCESS)) {
+            if (fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
                 goto out;
             }
 
@@ -565,7 +595,7 @@
 
             FIH_CALL(boot_nv_security_counter_get, fih_rc, image_index,
                                                            &security_cnt);
-            if (fih_not_eq(fih_rc, FIH_SUCCESS)) {
+            if (fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
                 goto out;
             }
 
@@ -576,8 +606,8 @@
 
             FIH_CALL(platform_security_counter_check_extract, fih_rc, 
                     (uint32_t)image_index, fih_uint_encode(img_security_cnt), &extracted_img_cnt);
-        
-            if (fih_not_eq(fih_rc, FIH_SUCCESS)) {
+
+            if (fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
                 /* The image's security counter exceeds registered value for this image */
                 goto out;
             }
@@ -593,7 +623,7 @@
             fih_rc = fih_int_encode_zero_equality( (int32_t)(img_security_cnt <
                                     fih_uint_decode(security_cnt)) );
 
-            if (fih_not_eq(fih_rc, FIH_SUCCESS)) {
+            if (fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
                 /* The image's security counter is not accepted. */
                 goto out;
             }
@@ -606,7 +636,7 @@
             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))) {
+            if (FIH_TRUE == fih_eq(security_counter_valid, fih_int_encode(HW_ROLLBACK_CNT_VALID))) {
                 /*
                 * Verify the image reprovisioning packet.
                 * This must always be present.
@@ -639,15 +669,17 @@
         goto out;
     }
 #ifdef EXPECTED_SIG_TLV
-    fih_rc = fih_int_encode_zero_equality(fih_not_eq(valid_signature,
-                                                     FIH_SUCCESS));
+    fih_rc = FIH_FAILURE;
+    if (FIH_TRUE == fih_eq(valid_signature, FIH_SUCCESS)) {
+        fih_rc = valid_signature;
+    }
 #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))) {
+    if (fih_eq(security_counter_valid, fih_int_encode(REPROV_PACK_VALID | HW_ROLLBACK_CNT_VALID)) != FIH_TRUE) {
         BOOT_LOG_DBG("Reprovisioning packet TLV 0x51 is not present image = %d", image_index);
 #else
-    if (fih_not_eq(security_counter_valid, FIH_SUCCESS)) {
+    if (fih_eq(security_counter_valid, FIH_SUCCESS) != FIH_TRUE) {
 #endif /* CYW20829 */
         rc = -1;
         goto out;
diff --git a/boot/bootutil/src/loader.c b/boot/bootutil/src/loader.c
index 7794b5a..bb623e5 100644
--- a/boot/bootutil/src/loader.c
+++ b/boot/bootutil/src/loader.c
@@ -36,6 +36,7 @@
 #include <stdlib.h>
 #include <string.h>
 #include "bootutil/bootutil.h"
+#include "bootutil/bootutil_public.h"
 #include "bootutil/image.h"
 #include "bootutil_priv.h"
 #include "swap_priv.h"
@@ -50,7 +51,7 @@
 #include "bootutil/enc_key.h"
 #endif
 
-#if !defined(MCUBOOT_DIRECT_XIP) && !defined(MCUBOOT_RAM_LOAD)
+#if (!defined(MCUBOOT_DIRECT_XIP) && !defined(MCUBOOT_RAM_LOAD)) || defined(MCUBOOT_MULTI_MEMORY_LOAD)
 #include <os/os_malloc.h>
 #endif
 
@@ -63,25 +64,9 @@
 #if (BOOT_IMAGE_NUMBER > 1)
 #define IMAGES_ITER(x) for ((x) = 0; (x) < BOOT_IMAGE_NUMBER; ++(x))
 #else
-#define IMAGES_ITER(x)
+#define IMAGES_ITER(x) for (int iter = 0; iter < 1; ++iter)
 #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
@@ -95,6 +80,14 @@
 #define TARGET_STATIC
 #endif
 
+#if BOOT_MAX_ALIGN > 1024
+#define BUF_SZ BOOT_MAX_ALIGN
+#else
+#define BUF_SZ 1024U
+#endif
+
+static fih_int FIH_SWAP_TYPE_NONE = FIH_INT_INIT(0x3A5C742E);
+
 static int
 boot_read_image_headers(struct boot_loader_state *state, bool require_all,
         struct boot_status *bs)
@@ -175,26 +168,36 @@
  * 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)
+fill_rsp(struct boot_loader_state *state, struct boot_rsp *rsp)
 {
-    uint32_t active_slot;
+    uint32_t active_slot = BOOT_PRIMARY_SLOT;
 
 #if (BOOT_IMAGE_NUMBER > 1)
-    /* Always boot from Image 0. */
+    /* Always boot from the first enabled image. */
     BOOT_CURR_IMG(state) = 0;
+    IMAGES_ITER(BOOT_CURR_IMG(state)) {
+        if (!state->img_mask[BOOT_CURR_IMG(state)]) {
+            break;
+        }
+    }
+    /* At least one image must be active, otherwise skip the execution */
+    if(BOOT_CURR_IMG(state) >= BOOT_IMAGE_NUMBER)
+    {
+        return;
+    }
 #endif
 
+#if defined(MCUBOOT_MULTI_MEMORY_LOAD)
+    if ((state->slot_usage[BOOT_CURR_IMG(state)].active_slot != BOOT_PRIMARY_SLOT) &&
+        (state->slot_usage[BOOT_CURR_IMG(state)].active_slot != NO_ACTIVE_SLOT))
+#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;
+    {
+        active_slot = state->slot_usage[BOOT_CURR_IMG(state)].active_slot;
+    }
 #endif
 
     rsp->br_flash_dev_id = flash_area_get_device_id(BOOT_IMG_AREA(state, active_slot));
@@ -213,6 +216,11 @@
     uint32_t slot;
 
     IMAGES_ITER(BOOT_CURR_IMG(state)) {
+#if BOOT_IMAGE_NUMBER > 1
+        if (state->img_mask[BOOT_CURR_IMG(state)]) {
+            continue;
+        }
+#endif
 #if MCUBOOT_SWAP_USING_SCRATCH
         flash_area_close(BOOT_SCRATCH_AREA(state));
 #endif
@@ -227,7 +235,7 @@
  * Compute the total size of the given image.  Includes the size of
  * the TLVs.
  */
-#if !defined(MCUBOOT_OVERWRITE_ONLY) || defined(MCUBOOT_OVERWRITE_ONLY_FAST)
+#if !defined(MCUBOOT_OVERWRITE_ONLY) || defined(MCUBOOT_OVERWRITE_ONLY_FAST) || defined(MCUBOOT_RAM_LOAD)
 static int
 boot_read_image_size(struct boot_loader_state *state, int slot, uint32_t *size)
 {
@@ -271,6 +279,10 @@
         rc = BOOT_EBADIMAGE;
         goto done;
     }
+    else 
+    {
+        /* acc. to MISRA R.15.7 */
+    }
 
     if (info.it_magic != IMAGE_TLV_INFO_MAGIC) {
         rc = BOOT_EBADIMAGE;
@@ -284,9 +296,8 @@
     flash_area_close(fap);
     return rc;
 }
-#endif /* !MCUBOOT_OVERWRITE_ONLY */
 
-#if !defined(MCUBOOT_RAM_LOAD)
+#endif
 
 static uint32_t
 boot_write_sz(struct boot_loader_state *state)
@@ -407,8 +418,8 @@
 {
 #ifdef MCUBOOT_ENC_IMAGES
     (void)memset(&bs->enckey, BOOT_UNINITIALIZED_KEY_FILL,
-                 BOOT_NUM_SLOTS * BOOT_ENC_KEY_SIZE);
-#if MCUBOOT_SWAP_SAVE_ENCTLV
+                 BOOT_NUM_SLOTS * BOOT_ENC_KEY_ALIGN_SIZE);
+#ifdef MCUBOOT_SWAP_SAVE_ENCTLV
     (void)memset(&bs->enctlv, BOOT_UNINITIALIZED_TLV_FILL,
                  BOOT_NUM_SLOTS * BOOT_ENC_TLV_ALIGN_SIZE);
 #endif
@@ -447,9 +458,9 @@
     const struct flash_area *fap = NULL;
     uint32_t off;
     int area_id;
-    int rc;
+    int rc = 0;
     uint8_t buf[BOOT_MAX_ALIGN];
-    size_t align;
+    uint32_t align;
     uint8_t erased_val;
 
     /* NOTE: The first sector copied (that is the last sector on slot) contains
@@ -498,12 +509,12 @@
 
 done:
     flash_area_close(fap);
+
     return rc;
 }
-#endif /* !MCUBOOT_RAM_LOAD */
-
 #endif /* MCUBOOT_SWAP_USING_STATUS */
 
+
 #endif /* !MCUBOOT_DIRECT_XIP */
 
 /*
@@ -515,7 +526,6 @@
 {
     TARGET_STATIC uint8_t tmpbuf[BOOT_TMPBUF_SZ];
     uint8_t image_index;
-    int rc;
     fih_int fih_rc = FIH_FAILURE;
 
 #if (BOOT_IMAGE_NUMBER == 1)
@@ -523,15 +533,14 @@
 #endif
 
     (void)bs;
-    (void)rc;
 
     image_index = BOOT_CURR_IMG(state);
 
 /* 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 = flash_area_id_to_multi_image_slot(image_index, fap->fa_id);
+#if defined(MCUBOOT_ENC_IMAGES)
+    if (MUST_DECRYPT(fap, image_index, hdr) && !IS_RAM_BOOTABLE(hdr)) {
+        int rc = flash_area_id_to_multi_image_slot(image_index, fap->fa_id);
         if (rc < 0) {
             FIH_RET(fih_rc);
         }
@@ -555,7 +564,6 @@
     FIH_RET(fih_rc);
 }
 
-#if !defined(MCUBOOT_DIRECT_XIP) && !defined(MCUBOOT_RAM_LOAD)
 static fih_int
 split_image_check(struct image_header *app_hdr,
                   const struct flash_area *app_fap,
@@ -575,7 +583,7 @@
 
     FIH_CALL(bootutil_img_validate, fih_rc, NULL, 0, loader_hdr, loader_fap,
              tmpbuf, BOOT_TMPBUF_SZ, NULL, 0, loader_hash);
-    if (fih_not_eq(fih_rc, FIH_SUCCESS)) {
+    if (fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
         FIH_RET(fih_rc);
     }
 
@@ -585,7 +593,6 @@
 out:
     FIH_RET(fih_rc);
 }
-#endif /* !MCUBOOT_DIRECT_XIP && !MCUBOOT_RAM_LOAD */
 
 /*
  * Check that this is a valid header.  Valid means that the magic is
@@ -711,14 +718,13 @@
  *          does not match the slot address.
  */
 static bool
-boot_rom_address_check(struct boot_loader_state *state,
-                       struct slot_usage_t slot_usage[])
+boot_rom_address_check(struct boot_loader_state *state)
 {
     uint32_t active_slot;
     const struct image_header *hdr;
     uint32_t f_off;
 
-    active_slot = slot_usage[BOOT_CURR_IMG(state)].active_slot;
+    active_slot = state->slot_usage[BOOT_CURR_IMG(state)].active_slot;
     hdr = boot_img_hdr(state, active_slot);
     f_off = boot_img_slot_off(state, active_slot);
 
@@ -764,6 +770,7 @@
     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 ||
         (hdr->ih_flags & IMAGE_F_NON_BOOTABLE)) {
 
@@ -787,7 +794,7 @@
 
         BOOT_LOG_DBG(" * No bootable image in slot(%d); continue booting from the primary slot.", slot);
         /* No bootable image in slot; continue booting from the primary slot. */
-        fih_rc = fih_int_encode(1);
+        fih_rc = FIH_SWAP_TYPE_NONE;
         goto out;
     }
 
@@ -803,18 +810,18 @@
             /* Image in the secondary slot does not satisfy version requirement.
              * Erase the image and continue booting from the primary slot.
              */
-            fih_rc = fih_int_encode(1);
+            fih_rc = FIH_SWAP_TYPE_NONE;
             goto out;
         }
     }
 #endif
     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)))
+    if (FIH_TRUE == 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 (!boot_is_header_valid(hdr, fap) || fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
         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, flash_area_get_size(fap));
@@ -826,15 +833,48 @@
         BOOT_LOG_ERR("Image in the %s slot is not valid!",
                      (slot == BOOT_PRIMARY_SLOT) ? "primary" : "secondary");
 #endif
-        fih_rc = fih_int_encode(1);
+        fih_rc = FIH_SWAP_TYPE_NONE;
         goto out;
     }
 
-    /* Image in the secondary slot is valid. */
+#if MCUBOOT_IMAGE_NUMBER > 1 && !defined(MCUBOOT_ENC_IMAGES) && defined(MCUBOOT_VERIFY_IMG_ADDRESS)
+    /* Verify that the image in the secondary slot has a reset address
+     * located in the primary slot. This is done to avoid users incorrectly
+     * overwriting an application written to the incorrect slot.
+     * This feature is only supported by ARM platforms.
+     */
+    if (area_id == FLASH_AREA_IMAGE_SECONDARY(BOOT_CURR_IMG(state))) {
+        const struct flash_area *pri_fa = BOOT_IMG_AREA(state, BOOT_PRIMARY_SLOT);
+        struct image_header *secondary_hdr = boot_img_hdr(state, slot);
+        uint32_t reset_value = 0;
+        uint32_t reset_addr = secondary_hdr->ih_hdr_size + sizeof(reset_value);
+
+        rc = flash_area_read(fap, reset_addr, &reset_value, sizeof(reset_value));
+        if (rc != 0) {
+            fih_rc = fih_int_encode(1);
+            goto out;
+        }
+
+        if (reset_value < pri_fa->fa_off || reset_value> (pri_fa->fa_off + pri_fa->fa_size)) {
+            BOOT_LOG_ERR("Reset address of image in secondary slot is not in the primary slot");
+            BOOT_LOG_ERR("Erasing image from secondary slot");
+
+            /* The vector table in the image located in the secondary
+             * slot does not target the primary slot. This might
+             * indicate that the image was loaded to the wrong slot.
+             *
+             * Erase the image and continue booting from the primary slot.
+             */
+            flash_area_erase(fap, 0, fap->fa_size);
+            fih_rc = fih_int_encode(1);
+            goto out;
+        }
+    }
+#endif
 
 out:
     flash_area_close(fap);
-    BOOT_LOG_DBG("< boot_validate_slot = %d", fih_int_decode(fih_rc));
+    BOOT_LOG_DBG("< boot_validate_slot: fa_id = %u", (unsigned)fap->fa_id);
     FIH_RET(fih_rc);
 }
 
@@ -857,12 +897,13 @@
                              struct image_header *hdr)
 {
     const struct flash_area *fap = NULL;
-    uint32_t img_security_cnt;
+    fih_int fih_rc = FIH_FAILURE;
+    fih_uint img_security_cnt = FIH_UINT_ZERO;
     void * custom_data = NULL;
     int rc;
-#ifdef CYW20829
+#if defined CYW20829
     uint8_t buff[REPROV_PACK_SIZE];
-#endif /* CYW20829 */
+#endif /* defined CYW20829 */
 
     rc = flash_area_open(flash_area_id_from_multi_image_slot(image_index, slot),
                          &fap);
@@ -871,26 +912,30 @@
         goto done;
     }
 
-    rc = bootutil_get_img_security_cnt(hdr, fap, &img_security_cnt);
-    if (rc != 0) {
+    rc = -1;
+    FIH_CALL(bootutil_get_img_security_cnt, fih_rc, hdr, fap, &img_security_cnt);
+    if (fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
         goto done;
     }
+    else 
+    {
+        fih_rc = FIH_FAILURE;
+    }
 
-#ifdef CYW20829
+#if defined 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);
+#endif /* defined CYW20829 */
 
+    rc = boot_nv_security_counter_update(image_index, img_security_cnt, custom_data);
 done:
     flash_area_close(fap);
     return rc;
 }
 #endif /* MCUBOOT_HW_ROLLBACK_PROT */
 
-#if !defined(MCUBOOT_DIRECT_XIP) && !defined(MCUBOOT_RAM_LOAD)
 /**
  * Determines which swap operation to perform, if any.  If it is determined
  * that a swap operation is required, the image in the secondary slot is checked
@@ -912,8 +957,8 @@
          * Ensure image is valid.
          */
         FIH_CALL(boot_validate_slot, fih_rc, state, BOOT_SECONDARY_SLOT, bs);
-        if (fih_not_eq(fih_rc, FIH_SUCCESS)) {
-            if (fih_eq(fih_rc, fih_int_encode(1))) {
+        if (fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
+            if (FIH_TRUE == fih_eq(fih_rc, FIH_SWAP_TYPE_NONE)) {
                 swap_type = BOOT_SWAP_TYPE_NONE;
             } else {
                 swap_type = BOOT_SWAP_TYPE_FAIL;
@@ -923,7 +968,6 @@
 
     return swap_type;
 }
-#endif
 
 /**
  * Erases a region of flash.
@@ -941,7 +985,6 @@
     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.
@@ -1031,35 +1074,41 @@
             }
 #endif
             if (IS_ENCRYPTED(hdr)) {
-                blk_sz = chunk_sz;
-                idx = 0;
-                if (off + bytes_copied < hdr->ih_hdr_size) {
+                uint32_t abs_off = off + bytes_copied;
+                if (abs_off < hdr->ih_hdr_size) {
                     /* do not decrypt header */
-                    blk_off = 0;
-                    if(chunk_sz > hdr->ih_hdr_size) {
-                        blk_sz = chunk_sz - hdr->ih_hdr_size;
-                        idx = hdr->ih_hdr_size - (off + bytes_copied);
+                    if (abs_off + chunk_sz > hdr->ih_hdr_size) {
+                        /* The lower part of the chunk contains header data */
+                        blk_off = 0;
+                        blk_sz = chunk_sz - (hdr->ih_hdr_size - abs_off);
+                        idx = hdr->ih_hdr_size  - abs_off;
                     } else {
-                        /* still in header-area, no need to decrypt */
-                        blk_sz = 0;
+                        /* The chunk contains exclusively header data */
+                        blk_sz = 0; /* nothing to decrypt */
                     }
                 } else {
-                    blk_off = ((off + bytes_copied) - hdr->ih_hdr_size) & 0xf;
+                    idx = 0;
+                    blk_sz = chunk_sz;
+                    blk_off = (abs_off - hdr->ih_hdr_size) & 0xf;
                 }
-                tlv_off = BOOT_TLV_OFF(hdr);
-                if (off + bytes_copied + chunk_sz > tlv_off) {
-                    /* do not decrypt TLVs */
-                    if (off + bytes_copied >= tlv_off) {
-                        blk_sz = 0;
-                    } else {
-                        blk_sz = tlv_off - (off + bytes_copied);
+
+                if (blk_sz > 0)
+                {
+                    tlv_off = BOOT_TLV_OFF(hdr);
+                    if (abs_off + chunk_sz > tlv_off) {
+                        /* do not decrypt TLVs */
+                        if (abs_off >= tlv_off) {
+                            blk_sz = 0;
+                        } else {
+                            blk_sz = tlv_off - abs_off;
+                        }
                     }
-                }
-                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;
+                    rc = boot_encrypt(BOOT_CURR_ENC(state), image_index, fap_src,
+                                      (abs_off + idx) - hdr->ih_hdr_size, blk_sz,
+                                      blk_off, &buf[idx]);
+                    if (rc != 0) {
+                        return rc;
+                    }
                 }
             }
         }
@@ -1297,7 +1346,7 @@
             }
         } else {
             (void)memset(bs->enckey[0], BOOT_UNINITIALIZED_KEY_FILL,
-                         BOOT_ENC_KEY_SIZE);
+                         BOOT_ENC_KEY_ALIGN_SIZE);
         }
 #endif
 
@@ -1322,7 +1371,7 @@
             }
         } else {
             (void)memset(bs->enckey[1], BOOT_UNINITIALIZED_KEY_FILL,
-                         BOOT_ENC_KEY_SIZE);
+                         BOOT_ENC_KEY_ALIGN_SIZE);
         }
 #endif
 
@@ -1381,8 +1430,7 @@
  * @return                  0 on success; nonzero on failure.
  */
 static int
-boot_verify_slot_dependency(struct boot_loader_state *state,
-                            struct image_dependency *dep)
+boot_verify_slot_dependency_flash(struct boot_loader_state *state, struct image_dependency *dep)
 {
     struct image_version *dep_version;
     size_t dep_slot;
@@ -1432,9 +1480,9 @@
  * @return                  0 on success; nonzero on failure.
  */
 static int
-boot_verify_slot_dependencies(struct boot_loader_state *state, uint32_t slot)
+boot_verify_slot_dependencies_flash(struct boot_loader_state *state, uint32_t slot)
 {
-    const struct flash_area *fap;
+    const struct flash_area *fap = NULL;
     struct image_tlv_iter it;
     struct image_dependency dep;
     uint32_t off;
@@ -1463,6 +1511,10 @@
             rc = 0;
             break;
         }
+        else
+        {
+            /* acc. to MISRA R.15.7 */
+        }
 
         if (len != sizeof(dep)) {
             rc = BOOT_EBADIMAGE;
@@ -1481,7 +1533,7 @@
         }
 
         /* Verify dependency and modify the swap type if not satisfied. */
-        rc = boot_verify_slot_dependency(state, &dep);
+        rc = boot_verify_slot_dependency_flash(state, &dep);
         if (rc != 0) {
             /* Dependency not satisfied. */
             goto done;
@@ -1498,13 +1550,17 @@
  * TLV area are all satisfied and update the related swap type if necessary.
  */
 static int
-boot_verify_dependencies(struct boot_loader_state *state)
+boot_verify_dependencies_flash(struct boot_loader_state *state)
 {
     int rc = -1;
     uint8_t slot;
 
     BOOT_CURR_IMG(state) = 0;
     while (BOOT_CURR_IMG(state) < BOOT_IMAGE_NUMBER) {
+        if (state->img_mask[BOOT_CURR_IMG(state)]) {
+            BOOT_CURR_IMG(state)++;
+            continue;
+        }
         if (BOOT_SWAP_TYPE(state) != BOOT_SWAP_TYPE_NONE &&
             BOOT_SWAP_TYPE(state) != BOOT_SWAP_TYPE_FAIL) {
             slot = BOOT_SECONDARY_SLOT;
@@ -1512,7 +1568,7 @@
             slot = BOOT_PRIMARY_SLOT;
         }
 
-        rc = boot_verify_slot_dependencies(state, slot);
+        rc = boot_verify_slot_dependencies_flash(state, slot);
         if (rc == 0) {
             /* All dependencies've been satisfied, continue with next image. */
             BOOT_CURR_IMG(state)++;
@@ -1565,7 +1621,7 @@
     fih_int fih_rc = FIH_FAILURE;
     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)) {
+    if (rc == 0 || fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
         /* Initialize swap status partition for primary slot, because
          * in swap mode it is needed to properly complete copying the image
          * to the primary slot.
@@ -1802,6 +1858,45 @@
         boot_status_reset(bs);
 
 #ifndef MCUBOOT_OVERWRITE_ONLY
+#ifdef MCUBOOT_SWAP_USING_STATUS
+
+        const struct flash_area *fap;
+        uint32_t img_size = 0;
+
+        /* Check here if image firmware + tlvs in slot do not
+         * overlap with last sector of slot. Last sector of slot
+         * contains trailer of the image which needs to be
+         * manupulated independently of other image parts. 
+         * If firmware overlaps with trailer sector it does not 
+         * make sense to move further since any attemps to perform
+         * swap upgrade would lead to failure or unexpected behaviour
+         */
+
+        for (uint32_t i = 0; i < BOOT_NUM_SLOTS; i++) {
+
+            rc = boot_read_image_size(state, i, &img_size);
+
+            if (rc == 0) {
+                fap = BOOT_IMG(state, i).area;
+                if (fap != NULL) {
+
+                    uint32_t trailer_sector_off = (BOOT_WRITE_SZ(state)) * boot_img_num_sectors(state, i) - BOOT_WRITE_SZ(state);
+
+                    BOOT_LOG_DBG("Slot %u firmware + tlvs size = %u, slot size = %u, write_size = %u, write_size * sect_num - write_size = %u",
+                                        i , img_size, fap->fa_size, BOOT_WRITE_SZ(state), trailer_sector_off);
+
+                    if (img_size > trailer_sector_off) {
+                        BOOT_LOG_ERR("Firmware + tlvs in slot %u overlaps with last sector, which contains trailer, erasing this image", i);
+                        rc = flash_area_erase(fap, 0, flash_area_get_size(fap));
+                    }
+                    else {
+                        /* image firmware + tlvs do not overlap with last sector of slot, continue */
+                    }
+                }
+            }
+        }
+#endif /* MCUBOOT_SWAP_USING_STATUS */
+
         rc = swap_read_status(state, bs);
         if (rc != 0) {
             BOOT_LOG_WRN("Failed reading boot status; Image=%u",
@@ -1873,7 +1968,7 @@
             } else {
                 FIH_CALL(boot_validate_slot, fih_rc,
                          state, BOOT_SECONDARY_SLOT, bs);
-                if (fih_not_eq(fih_rc, FIH_SUCCESS)) {
+                if (fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
                     BOOT_SWAP_TYPE(state) = BOOT_SWAP_TYPE_FAIL;
                 } else {
                     BOOT_SWAP_TYPE(state) = bs->swap_type;
@@ -1896,13 +1991,13 @@
                 FIH_CALL(boot_validate_slot, fih_rc,
                          state, BOOT_PRIMARY_SLOT, bs);
 
-                if (rc == 0 || fih_not_eq(fih_rc, FIH_SUCCESS)) {
+                if (rc == 0 || fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
 
                     rc = (boot_img_hdr(state, BOOT_SECONDARY_SLOT)->ih_magic == IMAGE_MAGIC) ? 1: 0;
                     FIH_CALL(boot_validate_slot, fih_rc,
                              state, BOOT_SECONDARY_SLOT, bs);
 
-                    if (rc == 1 && fih_eq(fih_rc, FIH_SUCCESS)) {
+                    if (rc == 1 && FIH_TRUE == fih_eq(fih_rc, FIH_SUCCESS)) {
                         /* Set swap type to REVERT to overwrite the primary
                          * slot with the image contained in secondary slot
                          * and to trigger the explicit setting of the
@@ -1929,7 +2024,7 @@
  * @return              0 on success; nonzero on failure.
  */
 static int
-boot_update_hw_rollback_protection(struct boot_loader_state *state)
+boot_update_hw_rollback_protection_flash(struct boot_loader_state *state)
 {
 #ifdef MCUBOOT_HW_ROLLBACK_PROT
     int rc;
@@ -1966,10 +2061,10 @@
 }
 
 fih_int
-context_boot_go(struct boot_loader_state *state, struct boot_rsp *rsp)
+context_boot_go_flash(struct boot_loader_state *state, struct boot_rsp *rsp)
 {
     size_t slot;
-    struct boot_status bs;
+    struct boot_status bs = {0};
     int rc = -1;
     fih_int fih_rc = FIH_FAILURE;
     int fa_id;
@@ -1990,8 +2085,6 @@
     TARGET_STATIC boot_sector_t status_sectors[BOOT_MAX_SWAP_STATUS_SECTORS];
 #endif
 
-    (void)memset(&bs, 0, sizeof(bs));
-    (void)memset(state, 0, sizeof(struct boot_loader_state));
     has_upgrade = false;
 
 #if (BOOT_IMAGE_NUMBER == 1)
@@ -2003,7 +2096,11 @@
      * completed.
      */
     IMAGES_ITER(BOOT_CURR_IMG(state)) {
-
+#if BOOT_IMAGE_NUMBER > 1
+        if (state->img_mask[BOOT_CURR_IMG(state)]) {
+            continue;
+        }
+#endif
 #if defined(MCUBOOT_ENC_IMAGES) && (BOOT_IMAGE_NUMBER > 1)
         /* The keys used for encryption may no longer be valid (could belong to
          * another images). Therefore, mark them as invalid to force their reload
@@ -2053,7 +2150,7 @@
         /* Iterate over all the images and verify whether the image dependencies
          * are all satisfied and update swap type if necessary.
          */
-        rc = boot_verify_dependencies(state);
+        rc = boot_verify_dependencies_flash(state);
         if (rc != 0) {
             /*
              * It was impossible to upgrade because the expected dependency version
@@ -2071,8 +2168,11 @@
      * all required update operations will have been finished.
      */
     IMAGES_ITER(BOOT_CURR_IMG(state)) {
-
 #if (BOOT_IMAGE_NUMBER > 1)
+        if (state->img_mask[BOOT_CURR_IMG(state)]) {
+            continue;
+        }
+
 #ifdef MCUBOOT_ENC_IMAGES
         /* The keys used for encryption may no longer be valid (could belong to
          * another images). Therefore, mark them as invalid to force their reload
@@ -2142,6 +2242,11 @@
      * have been re-validated.
      */
     IMAGES_ITER(BOOT_CURR_IMG(state)) {
+#if BOOT_IMAGE_NUMBER > 1
+        if (state->img_mask[BOOT_CURR_IMG(state)]) {
+            continue;
+        }
+#endif
         if (BOOT_SWAP_TYPE(state) != BOOT_SWAP_TYPE_NONE) {
             /* Attempt to read an image header from each slot. Ensure that image
              * headers in slots are aligned with headers in boot_data.
@@ -2159,7 +2264,7 @@
 
 #ifdef MCUBOOT_VALIDATE_PRIMARY_SLOT
         FIH_CALL(boot_validate_slot, fih_rc, state, BOOT_PRIMARY_SLOT, &bs);
-        if (fih_not_eq(fih_rc, FIH_SUCCESS)) {
+        if (fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
             goto out;
         }
 #else
@@ -2186,7 +2291,7 @@
         }
 #endif /* MCUBOOT_ENC_IMAGES_XIP */
 
-        rc = boot_update_hw_rollback_protection(state);
+        rc = boot_update_hw_rollback_protection_flash(state);
         if (rc != 0) {
             goto out;
         }
@@ -2212,7 +2317,7 @@
      */
     (void)memset(&bs, 0, sizeof(struct boot_status));
 
-    fill_rsp(state, NULL, rsp);
+    fill_rsp(state, rsp);
 
     fih_rc = FIH_SUCCESS;
 out:
@@ -2276,7 +2381,7 @@
              BOOT_IMG_AREA(&boot_data, split_slot),
              boot_img_hdr(&boot_data, loader_slot),
              BOOT_IMG_AREA(&boot_data, loader_slot));
-    if (fih_not_eq(fih_rc, FIH_SUCCESS)) {
+    if (fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
         goto done;
     }
 
@@ -2297,22 +2402,18 @@
     FIH_RET(fih_rc);
 }
 
-#else /* MCUBOOT_DIRECT_XIP || MCUBOOT_RAM_LOAD */
 
-#define NO_ACTIVE_SLOT UINT32_MAX
+#if defined(MCUBOOT_DIRECT_XIP) || defined(MCUBOOT_RAM_LOAD)
 
 /**
  * Opens all flash areas and checks which contain an image with a valid header.
  *
  * @param  state        Boot loader status information.
- * @param  slot_usage   Structure to fill with information about the available
- *                      slots.
  *
  * @return              0 on success; nonzero on failure.
  */
 static int
-boot_get_slot_usage(struct boot_loader_state *state,
-                    struct slot_usage_t slot_usage[])
+boot_get_slot_usage(struct boot_loader_state *state)
 {
     uint32_t slot;
     int fa_id;
@@ -2320,6 +2421,11 @@
     struct image_header *hdr = NULL;
 
     IMAGES_ITER(BOOT_CURR_IMG(state)) {
+#if BOOT_IMAGE_NUMBER > 1
+        if (state->img_mask[BOOT_CURR_IMG(state)]) {
+            continue;
+        }
+#endif
         /* Open all the slots */
         for (slot = 0; slot < BOOT_NUM_SLOTS; slot++) {
             fa_id = flash_area_id_from_multi_image_slot(
@@ -2340,10 +2446,10 @@
             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;
+                state->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;
+                state->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)
@@ -2351,7 +2457,7 @@
             }
         }
 
-        slot_usage[BOOT_CURR_IMG(state)].active_slot = NO_ACTIVE_SLOT;
+        state->slot_usage[BOOT_CURR_IMG(state)].active_slot = NO_ACTIVE_SLOT;
     }
 
     return 0;
@@ -2362,21 +2468,19 @@
  * 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
-find_slot_with_highest_version(struct boot_loader_state *state,
-                               struct slot_usage_t slot_usage[])
+find_slot_with_highest_version(struct boot_loader_state *state)
 {
     uint32_t slot;
     uint32_t candidate_slot = NO_ACTIVE_SLOT;
     int rc;
 
     for (slot = 0; slot < BOOT_NUM_SLOTS; slot++) {
-        if (slot_usage[BOOT_CURR_IMG(state)].slot_available[slot]) {
+        if (state->slot_usage[BOOT_CURR_IMG(state)].slot_available[slot]) {
             if (candidate_slot == NO_ACTIVE_SLOT) {
                 candidate_slot = slot;
             } else {
@@ -2401,18 +2505,21 @@
  * Prints the state of the loaded images.
  *
  * @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[])
+print_loaded_images(struct boot_loader_state *state)
 {
     uint32_t active_slot;
 
     (void)state;
 
     IMAGES_ITER(BOOT_CURR_IMG(state)) {
-        active_slot = slot_usage[BOOT_CURR_IMG(state)].active_slot;
+#if BOOT_IMAGE_NUMBER > 1
+        if (state->img_mask[BOOT_CURR_IMG(state)]) {
+            continue;
+        }
+#endif
+        active_slot = state->slot_usage[BOOT_CURR_IMG(state)].active_slot;
 
         BOOT_LOG_INF("Image %u loaded from the %s slot",
                      (unsigned)BOOT_CURR_IMG(state),
@@ -2429,13 +2536,11 @@
  * otherwise marks it as selected if it has not been before.
  *
  * @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_loader_state *state,
-                     struct slot_usage_t slot_usage[])
+boot_select_or_erase(struct boot_loader_state *state)
 {
     const struct flash_area *fap;
     int fa_id;
@@ -2443,13 +2548,13 @@
     uint32_t active_slot;
     struct boot_swap_state* active_swap_state;
 
-    active_slot = slot_usage[BOOT_CURR_IMG(state)].active_slot;
+    active_slot = state->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);
 
-    active_swap_state = &(slot_usage[BOOT_CURR_IMG(state)].swap_state);
+    active_swap_state = &(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);
@@ -2510,13 +2615,11 @@
  * predefined bounds that are allowed to be used by executable images.
  *
  * @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_verify_ram_load_address(struct boot_loader_state *state,
-                             struct slot_usage_t slot_usage[])
+boot_verify_ram_load_address(struct boot_loader_state *state)
 {
     uint32_t img_dst;
     uint32_t img_sz;
@@ -2539,8 +2642,8 @@
     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;
+    img_dst = state->slot_usage[BOOT_CURR_IMG(state)].img_dst;
+    img_sz = state->slot_usage[BOOT_CURR_IMG(state)].img_sz;
 
     if (img_dst < exec_ram_start) {
         return BOOT_EBADIMAGE;
@@ -2732,14 +2835,12 @@
  * 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.
+ * @param  state    Boot loader status information.
  *
  * @return                    0 if there is no overlap; nonzero otherwise.
  */
 static int
-boot_check_ram_load_overlapping(struct slot_usage_t slot_usage[],
-                                uint32_t image_id_to_check)
+boot_check_ram_load_overlapping(struct boot_loader_state *state)
 {
     uint32_t i;
 
@@ -2747,22 +2848,23 @@
     uint32_t end_a;
     uint32_t start_b;
     uint32_t end_b;
+    uint32_t image_id_to_check = BOOT_CURR_IMG(state);
 
-    start_a = slot_usage[image_id_to_check].img_dst;
+    start_a = state->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;
+    end_a = start_a + state->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
+        if (state->slot_usage[i].active_slot == NO_ACTIVE_SLOT
             || i == image_id_to_check) {
             continue;
         }
 
-        start_b = slot_usage[i].img_dst;
+        start_b = state->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;
+        end_b = start_b + state->slot_usage[i].img_sz;
 
         if (do_regions_overlap(start_a, end_a, start_b, end_b)) {
             return -1;
@@ -2778,24 +2880,22 @@
  * 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[])
+boot_load_image_to_sram(struct boot_loader_state *state)
 {
     uint32_t active_slot;
     struct image_header *hdr = NULL;
     uint32_t img_dst;
     uint32_t img_sz;
-    int rc;
+    int rc = 0;
 
-    active_slot = slot_usage[BOOT_CURR_IMG(state)].active_slot;
+    active_slot = state->slot_usage[BOOT_CURR_IMG(state)].active_slot;
     hdr = boot_img_hdr(state, active_slot);
 
-    if (hdr->ih_flags & IMAGE_F_RAM_LOAD) {
+    if (IS_RAM_BOOTABLE(hdr)) {
 
         img_dst = hdr->ih_load_addr;
 
@@ -2804,17 +2904,17 @@
             return rc;
         }
 
-        slot_usage[BOOT_CURR_IMG(state)].img_dst = img_dst;
-        slot_usage[BOOT_CURR_IMG(state)].img_sz = img_sz;
+        state->slot_usage[BOOT_CURR_IMG(state)].img_dst = img_dst;
+        state->slot_usage[BOOT_CURR_IMG(state)].img_sz = img_sz;
 
-        rc = boot_verify_ram_load_address(state, slot_usage);
+        rc = boot_verify_ram_load_address(state);
         if (rc != 0) {
             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));
+        rc = boot_check_ram_load_overlapping(state);
         if (rc != 0) {
             BOOT_LOG_INF("Image RAM loading to address 0x%" PRIx32
                          " would overlap with another image.", img_dst);
@@ -2839,7 +2939,8 @@
         } else {
             BOOT_LOG_INF("RAM loading to 0x%" PRIx32 " is succeeded.", img_dst);
         }
-    } else {
+    }
+    else {
         /* Only images that support IMAGE_F_RAM_LOAD are allowed if
          * MCUBOOT_RAM_LOAD is set.
          */
@@ -2847,8 +2948,8 @@
     }
 
     if (rc != 0) {
-        slot_usage[BOOT_CURR_IMG(state)].img_dst = 0;
-        slot_usage[BOOT_CURR_IMG(state)].img_sz = 0;
+        state->slot_usage[BOOT_CURR_IMG(state)].img_dst = 0;
+        state->slot_usage[BOOT_CURR_IMG(state)].img_sz = 0;
     }
 
     return rc;
@@ -2858,24 +2959,22 @@
  * Removes an image from SRAM, by overwriting it with zeros.
  *
  * @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[])
+boot_remove_image_from_sram(struct boot_loader_state *state)
 {
     (void)state;
 
-    BOOT_LOG_INF("Removing image from SRAM at address 0x%" PRIx32,
-                 slot_usage[BOOT_CURR_IMG(state)].img_dst);
+    BOOT_LOG_INF("Removing image from SRAM at address 0x%x",
+                 state->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);
+    (void)memset((void*)(IMAGE_RAM_BASE + state->slot_usage[BOOT_CURR_IMG(state)].img_dst),
+           0, state->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;
+    state->slot_usage[BOOT_CURR_IMG(state)].img_dst = 0;
+    state->slot_usage[BOOT_CURR_IMG(state)].img_sz = 0;
 
     return 0;
 }
@@ -2914,14 +3013,12 @@
  * 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[],
+boot_verify_slot_dependency_ram(struct boot_loader_state *state,
                             struct image_dependency *dep)
 {
     struct image_version *dep_version;
@@ -2931,7 +3028,7 @@
     /* Determine the source of the image which is the subject of
      * the dependency and get it's version.
      */
-    dep_slot = slot_usage[dep->image_id].active_slot;
+    dep_slot = state->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);
@@ -2948,13 +3045,11 @@
  * 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[])
+boot_verify_slot_dependencies_ram(struct boot_loader_state *state)
 {
     uint32_t active_slot;
     const struct flash_area *fap;
@@ -2965,7 +3060,7 @@
     int area_id;
     int rc;
 
-    active_slot = slot_usage[BOOT_CURR_IMG(state)].active_slot;
+    active_slot = state->slot_usage[BOOT_CURR_IMG(state)].active_slot;
 
     area_id = flash_area_id_from_multi_image_slot(BOOT_CURR_IMG(state),
                                                                 active_slot);
@@ -3007,7 +3102,7 @@
             goto done;
         }
 
-        rc = boot_verify_slot_dependency(state, slot_usage, &dep);
+        rc = boot_verify_slot_dependency_ram(state, &dep);
         if (rc != 0) {
             /* Dependency not satisfied. */
             goto done;
@@ -3025,29 +3120,30 @@
  * 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[])
+boot_verify_dependencies_ram(struct boot_loader_state *state)
 {
     int rc = -1;
     uint32_t active_slot;
 
     IMAGES_ITER(BOOT_CURR_IMG(state)) {
-        rc = boot_verify_slot_dependencies(state, slot_usage);
+        if (state->img_mask[BOOT_CURR_IMG(state)]) {
+            continue;
+        }
+        rc = boot_verify_slot_dependencies_ram(state);
         if (rc != 0) {
             /* Dependencies not met or invalid dependencies. */
 
 #ifdef MCUBOOT_RAM_LOAD
-            boot_remove_image_from_sram(state, slot_usage);
+            boot_remove_image_from_sram(state);
 #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;
+            active_slot = state->slot_usage[BOOT_CURR_IMG(state)].active_slot;
+            state->slot_usage[BOOT_CURR_IMG(state)].slot_available[active_slot] = false;
+            state->slot_usage[BOOT_CURR_IMG(state)].active_slot = NO_ACTIVE_SLOT;
 
             return rc;
         }
@@ -3061,13 +3157,11 @@
  * 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[])
+boot_load_and_validate_images(struct boot_loader_state *state)
 {
     uint32_t active_slot;
     int rc;
@@ -3079,16 +3173,14 @@
          * 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;
+            active_slot = state->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);
+            active_slot = find_slot_with_highest_version(state);
             if (active_slot == NO_ACTIVE_SLOT) {
                 BOOT_LOG_INF("No slot to load for image %u",
                              (unsigned)BOOT_CURR_IMG(state));
@@ -3096,23 +3188,29 @@
             }
 
             /* Save the number of the active slot. */
-            slot_usage[BOOT_CURR_IMG(state)].active_slot = active_slot;
+            state->slot_usage[BOOT_CURR_IMG(state)].active_slot = active_slot;
+
+#if BOOT_IMAGE_NUMBER > 1
+        if (state->img_mask[BOOT_CURR_IMG(state)]) {
+            continue;
+        }
+#endif
 
 #ifdef MCUBOOT_DIRECT_XIP
-            rc = boot_rom_address_check(state, slot_usage);
+            rc = boot_rom_address_check(state);
             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;
+                state->slot_usage[BOOT_CURR_IMG(state)].slot_available[active_slot] = false;
+                state->slot_usage[BOOT_CURR_IMG(state)].active_slot = NO_ACTIVE_SLOT;
                 continue;
             }
 
 #ifdef MCUBOOT_DIRECT_XIP_REVERT
-            rc = boot_select_or_erase(state, slot_usage);
+            rc = boot_select_or_erase(state);
             if (rc != 0) {
                 /* The selected image slot has been erased. */
-                slot_usage[BOOT_CURR_IMG(state)].slot_available[active_slot] = false;
-                slot_usage[BOOT_CURR_IMG(state)].active_slot = NO_ACTIVE_SLOT;
+                state->slot_usage[BOOT_CURR_IMG(state)].slot_available[active_slot] = false;
+                state->slot_usage[BOOT_CURR_IMG(state)].active_slot = NO_ACTIVE_SLOT;
                 continue;
             }
 #endif /* MCUBOOT_DIRECT_XIP_REVERT */
@@ -3124,24 +3222,24 @@
              * when loading images from external (untrusted) flash to internal
              * (trusted) RAM and image is authenticated before copying.
              */
-            rc = boot_load_image_to_sram(state, slot_usage);
+            rc = boot_load_image_to_sram(state);
             if (rc != 0 ) {
                 /* 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;
+                state->slot_usage[BOOT_CURR_IMG(state)].slot_available[active_slot] = false;
+                state->slot_usage[BOOT_CURR_IMG(state)].active_slot = NO_ACTIVE_SLOT;
                 continue;
             }
 #endif /* MCUBOOT_RAM_LOAD */
 
             FIH_CALL(boot_validate_slot, fih_rc, state, active_slot, NULL);
-            if (fih_not_eq(fih_rc, FIH_SUCCESS)) {
+            if (fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
                 /* Image is invalid. */
 #ifdef MCUBOOT_RAM_LOAD
-                boot_remove_image_from_sram(state, slot_usage);
+                boot_remove_image_from_sram(state);
 #endif /* MCUBOOT_RAM_LOAD */
-                slot_usage[BOOT_CURR_IMG(state)].slot_available[active_slot] = false;
-                slot_usage[BOOT_CURR_IMG(state)].active_slot = NO_ACTIVE_SLOT;
+                state->slot_usage[BOOT_CURR_IMG(state)].slot_available[active_slot] = false;
+                state->slot_usage[BOOT_CURR_IMG(state)].active_slot = NO_ACTIVE_SLOT;
                 continue;
             }
 
@@ -3150,6 +3248,7 @@
         }
     }
 
+    (void) rc;
     FIH_RET(FIH_SUCCESS);
 }
 
@@ -3157,13 +3256,11 @@
  * 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[])
+boot_update_hw_rollback_protection_ram(struct boot_loader_state *state)
 {
 #ifdef MCUBOOT_HW_ROLLBACK_PROT
     int rc;
@@ -3177,11 +3274,11 @@
      * 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) {
+    if (state->slot_usage[BOOT_CURR_IMG(state)].swap_state.image_ok == BOOT_FLAG_SET) {
 #endif
         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));
+                                          state->slot_usage[BOOT_CURR_IMG(state)].active_slot,
+                                          boot_img_hdr(state, state->slot_usage[BOOT_CURR_IMG(state)].active_slot));
         if (rc != 0) {
             BOOT_LOG_ERR("Security counter update failed after image "
                             "validation.");
@@ -3195,22 +3292,17 @@
 
 #else /* MCUBOOT_HW_ROLLBACK_PROT */
     (void) (state);
-    (void) (slot_usage);
     return 0;
 #endif
 }
 
 fih_int
-context_boot_go(struct boot_loader_state *state, struct boot_rsp *rsp)
+context_boot_go_ram(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);
+    fih_int fih_rc = FIH_FAILURE;
 
-    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);
+    rc = boot_get_slot_usage(state);
     if (rc != 0) {
         goto out;
     }
@@ -3218,13 +3310,13 @@
 #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)) {
+        FIH_CALL(boot_load_and_validate_images, fih_rc, state);
+        if (fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
             goto out;
         }
 
 #if (BOOT_IMAGE_NUMBER > 1)
-        rc = boot_verify_dependencies(state, slot_usage);
+        rc = boot_verify_dependencies_ram(state);
         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
@@ -3238,12 +3330,17 @@
 #endif
 
     IMAGES_ITER(BOOT_CURR_IMG(state)) {
-        rc = boot_update_hw_rollback_protection(state, slot_usage);
+#if BOOT_IMAGE_NUMBER > 1
+        if (state->img_mask[BOOT_CURR_IMG(state)]) {
+            continue;
+        }
+#endif
+        rc = boot_update_hw_rollback_protection_ram(state);
         if (rc != 0) {
             goto out;
         }
 
-        rc = boot_add_shared_data(state, slot_usage[BOOT_CURR_IMG(state)].active_slot);
+        rc = boot_add_shared_data(state, state->slot_usage[BOOT_CURR_IMG(state)].active_slot);
         if (rc != 0) {
             goto out;
         }
@@ -3251,16 +3348,16 @@
 
     /* All image loaded successfully. */
 #ifdef MCUBOOT_HAVE_LOGGING
-    print_loaded_images(state, slot_usage);
+    print_loaded_images(state);
 #endif
 
-    fill_rsp(state, slot_usage, rsp);
+    fill_rsp(state, rsp);
 
 out:
     close_all_flash_areas(state);
 
-    if (fih_eq(fih_rc, FIH_SUCCESS)) {
-        fih_rc = fih_int_encode(rc);
+    if (FIH_TRUE == fih_eq(fih_rc, FIH_SUCCESS)) {
+        fih_rc = fih_int_encode_zero_equality(rc);
     }
 
     FIH_RET(fih_rc);
@@ -3279,6 +3376,87 @@
 boot_go(struct boot_rsp *rsp)
 {
     fih_int fih_rc = FIH_FAILURE;
-    FIH_CALL(context_boot_go, fih_rc, &boot_data, rsp);
+
+    boot_state_clear(NULL);
+
+    FIH_CALL(context_boot_go_flash, fih_rc, &boot_data, rsp);
     FIH_RET(fih_rc);
 }
+
+/**
+ * Prepares the booting process, considering only a single image. This function
+ * moves images around in flash as appropriate, and tells you what address to
+ * boot from.
+ *
+ * @param rsp                   On success, indicates how booting should occur.
+ *
+ * @param image_id              The image ID to prepare the boot process for.
+ *
+ * @return                      FIH_SUCCESS on success; nonzero on failure.
+ */
+fih_int
+boot_go_for_image_id(struct boot_rsp *rsp, uint32_t image_id)
+{
+    fih_int fih_rc = FIH_FAILURE;
+
+    if (image_id >= BOOT_IMAGE_NUMBER) {
+        FIH_RET(FIH_FAILURE);
+    }
+
+#if BOOT_IMAGE_NUMBER > 1
+    (void)memset(&boot_data.img_mask, 1, BOOT_IMAGE_NUMBER);
+    boot_data.img_mask[image_id] = 0;
+#endif
+
+    FIH_CALL(context_boot_go_flash, fih_rc, &boot_data, rsp);
+    FIH_RET(fih_rc);
+}
+
+#if defined(MCUBOOT_RAM_LOAD)
+/**
+ * Prepares the booting process, considering only a single image. This function
+ * moves images around in flash as appropriate, and tells you what address to
+ * boot from.
+ *
+ * @param rsp                   On success, indicates how booting should occur.
+ *
+ * @param image_id              The image ID to prepare the boot process for.
+ *
+ * @return                      FIH_SUCCESS on success; nonzero on failure.
+ */
+fih_int
+boot_go_for_image_id_ram(struct boot_rsp *rsp, uint32_t image_id)
+{
+    fih_int fih_rc = FIH_FAILURE;
+
+    if (image_id >= BOOT_IMAGE_NUMBER) {
+        FIH_RET(FIH_FAILURE);
+    }
+
+#if BOOT_IMAGE_NUMBER > 1
+    (void)memset(&boot_data.img_mask, 1, BOOT_IMAGE_NUMBER);
+    boot_data.img_mask[image_id] = 0;
+#endif
+
+    FIH_CALL(context_boot_go_ram, fih_rc, &boot_data, rsp);
+    FIH_RET(fih_rc);
+}
+
+#endif /* MCUBOOT_RAM_LOAD */
+
+/**
+ * Clears the boot state, so that previous operations have no effect on new
+ * ones.
+ *
+ * @param state                 The state that should be cleared. If the value
+ *                              is NULL, the default bootloader state will be
+ *                              cleared.
+ */
+void boot_state_clear(struct boot_loader_state *state)
+{
+    if (state != NULL) {
+        (void)memset(state, 0, sizeof(struct boot_loader_state));
+    } else {
+        (void)memset(&boot_data, 0, sizeof(struct boot_loader_state));
+    }
+}
diff --git a/boot/bootutil/src/swap_misc.c b/boot/bootutil/src/swap_misc.c
index 13af238..64485db 100644
--- a/boot/bootutil/src/swap_misc.c
+++ b/boot/bootutil/src/swap_misc.c
@@ -169,7 +169,8 @@
         off = boot_swap_info_off(fap);
         rc = flash_area_read(fap, off, &swap_info, sizeof swap_info);
         if (rc != 0) {
-            return BOOT_EFLASH;
+            rc = BOOT_EFLASH;
+            goto done;
         }
 
         if (swap_info == flash_area_erased_val(fap)) {
@@ -181,6 +182,7 @@
         bs->swap_type = BOOT_GET_SWAP_TYPE(swap_info);
     }
 
+done:
     flash_area_close(fap);
 
     return rc;
diff --git a/boot/bootutil/src/swap_move.c b/boot/bootutil/src/swap_move.c
index 08c21da..a4ce84b 100644
--- a/boot/bootutil/src/swap_move.c
+++ b/boot/bootutil/src/swap_move.c
@@ -459,6 +459,8 @@
     const struct flash_area *fap_sec;
     int rc;
 
+    BOOT_LOG_INF("Starting swap using move algorithm.");
+
     sz = 0;
     g_last_idx = 0;
 
@@ -494,6 +496,9 @@
 
         if (g_last_idx >= first_trailer_idx) {
             BOOT_LOG_WRN("Not enough free space to run swap upgrade");
+            BOOT_LOG_WRN("required %d bytes but only %d are available",
+                         (g_last_idx + 1) * sector_sz ,
+                         first_trailer_idx * sector_sz);
             bs->swap_type = BOOT_SWAP_TYPE_NONE;
             return;
         }
diff --git a/boot/bootutil/src/swap_scratch.c b/boot/bootutil/src/swap_scratch.c
index 0097611..17d0255 100644
--- a/boot/bootutil/src/swap_scratch.c
+++ b/boot/bootutil/src/swap_scratch.c
@@ -56,7 +56,7 @@
 {
     const struct flash_area *fap = NULL;
     int area_id;
-    int rc;
+    int rc = 0;
 
     int saved_slot = slot;
 
@@ -95,6 +95,7 @@
     }
 
     area_id = flash_area_id_from_multi_image_slot(BOOT_CURR_IMG(state), slot);
+
     rc = flash_area_open(area_id, &fap);
     if (rc != 0) {
         rc = BOOT_EFLASH;
@@ -141,8 +142,6 @@
     return rc;
 }
 
-#if !defined(MCUBOOT_DIRECT_XIP) && !defined(MCUBOOT_RAM_LOAD)
-
 #ifndef MCUBOOT_SWAP_USING_STATUS
 /**
  * Reads the status of a partially-completed swap, if any.  This is necessary
@@ -569,13 +568,14 @@
     const struct flash_area *fap_scratch = NULL;
     uint32_t copy_sz;
     uint32_t trailer_sz;
+    uint32_t sector_sz;
     uint32_t img_off;
     uint32_t scratch_trailer_off;
     struct boot_swap_state swap_state = {0};
     size_t last_sector;
     bool erase_scratch;
     uint8_t image_index;
-    int rc;
+    __attribute__((unused)) int rc;
 
     /* Calculate offset from start of image area. */
     img_off = boot_img_sector_off(state, BOOT_PRIMARY_SLOT, idx);
@@ -584,6 +584,18 @@
 
 #ifdef MCUBOOT_SWAP_USING_STATUS
     trailer_sz = BOOT_WRITE_SZ(state); // TODO: deep investigation in swap_status use case
+    /* TODO: this code needs to be refined. It is introduced to overcome
+     * situation when MCUBootApp lives in internal memory, but user app
+     * is executed from different type memory - external in XIP mode in
+     * this case. This situation now arise on PSOC6 when XIP execution is
+     * used, bay may be applicable to other devices, where solution is
+     * distributed between memories with different write/erase sizes.
+     */
+#ifdef CY_BOOT_USE_EXTERNAL_FLASH
+    if (trailer_sz > MEMORY_ALIGN) {
+        trailer_sz = MEMORY_ALIGN;
+    }
+#endif
 #else
     trailer_sz = boot_trailer_sz(BOOT_WRITE_SZ(state));
 #endif
@@ -598,6 +610,21 @@
      * controls if special handling is needed (swapping last sector).
      */
     last_sector = boot_img_num_sectors(state, BOOT_PRIMARY_SLOT) - 1;
+    sector_sz = boot_img_sector_size(state, BOOT_PRIMARY_SLOT, last_sector);
+
+    if (sector_sz < trailer_sz) {
+        uint32_t trailer_sector_sz = sector_sz;
+
+        while (trailer_sector_sz < trailer_sz) {
+            /* Consider that the image trailer may span across sectors of
+             * different sizes.
+             */
+            sector_sz = boot_img_sector_size(state, BOOT_PRIMARY_SLOT, --last_sector);
+
+            trailer_sector_sz += sector_sz;
+        }
+    }
+
     if ((img_off + sz) >
         boot_img_sector_off(state, BOOT_PRIMARY_SLOT, last_sector)) {
         copy_sz -= trailer_sz;
@@ -780,6 +807,8 @@
     last_sector_idx = 0;
     last_idx_secondary_slot = 0;
 
+    BOOT_LOG_INF("Starting swap using scratch algorithm.");
+
     /*
      * Knowing the size of the largest image between both slots, here we
      * find what is the last sector in the primary slot that needs swapping.
@@ -824,6 +853,4 @@
 }
 #endif /* !MCUBOOT_OVERWRITE_ONLY */
 
-#endif /* !MCUBOOT_DIRECT_XIP && !MCUBOOT_RAM_LOAD */
-
 #endif /* !MCUBOOT_SWAP_USING_MOVE */
diff --git a/boot/bootutil/src/swap_status.c b/boot/bootutil/src/swap_status.c
index 7f7e065..d5be7c4 100644
--- a/boot/bootutil/src/swap_status.c
+++ b/boot/bootutil/src/swap_status.c
@@ -45,7 +45,6 @@
 
 #ifdef MCUBOOT_SWAP_USING_STATUS
 
-#if !defined(MCUBOOT_DIRECT_XIP) && !defined(MCUBOOT_RAM_LOAD)
 int
 swap_read_status_bytes(const struct flash_area *fap,
         struct boot_loader_state *state, struct boot_status *bs)
@@ -171,10 +170,7 @@
 uint32_t
 boot_status_internal_off(const struct boot_status *bs, uint32_t elem_sz)
 {
-    uint32_t off = (bs->idx - BOOT_STATUS_IDX_0) * elem_sz;
-
-    return off;
+    return (bs->idx - BOOT_STATUS_IDX_0) * elem_sz;
 }
-#endif /* !MCUBOOT_DIRECT_XIP && !MCUBOOT_RAM_LOAD */
 
 #endif /* MCUBOOT_SWAP_USING_STATUS */
diff --git a/boot/bootutil/src/swap_status.h b/boot/bootutil/src/swap_status.h
index cd96856..9cc8362 100644
--- a/boot/bootutil/src/swap_status.h
+++ b/boot/bootutil/src/swap_status.h
@@ -85,7 +85,7 @@
 #define BOOT_SWAP_STATUS_CNT_SZ         4UL
 #define BOOT_SWAP_STATUS_CRC_SZ         4UL
 
-#define BOOT_SWAP_STATUS_ROW_SZ         CY_FLASH_ALIGN
+#define BOOT_SWAP_STATUS_ROW_SZ         MEMORY_ALIGN
 
 /* agreed to name it "a record" */
 #define BOOT_SWAP_STATUS_PAYLD_SZ       (BOOT_SWAP_STATUS_ROW_SZ -\
@@ -155,7 +155,7 @@
                                      BOOT_SWAP_STATUS_SZ_PRIM)
 
 /* size Limit for primary slot trailer buffer */
-#define MAX_TRAILER_BUF_SIZE        CY_FLASH_ALIGN
+#define MAX_TRAILER_BUF_SIZE        PLATFORM_MAX_TRAILER_PAGE_SIZE
 
 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);
diff --git a/boot/bootutil/src/swap_status_misc.c b/boot/bootutil/src/swap_status_misc.c
index c3f4017..6299c8a 100644
--- a/boot/bootutil/src/swap_status_misc.c
+++ b/boot/bootutil/src/swap_status_misc.c
@@ -43,18 +43,17 @@
 
 MCUBOOT_LOG_MODULE_DECLARE(mcuboot);
 
-#if defined(MCUBOOT_SWAP_USING_STATUS)
+#define ERROR_VALUE      (UINT32_MAX)
 
-#define BOOT_MAGIC_ARR_SZ \
-    (sizeof boot_img_magic / sizeof boot_img_magic[0])
+#if defined(MCUBOOT_SWAP_USING_STATUS)
 
 static int
 boot_find_status(int image_index, const struct flash_area **fap);
 
 static int
-boot_magic_decode(const uint32_t *magic)
+boot_magic_decode(const union boot_img_magic_t *magic_p)
 {
-    if (memcmp(magic, boot_img_magic, BOOT_MAGIC_SZ) == 0) {
+    if (memcmp((const void*)magic_p->val, (const void *)&boot_img_magic.val, BOOT_MAGIC_SZ) == 0) {
         return BOOT_MAGIC_GOOD;
     }
     return BOOT_MAGIC_BAD;
@@ -111,15 +110,28 @@
 }
 
 #ifdef MCUBOOT_ENC_IMAGES
+/**
+ * @returns ERROR_VALUE on error, otherwise result.
+ */
 static inline uint32_t
 boot_enc_key_off(const struct flash_area *fap, uint8_t slot)
 {
+    uint32_t slot_offset;
+    uint32_t res = ERROR_VALUE;
+    uint32_t boot_swap_size_offset = boot_swap_size_off(fap);
+
 #ifdef MCUBOOT_SWAP_SAVE_ENCTLV
     /* suggest encryption key is also stored in status partition */
-    return boot_swap_size_off(fap) - (((uint32_t)slot + 1UL) * (uint32_t)BOOT_ENC_TLV_SIZE);
+    slot_offset = ((uint32_t)slot + 1UL) * (uint32_t)BOOT_ENC_TLV_SIZE;
 #else
-    return boot_swap_size_off(fap) - (((uint32_t)slot + 1UL) * (uint32_t)BOOT_ENC_KEY_SIZE);
+    slot_offset = ((uint32_t)slot + 1UL) * (uint32_t)BOOT_ENC_KEY_SIZE;
 #endif
+
+    if (boot_swap_size_offset >= slot_offset)
+    {
+        res = boot_swap_size_offset - slot_offset;
+    }
+    return res;
 }
 #endif
 
@@ -160,10 +172,12 @@
 boot_write_enc_key(const struct flash_area *fap, uint8_t slot,
         const struct boot_status *bs)
 {
-    uint32_t off;
     int rc;
-
-    off = boot_enc_key_off(fap, slot);
+    uint32_t off = boot_enc_key_off(fap, slot);
+    if (ERROR_VALUE == off)
+    {
+        return -1;
+    }
 #ifdef MCUBOOT_SWAP_SAVE_ENCTLV
     rc = swap_status_update(fap->fa_id, off,
                             bs->enctlv[slot], BOOT_ENC_TLV_ALIGN_SIZE);
@@ -188,6 +202,10 @@
     rc = boot_find_status(image_index, &fap);
     if (0 == rc) {
         off = boot_enc_key_off(fap, slot);
+        if (ERROR_VALUE == off)
+        {
+            return -1;
+        }
 #ifdef MCUBOOT_SWAP_SAVE_ENCTLV
         rc = swap_status_retrieve(fap->fa_id, off, bs->enctlv[slot], BOOT_ENC_TLV_ALIGN_SIZE);
         if (0 == rc) {
@@ -220,7 +238,7 @@
     off = boot_magic_off(fap);
 
     rc = swap_status_update(fap->fa_id, off,
-                            boot_img_magic, BOOT_MAGIC_SZ);
+                            (const uint8_t *)&boot_img_magic, BOOT_MAGIC_SZ);
 
     if (rc != 0) {
         return -1;
@@ -291,11 +309,11 @@
 boot_read_swap_state(const struct flash_area *fap,
                      struct boot_swap_state *state)
 {
-    uint32_t magic[BOOT_MAGIC_ARR_SZ];
-    uint32_t off;
+    union boot_img_magic_t magic = {0U};
+    uint32_t off = 0U;
     uint32_t trailer_off = 0U;
     uint8_t swap_info = 0U;
-    int rc;
+    int rc = 0;
     uint32_t erase_trailer = 0;
     bool buf_is_clean = false;
     bool is_primary = false;
@@ -311,7 +329,7 @@
 
     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);
+    rc = swap_status_retrieve(fap->fa_id, off, &magic, BOOT_MAGIC_SZ);
     if (rc < 0) {
         return -1;
     }
@@ -328,26 +346,26 @@
     }
 
     /* fill magic number value if equal to expected */
-    if (bootutil_buffer_is_erased(fap_stat, magic, BOOT_MAGIC_SZ)) {
+    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 (is_secondary) {
             trailer_off = fap->fa_size - BOOT_MAGIC_SZ;
 
-            rc = flash_area_read(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);
+            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);
+                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);
+                                    (uint8_t *)&magic, BOOT_MAGIC_SZ);
                 }
                 if (rc < 0) {
                     return -1;
@@ -357,7 +375,7 @@
             }
         }
     } else {
-        state->magic = (uint8_t)boot_magic_decode(magic);
+        state->magic = (uint8_t)boot_magic_decode(&magic);
     }
 
     off = boot_swap_info_off(fap);
@@ -468,7 +486,7 @@
 static int
 boot_find_status(int image_index, const struct flash_area **fap)
 {
-    uint32_t magic[BOOT_MAGIC_ARR_SZ] = {0};
+    union boot_img_magic_t magic = {0};
     uint32_t off;
     int rc = -1;
     uint8_t area = FLASH_AREA_ERROR;
@@ -492,10 +510,10 @@
          return rc;
     }
     off = boot_magic_off(*fap);
-    rc = swap_status_retrieve(area, off, magic, BOOT_MAGIC_SZ);
+    rc = swap_status_retrieve(area, off, &magic, BOOT_MAGIC_SZ);
 
     if (0 == rc) {
-        rc = memcmp(magic, boot_img_magic, BOOT_MAGIC_SZ);
+        rc = memcmp((const void*)&magic.val, (const void *)&boot_img_magic.val, BOOT_MAGIC_SZ);
     }
 
     flash_area_close(*fap);
diff --git a/boot/bootutil/src/swap_status_part.c b/boot/bootutil/src/swap_status_part.c
index f6be047..0056577 100644
--- a/boot/bootutil/src/swap_status_part.c
+++ b/boot/bootutil/src/swap_status_part.c
@@ -30,6 +30,7 @@
 #include <string.h>
 #include <stdlib.h>
 #include "swap_status.h"
+#include "sysflash/sysflash.h"
 
 #ifdef MCUBOOT_SWAP_USING_STATUS
 
@@ -67,32 +68,46 @@
 
 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
-      , 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) {
+#ifdef MCUBOOT_SWAP_USING_SCRATCH
+    #define ADD_ARRAY_MEMBER_FOR_SCRATCH (1U)
+#else
+    #define ADD_ARRAY_MEMBER_FOR_SCRATCH (0U)
+#endif /* MCUBOOT_SWAP_USING_SCRATCH */
+
+    /* we always have at least 2 images in BOOT and UPGRADE slots */
+#define ARR_SIZE  (SLOTS_FOR_IMAGE + ADD_ARRAY_MEMBER_FOR_SCRATCH + ((uint8_t)BOOT_IMAGE_NUMBER - 1U) * SLOTS_FOR_IMAGE)
+
+    uint8_t slots_ids[ARR_SIZE];
+
+    slots_ids[0] = FLASH_AREA_IMAGE_PRIMARY(0U);
+    slots_ids[1] = FLASH_AREA_IMAGE_SECONDARY(0U);
+
+#ifdef MCUBOOT_SWAP_USING_SCRATCH
+    /* The third position of SCRATCH is saved as it was before */
+    slots_ids[2] = FLASH_AREA_IMAGE_SCRATCH;
+#endif /* MCUBOOT_SWAP_USING_SCRATCH */
+
+#if (BOOT_IMAGE_NUMBER > 1)
+
+    uint8_t primary_slots_per_id = SLOTS_FOR_IMAGE + ADD_ARRAY_MEMBER_FOR_SCRATCH;
+    uint8_t secondary_slots_per_id = primary_slots_per_id + 1U;
+
+    for (i = 1U; i < (uint32_t) BOOT_IMAGE_NUMBER; ++i)
+    {
+        slots_ids[primary_slots_per_id] = FLASH_AREA_IMAGE_PRIMARY( i );
+        slots_ids[secondary_slots_per_id] = FLASH_AREA_IMAGE_SECONDARY( i );
+        primary_slots_per_id += 2U;
+        secondary_slots_per_id += 2U;
+    }
+
+#endif /* BOOT_IMAGE_NUMBER > 1 */
+
+    for (i = 0U; i < ARR_SIZE; i++) {
+        if (slots_ids[i] == area_id) {
             result = offset;
             break;
         }
@@ -247,7 +262,7 @@
 
 static int boot_magic_decode(uint8_t *magic)
 {
-    if (memcmp(magic, (const uint8_t *)boot_img_magic, BOOT_MAGIC_SZ) == 0) {
+    if (memcmp((const void *)magic, (const void *)&boot_img_magic.val, BOOT_MAGIC_SZ) == 0) {
         return BOOT_MAGIC_GOOD;
     }
     return BOOT_MAGIC_BAD;
@@ -430,7 +445,7 @@
     uint32_t cur_trailer_pos;
     uint32_t primary_trailer_sz;
     uint32_t primary_trailer_buf_sz;
-    uint32_t align = CY_FLASH_ALIGN;
+    uint32_t align = MEMORY_ALIGN;
     int rc = 0;
     const struct flash_area *fap_stat = NULL;
     uint8_t primary_trailer_buf[MAX_TRAILER_BUF_SIZE];
@@ -464,8 +479,20 @@
     /* align image trailer buffer size to minimal write size */
 #if !defined(__BOOTSIM__)
     align = flash_area_align(fap);
+    /* TODO: this code needs to be refined. It is introduced to overcome
+     * situation when MCUBootApp lives in internal memory, but user app
+     * is executed from different type memory - external in XIP mode in
+     * this case. This situation now arise on PSOC6 when XIP execution is
+     * used, bay may be applicable to other devices, where solution is
+     * distributed between memories with different write/erase sizes.
+     */
+#ifdef CY_BOOT_USE_EXTERNAL_FLASH
+    if (align > MEMORY_ALIGN) {
+        align = MEMORY_ALIGN;
+    }
+#endif
 #else
-    align = CY_FLASH_ALIGN;
+    align = MEMORY_ALIGN;
 #endif
 
     if ((align > MAX_TRAILER_BUF_SIZE) || (align == 0U)) {