diff --git a/boot/bootutil/include/bootutil/boot_record.h b/boot/bootutil/include/bootutil/boot_record.h
index e6aaa71..387fdf8 100644
--- a/boot/bootutil/include/bootutil/boot_record.h
+++ b/boot/bootutil/include/bootutil/boot_record.h
@@ -24,6 +24,16 @@
 #ifdef __cplusplus
 extern "C" {
 #endif
+/*
+ * User can redefine sw_module start id by passing SW_MODULE_START_ID value
+ * at compile time. This may be needed if mcuboot is a part of a system with
+ * several boot stages and previous stages are also fill measured boot data.
+ */ 
+#if defined(SW_MODULE_START_ID)
+#define GET_SW_MODULE_ID(sw_module) ((sw_module) + (SW_MODULE_START_ID))
+#else
+#define GET_SW_MODULE_ID(sw_module) sw_module
+#endif
 
 /**
  * @brief Add a data item to the shared data area between bootloader and
@@ -39,7 +49,8 @@
 int boot_add_data_to_shared_area(uint8_t        major_type,
                                  uint16_t       minor_type,
                                  size_t         size,
-                                 const uint8_t *data);
+                                 const uint8_t *data,
+                                 const struct flash_area *fap);
 
 /**
  * Add an image's all boot status information to the shared memory area
diff --git a/boot/bootutil/include/bootutil/crypto/aes_ctr.h b/boot/bootutil/include/bootutil/crypto/aes_ctr.h
index 3758ca8..90727f1 100644
--- a/boot/bootutil/include/bootutil/crypto/aes_ctr.h
+++ b/boot/bootutil/include/bootutil/crypto/aes_ctr.h
@@ -44,40 +44,141 @@
 extern "C" {
 #endif
 
-#if defined(MCUBOOT_USE_MBED_TLS)
+#if defined(MCUBOOT_USE_MBED_TLS) && !defined(MCUBOOT_USE_PSA_CRYPTO)
 typedef mbedtls_aes_context bootutil_aes_ctr_context;
 static inline void bootutil_aes_ctr_init(bootutil_aes_ctr_context *ctx)
 {
-    (void)mbedtls_aes_init(ctx);
+   (void)mbedtls_aes_init(ctx);
 }
 
 static inline void bootutil_aes_ctr_drop(bootutil_aes_ctr_context *ctx)
 {
-    /* XXX: config defines MBEDTLS_PLATFORM_NO_STD_FUNCTIONS so no need to free */
-    /* (void)mbedtls_aes_free(ctx); */
-    (void)ctx;
+   /* XXX: config defines MBEDTLS_PLATFORM_NO_STD_FUNCTIONS so no need to free */
+   /* (void)mbedtls_aes_free(ctx); */
+   (void)ctx;
 }
 
 static inline int bootutil_aes_ctr_set_key(bootutil_aes_ctr_context *ctx, const uint8_t *k)
 {
-    return mbedtls_aes_setkey_enc(ctx, k, BOOTUTIL_CRYPTO_AES_CTR_KEY_SIZE * 8);
+   return mbedtls_aes_setkey_enc(ctx, k, BOOTUTIL_CRYPTO_AES_CTR_KEY_SIZE * 8);
 }
 
 static inline int bootutil_aes_ctr_encrypt(bootutil_aes_ctr_context *ctx, uint8_t *counter, const uint8_t *m, uint32_t mlen, size_t blk_off, uint8_t *c)
 {
-    uint8_t stream_block[BOOTUTIL_CRYPTO_AES_CTR_BLOCK_SIZE];
-    (void)memset(&stream_block, 0, BOOTUTIL_CRYPTO_AES_CTR_BLOCK_SIZE);
-    return mbedtls_aes_crypt_ctr(ctx, mlen, &blk_off, counter, stream_block, m, c);
+   uint8_t stream_block[BOOTUTIL_CRYPTO_AES_CTR_BLOCK_SIZE];
+   (void)memset(&stream_block, 0, BOOTUTIL_CRYPTO_AES_CTR_BLOCK_SIZE);
+   return mbedtls_aes_crypt_ctr(ctx, mlen, &blk_off, counter, stream_block, m, c);
 }
 
 static inline int bootutil_aes_ctr_decrypt(bootutil_aes_ctr_context *ctx, uint8_t *counter, const uint8_t *c, uint32_t clen, size_t blk_off, uint8_t *m)
 {
-    uint8_t stream_block[BOOTUTIL_CRYPTO_AES_CTR_BLOCK_SIZE];
-    (void)memset(&stream_block, 0, BOOTUTIL_CRYPTO_AES_CTR_BLOCK_SIZE);
-    return mbedtls_aes_crypt_ctr(ctx, clen, &blk_off, counter, stream_block, c, m);
+   uint8_t stream_block[BOOTUTIL_CRYPTO_AES_CTR_BLOCK_SIZE];
+   (void)memset(&stream_block, 0, BOOTUTIL_CRYPTO_AES_CTR_BLOCK_SIZE);
+   return mbedtls_aes_crypt_ctr(ctx, clen, &blk_off, counter, stream_block, c, m);
 }
 #endif /* MCUBOOT_USE_MBED_TLS */
 
+#if defined(MCUBOOT_USE_PSA_CRYPTO)
+#include "crypto.h"
+#include "crypto_values.h"
+
+typedef struct
+{
+    psa_key_handle_t key_handle;
+    psa_cipher_operation_t operation;
+} bootutil_aes_ctr_context;
+
+static inline void bootutil_aes_ctr_init(bootutil_aes_ctr_context *ctx)
+{
+    ctx->operation = psa_cipher_operation_init();
+}
+
+static inline void bootutil_aes_ctr_drop(bootutil_aes_ctr_context *ctx)
+{
+    psa_cipher_abort(&ctx->operation);
+}
+
+static inline int bootutil_aes_ctr_set_key(bootutil_aes_ctr_context *ctx, const uint8_t *key)
+{
+    psa_status_t status;
+
+    psa_key_attributes_t key_attributes = PSA_KEY_ATTRIBUTES_INIT;
+    psa_set_key_algorithm(&key_attributes, PSA_ALG_CTR);
+    psa_set_key_type(&key_attributes, PSA_KEY_TYPE_AES);
+    psa_set_key_usage_flags(&key_attributes, PSA_KEY_USAGE_DECRYPT | PSA_KEY_USAGE_ENCRYPT);
+
+    status = psa_import_key(&key_attributes, key, BOOTUTIL_CRYPTO_AES_CTR_KEY_SIZE, &ctx->key_handle);
+
+    if (status != PSA_SUCCESS) {
+        return -1;
+    }
+
+    return 0;
+}
+
+static inline int bootutil_aes_ctr_encrypt(bootutil_aes_ctr_context *ctx, uint8_t *counter, const uint8_t *m, uint32_t mlen, size_t blk_off, uint8_t *c)
+{
+    (void)blk_off;
+
+    psa_status_t status;
+    size_t out_sz;
+    size_t f_sz;
+
+    ctx->operation = psa_cipher_operation_init();
+
+    status = psa_cipher_encrypt_setup(&ctx->operation, ctx->key_handle, PSA_ALG_CTR);
+
+    if (status == PSA_SUCCESS) {
+        status = psa_cipher_set_iv(&ctx->operation, counter, BOOTUTIL_CRYPTO_AES_CTR_KEY_SIZE);
+    }
+
+    if (status == PSA_SUCCESS) {
+        status = psa_cipher_update(&ctx->operation, m, mlen, c, mlen, &out_sz);
+    }
+
+    if (status == PSA_SUCCESS) {
+        status = psa_cipher_finish(&ctx->operation, c + out_sz, sizeof(mlen) - out_sz, &f_sz);
+    }
+
+    if ((status != PSA_SUCCESS) || ((out_sz + f_sz) != mlen)) {
+        return -1;
+    }
+
+    return 0;
+}
+
+static inline int bootutil_aes_ctr_decrypt(bootutil_aes_ctr_context *ctx, uint8_t *counter, const uint8_t *c, uint32_t clen, size_t blk_off, uint8_t *m)
+{
+    (void)blk_off;
+
+    psa_status_t status;
+    size_t out_sz;
+    size_t f_sz;
+
+    ctx->operation = psa_cipher_operation_init();
+
+    status = psa_cipher_decrypt_setup(&ctx->operation, ctx->key_handle, PSA_ALG_CTR);
+
+    if (status == PSA_SUCCESS) {
+        status = psa_cipher_set_iv(&ctx->operation, counter, BOOTUTIL_CRYPTO_AES_CTR_KEY_SIZE);
+    }
+
+    if (status == PSA_SUCCESS) {
+        status = psa_cipher_update(&ctx->operation, c, clen, m, clen, &out_sz);
+    }
+
+    if (status == PSA_SUCCESS) {
+        status = psa_cipher_finish(&ctx->operation, m + out_sz, sizeof(clen) - out_sz, &f_sz);
+    }
+
+    if ((status != PSA_SUCCESS) || ((out_sz + f_sz) != clen)) {
+        return -1;
+    }
+
+    return 0;
+}
+#endif /* MCUBOOT_USE_PSA_CRYPTO */
+
 #if defined(MCUBOOT_USE_TINYCRYPT)
 typedef struct tc_aes_key_sched_struct bootutil_aes_ctr_context;
 static inline void bootutil_aes_ctr_init(bootutil_aes_ctr_context *ctx)
diff --git a/boot/bootutil/include/bootutil/crypto/ecdh_p256.h b/boot/bootutil/include/bootutil/crypto/ecdh_p256.h
index 3811037..e0f6abd 100644
--- a/boot/bootutil/include/bootutil/crypto/ecdh_p256.h
+++ b/boot/bootutil/include/bootutil/crypto/ecdh_p256.h
@@ -67,7 +67,7 @@
 }
 #endif /* MCUBOOT_USE_TINYCRYPT */
 
-#if defined(MCUBOOT_USE_MBED_TLS)
+#if defined(MCUBOOT_USE_MBED_TLS) && !defined(MCUBOOT_USE_PSA_CRYPTO)
 #define NUM_ECC_BYTES 32
 
 #if MBEDTLS_VERSION_NUMBER >= 0x03000000
@@ -147,6 +147,67 @@
 }
 #endif /* MCUBOOT_USE_MBED_TLS */
 
+
+#if defined(MCUBOOT_USE_PSA_CRYPTO)
+
+#include "crypto.h"
+#include "crypto_values.h"
+
+#define NUM_ECC_BYTES 32
+
+typedef void* bootutil_ecdh_p256_context;
+
+static inline void bootutil_ecdh_p256_init(bootutil_ecdh_p256_context *ctx)
+{
+    (void) ctx;
+}
+
+static inline void bootutil_ecdh_p256_drop(bootutil_ecdh_p256_context *ctx)
+{
+    (void) ctx;
+}
+
+static inline int bootutil_ecdh_p256_shared_secret(bootutil_ecdh_p256_context *ctx, const uint8_t *pub_key, const uint8_t *priv_key, uint8_t *shared)
+{
+    (void) ctx;
+
+    psa_status_t status;
+    psa_key_handle_t private_key_handle;
+
+    psa_key_attributes_t private_key_attributes = psa_key_attributes_init();
+
+    psa_set_key_usage_flags(&private_key_attributes, PSA_KEY_USAGE_DERIVE);
+    psa_set_key_algorithm(&private_key_attributes, PSA_ALG_ECDH);
+    psa_set_key_type(&private_key_attributes, PSA_KEY_TYPE_ECC_KEY_PAIR(PSA_ECC_FAMILY_SECP_R1));
+    psa_set_key_bits(&private_key_attributes, 256u);
+
+    status = psa_import_key(&private_key_attributes, priv_key, 32, &private_key_handle);
+
+    if (status == PSA_SUCCESS)
+    {
+        size_t res_len;
+
+        status = psa_raw_key_agreement(PSA_ALG_ECDH,
+                                    private_key_handle,
+                                    pub_key,
+                                    65,
+                                    shared,
+                                    32,
+                                    &res_len);
+    }
+
+    psa_destroy_key(private_key_handle);
+
+    if (status != PSA_SUCCESS)
+    {
+        return -1;
+    }
+    
+    return 0;
+}
+#endif /* MCUBOOT_USE_PSA_CRYPTO */
+
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/boot/bootutil/include/bootutil/crypto/hmac_sha256.h b/boot/bootutil/include/bootutil/crypto/hmac_sha256.h
index e684018..d5aec15 100644
--- a/boot/bootutil/include/bootutil/crypto/hmac_sha256.h
+++ b/boot/bootutil/include/bootutil/crypto/hmac_sha256.h
@@ -84,7 +84,7 @@
 }
 #endif /* MCUBOOT_USE_TINYCRYPT */
 
-#if defined(MCUBOOT_USE_MBED_TLS)
+#if defined(MCUBOOT_USE_MBED_TLS) && !defined(MCUBOOT_USE_PSA_CRYPTO)
 /**
  * The generic message-digest context.
  */
@@ -127,6 +127,78 @@
 }
 #endif /* MCUBOOT_USE_MBED_TLS */
 
+#if defined(MCUBOOT_USE_PSA_CRYPTO)
+/**
+ * The generic message-digest context.
+ */
+typedef struct
+{
+    psa_key_handle_t key_handle;
+    psa_mac_operation_t operation;
+} bootutil_hmac_sha256_context;
+
+static inline void bootutil_hmac_sha256_init(bootutil_hmac_sha256_context *ctx)
+{
+    ctx->operation = psa_mac_operation_init();
+}
+
+static inline void bootutil_hmac_sha256_drop(bootutil_hmac_sha256_context *ctx)
+{
+    psa_mac_abort(&ctx->operation);
+    psa_destroy_key(ctx->key_handle);
+}
+
+static inline int bootutil_hmac_sha256_set_key(bootutil_hmac_sha256_context *ctx, const uint8_t *key, unsigned int key_size)
+{
+    psa_status_t status;
+    psa_key_attributes_t key_attributes = psa_key_attributes_init();
+
+    psa_set_key_usage_flags(&key_attributes, PSA_KEY_USAGE_SIGN_HASH);
+    psa_set_key_algorithm(&key_attributes, PSA_ALG_HMAC(PSA_ALG_SHA_256));
+    psa_set_key_type(&key_attributes, PSA_KEY_TYPE_HMAC);
+    psa_set_key_bits(&key_attributes, 256u);
+    
+    status = psa_import_key(&key_attributes, key, key_size, &ctx->key_handle);
+
+    if (status == PSA_SUCCESS) {
+        status = psa_mac_sign_setup(&ctx->operation, ctx->key_handle, PSA_ALG_HMAC(PSA_ALG_SHA_256));
+    }
+
+    if (status != PSA_SUCCESS) {
+        return -1;
+    }
+
+    return 0;
+}
+
+static inline int bootutil_hmac_sha256_update(bootutil_hmac_sha256_context *ctx, const void *data, unsigned int data_length)
+{
+    psa_status_t status;
+
+    status = psa_mac_update(&ctx->operation, data, data_length);
+    
+    if (status != PSA_SUCCESS) {
+        return -1;
+    }
+
+    return 0;
+}
+
+static inline int bootutil_hmac_sha256_finish(bootutil_hmac_sha256_context *ctx, uint8_t *tag, unsigned int taglen)
+{
+    size_t output_len;
+    psa_status_t status;
+
+    status = psa_mac_sign_finish(&ctx->operation, tag, taglen, &output_len);
+
+    if (status != PSA_SUCCESS) {
+        return -1;
+    }
+
+    return 0;
+}
+#endif /* MCUBOOT_USE_MBED_TLS */
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/boot/bootutil/include/bootutil/crypto/sha256.h b/boot/bootutil/include/bootutil/crypto/sha256.h
index b45cd63..82de07b 100644
--- a/boot/bootutil/include/bootutil/crypto/sha256.h
+++ b/boot/bootutil/include/bootutil/crypto/sha256.h
@@ -20,6 +20,10 @@
 
 #include "mcuboot_config/mcuboot_config.h"
 
+#if defined(MCUBOOT_SHA256_CUSTOM_INTERFACE)
+    #include "sha256_port.h"
+#else
+
 #if (defined(MCUBOOT_USE_MBED_TLS) + \
      defined(MCUBOOT_USE_TINYCRYPT) + \
      defined(MCUBOOT_USE_CC310)) != 1
@@ -143,4 +147,6 @@
 }
 #endif
 
+#endif /* MCUBOOT_SHA256_CUSTOM_INTERFACE */
+
 #endif /* __BOOTUTIL_CRYPTO_SHA256_H_ */
diff --git a/boot/bootutil/include/bootutil/fault_injection_hardening.h b/boot/bootutil/include/bootutil/fault_injection_hardening.h
index 8edf5ba..65b3023 100644
--- a/boot/bootutil/include/bootutil/fault_injection_hardening.h
+++ b/boot/bootutil/include/bootutil/fault_injection_hardening.h
@@ -83,8 +83,8 @@
 #define FIH_MISRA_BLOCK_END(MISRA)
 #endif /* CY_COVERITY_2012_CHECK */
 
-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');
+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)
@@ -107,13 +107,13 @@
 #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_TRUE_1              ((int32_t)0xC00AUL)
+#define FIH_TRUE_2              ((int32_t)0x0350UL)
+#define FIH_TRUE                ((int32_t)0xC35AUL) /* 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)
+#define FIH_NEGATIVE_VALUE      ((int32_t) 0xAAAA5555UL)
 
 #ifdef FIH_ENABLE_DOUBLE_VARS
 /*
@@ -122,15 +122,10 @@
  * 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
-#ifndef USE_IFX_SE_CRYPTO /* TODO: Remove this after TFM-1749 resolved */
-#define FIH_UINT_MASK_VALUE     0xB779A31CU
-#else
+#define FIH_MASK_VALUE          0xA5C35A3C
 #define FIH_UINT_MASK_VALUE     0xA5C35A3CU
-#endif /* USE_IFX_SE_CRYPTO */
 
-
-#define FIH_INT_VAL_MASK(val) ((int32_t)((val) ^ FIH_MASK_VALUE))
+#define FIH_INT_VAL_MASK(val) ((int32_t)((val) ^ (int32_t)FIH_MASK_VALUE))
 #define FIH_UINT_VAL_MASK(val) ((val) ^ FIH_UINT_MASK_VALUE)
 
 /*
@@ -142,14 +137,24 @@
     volatile int32_t msk;
 } fih_int;
 
-#define FIH_INT_INIT(x)         {(x), FIH_INT_VAL_MASK(x)}
+#define FIH_INT_INIT(x)                 ((fih_int){(x), FIH_INT_VAL_MASK(x)})
+
+/* FIH_INT_INIT_GLOBAL is created to declare global or static global veriables to 
+  avoid the compile time Error[Pe028]: expression must have a constant value
+  on IAR compiler */
+#define FIH_INT_INIT_GLOBAL(x)          {(x), FIH_INT_VAL_MASK(x)}
 
 typedef struct {
     volatile uint32_t val;
     volatile uint32_t msk;
 } fih_uint;
 
-#define FIH_UINT_INIT(x)        {(x), FIH_UINT_VAL_MASK(x)}
+#define FIH_UINT_INIT(x)                ((fih_uint){(x), FIH_UINT_VAL_MASK(x)})
+
+/* FIH_UINT_INIT_GLOBAL is created to declare global or static global veriables to 
+  avoid the compile time Error[Pe028]: expression must have a constant value
+  on IAR compiler */
+#define FIH_UINT_INIT_GLOBAL(x)         {(x), FIH_UINT_VAL_MASK(x)}
 
 #else /* FIH_ENABLE_DOUBLE_VARS */
 /*
@@ -165,15 +170,22 @@
     volatile uint32_t val;
 } fih_uint;
 
-#define FIH_INT_INIT(x)         {(x)}
-#define FIH_UINT_INIT(x)        {(x)}
+#define FIH_INT_INIT(x)                 ((fih_int){(x)})
+
+/* FIH_UINT_INIT_GLOBAL and FIH_INT_INIT_GLOBAL are created to declare global
+  or static global veriables to avoid the compile time Error[Pe028]: expression
+  must have a constant value on IAR compiler */
+#define FIH_INT_INIT_GLOBAL(x)          {(x)}
+#define FIH_UINT_INIT(x)                ((fih_uint){(x)})
+#define FIH_UINT_INIT_GLOBAL(x)         {(x)}
 #endif /* FIH_ENABLE_DOUBLE_VARS */
 
-#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_INT_ZERO    (fih_int_encode((signed)0))
-#define FIH_UINT_MAX    (fih_uint_encode(0xFFFFFFFFU))
+#define FIH_SUCCESS                     (FIH_INT_INIT(FIH_POSITIVE_VALUE))
+#define FIH_FAILURE                     (FIH_INT_INIT(FIH_NEGATIVE_VALUE))
+#define FIH_UINT_ZERO                   (FIH_UINT_INIT(0U))
+#define FIH_INT_ZERO                    (FIH_INT_INIT(0L))
+#define FIH_UINT_MAX                    (FIH_UINT_INIT(0xFFFFFFFFU))
+
 
 #ifdef FIH_ENABLE_GLOBAL_FAIL
 /**
@@ -207,48 +219,14 @@
  */
 void fih_delay_init(void);
 
-/**
- * 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);
+bool fih_delay(void);
 
-/**
- * Delaying logic, with randomness from a CSPRNG.
- */
-__attribute__((always_inline)) static inline
-void fih_delay(void)
-{
-    uint32_t i = 0;
-    volatile uint32_t delay = 10u; /* TODO: REMOVE */
-    volatile uint32_t counter = 0;
-
-#if 0
-    delay = fih_delay_random();
-
-    if (delay == FIH_NEGATIVE_VALUE) {
-        FIH_PANIC;
-    }
-
-    delay &= 0xFF;
-#endif
-
-    for (i = 0; i < delay; i++) {
-        counter++;
-    }
-
-    if (counter != delay) {
-        FIH_PANIC;
-    }
-}
 #else /* FIH_ENABLE_DELAY */
 /* NOOP */
 #define fih_delay_init()
 
 /* NOOP */
-#define fih_delay()
+#define fih_delay() (true)
 #endif /* FIH_ENABLE_DELAY */
 
 #ifdef FIH_ENABLE_DOUBLE_VARS
@@ -258,13 +236,15 @@
  * @param x  fih_int value to be validated.
  */
 __attribute__((always_inline)) static inline
-void fih_int_validate(fih_int x)
+bool fih_int_validate(fih_int x)
 {
     int32_t x_msk = x.msk;
 
     if (x.val != FIH_INT_VAL_MASK(x_msk)) {
         FIH_PANIC;
     }
+
+    return true;
 }
 
 /**
@@ -273,13 +253,15 @@
  * @param x  fih_uint value to be validated.
  */
 __attribute__((always_inline)) static inline
-void fih_uint_validate(fih_uint x)
+bool fih_uint_validate(fih_uint x)
 {
     uint32_t x_msk = x.msk;
 
     if (x.val != FIH_UINT_VAL_MASK(x_msk)) {
         FIH_PANIC;
     }
+
+    return true;
 }
 
 /**
@@ -292,7 +274,7 @@
 __attribute__((always_inline)) static inline
 int32_t fih_int_decode(fih_int x)
 {
-    fih_int_validate(x);
+    (void)fih_int_validate(x);
     return x.val;
 }
 
@@ -306,7 +288,7 @@
 __attribute__((always_inline)) static inline
 uint32_t fih_uint_decode(fih_uint x)
 {
-    fih_uint_validate(x);
+    (void)fih_uint_validate(x);
     return x.val;
 }
 
@@ -345,40 +327,17 @@
  * @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.
+ * @return   true if x == y, false 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;
-}
+#define fih_eq(x, y) \
+        ( fih_int_validate(x) && \
+          fih_int_validate(y) && \
+          ((x).val == (y).val) && \
+          fih_delay() && \
+          ((x).msk == (y).msk) && \
+          fih_delay() && \
+          ((x).val == FIH_INT_VAL_MASK((y).msk)) \
+        )
 
 /**
  * Standard equality for fih_uint values.
@@ -386,40 +345,17 @@
  * @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.
+ * @return   true if x == y, false 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;
-}
+#define fih_uint_eq(x, y) \
+        ( fih_uint_validate(x) && \
+          fih_uint_validate(y) && \
+          ((x).val == (y).val) && \
+          fih_delay() && \
+          ((x).msk == (y).msk) && \
+          fih_delay() && \
+          ((x).val == FIH_UINT_VAL_MASK((y).msk)) \
+        )
 
 /**
  * Standard non-equality for fih_int values.
@@ -427,40 +363,17 @@
  * @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.
+ * @return   true if x != y, 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;
-}
+#define fih_not_eq(x, y) \
+        ( fih_int_validate(x) && \
+          fih_int_validate(y) && \
+          ((x).val != (y).val) && \
+          fih_delay() && \
+          ((x).msk != (y).msk) && \
+          fih_delay() && \
+          ((x).val != FIH_INT_VAL_MASK((y).msk)) \
+        )
 
 /**
  * Standard non-equality for fih_uint values.
@@ -468,40 +381,17 @@
  * @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.
+ * @return   true if x != y, 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;
-}
+#define fih_uint_not_eq(x, y) \
+        ( fih_uint_validate(x) && \
+          fih_uint_validate(y) && \
+          ((x).val != (y).val) && \
+          fih_delay() && \
+          ((x).msk != (y).msk) && \
+          fih_delay() && \
+          ((x).val != FIH_UINT_VAL_MASK((y).msk)) \
+        )
 
 /**
  * Standard greater than comparison for fih_int values.
@@ -509,40 +399,17 @@
  * @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.
+ * @return   true if x > y, 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;
-}
+#define fih_gt(x, y) \
+        ( fih_int_validate(x)  && \
+          fih_int_validate(y) && \
+          ((x).val > (y).val) && \
+          fih_delay() && \
+          (FIH_INT_VAL_MASK((x).msk) > FIH_INT_VAL_MASK((y).msk)) && \
+          fih_delay() && \
+          ((x).val > FIH_INT_VAL_MASK((y).msk)) \
+        )
 
 /**
  * Standard greater than comparison for fih_uint values.
@@ -550,40 +417,17 @@
  * @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.
+ * @return   true if x > y, 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;
-}
+#define fih_uint_gt(x, y) \
+        ( fih_uint_validate(x)  && \
+          fih_uint_validate(y) && \
+          ((x).val > (y).val) && \
+          fih_delay() && \
+          (FIH_UINT_VAL_MASK((x).msk) > FIH_UINT_VAL_MASK((y).msk)) && \
+          fih_delay() && \
+          ((x).val > FIH_UINT_VAL_MASK((y).msk)) \
+        )
 
 /**
  * Standard greater than or equal comparison for fih_int values.
@@ -591,40 +435,17 @@
  * @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.
+ * @return   true if x >= y, 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;
-}
+#define fih_ge(x, y) \
+        ( fih_int_validate(x)  && \
+          fih_int_validate(y) && \
+          ((x).val >= (y).val) && \
+          fih_delay() && \
+          (FIH_INT_VAL_MASK((x).msk) >= FIH_INT_VAL_MASK((y).msk)) && \
+          fih_delay() && \
+          ((x).val >= FIH_INT_VAL_MASK((y).msk)) \
+        )
 
 /**
  * Standard greater than or equal comparison for fih_uint values.
@@ -632,40 +453,17 @@
  * @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.
+ * @return   true if x >= y, 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;
-}
+#define fih_uint_ge(x, y) \
+        ( fih_uint_validate(x)  && \
+          fih_uint_validate(y) && \
+          ((x).val >= (y).val) && \
+          fih_delay() && \
+          (FIH_UINT_VAL_MASK((x).msk) >= FIH_UINT_VAL_MASK((y).msk)) && \
+          fih_delay() && \
+          ((x).val >= FIH_UINT_VAL_MASK((y).msk)) \
+        )
 
 /**
  * Standard less than comparison for fih_int values.
@@ -673,40 +471,17 @@
  * @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.
+ * @return   true if x < y, 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;
-}
+#define fih_lt(x, y) \
+        ( fih_int_validate(x)  && \
+          fih_int_validate(y) && \
+          ((x).val < (y).val) && \
+          fih_delay() && \
+          (FIH_INT_VAL_MASK((x).msk) < FIH_INT_VAL_MASK((y).msk)) && \
+          fih_delay() && \
+          ((x).val < FIH_INT_VAL_MASK((y).msk)) \
+        )
 
 /**
  * Standard less than comparison for fih_uint values.
@@ -714,40 +489,17 @@
  * @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.
+ * @return   true if x < y, 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;
-}
+#define fih_uint_lt(x, y) \
+        ( fih_uint_validate(x)  && \
+          fih_uint_validate(y) && \
+          ((x).val < (y).val) && \
+          fih_delay() && \
+          (FIH_UINT_VAL_MASK((x).msk) < FIH_UINT_VAL_MASK((y).msk)) && \
+          fih_delay() && \
+          ((x).val < FIH_UINT_VAL_MASK((y).msk)) \
+        )
 
 /**
  * Standard less than or equal comparison for fih_int values.
@@ -755,40 +507,17 @@
  * @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.
+ * @return   true if x <= y, 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;
-}
+#define fih_le(x, y) \
+        ( fih_int_validate(x)  && \
+          fih_int_validate(y) && \
+          ((x).val <= (y).val) && \
+          fih_delay() && \
+          (FIH_INT_VAL_MASK((x).msk) <= FIH_INT_VAL_MASK((y).msk)) && \
+          fih_delay() && \
+          ((x).val <= FIH_INT_VAL_MASK((y).msk)) \
+        )
 
 /**
  * Standard less than or equal comparison for fih_uint values.
@@ -796,40 +525,17 @@
  * @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.
+ * @return   true if x <= y, 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;
-}
+#define fih_uint_le(x, y) \
+        ( fih_uint_validate(x)  && \
+          fih_uint_validate(y) && \
+          ((x).val <= (y).val) && \
+          fih_delay() && \
+          (FIH_UINT_VAL_MASK((x).msk) <= FIH_UINT_VAL_MASK((y).msk)) && \
+          fih_delay() && \
+          ((x).val <= FIH_UINT_VAL_MASK((y).msk)) \
+        )
 
 /**
  * Standard logical OR for fih_uint values.
@@ -842,22 +548,16 @@
 __attribute__((always_inline)) static inline
 fih_uint fih_uint_or(fih_uint x, fih_uint y)
 {
-    uint32_t y_val, y_msk;
+    /* Use local variable to avoid persistent side effect MISRA violation
+     * in operations with volatile variables. */
+    uint32_t y_val = y.val;
+    uint32_t y_msk = 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;
 }
 
@@ -872,22 +572,16 @@
 __attribute__((always_inline)) static inline
 fih_int fih_or(fih_int x, fih_int y)
 {
-    int32_t y_val, y_msk;
+    /* Use local variable to avoid persistent side effect MISRA violation
+     * in operations with volatile variables. */
+    int32_t y_val = y.val;
+    int32_t y_msk = y.msk;
     volatile fih_int rc = {0};
 
-    fih_int_validate(x);
-    fih_int_validate(y);
-
-    y_val = y.val;
     rc.val = x.val | y_val;
 
-    fih_delay();
-
-    y_msk = y.msk;
     rc.msk = FIH_INT_VAL_MASK(FIH_INT_VAL_MASK(x.msk) | FIH_INT_VAL_MASK(y_msk));
 
-    fih_int_validate(rc);
-
     return rc;
 }
 
@@ -902,38 +596,32 @@
 __attribute__((always_inline)) static inline
 fih_uint fih_uint_and(fih_uint x, fih_uint y)
 {
-    uint32_t y_val, y_msk;
+    /* Use local variable to avoid persistent side effect MISRA violation
+     * in operations with volatile variables. */
+    uint32_t y_val = y.val;
+    uint32_t y_msk = 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)
+#define fih_int_validate(x)  (true)
+#define fih_uint_validate(x) (true)
 
 /* 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))
+#define fih_int_encode(x)       (FIH_INT_INIT(x))
+#define fih_uint_encode(x)      (FIH_UINT_INIT(x))
 
 /**
  * Standard equality for fih_int values.
@@ -941,25 +629,13 @@
  * @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.
+ * @return   true if x == y, 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;
-}
+#define fih_eq(x, y) \
+        ( ((x).val == (y).val) && \
+          fih_delay() && \
+          !((x).val != (y).val) \
+        )
 
 /**
  * Standard equality for fih_uint values.
@@ -967,25 +643,13 @@
  * @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.
+ * @return   true if x == y, 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;
-}
+#define fih_uint_eq(x, y) \
+        ( ((x).val == (y).val) && \
+          fih_delay() && \
+          !((x).val != (y).val) \
+        )
 
 /**
  * Standard non-equality for fih_int values.
@@ -993,25 +657,13 @@
  * @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.
+ * @return    true if x != y, 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;
-}
+#define fih_not_eq(x, y) \
+        ( ((x).val != (y).val) && \
+          fih_delay() && \
+          !((x).val == (y).val) \
+        )
 
 /**
  * Standard non-equality for fih_uint values.
@@ -1019,103 +671,52 @@
  * @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.
+ * @return    true if x != y, 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;
-}
-
+#define fih_uint_not_eq(x, y) \
+        ( ((x).val != (y).val) && \
+          fih_delay() && \
+          !((x).val == (y).val) \
+        )
 /**
  * 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.
+ * @return   true if x > y, 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;
-}
-
+#define fih_gt(x, y) \
+        ( ((x).val > (y).val) && \
+          fih_delay() && \
+          !((x).val <= (y).val) \
+        )
 /**
  * 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.
+ * @return   true if x > y, 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;
-}
-
+#define fih_uint_gt(x, y) \
+        ( ((x).val > (y).val) && \
+          fih_delay() && \
+          !((x).val <= (y).val) \
+        )
 /**
  * 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.
+ * @return   true if x >= y, 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;
-}
+#define fih_ge(x, y) \
+        ( ((x).val >= (y).val) && \
+          fih_delay() && \
+          !((x).val < (y).val) \
+        )
 
 /**
  * Standard greater than or equal comparison for fih_uint values.
@@ -1123,51 +724,26 @@
  * @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.
+ * @return   true if x >= y, 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;
-}
-
+#define fih_uint_ge(x, y) \
+        ( ((x).val >= (y).val) && \
+          fih_delay() && \
+          !((x).val < (y).val) \
+        )
 /**
  * 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.
+ * @return   true if x < y, 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;
-}
+#define fih_lt(x, y) \
+        ( ((x).val < (y).val) && \
+          fih_delay() && \
+          !((x).val >= (y).val) \
+        )
 
 /**
  * Standard less than comparison for fih_uint values.
@@ -1175,51 +751,26 @@
  * @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.
+ * @return   true if x < y, 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;
-}
-
+#define fih_uint_lt(x, y) \
+        ( ((x).val < (y).val) && \
+          fih_delay() && \
+          !((x).val >= (y).val) \
+        )
 /**
  * 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.
+ * @return   true if x <= y, 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;
-}
+#define fih_le(x, y) \
+        ( ((x).val <= (y).val) && \
+          fih_delay() && \
+          !((x).val > (y).val) \
+        )
 
 /**
  * Standard less than or equal comparison for fih_uint values.
@@ -1227,25 +778,13 @@
  * @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.
+ * @return   true if x <= y, 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;
-}
+#define fih_uint_le(x, y) \
+        ( ((x).val <= (y).val) && \
+          fih_delay() && \
+          !((x).val > (y).val) \
+        )
 
 /**
  * Standard logical OR for fih_uint values.
@@ -1255,12 +794,13 @@
  *
  * @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();
+    (void)fih_delay();
 
     if (rc.val != (x.val | y.val)) {
         FIH_PANIC;
@@ -1282,7 +822,7 @@
 {
     fih_int rc = {x.val | y.val};
 
-    fih_delay();
+    (void)fih_delay();
 
     if (rc.val != (x.val | y.val)) {
         FIH_PANIC;
@@ -1304,7 +844,7 @@
 {
     fih_uint rc = {x.val & y.val};
 
-    fih_delay();
+    (void)fih_delay();
 
     if (rc.val != (x.val & y.val)) {
         FIH_PANIC;
@@ -1421,7 +961,7 @@
 #define FIH_CFI_STEP_ERR_RESET() \
         do { \
             fih_cfi_ctr = fih_cfi_step_saved_value; \
-            fih_int_validate(fih_cfi_ctr); \
+            (void)fih_int_validate(fih_cfi_ctr); \
         } while(0)
 
 #else /* FIH_ENABLE_CFI */
@@ -1462,10 +1002,10 @@
         FIH_LABEL("FIH_CALL_START_" # f); \
         FIH_CFI_PRECALL_BLOCK; \
         (ret) = FIH_FAILURE; \
-        fih_delay(); \
+        (void)fih_delay(); \
         (ret) = (f)(__VA_ARGS__); \
         FIH_CFI_POSTCALL_BLOCK; \
-        fih_int_validate(ret); \
+        (void)fih_int_validate(ret); \
         FIH_LABEL("FIH_CALL_END"); \
     } while (false)
 
@@ -1475,7 +1015,7 @@
 #define FIH_VOID(f, ...) \
     do { \
         FIH_CFI_PRECALL_BLOCK; \
-        fih_delay(); \
+        (void)fih_delay(); \
         (void)(f)(__VA_ARGS__); \
         FIH_CFI_POSTCALL_BLOCK; \
         FIH_LABEL("FIH_CALL_END"); \
@@ -1490,10 +1030,10 @@
         FIH_LABEL("FIH_CALL_START_" # f); \
         FIH_CFI_PRECALL_BLOCK; \
         (ret) = FIH_UINT_ZERO; \
-        fih_delay(); \
+        (void)fih_delay(); \
         (ret) = (f)(__VA_ARGS__); \
         FIH_CFI_POSTCALL_BLOCK; \
-        fih_uint_validate(ret); \
+        (void)fih_uint_validate(ret); \
         FIH_LABEL("FIH_CALL_END"); \
     } while (false)
 
@@ -1515,8 +1055,14 @@
 typedef fih_int fih_int;
 typedef fih_uint fih_uint;
 
+
+/* FIH_UINT_INIT_GLOBAL and FIH_INT_INIT_GLOBAL are created to declare global 
+  or static global veriables to avoid the compile time Error[Pe028]: expression
+  must have a constant value on IAR compiler */
 #define FIH_INT_INIT(x)         (x)
+#define FIH_INT_INIT_GLOBAL(x)  (x)
 #define FIH_UINT_INIT(x)        (x)
+#define FIH_UINT_INIT_GLOBAL(x) (x)
 
 #define FIH_SUCCESS             (0)
 #define FIH_FAILURE            (-1)
@@ -1599,7 +1145,7 @@
 }
 #endif /* __cplusplus */
 
-FIH_MISRA_BLOCK_END('MISRA C-2012 Rule 10.1');
-FIH_MISRA_BLOCK_END('MISRA C-2012 Rule 10.4');
+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 535e34e..906981d 100644
--- a/boot/bootutil/include/bootutil/image.h
+++ b/boot/bootutil/include/bootutil/image.h
@@ -97,6 +97,8 @@
 #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_BUILT_IN_KEY_ID   (0x77)   /* ID if Built in keys in device key storage */
 					   /*
 					    * vendor reserved TLVs at xxA0-xxFF,
 					    * where xx denotes the upper byte
diff --git a/boot/bootutil/src/boot_record.c b/boot/bootutil/src/boot_record.c
index 88b7573..82ec800 100644
--- a/boot/bootutil/src/boot_record.c
+++ b/boot/bootutil/src/boot_record.c
@@ -23,11 +23,6 @@
 
 #include "mcuboot_config/mcuboot_config.h"
 
-#if defined USE_IFX_SE_CRYPTO
-#include "ifx_se_platform.h"
-#define SE_RT_SYSCALL_NOT_SUPPORTED (ifx_se_status_t) FIH_UINT_INIT(0xF7000001u)
-#endif
-
 #if defined(MCUBOOT_MEASURED_BOOT) || defined(MCUBOOT_DATA_SHARING)
 #include "bootutil/crypto/sha256.h"
 #include "bootutil/boot_record.h"
@@ -41,75 +36,8 @@
 #define SHARED_MEMORY_OVERFLOW      (1)
 #define SHARED_MEMORY_OVERWRITE     (2)
 #define SHARED_MEMORY_GEN_ERROR     (3)
-#define SHARED_MEMORY_CORRUPTED     (4)
 
-#if defined USE_IFX_SE_CRYPTO
-/* See in boot_record.h */
-int
-boot_add_data_to_shared_area(uint8_t        major_type,
-                             uint16_t       minor_type,
-                             size_t         size,
-                             const uint8_t *data)
-{
-    struct shared_data_tlv_entry tlv_entry = {0};
-    uint16_t type = SET_TLV_TYPE(major_type, minor_type);
-    uint8_t boot_data_arr[MCUBOOT_SHARED_DATA_SIZE] = {0};
-    struct shared_boot_data *boot_data = (struct shared_boot_data *)boot_data_arr;
-    uint16_t boot_data_size = 0;
-
-    ifx_se_status_t if_se_rc = IFX_SE_INVALID;
-
-    if (data == NULL) {
-        return SHARED_MEMORY_GEN_ERROR;
-    }
-    
-    /* Fill boot_data structure - tlv header portions */
-    boot_data->header.tlv_magic   = SHARED_DATA_TLV_INFO_MAGIC;
-    boot_data->header.tlv_tot_len = (uint16_t)SHARED_DATA_HEADER_SIZE;
-
-    /* Add TLV entry type portion */
-    tlv_entry.tlv_type = type;
-
-    if (size > (unsigned)UINT16_MAX - SHARED_DATA_ENTRY_HEADER_SIZE) {
-        return SHARED_MEMORY_GEN_ERROR;
-    }
-
-    /* Add TLV entry length portion */
-    tlv_entry.tlv_len = (uint16_t)size;
-    if (!boot_u16_safe_add(&boot_data_size, boot_data->header.tlv_tot_len,
-                            (uint16_t)SHARED_DATA_ENTRY_SIZE(size))) {
-        return SHARED_MEMORY_GEN_ERROR;
-    }
-
-    /* Verify overflow of shared area */
-    if (boot_data_size > MCUBOOT_SHARED_DATA_SIZE) {
-        return SHARED_MEMORY_OVERFLOW;
-    }
-
-    (void)memcpy((void *)boot_data + SHARED_DATA_HEADER_SIZE, (const void *)&tlv_entry, SHARED_DATA_ENTRY_HEADER_SIZE);
-    (void)memcpy((void *)boot_data + SHARED_DATA_HEADER_SIZE + SHARED_DATA_ENTRY_HEADER_SIZE, (const void *)data, size);
-
-    boot_data->header.tlv_tot_len = boot_data_size;
-
-    if_se_rc = ifx_se_set_shared_data(fih_ptr_encode(boot_data), fih_uint_encode(boot_data_size), IFX_SE_NULL_CTX);
-
-    if (fih_uint_eq(if_se_rc, SE_RT_SYSCALL_NOT_SUPPORTED)) {
-        /* return as okay, because syscall is not implemented in SE RT nowm but our job is done here */
-        return SHARED_MEMORY_OK;
-    }
-    else if (fih_uint_eq(if_se_rc, IFX_SE_SUCCESS)) {
-        /* return error if we get here and rc is supported */
-        return SHARED_MEMORY_GEN_ERROR;
-    }
-    else {
-        /* return success otherwise */
-        return SHARED_MEMORY_OK;
-    }
-    
-}
-
-#else /* USE_IFX_SE_CRYPTO */
-
+#ifndef USE_PLATFORM_SHARED_DATA_STORAGE
 /**
  * @var shared_memory_init_done
  *
@@ -123,7 +51,8 @@
 boot_add_data_to_shared_area(uint8_t        major_type,
                              uint16_t       minor_type,
                              size_t         size,
-                             const uint8_t *data)
+                             const uint8_t *data,
+                             const struct flash_area *fap)
 {
     struct shared_data_tlv_entry tlv_entry = {0};
     uint16_t type = SET_TLV_TYPE(major_type, minor_type);
@@ -131,6 +60,8 @@
     uint16_t boot_data_size;
     uintptr_t tlv_end, offset;
 
+    (void)fap;
+
     if (data == NULL) {
         return SHARED_MEMORY_GEN_ERROR;
     }
@@ -196,7 +127,9 @@
 
     return SHARED_MEMORY_OK;
 }
-#endif /* USE_IFX_SE_CRYPTO */
+
+
+#endif /* USE_PLATFORM_SHARED_DATA_STORAGE */
 #endif /* MCUBOOT_MEASURED_BOOT OR MCUBOOT_DATA_SHARING */
 
 #ifdef MCUBOOT_MEASURED_BOOT
@@ -310,7 +243,8 @@
     rc = boot_add_data_to_shared_area(TLV_MAJOR_IAS,
                                       ias_minor,
                                       record_len,
-                                      buf);
+                                      buf,
+                                      fap);
     if (rc != SHARED_MEMORY_OK) {
         return rc;
     }
@@ -346,6 +280,27 @@
         }
     }
 
+/* Since the ram load active slot is chosen by the highest present 
+ * version we should compare not only with first slots of all images
+ * but with secondary slots also. So when there is no appropriate
+ * between first slots it searches in secondaries.
+*/
+#if defined(MCUBOOT_RAM_LOAD)
+    if ((uint8_t)MCUBOOT_IMAGE_NUMBER == i) {
+        for (i = 0; i < (uint8_t)MCUBOOT_IMAGE_NUMBER; i++) {
+            if (flash_area_open(FLASH_AREA_IMAGE_SECONDARY(i),
+                            &temp_fap) != 0) {
+                return -1;
+            }
+
+            if (fap == temp_fap) {
+                fwu_img_id = i;
+                break;
+            }
+        }
+    }
+#endif
+
     if ((uint8_t)MCUBOOT_IMAGE_NUMBER == i) {
         return -1;
     }
@@ -355,6 +310,7 @@
     return boot_add_data_to_shared_area(TLV_MAJOR_FWU,
                                         fwu_minor,
                                         sizeof(hdr->ih_ver),
-                                        (const uint8_t *)&hdr->ih_ver);
+                                        (const uint8_t *)&hdr->ih_ver,
+                                        fap);
 }
 #endif /* MCUBOOT_DATA_SHARING */
diff --git a/boot/bootutil/src/bootutil_priv.h b/boot/bootutil/src/bootutil_priv.h
index 045cacf..04c61f9 100644
--- a/boot/bootutil/src/bootutil_priv.h
+++ b/boot/bootutil/src/bootutil_priv.h
@@ -50,7 +50,12 @@
 
 struct flash_area;
 
+#ifdef USE_IFX_SE_CRYPTO
+#define BOOT_TMPBUF_SZ  4096
+#else
 #define BOOT_TMPBUF_SZ  256
+#endif /* USE_IFX_SE_CRYPTO */
+
 
 /** Number of image slots in flash; currently limited to two. */
 #define BOOT_NUM_SLOTS                  2
@@ -309,6 +314,9 @@
 
 
 #ifdef MCUBOOT_ENC_IMAGES
+uint32_t boot_iv_offset(const struct flash_area *fap, uint8_t slot);
+int boot_read_iv(int image_index, uint8_t slot, uint8_t* dst);
+int boot_write_iv(const struct flash_area *fap, uint8_t* iv);
 int boot_write_enc_key(const struct flash_area *fap, uint8_t slot,
                        const struct boot_status *bs);
 int boot_read_enc_key(int image_index, uint8_t slot, struct boot_status *bs);
diff --git a/boot/bootutil/src/encrypted.c b/boot/bootutil/src/encrypted.c
index 02f8eef..6d37ecb 100644
--- a/boot/bootutil/src/encrypted.c
+++ b/boot/bootutil/src/encrypted.c
@@ -48,6 +48,10 @@
 
 #include "bootutil_priv.h"
 
+#ifdef MCUBOOT_ENC_IMAGES_XIP_MULTI
+#include "xip_encryption.h"
+#endif /* MCUBOOT_ENC_IMAGES_XIP_MULTI */
+
 #if defined(MCUBOOT_ENCRYPT_EC256) || defined(MCUBOOT_ENCRYPT_X25519)
 #if defined(_compare)
 static inline int bootutil_constant_time_compare(const uint8_t *a, const uint8_t *b, size_t size)
@@ -158,8 +162,10 @@
 #endif
 
 #if defined(MCUBOOT_ENCRYPT_EC256)
+#if !defined(MCUBOOT_USE_PSA_CRYPTO)
 static const uint8_t ec_pubkey_oid[] = MBEDTLS_OID_EC_ALG_UNRESTRICTED;
 static const uint8_t ec_secp256r1_oid[] = MBEDTLS_OID_EC_GRP_SECP256R1;
+#endif
 
 #define SHARED_KEY_LEN NUM_ECC_BYTES
 #define PRIV_KEY_LEN   NUM_ECC_BYTES
@@ -195,6 +201,7 @@
         return -5;
     }
 
+#if !defined(MCUBOOT_USE_PSA_CRYPTO)
     if (alg.MBEDTLS_CONTEXT_MEMBER(len) != sizeof(ec_pubkey_oid) - 1 ||
         memcmp(alg.MBEDTLS_CONTEXT_MEMBER(p), ec_pubkey_oid, sizeof(ec_pubkey_oid) - 1)) {
         return -6;
@@ -203,6 +210,7 @@
         memcmp(param.MBEDTLS_CONTEXT_MEMBER(p), ec_secp256r1_oid, sizeof(ec_secp256r1_oid) - 1)) {
         return -7;
     }
+#endif
 
     if ((rc = mbedtls_asn1_get_tag(p, end, &len, MBEDTLS_ASN1_OCTET_STRING)) != 0) {
         return -8;
@@ -240,8 +248,10 @@
 
 #if defined(MCUBOOT_ENCRYPT_X25519)
 #define X25519_OID "\x6e"
+#if !defined(MCUBOOT_USE_PSA_CRYPTO)
 static const uint8_t ec_pubkey_oid[] = MBEDTLS_OID_ISO_IDENTIFIED_ORG \
                                        MBEDTLS_OID_ORG_GOV X25519_OID;
+#endif
 
 #define SHARED_KEY_LEN 32
 #define PRIV_KEY_LEN   32
@@ -272,10 +282,12 @@
         return -4;
     }
 
+#if !defined(MCUBOOT_USE_PSA_CRYPTO)
     if (alg.MBEDTLS_CONTEXT_MEMBER(len) != sizeof(ec_pubkey_oid) - 1 ||
         memcmp(alg.MBEDTLS_CONTEXT_MEMBER(p), ec_pubkey_oid, sizeof(ec_pubkey_oid) - 1)) {
         return -5;
     }
+#endif
 
     if (mbedtls_asn1_get_tag(p, end, &len, MBEDTLS_ASN1_OCTET_STRING) != 0) {
         return -6;
@@ -311,6 +323,7 @@
 hkdf(const uint8_t *ikm, uint16_t ikm_len, const uint8_t *info, uint16_t info_len,
         const uint8_t *salt, uint16_t salt_len, uint8_t *okm, uint16_t *okm_len)
 {
+#if !defined(MCUBOOT_USE_PSA_CRYPTO)
     bootutil_hmac_sha256_context hmac;
     uint8_t prk[BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE];
     uint8_t T[BOOTUTIL_CRYPTO_SHA256_DIGEST_SIZE];
@@ -399,6 +412,56 @@
 error:
     bootutil_hmac_sha256_drop(&hmac);
     return -1;
+#else
+    psa_status_t status;
+    psa_key_handle_t key_handle;
+    psa_key_derivation_operation_t operation = psa_key_derivation_operation_init();
+
+    {
+        psa_key_attributes_t key_attributes = psa_key_attributes_init();
+        psa_set_key_usage_flags(&key_attributes, PSA_KEY_USAGE_DERIVE);
+        psa_set_key_algorithm(&key_attributes, PSA_ALG_HKDF(PSA_ALG_SHA_256));
+        psa_set_key_type(&key_attributes, PSA_KEY_TYPE_DERIVE);
+        psa_set_key_bits(&key_attributes, 256u);
+
+        status = psa_import_key(&key_attributes, ikm, ikm_len, &key_handle);
+    }
+
+    if (status == PSA_SUCCESS)
+    {
+        status = psa_key_derivation_setup(&operation, PSA_ALG_HKDF(PSA_ALG_SHA_256));
+    }
+
+    if (status == PSA_SUCCESS)
+    {
+        status = psa_key_derivation_input_bytes(&operation, PSA_KEY_DERIVATION_INPUT_SALT, salt, salt_len);
+    }
+
+    if (status == PSA_SUCCESS)
+    {
+        status = psa_key_derivation_input_key(&operation, PSA_KEY_DERIVATION_INPUT_SECRET, key_handle);
+    }
+
+    if (status == PSA_SUCCESS)
+    {
+        status = psa_key_derivation_input_bytes(&operation, PSA_KEY_DERIVATION_INPUT_INFO, info, info_len);
+    }
+
+    if (status == PSA_SUCCESS)
+    {
+        status = psa_key_derivation_output_bytes(&operation, okm, *okm_len);
+    }
+
+    psa_key_derivation_abort(&operation);
+    psa_destroy_key(key_handle);
+
+    if (status != PSA_SUCCESS)
+    {
+        return -1;
+    }
+
+    return 0;
+#endif
 }
 #endif
 
@@ -464,6 +527,7 @@
 #if defined(MCUBOOT_ENCRYPT_RSA) || \
     (defined(MCUBOOT_ENCRYPT_EC256) && defined(MCUBOOT_USE_MBED_TLS))
 #if MBEDTLS_VERSION_NUMBER >= 0x03000000
+#if !defined(MCUBOOT_USE_PSA_CRYPTO)
 static int fake_rng(void *p_rng, unsigned char *output, size_t len)
 {
     size_t i;
@@ -475,10 +539,10 @@
 
     return 0;
 }
+#endif /* MCUBOOT_USE_PSA_CRYPTO */
 #endif
 #endif /* defined(MCUBOOT_ENCRYPT_RSA) ||
           defined(MCUBOOT_ENCRYPT_EC256) && defined(MCUBOOT_USE_MBED_TLS) */
-
 /*
  * Decrypt an encryption key TLV.
  *
@@ -818,18 +882,18 @@
         }
     }
 
-    enc = &enc_state[rc];
-    assert(enc->valid == 1);
+   enc = &enc_state[rc];
+   assert(enc->valid == 1);
 
-    nonce = enc->aes_iv;
+   nonce = enc->aes_iv;
 
-    off >>= 4;
-    nonce[12] = (uint8_t)(off >> 24);
-    nonce[13] = (uint8_t)(off >> 16);
-    nonce[14] = (uint8_t)(off >> 8);
-    nonce[15] = (uint8_t)off;
+   off >>= 4;
+   nonce[12] = (uint8_t)(off >> 24);
+   nonce[13] = (uint8_t)(off >> 16);
+   nonce[14] = (uint8_t)(off >> 8);
+   nonce[15] = (uint8_t)off;
 
-    return bootutil_aes_ctr_encrypt(&enc->aes_ctr, nonce, buf, sz, blk_off, buf);
+   return bootutil_aes_ctr_encrypt(&enc->aes_ctr, nonce, buf, sz, blk_off, buf);
 }
 
 /**
@@ -846,3 +910,9 @@
 }
 
 #endif /* MCUBOOT_ENC_IMAGES */
+
+/*
+ * Avoid warning from -pedantic. This is included
+ * because ISO C forbids an empty translation unit.
+ */
+typedef int encrypted_iso_c_forbids_empty_translation_units;
\ No newline at end of file
diff --git a/boot/bootutil/src/fault_injection_hardening.c b/boot/bootutil/src/fault_injection_hardening.c
index 4dcbdd9..1b9397d 100644
--- a/boot/bootutil/src/fault_injection_hardening.c
+++ b/boot/bootutil/src/fault_injection_hardening.c
@@ -7,8 +7,14 @@
 
 #include "bootutil/fault_injection_hardening.h"
 
+#ifdef FIH_ENABLE_DELAY
+#include "boot_rng.h"
+
+#define FIH_DELAY_MSK 0x3FUL
+#endif /* FIH_ENABLE_DELAY */
+
 #ifdef FIH_ENABLE_CFI
-fih_uint fih_cfi_ctr = FIH_UINT_INIT(0u);
+fih_uint fih_cfi_ctr = FIH_UINT_INIT_GLOBAL(0u);
 
 fih_uint fih_cfi_get_and_increment(uint8_t cnt)
 {
@@ -21,18 +27,16 @@
 
     fih_cfi_ctr = fih_uint_encode(fih_uint_decode(fih_cfi_ctr) + cnt);
 
-    fih_uint_validate(fih_cfi_ctr);
-    fih_uint_validate(saved_ctr);
+    (void)fih_uint_validate(fih_cfi_ctr);
+    (void)fih_uint_validate(saved_ctr);
 
     return saved_ctr;
 }
 
 void fih_cfi_validate(fih_uint saved)
 {
-    volatile int32_t rc = FIH_FALSE;
-
-    rc = fih_uint_eq(saved, fih_cfi_ctr);
-    if (rc != FIH_TRUE) {
+    if (!fih_uint_eq(saved, fih_cfi_ctr))
+    {
         FIH_PANIC;
     }
 }
@@ -45,7 +49,7 @@
 
     fih_cfi_ctr = fih_uint_encode(fih_uint_decode(fih_cfi_ctr) - 1u);
 
-    fih_uint_validate(fih_cfi_ctr);
+    (void)fih_uint_validate(fih_cfi_ctr);
 }
 #endif /* FIH_ENABLE_CFI */
 
@@ -74,15 +78,61 @@
 #endif /* FIH_ENABLE_GLOBAL_FAIL */
 
 #ifdef FIH_ENABLE_DELAY
+
+/*******************************************************************************
+ * Function Name: fih_delay_init
+ *******************************************************************************
+ * \brief Initialize assets which are required for random delay generation.
+ *
+ ******************************************************************************/
 void fih_delay_init(void)
 {
-    /* Implement here */
+    if(!boot_rng_init())
+    {
+        FIH_PANIC;
+    }
 }
 
-uint8_t fih_delay_random(void)
+
+/*******************************************************************************
+ * Function Name: fih_delay_random
+ *******************************************************************************
+ * \brief Generate 8-bit random delay number masked with FIH_DELAY_MSK.
+ *
+ * \return  8-bit random delay number masked with FIH_DELAY_MSK.
+ *
+ ******************************************************************************/
+static uint8_t fih_delay_random(void)
 {
-    /* Implement here */
-
-    return 0xFF;
+    return (uint8_t)(boot_rng_random_generate() & FIH_DELAY_MSK);
 }
+
+
+/*******************************************************************************
+ * Function Name: fih_delay
+ *******************************************************************************
+ * \brief FIH delay execution.
+ *
+ * \return  status of execution. The return value is required by calling macros
+ *          like fih_uint_eq(). It always returns true or hang in FIH_PANIC.
+ *
+ ******************************************************************************/
+bool fih_delay(void)
+{
+    volatile uint8_t counter = 0U;
+    uint8_t delay = fih_delay_random();
+
+    for (uint8_t i = 0; i < delay; i++)
+    {
+        counter++;
+    }
+
+    if (counter != delay)
+    {
+        FIH_PANIC;
+    }
+
+    return true;
+}
+
 #endif /* FIH_ENABLE_DELAY */
diff --git a/boot/bootutil/src/image_ec256.c b/boot/bootutil/src/image_ec256.c
index e5c0cd6..4d67063 100644
--- a/boot/bootutil/src/image_ec256.c
+++ b/boot/bootutil/src/image_ec256.c
@@ -120,7 +120,7 @@
     if (mbedtls_asn1_get_alg(cp, end, &alg, &param)) {
         return -2;
     }
-    /* id-ecPublicKey (RFC5480) */
+
     if (alg.MBEDTLS_CONTEXT_MEMBER(len) != sizeof(ec_pubkey_oid) - 1 ||
         memcmp(alg.MBEDTLS_CONTEXT_MEMBER(p), ec_pubkey_oid, sizeof(ec_pubkey_oid) - 1)) {
         return -3;
@@ -130,6 +130,7 @@
         memcmp(param.MBEDTLS_CONTEXT_MEMBER(p), ec_secp256r1_oid, sizeof(ec_secp256r1_oid) - 1)) {
         return -4;
     }
+
     /* ECPoint (RFC5480) */
     if (mbedtls_asn1_get_bitstring_null(cp, end, &len)) {
         return -6;
diff --git a/boot/bootutil/src/image_validate.c b/boot/bootutil/src/image_validate.c
index d4cbade..79dc2fd 100644
--- a/boot/bootutil/src/image_validate.c
+++ b/boot/bootutil/src/image_validate.c
@@ -30,6 +30,7 @@
 #include <stdint.h>
 #include <inttypes.h>
 #include <string.h>
+#include <assert.h>
 
 #include <flash_map_backend/flash_map_backend.h>
 
@@ -123,7 +124,7 @@
     size += hdr->ih_protect_tlv_size;
 
     do
-    {      
+    {
 #if defined(MCUBOOT_RAM_LOAD)
 #if defined(MCUBOOT_MULTI_MEMORY_LOAD)
         if (IS_RAM_BOOTABLE(hdr) && IS_RAM_BOOT_STAGE())
@@ -229,15 +230,15 @@
 
 /* Complex result masks for bootutil_img_validate() */
 #define SET_MASK_IMAGE_TLV_SHA256       ((signed)0x0000002E)
-fih_int FIH_MASK_IMAGE_TLV_SHA256 = FIH_INT_INIT(
+fih_int FIH_MASK_IMAGE_TLV_SHA256 = FIH_INT_INIT_GLOBAL(
         SET_MASK_IMAGE_TLV_SHA256);
 
 #define SET_MASK_SIG_TLV_EXPECTED       ((signed)0x00007400)
-fih_int FIH_MASK_SIG_TLV_EXPECTED = FIH_INT_INIT(
+fih_int FIH_MASK_SIG_TLV_EXPECTED = FIH_INT_INIT_GLOBAL(
         SET_MASK_SIG_TLV_EXPECTED);
 
 #define SET_MASK_IMAGE_TLV_SEC_CNT      ((signed)0x005C0000)
-fih_int FIH_MASK_IMAGE_TLV_SEC_CNT = FIH_INT_INIT(
+fih_int FIH_MASK_IMAGE_TLV_SEC_CNT = FIH_INT_INIT_GLOBAL(
         SET_MASK_IMAGE_TLV_SEC_CNT);
 
 #define CHK_MASK_IMAGE_TLV_SHA256       SET_MASK_IMAGE_TLV_SHA256
@@ -271,7 +272,7 @@
     #define CHK_MASK_IMAGE_TLV_SEC_CNT      ((signed)0)
 #endif /* MCUBOOT_HW_ROLLBACK_PROT */
 
-fih_int FIH_IMG_VALIDATE_COMPLEX_OK = FIH_INT_INIT( \
+fih_int FIH_IMG_VALIDATE_COMPLEX_OK = FIH_INT_INIT_GLOBAL( \
     CHK_MASK_IMAGE_TLV_SHA256  |                      \
     CHK_MASK_SIG_TLV_EXPECTED  |                      \
     CHK_MASK_IMAGE_TLV_SEC_CNT);
@@ -340,7 +341,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_TRUE == fih_eq(fih_rc, FIH_SUCCESS)) {
+    if (fih_eq(fih_rc, FIH_SUCCESS)) {
         bootutil_keys[0].key = key;
         pub_key_len = key_len;
         return 0;
@@ -419,8 +420,7 @@
         goto out;
     }
 
-    if (FIH_TRUE == fih_uint_eq(fih_uint_encode(img_chk_cnt),
-                                *img_security_cnt)) {
+    if (fih_uint_eq(fih_uint_encode(img_chk_cnt), *img_security_cnt)) {
 
         if (img_sec_cnt == img_chk_cnt) {
             fih_rc = FIH_SUCCESS;
@@ -529,11 +529,11 @@
 #ifdef MCUBOOT_HW_ROLLBACK_PROT
     fih_uint security_cnt = FIH_UINT_MAX;
     uint32_t img_security_cnt = 0;
+#ifdef CYW20829
     uint8_t reprov_packet[REPROV_PACK_SIZE];
     fih_int security_counter_valid = FIH_FAILURE;
-    #ifdef CYW20829
     fih_uint extracted_img_cnt = FIH_UINT_MAX;
-    #endif /* CYW20829 */
+#endif /* CYW20829 */
 #endif /* MCUBOOT_HW_ROLLBACK_PROT */
 
     rc = bootutil_img_hash(enc_state, image_index, hdr, fap, tmp_buf,
@@ -579,7 +579,7 @@
 
             FIH_CALL(boot_fih_memequal, fih_rc, hash, buf, sizeof(hash));
 
-            if (fih_eq(fih_rc, FIH_SUCCESS) == FIH_TRUE) {
+            if (fih_eq(fih_rc, FIH_SUCCESS)) {
                 /* Encode succesful completion pattern to complex_result */
                 fih_complex_result = fih_or(fih_complex_result,
                                                  FIH_MASK_IMAGE_TLV_SHA256);
@@ -646,7 +646,7 @@
                                                            buf, len, key_id);
             key_id = -1;
 
-            if (FIH_TRUE == fih_eq(FIH_SUCCESS, valid_signature)) {
+            if (fih_eq(FIH_SUCCESS, valid_signature)) {
                 /* Encode succesful completion pattern to complex_result */
                 fih_complex_result = fih_or(fih_complex_result,
                                                  FIH_MASK_SIG_TLV_EXPECTED);
@@ -677,7 +677,7 @@
 
             FIH_CALL(boot_nv_security_counter_get, fih_rc, image_index,
                                                            &security_cnt);
-            if (fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
+            if (!fih_eq(fih_rc, FIH_SUCCESS)) {
                 rc = -1;
                 goto out;
             }
@@ -690,7 +690,7 @@
             FIH_CALL(platform_security_counter_check_extract, fih_rc, 
                     (uint32_t)image_index, fih_uint_encode(img_security_cnt), &extracted_img_cnt);
 
-            if (fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
+            if (!fih_eq(fih_rc, FIH_SUCCESS)) {
                 /* The image's security counter exceeds registered value for this image */
                 rc = -1;
                 goto out;
@@ -704,13 +704,13 @@
             /* Compare the new image's security counter value against the
              * stored security counter value.
              */
-            if (FIH_TRUE == fih_uint_ge(fih_uint_encode(img_security_cnt), security_cnt)) {
+            if (fih_uint_ge(fih_uint_encode(img_security_cnt), security_cnt)) {
                 /* Encode succesful completion pattern to complex_result */
                 fih_complex_result = fih_or(fih_complex_result,
                                                  FIH_MASK_IMAGE_TLV_SEC_CNT);
 #ifdef CYW20829
                 /* The image's security counter has been successfully verified. */
-                security_counter_valid = fih_int_encode(HW_ROLLBACK_CNT_VALID);
+                security_counter_valid = FIH_INT_INIT(HW_ROLLBACK_CNT_VALID);
 #endif
             } else {
                 /* The image's security counter is not accepted. */
@@ -721,7 +721,7 @@
 #ifdef CYW20829
         } else if (type == IMAGE_TLV_PROV_PACK) {
 
-            if (FIH_TRUE == fih_eq(security_counter_valid, fih_int_encode(HW_ROLLBACK_CNT_VALID))) {
+            if (fih_eq(security_counter_valid, FIH_INT_INIT(HW_ROLLBACK_CNT_VALID))) {
                 /*
                 * Verify the image reprovisioning packet.
                 * This must always be present.
@@ -752,7 +752,7 @@
 
 #ifdef MCUBOOT_HW_ROLLBACK_PROT
 #ifdef CYW20829
-    if (fih_eq(security_counter_valid, fih_int_encode(REPROV_PACK_VALID | HW_ROLLBACK_CNT_VALID)) != FIH_TRUE) {
+    if (!fih_eq(security_counter_valid, FIH_INT_INIT(REPROV_PACK_VALID | HW_ROLLBACK_CNT_VALID))) {
         BOOT_LOG_DBG("Reprovisioning packet TLV 0x51 is not present image = %d", image_index);
         rc = -1;
         goto out;
diff --git a/boot/bootutil/src/loader.c b/boot/bootutil/src/loader.c
index 8ede0f1..ba5350e 100644
--- a/boot/bootutil/src/loader.c
+++ b/boot/bootutil/src/loader.c
@@ -49,7 +49,10 @@
 
 #ifdef MCUBOOT_ENC_IMAGES
 #include "bootutil/enc_key.h"
-#endif
+#ifdef MCUBOOT_ENC_IMAGES_XIP_MULTI
+#include "xip_encryption.h"
+#endif /* MCUBOOT_ENC_IMAGES_XIP_MULTI */
+#endif /* MCUBOOT_ENC_IMAGES */
 
 #if (!defined(MCUBOOT_DIRECT_XIP) && !defined(MCUBOOT_RAM_LOAD)) || defined(MCUBOOT_MULTI_MEMORY_LOAD)
 #include <os/os_malloc.h>
@@ -91,7 +94,7 @@
 #define BUF_SZ 1024U
 #endif
 
-static fih_int FIH_SWAP_TYPE_NONE = FIH_INT_INIT(0x3A5C742E);
+static fih_int FIH_SWAP_TYPE_NONE = FIH_INT_INIT_GLOBAL(0x3A5C742E);
 
 static int
 boot_read_image_headers(struct boot_loader_state *state, bool require_all,
@@ -141,13 +144,16 @@
     int rc;
 
 #ifdef MCUBOOT_MEASURED_BOOT
-    rc = boot_save_boot_status(BOOT_CURR_IMG(state),
+    rc = boot_save_boot_status(GET_SW_MODULE_ID(BOOT_CURR_IMG(state)),
                                 boot_img_hdr(state, active_slot),
                                 BOOT_IMG_AREA(state, active_slot));
     if (rc != 0) {
         BOOT_LOG_ERR("Failed to add image data to shared area");
         return rc;
     }
+    else {
+        BOOT_LOG_INF("Successfully added image data to shared area");
+    }
 #endif /* MCUBOOT_MEASURED_BOOT */
 
 #ifdef MCUBOOT_DATA_SHARING
@@ -567,9 +573,13 @@
             if (0 == rc && boot_enc_set_key(BOOT_CURR_ENC(state), slot, bs)) {
                 FIH_RET(fih_rc);
             }
+#ifdef MCUBOOT_ENC_IMAGES_XIP_MULTI
+            ifx_epb_set_xip_crypto_params((uint32_t *)bs->enckey[slot],
+                                        (uint32_t *)BOOT_CURR_ENC(state)[slot].aes_iv);
+#endif /* MCUBOOT_ENC_IMAGES_XIP_MULTI */
         }
     }
-#endif
+#endif /* MCUBOOT_ENC_IMAGES */
 
 #ifdef USE_IFX_SE_CRYPTO
     FIH_UCALL(bootutil_psa_img_validate, fih_complex_result, \
@@ -581,8 +591,7 @@
                  fih_uint_decode(IFX_FIH_IMG_VALIDATE_COMPLEX_OK),   \
                  fih_uint_decode(fih_complex_result));
 
-    if (FIH_TRUE == fih_uint_eq(fih_complex_result,
-                                IFX_FIH_IMG_VALIDATE_COMPLEX_OK)) {
+    if (fih_uint_eq(fih_complex_result, IFX_FIH_IMG_VALIDATE_COMPLEX_OK)) {
         fih_rc = fih_int_encode_zero_equality(
                             fih_uint_decode(IFX_FIH_IMG_VALIDATE_COMPLEX_OK) &
                             ~fih_uint_decode(fih_complex_result));
@@ -598,8 +607,7 @@
                  fih_int_decode(FIH_IMG_VALIDATE_COMPLEX_OK),   \
                  fih_int_decode(fih_complex_result));
 
-    if (FIH_TRUE == fih_eq(fih_complex_result,
-                                FIH_IMG_VALIDATE_COMPLEX_OK)) {
+    if (fih_eq(fih_complex_result, FIH_IMG_VALIDATE_COMPLEX_OK)) {
         fih_rc = fih_int_encode_zero_equality(
                             fih_int_decode(FIH_IMG_VALIDATE_COMPLEX_OK) &
                             ~fih_int_decode(fih_complex_result));
@@ -619,9 +627,10 @@
                   struct image_header *loader_hdr,
                   const struct flash_area *loader_fap)
 {
+    fih_int fih_rc = FIH_FAILURE;
+#if !defined USE_IFX_SE_CRYPTO
     static void *tmpbuf;
     uint8_t loader_hash[32];
-    fih_int fih_rc = FIH_FAILURE;
 
     if (!tmpbuf) {
         tmpbuf = malloc(BOOT_TMPBUF_SZ);
@@ -632,12 +641,22 @@
 
     FIH_CALL(bootutil_img_validate, fih_rc, NULL, 0, loader_hdr, loader_fap,
              tmpbuf, BOOT_TMPBUF_SZ, NULL, 0, loader_hash);
-    if (fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
+    if (!fih_eq(fih_rc, FIH_SUCCESS)) {
         FIH_RET(fih_rc);
     }
 
     FIH_CALL(bootutil_img_validate, fih_rc, NULL, 0, app_hdr, app_fap,
              tmpbuf, BOOT_TMPBUF_SZ, loader_hash, 32, NULL);
+#else
+    /*  Not implemented for USE_IFX_SE_CRYPTO
+        The code below is just to avoid warnings
+    */
+    (void)app_hdr;
+    (void)app_fap;
+    (void)loader_hdr;
+    (void)loader_fap;
+    goto out;
+#endif
 
 out:
     FIH_RET(fih_rc);
@@ -711,9 +730,8 @@
     return 0;
 }
 
-#if (BOOT_IMAGE_NUMBER > 1) || \
+#if (BOOT_IMAGE_NUMBER > 1 && defined(MCUBOOT_DEPENDENCY_CHECK)) || \
     defined(MCUBOOT_DIRECT_XIP) || \
-    defined(MCUBOOT_RAM_LOAD) || \
     (defined(MCUBOOT_OVERWRITE_ONLY) && defined(MCUBOOT_DOWNGRADE_PREVENTION))
 /**
  * Compare image version numbers not including the build number
@@ -820,6 +838,17 @@
 
     hdr = boot_img_hdr(state, slot);
 
+#ifdef MCUBOOT_ENC_IMAGES_XIP_MULTI
+/* In the XIP encryption multi image case if XIP encryption is turned on then 
+ * the boot_check_header_erased() can't detect erased header correctly for the second and next images
+ * because erased value is not read as 0xFF.
+ * So, the bootloader has one option only to detect correctness of image header: it is
+ * to check header magic */
+    if (hdr->ih_magic != IMAGE_MAGIC) {
+        FIH_RET(fih_rc);
+    }
+#endif /* MCUBOOT_ENC_IMAGES_XIP_MULTI */
+
     if (boot_check_header_erased(state, slot) == 0 ||
         (hdr->ih_flags & IMAGE_F_NON_BOOTABLE)) {
 
@@ -864,13 +893,13 @@
         }
     }
 #endif
-    BOOT_HOOK_CALL_FIH(boot_image_check_hook, fih_int_encode(BOOT_HOOK_REGULAR),
+    BOOT_HOOK_CALL_FIH(boot_image_check_hook, FIH_INT_INIT(BOOT_HOOK_REGULAR),
                        fih_rc, BOOT_CURR_IMG(state), slot);
-    if (FIH_TRUE == fih_eq(fih_rc, fih_int_encode(BOOT_HOOK_REGULAR)))
+    if (fih_eq(fih_rc, FIH_INT_INIT(BOOT_HOOK_REGULAR)))
     {
         FIH_CALL(boot_image_check, fih_rc, state, hdr, fap, bs);
     }
-    if (!boot_is_header_valid(hdr, fap) || fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
+    if (!boot_is_header_valid(hdr, fap) || !fih_eq(fih_rc, FIH_SUCCESS)) {
         if ((slot != BOOT_PRIMARY_SLOT) || ARE_SLOTS_EQUIVALENT()) {
             BOOT_LOG_DBG(" * Image in the secondary slot is invalid. Erase the image");
             flash_area_erase(fap, 0, flash_area_get_size(fap));
@@ -900,7 +929,7 @@
 
         rc = flash_area_read(fap, reset_addr, &reset_value, sizeof(reset_value));
         if (rc != 0) {
-            fih_rc = fih_int_encode(1);
+            fih_rc = FIH_INT_INIT(1);
             goto out;
         }
 
@@ -915,7 +944,7 @@
              * Erase the image and continue booting from the primary slot.
              */
             flash_area_erase(fap, 0, fap->fa_size);
-            fih_rc = fih_int_encode(1);
+            fih_rc = FIH_INT_INIT(1);
             goto out;
         }
     }
@@ -963,7 +992,7 @@
 
     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) {
+    if (!fih_eq(fih_rc, FIH_SUCCESS)) {
         goto done;
     }
     else 
@@ -982,7 +1011,7 @@
 #ifdef USE_IFX_SE_CRYPTO
     fih_uint img_security_check = FIH_UINT_ZERO;
     FIH_CALL(boot_nv_security_counter_get, fih_rc, image_index, &img_security_check);
-    if (fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
+    if (!fih_eq(fih_rc, FIH_SUCCESS)) {
         goto done;
     }
     else 
@@ -1020,8 +1049,8 @@
          * Ensure image is valid.
          */
         FIH_CALL(boot_validate_slot, fih_rc, state, BOOT_SECONDARY_SLOT, bs);
-        if (fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
-            if (FIH_TRUE == fih_eq(fih_rc, FIH_SWAP_TYPE_NONE)) {
+        if (!fih_eq(fih_rc, FIH_SUCCESS)) {
+            if (fih_eq(fih_rc, FIH_SWAP_TYPE_NONE)) {
                 swap_type = BOOT_SWAP_TYPE_NONE;
             } else {
                 swap_type = BOOT_SWAP_TYPE_FAIL;
@@ -1071,7 +1100,8 @@
     uint32_t bytes_copied;
     int chunk_sz;
     int rc;
-#if defined (MCUBOOT_ENC_IMAGES) && !defined(MCUBOOT_ENC_IMAGES_XIP)
+#if (defined (MCUBOOT_ENC_IMAGES) && !defined(MCUBOOT_ENC_IMAGES_XIP)) || \
+    (defined (MCUBOOT_ENC_IMAGES) && defined(MCUBOOT_ENC_IMAGES_XIP) && defined(MCUBOOT_ENC_IMAGES_XIP_MULTI))
     uint32_t off;
     uint32_t tlv_off;
     size_t blk_off;
@@ -1079,7 +1109,8 @@
     uint16_t idx;
     uint32_t blk_sz;
     uint8_t image_index;
-#endif
+#endif /* (defined (MCUBOOT_ENC_IMAGES) && !defined(MCUBOOT_ENC_IMAGES_XIP)) || \
+          (defined (MCUBOOT_ENC_IMAGES) && defined(MCUBOOT_ENC_IMAGES_XIP) && defined(MCUBOOT_ENC_IMAGES_XIP_MULTI)) */
 
 /* NOTE:
  * Default value 1024 is not suitable for platforms with larger erase size.
@@ -1111,7 +1142,8 @@
             return BOOT_EFLASH;
         }
 
-#if defined(MCUBOOT_ENC_IMAGES) && !defined(MCUBOOT_ENC_IMAGES_XIP)
+#if (defined (MCUBOOT_ENC_IMAGES) && !defined(MCUBOOT_ENC_IMAGES_XIP)) || \
+    (defined (MCUBOOT_ENC_IMAGES) && defined(MCUBOOT_ENC_IMAGES_XIP) && defined(MCUBOOT_ENC_IMAGES_XIP_MULTI))
         image_index = BOOT_CURR_IMG(state);
         if ((flash_area_get_id(fap_src) == FLASH_AREA_IMAGE_PRIMARY(image_index) ||
              flash_area_get_id(fap_dst) == FLASH_AREA_IMAGE_PRIMARY(image_index)) &&
@@ -1166,18 +1198,41 @@
                             blk_sz = tlv_off - abs_off;
                         }
                     }
+#ifndef MCUBOOT_ENC_IMAGES_XIP_MULTI
                     rc = boot_encrypt(BOOT_CURR_ENC(state), image_index, fap_src,
                                       (abs_off + idx) - hdr->ih_hdr_size, blk_sz,
                                       blk_off, &buf[idx]);
+#else /* MCUBOOT_ENC_IMAGES_XIP_MULTI */
+                    rc = boot_encrypt_xip(fap_src, fap_dst,
+                        (abs_off + idx), blk_sz, &buf[idx]);
+#endif
                     if (rc != 0) {
                         return rc;
                     }
                 }
+#ifdef MCUBOOT_ENC_IMAGES_XIP_MULTI
+                rc = flash_area_write(fap_dst, off_dst + bytes_copied, buf, chunk_sz);
+            }
+            else {
+                (void)blk_off;
+                rc = boot_encrypt_xip(fap_src, fap_dst,
+                                        off_dst + bytes_copied,
+                                        chunk_sz, buf);
+
+                rc = flash_area_write(fap_dst, off_dst + bytes_copied, buf, chunk_sz);
+                SMIF_SET_CRYPTO_MODE(Enable);
             }
         }
-#endif
-
+#else
+        	}
+        }
         rc = flash_area_write(fap_dst, off_dst + bytes_copied, buf, chunk_sz);
+#endif /* MCUBOOT_ENC_IMAGES_XIP_MULTI */
+#else
+        rc = flash_area_write(fap_dst, off_dst + bytes_copied, buf, chunk_sz);
+#endif /* (defined (MCUBOOT_ENC_IMAGES) && !defined(MCUBOOT_ENC_IMAGES_XIP)) || \
+          (defined (MCUBOOT_ENC_IMAGES) && defined(MCUBOOT_ENC_IMAGES_XIP) && defined(MCUBOOT_ENC_IMAGES_XIP_MULTI)) */
+
         if (rc != 0) {
             return BOOT_EFLASH;
         }
@@ -1466,6 +1521,10 @@
                 assert(rc == 0);
             }
         }
+#if defined(MCUBOOT_SAVE_ENC_IV)
+        rc = boot_read_iv(image_index, 1, state->enc[image_index][1].aes_iv);
+        assert(rc == 0);
+#endif
 #endif
     }
 
@@ -1484,6 +1543,7 @@
 #endif
 
 #if (BOOT_IMAGE_NUMBER > 1)
+#if defined(MCUBOOT_DEPENDENCY_CHECK)
 /**
  * Check the image dependency whether it is satisfied and modify
  * the swap type if necessary.
@@ -1509,6 +1569,7 @@
 
     rc = boot_version_cmp(dep_version, &dep->image_min_version);
     if (rc < 0) {
+#ifndef MCUBOOT_OVERWRITE_ONLY
         /* Dependency not satisfied.
          * Modify the swap type to decrease the version number of the image
          * (which will be located in the primary slot after the boot process),
@@ -1518,14 +1579,20 @@
         switch (BOOT_SWAP_TYPE(state)) {
         case BOOT_SWAP_TYPE_TEST:
         case BOOT_SWAP_TYPE_PERM:
-            BOOT_SWAP_TYPE(state) = BOOT_SWAP_TYPE_NONE;
+            /* BOOT_SWAP_TYPE_NONE has been changed to BOOT_SWAP_TYPE_FAIL to avoid 
+             * reversion again after device reset */
+            BOOT_SWAP_TYPE(state) = BOOT_SWAP_TYPE_FAIL;
             break;
         case BOOT_SWAP_TYPE_NONE:
+            BOOT_LOG_DBG("Dependency is unsatisfied. Slot will be reverted.");
             BOOT_SWAP_TYPE(state) = BOOT_SWAP_TYPE_REVERT;
             break;
         default:
             break;
         }
+#else
+        BOOT_LOG_DBG("Dependency is unsatisfied.");
+#endif
     } else {
         /* Dependency satisfied. */
         rc = 0;
@@ -1646,7 +1713,13 @@
              */
             for (int idx = 0; idx < BOOT_IMAGE_NUMBER; idx++) {
                 BOOT_CURR_IMG(state) = idx;
-                BOOT_SWAP_TYPE(state) = BOOT_SWAP_TYPE_NONE;
+                /*When dependency is not satisfied, the boot_verify_slot_dependencies_flash 
+                changes swap type to BOOT_SWAP_TYPE_REVERT to have ability of reversion of a
+                dependent image. That's why BOOT_SWAP_TYPE_REVERT must not be changed to 
+                BOOT_SWAP_TYPE_NONE */
+                if (BOOT_SWAP_TYPE(state) != BOOT_SWAP_TYPE_REVERT) {
+                    BOOT_SWAP_TYPE(state) = BOOT_SWAP_TYPE_NONE;
+                }
             }
             break;
 #endif /* (USE_SHARED_SLOT == 1) */
@@ -1654,6 +1727,7 @@
     }
     return rc;
 }
+#endif /* (MCUBOOT_DEPENDENCY_CHECK) */
 #endif /* (BOOT_IMAGE_NUMBER > 1) */
 
 /**
@@ -1690,7 +1764,7 @@
     fih_rc = FIH_SUCCESS;
 #endif
 
-    if (rc == 0 || fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
+    if (rc == 0 || !fih_eq(fih_rc, FIH_SUCCESS)) {
         /* Initialize swap status partition for primary slot, because
          * in swap mode it is needed to properly complete copying the image
          * to the primary slot.
@@ -2045,7 +2119,7 @@
             } else {
                 FIH_CALL(boot_validate_slot, fih_rc,
                          state, BOOT_SECONDARY_SLOT, bs);
-                if (fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
+                if (!fih_eq(fih_rc, FIH_SUCCESS)) {
                     BOOT_SWAP_TYPE(state) = BOOT_SWAP_TYPE_FAIL;
                 } else {
                     BOOT_SWAP_TYPE(state) = bs->swap_type;
@@ -2071,13 +2145,13 @@
 #else
                 fih_rc = FIH_SUCCESS;
 #endif
-                if (rc == 0 || fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
+                if (rc == 0 || !fih_eq(fih_rc, FIH_SUCCESS)) {
 
                     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_TRUE == fih_eq(fih_rc, FIH_SUCCESS)) {
+                    if (rc == 1 && 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
@@ -2149,7 +2223,6 @@
     fih_int fih_rc = FIH_FAILURE;
     int fa_id;
     int image_index;
-    bool has_upgrade;
 
     /* The array of slot sectors are defined here (as opposed to file scope) so
      * that they don't get allocated for non-boot-loader apps.  This is
@@ -2165,22 +2238,18 @@
     TARGET_STATIC boot_sector_t status_sectors[BOOT_MAX_SWAP_STATUS_SECTORS];
 #endif
 
-    has_upgrade = false;
-
-#if (BOOT_IMAGE_NUMBER == 1)
-    (void)has_upgrade;
-#endif
-
     /* Iterate over all the images. By the end of the loop the swap type has
      * to be determined for each image and all aborted swaps have to be
      * completed.
      */
     IMAGES_ITER(BOOT_CURR_IMG(state)) {
+#if !defined(MCUBOOT_DEPENDENCY_CHECK)
 #if BOOT_IMAGE_NUMBER > 1
         if (state->img_mask[BOOT_CURR_IMG(state)]) {
             continue;
         }
 #endif
+#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
@@ -2220,13 +2289,10 @@
         /* Determine swap type and complete swap if it has been aborted. */
         boot_prepare_image_for_update(state, &bs);
 
-        if (BOOT_IS_UPGRADE(BOOT_SWAP_TYPE(state))) {
-            has_upgrade = true;
-        }
     }
 
 #if (BOOT_IMAGE_NUMBER > 1)
-    if (has_upgrade) {
+#if defined(MCUBOOT_DEPENDENCY_CHECK)
         /* Iterate over all the images and verify whether the image dependencies
          * are all satisfied and update swap type if necessary.
          */
@@ -2240,8 +2306,8 @@
              */
             rc = 0;
         }
-    }
-#endif
+#endif /* (MCUBOOT_DEPENDENCY_CHECK) */
+#endif /* (BOOT_IMAGE_NUMBER > 1) */
 
     /* Iterate over all the images. At this point there are no aborted swaps
      * and the swap types are determined for each image. By the end of the loop
@@ -2347,7 +2413,7 @@
         if(IS_RAM_BOOTABLE(boot_img_hdr(state, BOOT_PRIMARY_SLOT)) == false) {
 #endif /* defined(MCUBOOT_RAM_LOAD) */
             FIH_CALL(boot_validate_slot, fih_rc, state, BOOT_PRIMARY_SLOT, &bs);
-            if (fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
+            if (!fih_eq(fih_rc, FIH_SUCCESS)) {
                 goto out;
             }
 #if defined(MCUBOOT_RAM_LOAD) /* to fix Rule 14.3 violation */
@@ -2386,9 +2452,11 @@
             goto out;
         }
 
-        rc = boot_add_shared_data(state, BOOT_PRIMARY_SLOT);
-        if (rc != 0) {
-            goto out;
+        if(IS_RAM_BOOTABLE(boot_img_hdr(state, BOOT_PRIMARY_SLOT)) == false) {
+            rc = boot_add_shared_data(state, BOOT_PRIMARY_SLOT);
+            if (rc != 0) {
+                goto out;
+            }
         }
     }
 
@@ -2413,6 +2481,10 @@
 out:
     close_all_flash_areas(state);
 
+#ifdef MCUBOOT_ENC_IMAGES_XIP_MULTI
+    SMIF_SET_CRYPTO_MODE(Enable);
+#endif /* MCUBOOT_ENC_IMAGES_XIP_MULTI */
+
     if (rc) {
         fih_rc = fih_int_encode(rc);
     }
@@ -2471,7 +2543,7 @@
              BOOT_IMG_AREA(&boot_data, split_slot),
              boot_img_hdr(&boot_data, loader_slot),
              BOOT_IMG_AREA(&boot_data, loader_slot));
-    if (fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
+    if (!fih_eq(fih_rc, FIH_SUCCESS)) {
         goto done;
     }
 
@@ -2511,11 +2583,13 @@
     struct image_header *hdr = NULL;
 
     IMAGES_ITER(BOOT_CURR_IMG(state)) {
+#if !defined(MCUBOOT_DEPENDENCY_CHECK)
 #if BOOT_IMAGE_NUMBER > 1
         if (state->img_mask[BOOT_CURR_IMG(state)]) {
             continue;
         }
 #endif
+#endif
         /* Open all the slots */
         for (slot = 0; slot < BOOT_NUM_SLOTS; slot++) {
             fa_id = flash_area_id_from_multi_image_slot(
@@ -2555,13 +2629,16 @@
 
 /**
  * Finds the slot containing the image with the highest version number for the
- * current image.
+ * current image. Also dependency check feature verifies version of the first
+ * slot of dependent image and assumes to load from the first slot. In order to
+ * avoid conflicts dependency ckeck feature is disabled.
  *
  * @param  state        Boot loader status information.
  *
  * @return              NO_ACTIVE_SLOT if no available slot found, number of
  *                      the found slot otherwise.
  */
+#if !defined(MCUBOOT_DEPENDENCY_CHECK) && !defined(MCUBOOT_RAM_LOAD)
 static uint32_t
 find_slot_with_highest_version(struct boot_loader_state *state)
 {
@@ -2589,6 +2666,7 @@
 
     return candidate_slot;
 }
+#endif /* !defined(MCUBOOT_DEPENDENCY_CHECK) && !defined(MCUBOOT_RAM_LOAD) */
 
 #ifdef MCUBOOT_HAVE_LOGGING
 /**
@@ -2979,7 +3057,7 @@
     uint32_t active_slot;
     struct image_header *hdr = NULL;
     uint32_t img_dst;
-    uint32_t img_sz;
+    uint32_t img_sz = 0;
     int rc = 0;
 
     active_slot = state->slot_usage[BOOT_CURR_IMG(state)].active_slot;
@@ -3099,6 +3177,7 @@
 #endif /* MCUBOOT_RAM_LOAD */
 
 #if (BOOT_IMAGE_NUMBER > 1)
+#if defined(MCUBOOT_DEPENDENCY_CHECK)
 /**
  * Checks the image dependency whether it is satisfied.
  *
@@ -3241,6 +3320,7 @@
 
     return rc;
 }
+#endif /* (MCUBOOT_DEPENDENCY_CHECK) */
 #endif /* (BOOT_IMAGE_NUMBER > 1) */
 
 /**
@@ -3271,8 +3351,25 @@
                 /* A slot is already active, go to next image. */
                 break;
             }
+            
+            /* Ram load assumes to find the highest version of available slots
+             * and load it. Also dependency check feature verifies version
+             * of first slot of dependent image and assumes to load from the
+             * first slot. So logic is separated into two cases to avoid conflicts,
+             * where the first is when dependency check is disabled,
+             * and the second is when it is enabled.
+             * Notation: to avoid situations when reverted image with higher version is
+             * ram-loaded, the current logic is changed to loading 'BOOT_PRIMARY_SLOT'
+             * on a constant basis.
+             * */
 
+#if !defined(MCUBOOT_DEPENDENCY_CHECK) && !defined(MCUBOOT_RAM_LOAD)
+            /* Go over all slots and find the highest version. */
             active_slot = find_slot_with_highest_version(state);
+#else
+            /* Dependecy check feature assumes to load from the first slot */
+            active_slot = BOOT_PRIMARY_SLOT;
+#endif
             if (active_slot == NO_ACTIVE_SLOT) {
                 BOOT_LOG_INF("No slot to load for image %u",
                              (unsigned)BOOT_CURR_IMG(state));
@@ -3320,19 +3417,27 @@
                 boot_remove_image_from_flash(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;
-                continue;
+                /* Since active_slot is set BOOT_PRIMARY_SLOT only, then after its deletion
+                 * no sense to check BOOT_SECONDARY_SLOT. So go outside with an error */
+                BOOT_LOG_ERR("BOOT slot of image %u has been removed from flash",
+                                                          (unsigned)BOOT_CURR_IMG(state));
+                FIH_RET(FIH_FAILURE);
             }
 #endif /* MCUBOOT_RAM_LOAD */
 #ifdef MCUBOOT_VALIDATE_PRIMARY_SLOT
             FIH_CALL(boot_validate_slot, fih_rc, state, active_slot, NULL);
-            if (fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
+            if (!fih_eq(fih_rc, FIH_SUCCESS)) {
                 /* Image is invalid. */
 #ifdef MCUBOOT_RAM_LOAD
                 boot_remove_image_from_sram(state);
 #endif /* MCUBOOT_RAM_LOAD */
                 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;
+                /* Since active_slot is set BOOT_PRIMARY_SLOT only, then after its deletion
+                 * no sense to check BOOT_SECONDARY_SLOT. So go outside with an error */
+                BOOT_LOG_ERR("BOOT slot of image %u has been removed from SRAM",
+                                                           (unsigned)BOOT_CURR_IMG(state));
+                FIH_RET(FIH_FAILURE);
             }
 #endif
             /* Valid image loaded from a slot, go to next image. */
@@ -3404,23 +3509,24 @@
     while (true) {
 #endif
         FIH_CALL(boot_load_and_validate_images, fih_rc, state);
-        if (fih_eq(fih_rc, FIH_SUCCESS) != FIH_TRUE) {
+        if (!fih_eq(fih_rc, FIH_SUCCESS)) {
             goto out;
         }
 
 #if (BOOT_IMAGE_NUMBER > 1)
+#if defined(MCUBOOT_DEPENDENCY_CHECK)
         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
-             * unavailable. Try to load an image from another slot.
-             */
-            continue;
+             * unavailable. */
+            goto out;
         }
         /* Dependency check was successful. */
+#endif /* defined(MCUBOOT_DEPENDENCY_CHECK) */
         break;
     }
-#endif
+#endif /* (BOOT_IMAGE_NUMBER > 1) */
 
     IMAGES_ITER(BOOT_CURR_IMG(state)) {
 #if BOOT_IMAGE_NUMBER > 1
@@ -3449,7 +3555,7 @@
 out:
     close_all_flash_areas(state);
 
-    if (FIH_TRUE == fih_eq(fih_rc, FIH_SUCCESS)) {
+    if (fih_eq(fih_rc, FIH_SUCCESS)) {
         fih_rc = fih_int_encode_zero_equality(rc);
     }
 
diff --git a/boot/bootutil/src/swap_scratch.c b/boot/bootutil/src/swap_scratch.c
index 17d0255..8b55d2a 100644
--- a/boot/bootutil/src/swap_scratch.c
+++ b/boot/bootutil/src/swap_scratch.c
@@ -685,6 +685,11 @@
                               img_off, 0, copy_sz);
         assert(rc == 0);
 
+#if defined(MCUBOOT_ENC_IMAGES) && defined(MCUBOOT_SAVE_ENC_IV)
+        rc = boot_write_iv(fap_primary_slot, state->enc[image_index][1].aes_iv);
+        assert(rc == 0);
+#endif
+
         rc = boot_write_status(state, bs);
         bs->state = BOOT_STATUS_STATE_1;
         BOOT_STATUS_ASSERT(rc == 0);
@@ -706,6 +711,11 @@
             assert(rc == 0);
         }
 
+#if defined(MCUBOOT_ENC_IMAGES) && defined(MCUBOOT_SAVE_ENC_IV)
+        rc = boot_write_iv(fap_primary_slot, state->enc[image_index][1].aes_iv);
+        assert(rc == 0);
+#endif
+
         rc = boot_write_status(state, bs);
         bs->state = BOOT_STATUS_STATE_2;
         BOOT_STATUS_ASSERT(rc == 0);
@@ -767,6 +777,11 @@
         erase_scratch = bs->use_scratch;
         bs->use_scratch = 0;
 
+#if defined(MCUBOOT_ENC_IMAGES) && defined(MCUBOOT_SAVE_ENC_IV)
+        rc = boot_write_iv(fap_primary_slot, state->enc[image_index][1].aes_iv);
+        assert(rc == 0);
+#endif
+
         rc = boot_write_status(state, bs);
         bs->idx++;
         bs->state = BOOT_STATUS_STATE_0;
diff --git a/boot/bootutil/src/swap_status_misc.c b/boot/bootutil/src/swap_status_misc.c
index bc44feb..2794a6f 100644
--- a/boot/bootutil/src/swap_status_misc.c
+++ b/boot/bootutil/src/swap_status_misc.c
@@ -140,17 +140,23 @@
  *
  * @returns 0 on success, -1 on error.
  */
-int
-boot_write_trailer(const struct flash_area *fap, uint32_t off,
-        const uint8_t *inbuf, uint8_t inlen)
+int boot_write_trailer(const struct flash_area *fap, uint32_t off,
+                       const uint8_t *inbuf, uint8_t inlen)
 {
     int rc;
 
-    /* copy status part trailer to primary image before set copy_done flag */
-    if (boot_copy_done_off(fap) == off &&
-        fap->fa_id == FLASH_AREA_IMAGE_PRIMARY(0u) &&
-        BOOT_SWAP_STATUS_COPY_DONE_SZ == inlen) {
+    bool is_primary = false;
 
+    for (uint32_t i = 0u; i < BOOT_IMAGE_NUMBER; i++) {
+        if (fap->fa_id == FLASH_AREA_IMAGE_PRIMARY(i)) {
+            is_primary = true;
+            break;
+        }
+    }
+
+    /* copy status part trailer to primary image before set copy_done flag */
+    if (boot_copy_done_off(fap) == off && is_primary &&
+        BOOT_SWAP_STATUS_COPY_DONE_SZ == inlen) {
         BOOT_LOG_DBG("copy status part trailer to primary image slot");
         rc = swap_status_to_image_trailer(fap);
         if (rc != 0) {
@@ -168,6 +174,43 @@
 }
 
 #ifdef MCUBOOT_ENC_IMAGES
+uint32_t boot_iv_offset(const struct flash_area *fap, uint8_t slot)
+{
+    (void)(slot);
+
+    return boot_enc_key_off(fap, 0) - 32;
+}
+
+int
+boot_read_iv(int image_index, uint8_t slot, uint8_t* dst)
+{
+    uint32_t off;
+    const struct flash_area *fap;
+    int rc;
+
+    rc = boot_find_status(image_index, &fap);
+
+    if (0 == rc) {
+        off = boot_iv_offset(fap, slot);
+
+        rc = swap_status_retrieve(fap->fa_id, off, dst, BOOTUTIL_CRYPTO_AES_CTR_BLOCK_SIZE);
+    }
+
+    return rc;
+}
+
+int
+boot_write_iv(const struct flash_area *fap, uint8_t* iv)
+{
+    int rc;
+    uint32_t off = boot_iv_offset(fap, 0);
+
+    rc = swap_status_update(fap->fa_id, off,
+                            iv, BOOTUTIL_CRYPTO_AES_CTR_BLOCK_SIZE);
+
+    return rc;
+}
+
 int
 boot_write_enc_key(const struct flash_area *fap, uint8_t slot,
         const struct boot_status *bs)
@@ -249,9 +292,9 @@
 int
 boot_clear_magic(const struct flash_area *fap)
 {
-    uint32_t off;
-    int rc;
-    uint8_t tmp[BOOT_MAGIC_SZ];
+    uint32_t off = 0;
+    int rc = -1;
+    uint8_t tmp[BOOT_MAGIC_SZ] = {0};
 
     off = fap->fa_size - BOOT_MAGIC_SZ;
 
@@ -324,110 +367,6 @@
     return rc;
 }
 
-#if defined (MCUBOOT_SWAP_STATUS_FAST_BOOT)
-static inline int
-boot_read_flag(const struct flash_area *fap, uint8_t *flag, uint32_t off)
-{
-    int rc;
-
-    do {
-        rc = flash_area_read(fap, off, flag, sizeof *flag);
-
-        if (rc != 0) {
-            break;
-        }
-
-        if (*flag == flash_area_erased_val(fap)) {
-            *flag = BOOT_FLAG_UNSET;
-        } else {
-            *flag = boot_flag_decode(*flag);
-        }
-
-    } while (false);
-
-    return rc;
-}
-
-static inline uint32_t
-boot_magic_off_trailer(const struct flash_area *fap)
-{
-    return flash_area_get_size(fap) - BOOT_MAGIC_SZ;
-}
-
-static inline uint32_t
-boot_image_ok_off_trailer(const struct flash_area *fap)
-{
-    return ALIGN_DOWN(boot_magic_off_trailer(fap) - BOOT_MAX_ALIGN, BOOT_MAX_ALIGN);
-}
-
-static inline uint32_t
-boot_copy_done_off_trailer(const struct flash_area *fap)
-{
-    return boot_image_ok_off_trailer(fap) - BOOT_MAX_ALIGN;
-}
-
-static inline uint32_t
-boot_swap_info_off_trailer(const struct flash_area *fap)
-{
-    return boot_copy_done_off_trailer(fap) - BOOT_MAX_ALIGN;
-}
-
-int
-boot_read_swap_state_trailer(const struct flash_area *fap,
-                     struct boot_swap_state *state)
-{
-    union boot_img_magic_t magic = {0U};
-    uint32_t off;
-    uint8_t swap_info;
-    int rc;
-
-    do {
-        off = boot_magic_off_trailer(fap);
-        rc = flash_area_read(fap, off, &magic, BOOT_MAGIC_SZ);
-
-        if (rc != 0) {
-            break;
-        }
-
-        if (bootutil_buffer_is_erased(fap, &magic, BOOT_MAGIC_SZ)) {
-            state->magic = BOOT_MAGIC_UNSET;
-        } else {
-            state->magic = boot_magic_decode(&magic);
-        }
-
-        off = boot_swap_info_off_trailer(fap);
-        rc = flash_area_read(fap, off, &swap_info, sizeof swap_info);
-
-        if (rc != 0) {
-            break;
-        }
-
-        /* Extract the swap type and image number */
-        state->swap_type = BOOT_GET_SWAP_TYPE(swap_info);
-        state->image_num = BOOT_GET_IMAGE_NUM(swap_info);
-
-        if (swap_info == flash_area_erased_val(fap) ||
-            state->swap_type > BOOT_SWAP_TYPE_REVERT) {
-            state->swap_type = BOOT_SWAP_TYPE_NONE;
-            state->image_num = 0;
-        }
-
-        off = boot_copy_done_off_trailer(fap);
-        rc = boot_read_flag(fap, &state->copy_done, off);
-
-        if (rc != 0) {
-            break;
-        }
-
-        off = boot_image_ok_off_trailer(fap);
-        rc = boot_read_flag(fap, &state->image_ok, off);
-
-    } while (false);
-
-    return rc;
-}
-#endif
-
 int
 boot_read_swap_state(const struct flash_area *fap,
                      struct boot_swap_state *state)
@@ -445,36 +384,6 @@
 
     const struct flash_area *fap_stat = NULL;
 
-    for (i = 0u; i < (uint32_t)BOOT_IMAGE_NUMBER; i++) {
-        if (fap->fa_id == FLASH_AREA_IMAGE_PRIMARY(i)) {
-            is_primary = true;
-            break;
-        }
-        if (fap->fa_id == FLASH_AREA_IMAGE_SECONDARY(i)) {
-            is_secondary = true;
-            break;
-        }
-    }
-
-#if defined(MCUBOOT_SWAP_STATUS_FAST_BOOT)
-    {
-        bool is_scratch = fap->fa_id == FLASH_AREA_IMAGE_SCRATCH;
-        boot_read_swap_state_trailer(fap, state);
-
-        if (is_primary) {
-            if (state->image_ok == BOOT_FLAG_SET && state->copy_done == BOOT_FLAG_SET && state->magic == BOOT_MAGIC_GOOD) {
-                return 0;
-            }
-        }
-
-        if (is_secondary || is_scratch) {
-            if (state->image_ok == BOOT_FLAG_UNSET && state->copy_done == BOOT_FLAG_UNSET && state->magic == BOOT_MAGIC_UNSET) {
-                return 0;
-            }
-        }
-    }
-#endif
-
     rc = flash_area_open(FLASH_AREA_IMAGE_SWAP_STATUS, &fap_stat);
     if (rc != 0) {
         return -1;
@@ -487,6 +396,17 @@
         return -1;
     }
 
+    for (i = 0u; i < (uint32_t)BOOT_IMAGE_NUMBER; i++) {
+        if (fap->fa_id == FLASH_AREA_IMAGE_PRIMARY(i)) {
+            is_primary = true;
+            break;
+        }
+        if (fap->fa_id == FLASH_AREA_IMAGE_SECONDARY(i)) {
+            is_secondary = true;
+            break;
+        }
+    }
+
     /* fill magic number value if equal to expected */
     if (bootutil_buffer_is_erased(fap_stat, &magic, BOOT_MAGIC_SZ)) {
         state->magic = BOOT_MAGIC_UNSET;
diff --git a/boot/bootutil/src/swap_status_part.c b/boot/bootutil/src/swap_status_part.c
index 9fb7420..6498787 100644
--- a/boot/bootutil/src/swap_status_part.c
+++ b/boot/bootutil/src/swap_status_part.c
@@ -152,46 +152,49 @@
     if (rc != 0) {
         return -1;
     }
-    else {
-        /* loop over copies/duplicates */
-        for (uint32_t i = 0; i < BOOT_SWAP_STATUS_MULT; i++) {
-            /* calculate final duplicate offset */
-            fin_offset = rec_offset + i * BOOT_SWAP_STATUS_D_SIZE;
 
+    /* loop over copies/duplicates */
+    for (uint32_t i = 0; i < BOOT_SWAP_STATUS_MULT; i++) {
+        /* calculate final duplicate offset */
+        fin_offset = rec_offset + i * BOOT_SWAP_STATUS_D_SIZE;
+
+        rc = flash_area_read(fap_stat, fin_offset + BOOT_SWAP_STATUS_PAYLD_SZ, &magic, sizeof(magic));
+        if (rc != 0) {
+            return -1;
+        }
+
+        if (magic == BOOT_SWAP_STATUS_MAGIC) {
             rc = flash_area_read(fap_stat, fin_offset, record_buff, sizeof(record_buff));
             if (rc != 0) {
                 return -1;
             }
-            else {
-                /* read magic value to know if area was pre-erased */
-                magic = pack_bytes_u32(&record_buff[BOOT_SWAP_STATUS_PAYLD_SZ]);
-                if (magic == BOOT_SWAP_STATUS_MAGIC) {   /* read CRC */
-                    crc = pack_bytes_u32(&record_buff[BOOT_SWAP_STATUS_ROW_SZ -
-                                            BOOT_SWAP_STATUS_CRC_SZ]);
-                    /* check record data integrity first */
-                    if (crc == calc_record_crc(record_buff, BOOT_SWAP_STATUS_ROW_SZ-BOOT_SWAP_STATUS_CRC_SZ)) {
-                        /* look for counter */
-                        counter = pack_bytes_u32(&record_buff[BOOT_SWAP_STATUS_ROW_SZ -
-                                                    BOOT_SWAP_STATUS_CNT_SZ -
-                                                    BOOT_SWAP_STATUS_CRC_SZ]);
-                        /* find out counter max */
-                        if (counter >= max_cnt) {
-                            max_cnt = counter;
-                            *max_idx = i;
-                            data_offset = fin_offset;
-                        }
-                    }
-                    /* if crc != calculated() */
-                    else {
-                        crc_fail++;
-                    }
-                }
-                else {
-                    magic_fail++;
+
+            crc = pack_bytes_u32(&record_buff[BOOT_SWAP_STATUS_ROW_SZ -
+                                              BOOT_SWAP_STATUS_CRC_SZ]);
+            /* check record data integrity first */
+            if (crc == calc_record_crc(record_buff, BOOT_SWAP_STATUS_ROW_SZ - BOOT_SWAP_STATUS_CRC_SZ)) {
+                /* look for counter */
+                counter = pack_bytes_u32(&record_buff[BOOT_SWAP_STATUS_ROW_SZ -
+                                                      BOOT_SWAP_STATUS_CNT_SZ -
+                                                      BOOT_SWAP_STATUS_CRC_SZ]);
+                /* find out counter max */
+                if (counter >= max_cnt) {
+                    max_cnt = counter;
+                    *max_idx = i;
+                    data_offset = fin_offset;
                 }
             }
+            /* if crc != calculated() */
+            else {
+                (void) flash_area_erase(fap_stat, fin_offset, sizeof(record_buff));
+                crc_fail++;
+            }
+
+        } else {
+            magic_fail++;
         }
     }
+
     /* no magic found - status area is pre-erased, start from scratch */
     if (magic_fail == BOOT_SWAP_STATUS_MULT) {
         /* emulate last index was received, so next will start from beginning */
@@ -228,10 +231,11 @@
 { /* it receives explicitly BOOT_SWAP_STATUS_PAYLD_SZ of data */
     int rc = -1;
 
-    uint32_t fin_offset;
+    uint32_t fin_offset, fin_offset_prev;
     /* increment counter field */
     uint32_t next_counter = copy_counter + 1U;
     uint32_t next_crc;
+    uint32_t copy_num_prev;
 
     const struct flash_area *fap_stat = NULL;
 
@@ -260,12 +264,15 @@
     /* we already know what copy number was last and correct */
     /* increment duplicate index */
     /* calculate final duplicate offset */
+    copy_num_prev = copy_num;
+
     if (copy_num == (BOOT_SWAP_STATUS_MULT - 1U)) {
         copy_num = 0;
     }
     else {
         copy_num++;
     }
+    fin_offset_prev = rec_offset + copy_num_prev*BOOT_SWAP_STATUS_D_SIZE;
     fin_offset = rec_offset + copy_num*BOOT_SWAP_STATUS_D_SIZE;
 
     /* erase obsolete status record before write */
@@ -276,6 +283,11 @@
 
     /* write prepared record into flash */
     rc = flash_area_write(fap_stat, fin_offset, record_buff, sizeof(record_buff));
+    if (rc != 0) {
+        return -1;
+    }
+
+    rc = flash_area_erase(fap_stat, fin_offset_prev, sizeof(record_buff));
 
     flash_area_close(fap_stat);