diff --git a/docs/getting_started/porting-guide.rst b/docs/getting_started/porting-guide.rst
index 92ff39f..24af13e 100644
--- a/docs/getting_started/porting-guide.rst
+++ b/docs/getting_started/porting-guide.rst
@@ -1195,7 +1195,7 @@
 ::
 
     Argument : unsigned int, image_info_t *
-    Return   : void
+    Return   : int
 
 When the MEASURED_BOOT flag is enabled:
 
@@ -1204,7 +1204,25 @@
 -  On the Arm FVP port, this function measures the given image using its
    passed id and information and then records that measurement in the
    Event Log buffer.
--  This function must return 0 on success, a negative error code otherwise.
+-  This function must return 0 on success, a signed integer error code
+   otherwise.
+
+When the MEASURED_BOOT flag is disabled, this function doesn't do anything.
+
+Function : plat_mboot_measure_critical_data()
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+::
+
+    Argument : unsigned int, const void *, size_t
+    Return   : int
+
+When the MEASURED_BOOT flag is enabled:
+
+-  This function measures the given critical data structure and records its
+   measurement using the measured boot backend driver.
+-  This function must return 0 on success, a signed integer error code
+   otherwise.
 
 When the MEASURED_BOOT flag is disabled, this function doesn't do anything.
 
diff --git a/drivers/auth/crypto_mod.c b/drivers/auth/crypto_mod.c
index c63ff08..127eb0d 100644
--- a/drivers/auth/crypto_mod.c
+++ b/drivers/auth/crypto_mod.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2021, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -114,8 +114,9 @@
  *   data_ptr, data_len: data to be hashed
  *   output: resulting hash
  */
-int crypto_mod_calc_hash(unsigned int alg, void *data_ptr,
-			 unsigned int data_len, unsigned char *output)
+int crypto_mod_calc_hash(enum crypto_md_algo alg, void *data_ptr,
+			 unsigned int data_len,
+			 unsigned char output[CRYPTO_MD_MAX_SIZE])
 {
 	assert(data_ptr != NULL);
 	assert(data_len != 0);
diff --git a/drivers/auth/mbedtls/mbedtls_common.mk b/drivers/auth/mbedtls/mbedtls_common.mk
index 53ebe30..54c819c 100644
--- a/drivers/auth/mbedtls/mbedtls_common.mk
+++ b/drivers/auth/mbedtls/mbedtls_common.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2015-2020, Arm Limited. All rights reserved.
+# Copyright (c) 2015-2021, Arm Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -96,6 +96,18 @@
     TF_MBEDTLS_USE_AES_GCM	:=	0
 endif
 
+ifeq ($(MEASURED_BOOT),1)
+    ifeq (${TPM_HASH_ALG}, sha256)
+        TF_MBEDTLS_TPM_HASH_ALG_ID	:=	TF_MBEDTLS_SHA256
+    else ifeq (${TPM_HASH_ALG}, sha384)
+        TF_MBEDTLS_TPM_HASH_ALG_ID	:=	TF_MBEDTLS_SHA384
+    else ifeq (${TPM_HASH_ALG}, sha512)
+        TF_MBEDTLS_TPM_HASH_ALG_ID	:=	TF_MBEDTLS_SHA512
+    else
+        $(error "TPM_HASH_ALG not defined.")
+    endif
+endif
+
 # Needs to be set to drive mbed TLS configuration correctly
 $(eval $(call add_defines,\
     $(sort \
@@ -105,6 +117,10 @@
         TF_MBEDTLS_USE_AES_GCM \
 )))
 
+ifeq ($(MEASURED_BOOT),1)
+  $(eval $(call add_define,TF_MBEDTLS_TPM_HASH_ALG_ID))
+endif
+
 $(eval $(call MAKE_LIB,mbedtls))
 
 endif
diff --git a/drivers/auth/mbedtls/mbedtls_crypto.c b/drivers/auth/mbedtls/mbedtls_crypto.c
index 6d6efb5..114e6ad 100644
--- a/drivers/auth/mbedtls/mbedtls_crypto.c
+++ b/drivers/auth/mbedtls/mbedtls_crypto.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2021, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -24,6 +24,16 @@
 
 #define LIB_NAME		"mbed TLS"
 
+#if MEASURED_BOOT
+/*
+ * CRYPTO_MD_MAX_SIZE value is as per current stronger algorithm available
+ * so make sure that mbed TLS MD maximum size must be lesser than this.
+ */
+CASSERT(CRYPTO_MD_MAX_SIZE >= MBEDTLS_MD_MAX_SIZE,
+	assert_mbedtls_md_size_overflow);
+
+#endif /* MEASURED_BOOT */
+
 /*
  * AlgorithmIdentifier  ::=  SEQUENCE  {
  *     algorithm               OBJECT IDENTIFIER,
@@ -211,21 +221,45 @@
 
 #if MEASURED_BOOT
 /*
+ * Map a generic crypto message digest algorithm to the corresponding macro used
+ * by Mbed TLS.
+ */
+static inline mbedtls_md_type_t md_type(enum crypto_md_algo algo)
+{
+	switch (algo) {
+	case CRYPTO_MD_SHA512:
+		return MBEDTLS_MD_SHA512;
+	case CRYPTO_MD_SHA384:
+		return MBEDTLS_MD_SHA384;
+	case CRYPTO_MD_SHA256:
+		return MBEDTLS_MD_SHA256;
+	default:
+		/* Invalid hash algorithm. */
+		return MBEDTLS_MD_NONE;
+	}
+}
+
+/*
  * Calculate a hash
  *
  * output points to the computed hash
  */
-int calc_hash(unsigned int alg, void *data_ptr,
-	      unsigned int data_len, unsigned char *output)
+static int calc_hash(enum crypto_md_algo md_algo, void *data_ptr,
+		     unsigned int data_len,
+		     unsigned char output[CRYPTO_MD_MAX_SIZE])
 {
 	const mbedtls_md_info_t *md_info;
 
-	md_info = mbedtls_md_info_from_type((mbedtls_md_type_t)alg);
+	md_info = mbedtls_md_info_from_type(md_type(md_algo));
 	if (md_info == NULL) {
 		return CRYPTO_ERR_HASH;
 	}
 
-	/* Calculate the hash of the data */
+	/*
+	 * Calculate the hash of the data, it is safe to pass the
+	 * 'output' hash buffer pointer considering its size is always
+	 * bigger than or equal to MBEDTLS_MD_MAX_SIZE.
+	 */
 	return mbedtls_md(md_info, data_ptr, data_len, output);
 }
 #endif /* MEASURED_BOOT */
diff --git a/drivers/measured_boot/event_log/event_log.c b/drivers/measured_boot/event_log/event_log.c
index 1755dd9..792f235 100644
--- a/drivers/measured_boot/event_log/event_log.c
+++ b/drivers/measured_boot/event_log/event_log.c
@@ -13,10 +13,19 @@
 #include <common/debug.h>
 #include <drivers/auth/crypto_mod.h>
 #include <drivers/measured_boot/event_log/event_log.h>
-#include <mbedtls/md.h>
 
 #include <plat/common/platform.h>
 
+#if TPM_ALG_ID == TPM_ALG_SHA512
+#define	CRYPTO_MD_ID	CRYPTO_MD_SHA512
+#elif TPM_ALG_ID == TPM_ALG_SHA384
+#define	CRYPTO_MD_ID	CRYPTO_MD_SHA384
+#elif TPM_ALG_ID == TPM_ALG_SHA256
+#define	CRYPTO_MD_ID	CRYPTO_MD_SHA256
+#else
+#  error Invalid TPM algorithm.
+#endif /* TPM_ALG_ID */
+
 /* Running Event Log Pointer */
 static uint8_t *log_ptr;
 
@@ -245,20 +254,20 @@
 int event_log_measure_and_record(uintptr_t data_base, uint32_t data_size,
 				 uint32_t data_id)
 {
-	unsigned char hash_data[MBEDTLS_MD_MAX_SIZE];
+	unsigned char hash_data[CRYPTO_MD_MAX_SIZE];
 	int rc;
 	const event_log_metadata_t *metadata_ptr = plat_metadata_ptr;
 
 	/* Get the metadata associated with this image. */
-	while ((metadata_ptr->id != INVALID_ID) &&
+	while ((metadata_ptr->id != EVLOG_INVALID_ID) &&
 		(metadata_ptr->id != data_id)) {
 		metadata_ptr++;
 	}
-	assert(metadata_ptr->id != INVALID_ID);
+	assert(metadata_ptr->id != EVLOG_INVALID_ID);
 
 	/* Calculate hash */
-	rc = crypto_mod_calc_hash((unsigned int)MBEDTLS_MD_ID,
-				(void *)data_base, data_size, hash_data);
+	rc = crypto_mod_calc_hash(CRYPTO_MD_ID,
+				  (void *)data_base, data_size, hash_data);
 	if (rc != 0) {
 		return rc;
 	}
diff --git a/drivers/measured_boot/event_log/event_log.mk b/drivers/measured_boot/event_log/event_log.mk
index 37e5e29..d3fbbb5 100644
--- a/drivers/measured_boot/event_log/event_log.mk
+++ b/drivers/measured_boot/event_log/event_log.mk
@@ -12,35 +12,24 @@
 TPM_HASH_ALG			:=	sha256
 
 ifeq (${TPM_HASH_ALG}, sha512)
-    MBEDTLS_MD_ID		:=	MBEDTLS_MD_SHA512
     TPM_ALG_ID			:=	TPM_ALG_SHA512
     TCG_DIGEST_SIZE		:=	64U
 else ifeq (${TPM_HASH_ALG}, sha384)
-    MBEDTLS_MD_ID		:=	MBEDTLS_MD_SHA384
     TPM_ALG_ID			:=	TPM_ALG_SHA384
     TCG_DIGEST_SIZE		:=	48U
 else
-    MBEDTLS_MD_ID		:=	MBEDTLS_MD_SHA256
     TPM_ALG_ID			:=	TPM_ALG_SHA256
     TCG_DIGEST_SIZE		:=	32U
-endif
+endif #TPM_HASH_ALG
 
-
-# Set definitions for mbed TLS library and Measured Boot driver
+# Set definitions for Measured Boot driver.
 $(eval $(call add_defines,\
     $(sort \
-        MBEDTLS_MD_ID \
         TPM_ALG_ID \
         TCG_DIGEST_SIZE \
         EVENT_LOG_LEVEL \
 )))
 
-ifeq (${HASH_ALG}, sha256)
-    ifneq (${TPM_HASH_ALG}, sha256)
-        $(eval $(call add_define,MBEDTLS_SHA512_C))
-    endif
-endif
-
 MEASURED_BOOT_SRC_DIR	:= drivers/measured_boot/event_log/
 
 MEASURED_BOOT_SOURCES	:= ${MEASURED_BOOT_SRC_DIR}event_log.c		\
diff --git a/drivers/st/clk/stm32mp1_clk.c b/drivers/st/clk/stm32mp1_clk.c
index 3ebc376..5d4b8fb 100644
--- a/drivers/st/clk/stm32mp1_clk.c
+++ b/drivers/st/clk/stm32mp1_clk.c
@@ -1744,6 +1744,8 @@
 	bool pll4_bootrom = false;
 	const fdt32_t *pkcs_cell;
 	void *fdt;
+	int stgen_p = stm32mp1_clk_get_parent(STGEN_K);
+	int usbphy_p = stm32mp1_clk_get_parent(USBPHY_K);
 
 	if (fdt_get_address(&fdt) == 0) {
 		return -FDT_ERR_NOTFOUND;
@@ -1843,6 +1845,13 @@
 							pllcfg[_PLL4],
 							plloff[_PLL4]);
 	}
+	/* Don't initialize PLL4, when used by BOOTROM */
+	if ((stm32mp_get_boot_itf_selected() ==
+	     BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_USB) &&
+	    ((stgen_p == (int)_PLL4_R) || (usbphy_p == (int)_PLL4_R))) {
+		pll4_bootrom = true;
+		pll4_preserve = true;
+	}
 
 	for (i = (enum stm32mp1_pll_id)0; i < _PLL_NB; i++) {
 		if (((i == _PLL3) && pll3_preserve) ||
@@ -1994,6 +2003,11 @@
 	if (pkcs_cell != NULL) {
 		bool ckper_disabled = false;
 		uint32_t j;
+		uint32_t usbreg_bootrom = 0U;
+
+		if (pll4_bootrom) {
+			usbreg_bootrom = mmio_read_32(rcc_base + RCC_USBCKSELR);
+		}
 
 		for (j = 0; j < ((uint32_t)len / sizeof(uint32_t)); j++) {
 			uint32_t pkcs = fdt32_to_cpu(pkcs_cell[j]);
@@ -2014,6 +2028,25 @@
 		if (ckper_disabled) {
 			stm32mp1_pkcs_config(CLK_CKPER_DISABLED);
 		}
+
+		if (pll4_bootrom) {
+			uint32_t usbreg_value, usbreg_mask;
+			const struct stm32mp1_clk_sel *sel;
+
+			sel = clk_sel_ref(_USBPHY_SEL);
+			usbreg_mask = (uint32_t)sel->msk << sel->src;
+			sel = clk_sel_ref(_USBO_SEL);
+			usbreg_mask |= (uint32_t)sel->msk << sel->src;
+
+			usbreg_value = mmio_read_32(rcc_base + RCC_USBCKSELR) &
+				       usbreg_mask;
+			usbreg_bootrom &= usbreg_mask;
+			if (usbreg_bootrom != usbreg_value) {
+				VERBOSE("forbidden new USB clk path\n");
+				VERBOSE("vs bootrom on USB boot\n");
+				return -FDT_ERR_BADVALUE;
+			}
+		}
 	}
 
 	/* Switch OFF HSI if not found in device-tree */
diff --git a/include/drivers/auth/crypto_mod.h b/include/drivers/auth/crypto_mod.h
index 71cf673..cdcf504 100644
--- a/include/drivers/auth/crypto_mod.h
+++ b/include/drivers/auth/crypto_mod.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2021, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -25,6 +25,16 @@
 	CRYPTO_GCM_DECRYPT = 0
 };
 
+/* Message digest algorithm */
+enum crypto_md_algo {
+	CRYPTO_MD_SHA256,
+	CRYPTO_MD_SHA384,
+	CRYPTO_MD_SHA512,
+};
+
+/* Maximum size as per the known stronger hash algorithm i.e.SHA512 */
+#define CRYPTO_MD_MAX_SIZE		64U
+
 /*
  * Cryptographic library descriptor
  */
@@ -49,8 +59,9 @@
 
 #if MEASURED_BOOT
 	/* Calculate a hash. Return hash value */
-	int (*calc_hash)(unsigned int alg, void *data_ptr,
-			 unsigned int data_len, unsigned char *output);
+	int (*calc_hash)(enum crypto_md_algo md_alg, void *data_ptr,
+			 unsigned int data_len,
+			 unsigned char output[CRYPTO_MD_MAX_SIZE]);
 #endif /* MEASURED_BOOT */
 
 	/*
@@ -79,8 +90,9 @@
 			    unsigned int tag_len);
 
 #if MEASURED_BOOT
-int crypto_mod_calc_hash(unsigned int alg, void *data_ptr,
-			 unsigned int data_len, unsigned char *output);
+int crypto_mod_calc_hash(enum crypto_md_algo alg, void *data_ptr,
+			 unsigned int data_len,
+			 unsigned char output[CRYPTO_MD_MAX_SIZE]);
 
 /* Macro to register a cryptographic library */
 #define REGISTER_CRYPTO_LIB(_name, _init, _verify_signature, _verify_hash, \
diff --git a/include/drivers/auth/mbedtls/mbedtls_config.h b/include/drivers/auth/mbedtls/mbedtls_config.h
index ad39fa9..8ad6d7a 100644
--- a/include/drivers/auth/mbedtls/mbedtls_config.h
+++ b/include/drivers/auth/mbedtls/mbedtls_config.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, Arm Limited. All rights reserved.
+ * Copyright (c) 2015-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -71,9 +71,20 @@
 #endif
 
 #define MBEDTLS_SHA256_C
-#if (TF_MBEDTLS_HASH_ALG_ID != TF_MBEDTLS_SHA256)
+
+/*
+ * If either Trusted Boot or Measured Boot require a stronger algorithm than
+ * SHA-256, pull in SHA-512 support.
+ */
+#if (TF_MBEDTLS_HASH_ALG_ID != TF_MBEDTLS_SHA256) /* TBB hash algo */
+#define	MBEDTLS_SHA512_C
+#else
+   /* TBB uses SHA-256, what about measured boot? */
+#if defined(TF_MBEDTLS_TPM_HASH_ALG_ID) && \
+	(TF_MBEDTLS_TPM_HASH_ALG_ID != TF_MBEDTLS_SHA256)
 #define MBEDTLS_SHA512_C
 #endif
+#endif
 
 #define MBEDTLS_VERSION_C
 
diff --git a/include/drivers/measured_boot/event_log/event_log.h b/include/drivers/measured_boot/event_log/event_log.h
index c6eb29c..a687d41 100644
--- a/include/drivers/measured_boot/event_log/event_log.h
+++ b/include/drivers/measured_boot/event_log/event_log.h
@@ -36,9 +36,9 @@
 #endif
 
 /* Number of hashing algorithms supported */
-#define HASH_ALG_COUNT	1U
+#define HASH_ALG_COUNT		1U
 
-#define	INVALID_ID	MAX_NUMBER_IDS
+#define EVLOG_INVALID_ID	UINT32_MAX
 
 #define MEMBER_SIZE(type, member) sizeof(((type *)0)->member)
 
diff --git a/include/plat/common/common_def.h b/include/plat/common/common_def.h
index 14ae603..632f122 100644
--- a/include/plat/common/common_def.h
+++ b/include/plat/common/common_def.h
@@ -85,4 +85,12 @@
 #endif /* BL2_IN_XIP_MEM */
 #endif /* SEPARATE_CODE_AND_RODATA */
 
+#if MEASURED_BOOT
+/*
+ * Start critical data Ids from 2^32/2 reserving Ids from 0 to (2^32/2 - 1)
+ * for Images, It is a critical data Id base for all platforms.
+ */
+#define CRITICAL_DATA_ID_BASE	U(0x80000000)
+#endif /* MEASURED_BOOT */
+
 #endif /* COMMON_DEF_H */
diff --git a/include/plat/common/platform.h b/include/plat/common/platform.h
index 3fa63f5..9a61b50 100644
--- a/include/plat/common/platform.h
+++ b/include/plat/common/platform.h
@@ -124,12 +124,22 @@
 
 #if MEASURED_BOOT
 int plat_mboot_measure_image(unsigned int image_id, image_info_t *image_data);
+int plat_mboot_measure_critical_data(unsigned int critical_data_id,
+				     const void *base,
+				     size_t size);
 #else
 static inline int plat_mboot_measure_image(unsigned int image_id __unused,
 					   image_info_t *image_data __unused)
 {
 	return 0;
 }
+static inline int plat_mboot_measure_critical_data(
+					unsigned int critical_data_id __unused,
+					const void *base __unused,
+					size_t size __unused)
+{
+	return 0;
+}
 #endif /* MEASURED_BOOT */
 
 /*******************************************************************************
diff --git a/include/services/gtsi_svc.h b/include/services/gtsi_svc.h
index cb942ed..ef4289f 100644
--- a/include/services/gtsi_svc.h
+++ b/include/services/gtsi_svc.h
@@ -14,12 +14,16 @@
 #define GTSI_ERROR_INVALID_PAS		-3
 
 /* The macros below are used to identify GTSI calls from the SMC function ID */
-#define GTSI_FNUM_MIN_VALUE	U(0x100)
-#define GTSI_FNUM_MAX_VALUE	U(0x101)
+#define GTSI_FNUM_MIN_VALUE	U(0x1B0)
+#define GTSI_FNUM_MAX_VALUE	U(0x1B1)
 #define is_gtsi_fid(fid) __extension__ ({		\
 	__typeof__(fid) _fid = (fid);			\
 	((GET_SMC_NUM(_fid) >= GTSI_FNUM_MIN_VALUE) &&	\
-	 (GET_SMC_NUM(_fid) <= GTSI_FNUM_MAX_VALUE)); })
+	(GET_SMC_NUM(_fid) <= GTSI_FNUM_MAX_VALUE)  &&	\
+	(GET_SMC_TYPE(_fid) == SMC_TYPE_FAST)	    &&	\
+	(GET_SMC_CC(_fid) == SMC_64)                &&	\
+	(GET_SMC_OEN(_fid) == OEN_STD_START)        &&	\
+	((_fid & 0x00FE0000) == 0U)); })
 
 /* Get GTSI fastcall std FID from function number */
 #define GTSI_FID(smc_cc, func_num)			\
@@ -28,8 +32,8 @@
 	 (OEN_STD_START << FUNCID_OEN_SHIFT)	|	\
 	 ((func_num) << FUNCID_NUM_SHIFT))
 
-#define GRAN_TRANS_TO_REALM_FNUM	U(0x100)
-#define GRAN_TRANS_TO_NS_FNUM		U(0x101)
+#define GRAN_TRANS_TO_REALM_FNUM	U(0x1B0)
+#define GRAN_TRANS_TO_NS_FNUM		U(0x1B1)
 
 #define SMC_ASC_MARK_REALM	GTSI_FID(SMC_64, GRAN_TRANS_TO_REALM_FNUM)
 #define SMC_ASC_MARK_NONSECURE	GTSI_FID(SMC_64, GRAN_TRANS_TO_NS_FNUM)
diff --git a/include/services/rmi_svc.h b/include/services/rmi_svc.h
index 22f635b..46fd510 100644
--- a/include/services/rmi_svc.h
+++ b/include/services/rmi_svc.h
@@ -17,22 +17,22 @@
 #define RMI_ERROR_INVALID_PAS		-3
 
 /* The macros below are used to identify RMI calls from the SMC function ID */
-#define RMI_FNUM_MIN_VALUE	U(0x00)
-#define RMI_FNUM_MAX_VALUE	U(0x20)
+#define RMI_FNUM_MIN_VALUE	U(0x150)
+#define RMI_FNUM_MAX_VALUE	U(0x18F)
 #define is_rmi_fid(fid) __extension__ ({		\
 	__typeof__(fid) _fid = (fid);			\
 	((GET_SMC_NUM(_fid) >= RMI_FNUM_MIN_VALUE) &&	\
 	 (GET_SMC_NUM(_fid) <= RMI_FNUM_MAX_VALUE) &&	\
 	 (GET_SMC_TYPE(_fid) == SMC_TYPE_FAST)	   &&	\
 	 (GET_SMC_CC(_fid) == SMC_64)              &&	\
-	 (GET_SMC_OEN(_fid) == OEN_ARM_START)      &&	\
+	 (GET_SMC_OEN(_fid) == OEN_STD_START)      &&	\
 	 ((_fid & 0x00FE0000) == 0U)); })
 
 /* Get RMI fastcall std FID from function number */
 #define RMI_FID(smc_cc, func_num)			\
 	((SMC_TYPE_FAST << FUNCID_TYPE_SHIFT)	|	\
 	((smc_cc) << FUNCID_CC_SHIFT)		|	\
-	(OEN_ARM_START << FUNCID_OEN_SHIFT)	|	\
+	(OEN_STD_START << FUNCID_OEN_SHIFT)	|	\
 	((func_num) << FUNCID_NUM_SHIFT))
 
 /*
@@ -41,19 +41,20 @@
  * always invoked by the Normal world, forwarded by RMMD and handled by the
  * RMM
  */
-#define RMI_FNUM_REQ_COMPLETE		U(0x10)
-#define RMI_FNUM_VERSION_REQ		U(0x00)
+#define RMI_FNUM_REQ_COMPLETE		U(0x18F)
+#define RMI_FNUM_VERSION_REQ		U(0x150)
 
-#define RMI_FNUM_GRAN_NS_REALM		U(0x01)
-#define RMI_FNUM_GRAN_REALM_NS		U(0x02)
+#define RMI_FNUM_GRANULE_DELEGATE	U(0x151)
+#define RMI_FNUM_GRANULE_UNDELEGATE	U(0x152)
 
 /* RMI SMC64 FIDs handled by the RMMD */
 #define RMI_RMM_REQ_COMPLETE		RMI_FID(SMC_64, RMI_FNUM_REQ_COMPLETE)
 #define RMI_RMM_REQ_VERSION		RMI_FID(SMC_64, RMI_FNUM_VERSION_REQ)
 
-#define RMI_RMM_GRANULE_DELEGATE	RMI_FID(SMC_64, RMI_FNUM_GRAN_NS_REALM)
-#define RMI_RMM_GRANULE_UNDELEGATE	RMI_FID(SMC_64, RMI_FNUM_GRAN_REALM_NS)
-
+#define RMI_RMM_GRANULE_DELEGATE	RMI_FID(SMC_64, \
+						RMI_FNUM_GRANULE_DELEGATE)
+#define RMI_RMM_GRANULE_UNDELEGATE	RMI_FID(SMC_64, \
+						RMI_FNUM_GRANULE_UNDELEGATE)
 
 #define RMI_ABI_VERSION_GET_MAJOR(_version) ((_version) >> 16)
 #define RMI_ABI_VERSION_GET_MINOR(_version) ((_version) & 0xFFFF)
diff --git a/plat/arm/board/fvp/fvp_bl1_measured_boot.c b/plat/arm/board/fvp/fvp_bl1_measured_boot.c
index 47af1f5..5468555 100644
--- a/plat/arm/board/fvp/fvp_bl1_measured_boot.c
+++ b/plat/arm/board/fvp/fvp_bl1_measured_boot.c
@@ -17,7 +17,8 @@
 	{ FW_CONFIG_ID, EVLOG_FW_CONFIG_STRING, PCR_0 },
 	{ TB_FW_CONFIG_ID, EVLOG_TB_FW_CONFIG_STRING, PCR_0 },
 	{ BL2_IMAGE_ID, EVLOG_BL2_STRING, PCR_0 },
-	{ INVALID_ID, NULL, (unsigned int)(-1) }	/* Terminator */
+
+	{ EVLOG_INVALID_ID, NULL, (unsigned int)(-1) }	/* Terminator */
 };
 
 void bl1_plat_mboot_init(void)
diff --git a/plat/arm/board/fvp/fvp_bl2_measured_boot.c b/plat/arm/board/fvp/fvp_bl2_measured_boot.c
index 5ebfede..4943e58 100644
--- a/plat/arm/board/fvp/fvp_bl2_measured_boot.c
+++ b/plat/arm/board/fvp/fvp_bl2_measured_boot.c
@@ -7,7 +7,11 @@
 #include <stdint.h>
 
 #include <drivers/measured_boot/event_log/event_log.h>
+#include <tools_share/tbbr_oid.h>
+#include <fvp_critical_data.h>
+
 #include <plat/arm/common/plat_arm.h>
+#include <plat/common/common_def.h>
 
 /* Event Log data */
 static uint64_t event_log_base;
@@ -24,7 +28,10 @@
 	{ SCP_BL2_IMAGE_ID, EVLOG_SCP_BL2_STRING, PCR_0 },
 	{ SOC_FW_CONFIG_ID, EVLOG_SOC_FW_CONFIG_STRING, PCR_0 },
 	{ TOS_FW_CONFIG_ID, EVLOG_TOS_FW_CONFIG_STRING, PCR_0 },
-	{ INVALID_ID, NULL, (unsigned int)(-1) }	/* Terminator */
+
+	{ CRITICAL_DATA_ID, EVLOG_CRITICAL_DATA_STRING, PCR_1 },
+
+	{ EVLOG_INVALID_ID, NULL, (unsigned int)(-1) }	/* Terminator */
 };
 
 void bl2_plat_mboot_init(void)
@@ -58,6 +65,60 @@
 	event_log_init((uint8_t *)event_log_start, event_log_finish);
 }
 
+int plat_mboot_measure_critical_data(unsigned int critical_data_id,
+				     const void *base, size_t size)
+{
+	/*
+	 * It is very unlikely that the critical data size would be
+	 * bigger than 2^32 bytes
+	 */
+	assert(size < UINT32_MAX);
+	assert(base != NULL);
+
+	/* Calculate image hash and record data in Event Log */
+	int err = event_log_measure_and_record((uintptr_t)base, (uint32_t)size,
+					       critical_data_id);
+	if (err != 0) {
+		ERROR("%s%s critical data (%i)\n",
+		      "Failed to ", "record",  err);
+		return err;
+	}
+
+	return 0;
+}
+
+static int fvp_populate_critical_data(struct fvp_critical_data *critical_data)
+{
+	char *nv_ctr_oids[MAX_NV_CTR_IDS] = {
+		[TRUSTED_NV_CTR_ID] = TRUSTED_FW_NVCOUNTER_OID,
+		[NON_TRUSTED_NV_CTR_ID] = NON_TRUSTED_FW_NVCOUNTER_OID,
+	};
+
+	for (int i = 0; i < MAX_NV_CTR_IDS; i++) {
+		int rc = plat_get_nv_ctr(nv_ctr_oids[i],
+					 &critical_data->nv_ctr[i]);
+		if (rc != 0) {
+			return rc;
+		}
+	}
+
+	return 0;
+}
+
+static int fvp_populate_and_measure_critical_data(void)
+{
+	struct fvp_critical_data populate_critical_data;
+
+	int rc = fvp_populate_critical_data(&populate_critical_data);
+	if (rc == 0) {
+		rc = plat_mboot_measure_critical_data(CRITICAL_DATA_ID,
+						&populate_critical_data,
+						sizeof(populate_critical_data));
+	}
+
+	return rc;
+}
+
 void bl2_plat_mboot_finish(void)
 {
 	int rc;
@@ -68,6 +129,11 @@
 	/* Event Log filled size */
 	size_t event_log_cur_size;
 
+	rc = fvp_populate_and_measure_critical_data();
+	if (rc != 0) {
+		panic();
+	}
+
 	event_log_cur_size = event_log_get_cur_size((uint8_t *)event_log_base);
 
 	rc = arm_set_nt_fw_info(
diff --git a/plat/arm/board/fvp/include/fvp_critical_data.h b/plat/arm/board/fvp/include/fvp_critical_data.h
new file mode 100644
index 0000000..3010d21
--- /dev/null
+++ b/plat/arm/board/fvp/include/fvp_critical_data.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/nv_cntr_ids.h>
+#include <lib/utils_def.h>
+#include <plat/common/platform.h>
+
+#define EVLOG_CRITICAL_DATA_STRING	"CRITICAL DATA"
+
+#define CRITICAL_DATA_ID		CRITICAL_DATA_ID_BASE
+
+struct fvp_critical_data {
+
+	/* platform NV counters */
+	unsigned int nv_ctr[MAX_NV_CTR_IDS];
+};
diff --git a/plat/arm/common/arm_common.mk b/plat/arm/common/arm_common.mk
index 78efb0f..3236596 100644
--- a/plat/arm/common/arm_common.mk
+++ b/plat/arm/common/arm_common.mk
@@ -389,6 +389,15 @@
 
     $(eval $(call TOOL_ADD_IMG,ns_bl2u,--fwu,FWU_))
 
+# Include Measured Boot makefile before any Crypto library makefile.
+# Crypto library makefile may need default definitions of Measured Boot build
+# flags present in Measured Boot makefile.
+ifeq (${MEASURED_BOOT},1)
+    MEASURED_BOOT_MK := drivers/measured_boot/event_log/event_log.mk
+    $(info Including ${MEASURED_BOOT_MK})
+    include ${MEASURED_BOOT_MK}
+endif
+
     # We expect to locate the *.mk files under the directories specified below
 ifeq (${ARM_CRYPTOCELL_INTEG},0)
     CRYPTO_LIB_MK := drivers/auth/mbedtls/mbedtls_crypto.mk
@@ -411,8 +420,3 @@
     endif
 endif
 
-ifeq (${MEASURED_BOOT},1)
-    MEASURED_BOOT_MK := drivers/measured_boot/event_log/event_log.mk
-    $(info Including ${MEASURED_BOOT_MK})
-    include ${MEASURED_BOOT_MK}
-endif
diff --git a/plat/socionext/synquacer/drivers/scp/sq_scmi.c b/plat/socionext/synquacer/drivers/scp/sq_scmi.c
index e2013cc..0e99256 100644
--- a/plat/socionext/synquacer/drivers/scp/sq_scmi.c
+++ b/plat/socionext/synquacer/drivers/scp/sq_scmi.c
@@ -189,6 +189,11 @@
 /*
  * Helper function to reset the system via SCMI.
  */
+void __dead2 sq_scmi_sys_shutdown(void)
+{
+	sq_scmi_system_off(SCMI_SYS_PWR_SHUTDOWN);
+}
+
 void __dead2 sq_scmi_sys_reboot(void)
 {
 	sq_scmi_system_off(SCMI_SYS_PWR_COLD_RESET);
diff --git a/plat/socionext/synquacer/include/platform_def.h b/plat/socionext/synquacer/include/platform_def.h
index 2f8613a..49ffbf9 100644
--- a/plat/socionext/synquacer/include/platform_def.h
+++ b/plat/socionext/synquacer/include/platform_def.h
@@ -76,6 +76,7 @@
 
 #define SQ_SYS_TIMCTL_BASE		0x2a810000
 #define PLAT_SQ_NSTIMER_FRAME_ID	0
+#define SQ_SYS_CNT_BASE_NS		0x2a830000
 
 #define DRAMINFO_BASE			0x2E00FFC0
 
diff --git a/plat/socionext/synquacer/include/sq_common.h b/plat/socionext/synquacer/include/sq_common.h
index a985822..b09d22a 100644
--- a/plat/socionext/synquacer/include/sq_common.h
+++ b/plat/socionext/synquacer/include/sq_common.h
@@ -45,6 +45,7 @@
 /* SCMI API for power management by SCP */
 void sq_scmi_off(const struct psci_power_state *target_state);
 void sq_scmi_on(u_register_t mpidr);
+void __dead2 sq_scmi_sys_shutdown(void);
 void __dead2 sq_scmi_sys_reboot(void);
 void __dead2 sq_scmi_system_off(int state);
 /* SCMI API for vendor specific protocol */
diff --git a/plat/socionext/synquacer/sq_bl31_setup.c b/plat/socionext/synquacer/sq_bl31_setup.c
index 9723ef9..a7a0ce0 100644
--- a/plat/socionext/synquacer/sq_bl31_setup.c
+++ b/plat/socionext/synquacer/sq_bl31_setup.c
@@ -24,6 +24,20 @@
 IMPORT_SYM(uintptr_t, __SPM_SHIM_EXCEPTIONS_END__,   SPM_SHIM_EXCEPTIONS_END);
 IMPORT_SYM(uintptr_t, __SPM_SHIM_EXCEPTIONS_LMA__,   SPM_SHIM_EXCEPTIONS_LMA);
 
+unsigned int plat_get_syscnt_freq2(void)
+{
+	unsigned int counter_base_frequency;
+
+	/* Read the frequency from Frequency modes table */
+	counter_base_frequency = mmio_read_32(SQ_SYS_CNTCTL_BASE + CNTFID_OFF);
+
+	/* The first entry of the frequency modes table must not be 0 */
+	if (counter_base_frequency == 0)
+		panic();
+
+	return counter_base_frequency;
+}
+
 entry_point_info_t *bl31_plat_get_next_image_ep_info(uint32_t type)
 {
 	assert(sec_state_is_valid(type));
@@ -119,6 +133,7 @@
 static void sq_configure_sys_timer(void)
 {
 	unsigned int reg_val;
+	unsigned int freq_val = plat_get_syscnt_freq2();
 
 	reg_val = (1 << CNTACR_RPCT_SHIFT) | (1 << CNTACR_RVCT_SHIFT);
 	reg_val |= (1 << CNTACR_RFRQ_SHIFT) | (1 << CNTACR_RVOFF_SHIFT);
@@ -128,6 +143,17 @@
 
 	reg_val = (1 << CNTNSAR_NS_SHIFT(PLAT_SQ_NSTIMER_FRAME_ID));
 	mmio_write_32(SQ_SYS_TIMCTL_BASE + CNTNSAR, reg_val);
+
+	/* Initialize CNTFRQ register in CNTCTLBase frame */
+	mmio_write_32(SQ_SYS_TIMCTL_BASE + CNTCTLBASE_CNTFRQ, freq_val);
+
+	/*
+	 * Initialize CNTFRQ register in Non-secure CNTBase frame.
+	 * This is required for SynQuacer, because it does not
+	 * follow ARM ARM in that the value updated in CNTFRQ is not
+	 * reflected in CNTBASEN_CNTFRQ. Hence update the value manually.
+	 */
+	mmio_write_32(SQ_SYS_CNT_BASE_NS + CNTBASEN_CNTFRQ, freq_val);
 }
 
 void bl31_platform_setup(void)
@@ -184,17 +210,3 @@
 {
 	enable_mmu_el3(flags | XLAT_TABLE_NC);
 }
-
-unsigned int plat_get_syscnt_freq2(void)
-{
-	unsigned int counter_base_frequency;
-
-	/* Read the frequency from Frequency modes table */
-	counter_base_frequency = mmio_read_32(SQ_SYS_CNTCTL_BASE + CNTFID_OFF);
-
-	/* The first entry of the frequency modes table must not be 0 */
-	if (counter_base_frequency == 0)
-		panic();
-
-	return counter_base_frequency;
-}
diff --git a/plat/socionext/synquacer/sq_psci.c b/plat/socionext/synquacer/sq_psci.c
index 4168df9..3062f63 100644
--- a/plat/socionext/synquacer/sq_psci.c
+++ b/plat/socionext/synquacer/sq_psci.c
@@ -113,6 +113,9 @@
 
 void __dead2 sq_system_off(void)
 {
+#if SQ_USE_SCMI_DRIVER
+	sq_scmi_sys_shutdown();
+#else
 	volatile uint32_t *gpio = (uint32_t *)PLAT_SQ_GPIO_BASE;
 
 	/* set PD[9] high to power off the system */
@@ -139,6 +142,7 @@
 	wfi();
 	ERROR("SQ System Off: operation not handled.\n");
 	panic();
+#endif
 }
 
 void __dead2 sq_system_reset(void)
diff --git a/services/arm_arch_svc/arm_arch_svc_setup.c b/services/arm_arch_svc/arm_arch_svc_setup.c
index 1d4423c..5523a1c 100644
--- a/services/arm_arch_svc/arm_arch_svc_setup.c
+++ b/services/arm_arch_svc/arm_arch_svc_setup.c
@@ -11,19 +11,9 @@
 #include <lib/cpus/wa_cve_2018_3639.h>
 #include <lib/smccc.h>
 #include <services/arm_arch_svc.h>
-#include <services/rmi_svc.h>
-#include <services/rmmd_svc.h>
 #include <smccc_helpers.h>
 #include <plat/common/platform.h>
 
-#if ENABLE_RME
-/* Setup Arm architecture Services */
-static int32_t arm_arch_svc_setup(void)
-{
-	return rmmd_setup();
-}
-#endif
-
 static int32_t smccc_version(void)
 {
 	return MAKE_SMCCC_VERSION(SMCCC_MAJOR_VERSION, SMCCC_MINOR_VERSION);
@@ -143,16 +133,6 @@
 		SMC_RET0(handle);
 #endif
 	default:
-#if ENABLE_RME
-		/*
-		 * RMI functions are allocated from the Arch service range. Call
-		 * the RMM dispatcher to handle RMI calls.
-		 */
-		if (is_rmi_fid(smc_fid)) {
-			return rmmd_rmi_handler(smc_fid, x1, x2, x3, x4, cookie,
-						handle, flags);
-		}
-#endif
 		WARN("Unimplemented Arm Architecture Service Call: 0x%x \n",
 			smc_fid);
 		SMC_RET1(handle, SMC_UNK);
@@ -165,10 +145,6 @@
 		OEN_ARM_START,
 		OEN_ARM_END,
 		SMC_TYPE_FAST,
-#if ENABLE_RME
-		arm_arch_svc_setup,
-#else
 		NULL,
-#endif
 		arm_arch_svc_smc_handler
 );
diff --git a/services/std_svc/rmmd/rmmd_main.c b/services/std_svc/rmmd/rmmd_main.c
index e9004c7..c4ea706 100644
--- a/services/std_svc/rmmd/rmmd_main.c
+++ b/services/std_svc/rmmd/rmmd_main.c
@@ -30,6 +30,7 @@
 #include <services/rmi_svc.h>
 #include <services/rmmd_svc.h>
 #include <smccc_helpers.h>
+#include <lib/extensions/sve.h>
 #include "rmmd_initial_context.h"
 #include "rmmd_private.h"
 
@@ -112,6 +113,26 @@
 }
 
 /*******************************************************************************
+ * Enable architecture extensions on first entry to Realm world.
+ ******************************************************************************/
+static void manage_extensions_realm(cpu_context_t *ctx)
+{
+#if ENABLE_SVE_FOR_NS
+	/*
+	 * Enable SVE and FPU in realm context when it is enabled for NS.
+	 * Realm manager must ensure that the SVE and FPU register
+	 * contexts are properly managed.
+	 */
+	sve_enable(ctx);
+#else
+	/*
+	 * Disable SVE and FPU in realm context when it is disabled for NS.
+	 */
+	sve_disable(ctx);
+#endif /* ENABLE_SVE_FOR_NS */
+}
+
+/*******************************************************************************
  * Jump to the RMM for the first time.
  ******************************************************************************/
 static int32_t rmm_init(void)
@@ -124,6 +145,9 @@
 	INFO("RMM init start.\n");
 	ctx->state = RMM_STATE_RESET;
 
+	/* Enable architecture extensions */
+	manage_extensions_realm(&ctx->cpu_ctx);
+
 	/* Initialize RMM EL2 context. */
 	rmm_el2_context_init(&ctx->cpu_ctx.el2_sysregs_ctx);
 
@@ -281,6 +305,9 @@
 	/* Initialise RMM context with this entry point information */
 	cm_setup_context(&ctx->cpu_ctx, rmm_ep_info);
 
+	/* Enable architecture extensions */
+	manage_extensions_realm(&ctx->cpu_ctx);
+
 	/* Initialize RMM EL2 context. */
 	rmm_el2_context_init(&ctx->cpu_ctx.el2_sysregs_ctx);
 
diff --git a/services/std_svc/std_svc_setup.c b/services/std_svc/std_svc_setup.c
index 39db429..eea7e14 100644
--- a/services/std_svc/std_svc_setup.c
+++ b/services/std_svc/std_svc_setup.c
@@ -15,6 +15,7 @@
 #include <lib/runtime_instr.h>
 #include <services/gtsi_svc.h>
 #include <services/pci_svc.h>
+#include <services/rmi_svc.h>
 #include <services/rmmd_svc.h>
 #include <services/sdei.h>
 #include <services/spm_mm_svc.h>
@@ -62,6 +63,12 @@
 	}
 #endif
 
+#if ENABLE_RME
+	if (rmmd_setup() != 0) {
+		ret = 1;
+	}
+#endif
+
 #if SDEI_SUPPORT
 	/* SDEI initialisation */
 	sdei_init();
@@ -169,6 +176,11 @@
 		return rmmd_gtsi_handler(smc_fid, x1, x2, x3, x4, cookie,
 						handle, flags);
 	}
+
+	if (is_rmi_fid(smc_fid)) {
+		return rmmd_rmi_handler(smc_fid, x1, x2, x3, x4, cookie,
+					handle, flags);
+	}
 #endif
 
 #if SMC_PCI_SUPPORT