diff --git a/Makefile b/Makefile
index 73007b4..32eb501 100644
--- a/Makefile
+++ b/Makefile
@@ -267,6 +267,16 @@
 ENABLE_FEAT_SB		= 	1
 endif
 
+# Determine and enable FEAT_FGT to access HDFGRTR_EL2 register for v8.6 and higher versions.
+ifeq "8.6" "$(word 1, $(sort 8.6 $(ARM_ARCH_MAJOR).$(ARM_ARCH_MINOR)))"
+ENABLE_FEAT_FGT		=	1
+endif
+
+# Determine and enable FEAT_ECV to access CNTPOFF_EL2 register for v8.6 and higher versions.
+ifeq "8.6" "$(word 1, $(sort 8.6 $(ARM_ARCH_MAJOR).$(ARM_ARCH_MINOR)))"
+ENABLE_FEAT_ECV		=	1
+endif
+
 ifneq ($(findstring armclang,$(notdir $(CC))),)
 TF_CFLAGS_aarch32	=	-target arm-arm-none-eabi $(march32-directive)
 TF_CFLAGS_aarch64	=	-target aarch64-arm-none-eabi $(march64-directive)
@@ -1041,6 +1051,9 @@
         ENABLE_FEAT_HCX \
         ENABLE_MPMM \
         ENABLE_MPMM_FCONF \
+        ENABLE_FEAT_FGT \
+        ENABLE_FEAT_AMUv1 \
+        ENABLE_FEAT_ECV \
 )))
 
 $(eval $(call assert_numerics,\
@@ -1153,6 +1166,9 @@
         ENABLE_FEAT_HCX \
         ENABLE_MPMM \
         ENABLE_MPMM_FCONF \
+        ENABLE_FEAT_FGT \
+        ENABLE_FEAT_AMUv1 \
+        ENABLE_FEAT_ECV \
 )))
 
 ifeq (${SANITIZE_UB},trap)
diff --git a/docs/components/secure-partition-manager.rst b/docs/components/secure-partition-manager.rst
index 4faabf9..a598e5f 100644
--- a/docs/components/secure-partition-manager.rst
+++ b/docs/components/secure-partition-manager.rst
@@ -533,7 +533,10 @@
 
 SP manifests provide an optional boot order attribute meant to resolve
 dependencies such as an SP providing a service required to properly boot
-another SP.
+another SP. SPMC boots the SPs in accordance to the boot order attribute,
+lowest to the highest value. If the boot order attribute is absent from the FF-A
+manifest, the SP is treated as if it had the highest boot order value
+(i.e. lowest booting priority).
 
 It is possible for an SP to call into another SP through a direct request
 provided the latter SP has already been booted.
diff --git a/docs/getting_started/build-options.rst b/docs/getting_started/build-options.rst
index 7662a14..d77875e 100644
--- a/docs/getting_started/build-options.rst
+++ b/docs/getting_started/build-options.rst
@@ -246,6 +246,24 @@
    builds, but this behaviour can be overridden in each platform's Makefile or
    in the build command line.
 
+-  ``ENABLE_FEAT_AMUv1``: Boolean option to enable access to the HAFGRTR_EL2
+   (Hypervisor Activity Monitors Fine-Grained Read Trap Register) during EL2
+   to EL3 context save/restore operations. It is an optional feature available
+   on v8.4 and onwards and must be set to 1 alongside ``ENABLE_FEAT_FGT``, to
+   access the HAFGRTR_EL2 register. Defaults to ``0``.
+
+-  ``ENABLE_FEAT_ECV``: Boolean option to enable support for the Enhanced Counter
+   Virtualization feature, allowing for access to the CNTPOFF_EL2 (Counter-timer
+   Physical Offset register) during EL2 to EL3 context save/restore operations.
+   Its a mandatory architectural feature in Armv8.6 and defaults to ``1`` for
+   v8.6 or later CPUs.
+
+-  ``ENABLE_FEAT_FGT``: Boolean option to enable support for FGT (Fine Grain Traps)
+   feature allowing for access to the HDFGRTR_EL2 (Hypervisor Debug Fine-Grained
+   Read Trap Register)  during EL2 to EL3 context save/restore operations.
+   Its a mandatory architectural feature in Armv8.6 and defaults to ``1`` for
+   v8.6 or later CPUs.
+
 -  ``ENABLE_FEAT_HCX``: This option sets the bit SCR_EL3.HXEn in EL3 to allow
    access to HCRX_EL2 (extended hypervisor control register) from EL2 as well as
    adding HCRX_EL2 to the EL2 context save/restore operations.
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/arm/css/scmi/scmi_private.h b/drivers/arm/css/scmi/scmi_private.h
index 61437f6..a684ca5 100644
--- a/drivers/arm/css/scmi/scmi_private.h
+++ b/drivers/arm/css/scmi/scmi_private.h
@@ -136,7 +136,7 @@
 	uint64_t res_b; /* Reserved */
 	uint32_t flags;
 	volatile uint32_t len;
-	uint32_t msg_header;
+	volatile uint32_t msg_header;
 	uint32_t payload[];
 } mailbox_mem_t;
 
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/marvell/comphy/phy-comphy-3700.c b/drivers/marvell/comphy/phy-comphy-3700.c
index a3e414c..1a97753 100644
--- a/drivers/marvell/comphy/phy-comphy-3700.c
+++ b/drivers/marvell/comphy/phy-comphy-3700.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2018 Marvell International Ltd.
+ * Copyright (C) 2018-2021 Marvell International Ltd.
  *
  * SPDX-License-Identifier:     BSD-3-Clause
  * https://spdx.org/licenses
@@ -301,7 +301,7 @@
 	}
 
 	/* Clear phy isolation mode to make it work in normal mode */
-	offset =  COMPHY_ISOLATION_CTRL_REG + SATAPHY_LANE2_REG_BASE_OFFSET;
+	offset =  COMPHY_ISOLATION_CTRL + SATAPHY_LANE2_REG_BASE_OFFSET;
 	comphy_sata_set_indirect(comphy_indir_regs, offset, 0, PHY_ISOLATE_MODE);
 
 	/* 0. Check the Polarity invert bits */
@@ -310,21 +310,21 @@
 	if (invert & COMPHY_POLARITY_RXD_INVERT)
 		data |= RXD_INVERT_BIT;
 
-	offset = COMPHY_SYNC_PATTERN_REG + SATAPHY_LANE2_REG_BASE_OFFSET;
+	offset = COMPHY_SYNC_PATTERN + SATAPHY_LANE2_REG_BASE_OFFSET;
 	comphy_sata_set_indirect(comphy_indir_regs, offset, data, TXD_INVERT_BIT |
 				 RXD_INVERT_BIT);
 
 	/* 1. Select 40-bit data width width */
-	offset = COMPHY_LOOPBACK_REG0 + SATAPHY_LANE2_REG_BASE_OFFSET;
+	offset = COMPHY_DIG_LOOPBACK_EN + SATAPHY_LANE2_REG_BASE_OFFSET;
 	comphy_sata_set_indirect(comphy_indir_regs, offset, DATA_WIDTH_40BIT,
 				 SEL_DATA_WIDTH_MASK);
 
 	/* 2. Select reference clock(25M) and PHY mode (SATA) */
 	offset = COMPHY_POWER_PLL_CTRL + SATAPHY_LANE2_REG_BASE_OFFSET;
 	if (get_ref_clk() == 40)
-		ref_clk = REF_CLOCK_SPEED_40M;
+		ref_clk = REF_FREF_SEL_SERDES_40MHZ;
 	else
-		ref_clk = REF_CLOCK_SPEED_25M;
+		ref_clk = REF_FREF_SEL_SERDES_25MHZ;
 
 	comphy_sata_set_indirect(comphy_indir_regs, offset, ref_clk | PHY_MODE_SATA,
 				 REF_FREF_SEL_MASK | PHY_MODE_MASK);
@@ -352,7 +352,7 @@
 
 	/* Polling status */
 	mmio_write_32(comphy_indir_regs + COMPHY_LANE2_INDIR_ADDR_OFFSET,
-		      COMPHY_LOOPBACK_REG0 + SATAPHY_LANE2_REG_BASE_OFFSET);
+		      COMPHY_DIG_LOOPBACK_EN + SATAPHY_LANE2_REG_BASE_OFFSET);
 
 	ret = polling_with_timeout(comphy_indir_regs +
 				   COMPHY_LANE2_INDIR_DATA_OFFSET,
@@ -401,8 +401,8 @@
 	 *    PHY TXP/TXN output to idle state during PHY initialization
 	 * 3. Set PHY input port PIN_PU_PLL=0, PIN_PU_RX=0, PIN_PU_TX=0.
 	 */
-	data = PIN_PU_IVEREF_BIT | PIN_TX_IDLE_BIT | PIN_RESET_COMPHY_BIT;
-	mask = PIN_RESET_CORE_BIT | PIN_PU_PLL_BIT | PIN_PU_RX_BIT |
+	data = PIN_PU_IVREF_BIT | PIN_TX_IDLE_BIT | PIN_RESET_COMPHY_BIT;
+	mask = data | PIN_RESET_CORE_BIT | PIN_PU_PLL_BIT | PIN_PU_RX_BIT |
 		PIN_PU_TX_BIT;
 	offset = MVEBU_COMPHY_REG_BASE + COMPHY_PHY_CFG1_OFFSET(comphy_index);
 	reg_set(offset, data, mask);
@@ -422,8 +422,8 @@
 		data |= SD_SPEED_1_25_G << GEN_TX_SEL_OFFSET;
 	} else if (mode == COMPHY_2500BASEX_MODE) {
 		/* 2500Base-X, SerDes speed 3.125G */
-		data |= SD_SPEED_2_5_G << GEN_RX_SEL_OFFSET;
-		data |= SD_SPEED_2_5_G << GEN_TX_SEL_OFFSET;
+		data |= SD_SPEED_3_125_G << GEN_RX_SEL_OFFSET;
+		data |= SD_SPEED_3_125_G << GEN_TX_SEL_OFFSET;
 	} else {
 		/* Other rates are not supported */
 		ERROR("unsupported SGMII speed on comphy lane%d\n",
@@ -450,16 +450,16 @@
 	 */
 	data = 0;
 	mask = PHY_REF_CLK_SEL;
-	reg_set16(SGMIIPHY_ADDR(COMPHY_MISC_REG0_ADDR, sd_ip_addr), data, mask);
+	reg_set16(SGMIIPHY_ADDR(COMPHY_MISC_CTRL0, sd_ip_addr), data, mask);
 
 	/*
 	 * 9. Set correct reference clock frequency in COMPHY register
 	 * REF_FREF_SEL.
 	 */
 	if (get_ref_clk() == 40)
-		data = REF_CLOCK_SPEED_50M;
+		data = REF_FREF_SEL_SERDES_50MHZ;
 	else
-		data = REF_CLOCK_SPEED_25M;
+		data = REF_FREF_SEL_SERDES_25MHZ;
 
 	mask = REF_FREF_SEL_MASK;
 	reg_set16(SGMIIPHY_ADDR(COMPHY_POWER_PLL_CTRL, sd_ip_addr), data, mask);
@@ -477,7 +477,8 @@
 	 */
 	data = DATA_WIDTH_10BIT;
 	mask = SEL_DATA_WIDTH_MASK;
-	reg_set16(SGMIIPHY_ADDR(COMPHY_LOOPBACK_REG0, sd_ip_addr), data, mask);
+	reg_set16(SGMIIPHY_ADDR(COMPHY_DIG_LOOPBACK_EN, sd_ip_addr),
+		  data, mask);
 
 	/*
 	 * 12. As long as DFE function needs to be enabled in any mode,
@@ -523,7 +524,7 @@
 	if (invert & COMPHY_POLARITY_RXD_INVERT)
 		data |= RXD_INVERT_BIT;
 	mask = TXD_INVERT_BIT | RXD_INVERT_BIT;
-	reg_set16(SGMIIPHY_ADDR(COMPHY_SYNC_PATTERN_REG, sd_ip_addr), data, mask);
+	reg_set16(SGMIIPHY_ADDR(COMPHY_SYNC_PATTERN, sd_ip_addr), data, mask);
 
 	/*
 	 * 17. Set PHY input ports PIN_PU_PLL, PIN_PU_TX and PIN_PU_RX to 1 to
@@ -645,68 +646,68 @@
 	 */
 	mask = PRD_TXDEEMPH0_MASK | PRD_TXMARGIN_MASK | PRD_TXSWING_MASK |
 		CFG_TX_ALIGN_POS_MASK;
-	usb3_reg_set(reg_base, COMPHY_REG_LANE_CFG0_ADDR, PRD_TXDEEMPH0_MASK,
-		     mask);
+	usb3_reg_set(reg_base, COMPHY_LANE_CFG0, PRD_TXDEEMPH0_MASK, mask);
 
 	/*
 	 * 2. Set BIT0: enable transmitter in high impedance mode
 	 *    Set BIT[3:4]: delay 2 clock cycles for HiZ off latency
 	 *    Set BIT6: Tx detect Rx at HiZ mode
 	 *    Unset BIT15: set to 0 to set USB3 De-emphasize level to -3.5db
-	 *            together with bit 0 of COMPHY_REG_LANE_CFG0_ADDR register
+	 *            together with bit 0 of COMPHY_LANE_CFG0 register
 	 */
 	mask = PRD_TXDEEMPH1_MASK | TX_DET_RX_MODE | GEN2_TX_DATA_DLY_MASK |
 		TX_ELEC_IDLE_MODE_EN;
 	data = TX_DET_RX_MODE | GEN2_TX_DATA_DLY_DEFT | TX_ELEC_IDLE_MODE_EN;
-	usb3_reg_set(reg_base, COMPHY_REG_LANE_CFG1_ADDR, data, mask);
+	usb3_reg_set(reg_base, COMPHY_LANE_CFG1, data, mask);
 
 	/*
 	 * 3. Set Spread Spectrum Clock Enabled
 	 */
-	usb3_reg_set(reg_base, COMPHY_REG_LANE_CFG4_ADDR,
+	usb3_reg_set(reg_base, COMPHY_LANE_CFG4,
 		     SPREAD_SPECTRUM_CLK_EN, SPREAD_SPECTRUM_CLK_EN);
 
 	/*
 	 * 4. Set Override Margining Controls From the MAC:
 	 *    Use margining signals from lane configuration
 	 */
-	usb3_reg_set(reg_base, COMPHY_REG_TEST_MODE_CTRL_ADDR,
+	usb3_reg_set(reg_base, COMPHY_TEST_MODE_CTRL,
 		     MODE_MARGIN_OVERRIDE, REG_16_BIT_MASK);
 
 	/*
 	 * 5. Set Lane-to-Lane Bundle Clock Sampling Period = per PCLK cycles
 	 *    set Mode Clock Source = PCLK is generated from REFCLK
 	 */
-	usb3_reg_set(reg_base, COMPHY_REG_GLOB_CLK_SRC_LO_ADDR, 0x0,
-		     (MODE_CLK_SRC | BUNDLE_PERIOD_SEL | BUNDLE_PERIOD_SCALE |
-		      BUNDLE_SAMPLE_CTRL | PLL_READY_DLY));
+	usb3_reg_set(reg_base, COMPHY_CLK_SRC_LO, 0x0,
+		     (MODE_CLK_SRC | BUNDLE_PERIOD_SEL |
+		      BUNDLE_PERIOD_SCALE_MASK | BUNDLE_SAMPLE_CTRL |
+		      PLL_READY_DLY_MASK));
 
 	/*
 	 * 6. Set G2 Spread Spectrum Clock Amplitude at 4K
 	 */
-	usb3_reg_set(reg_base, COMPHY_REG_GEN2_SET_2,
-		     G2_TX_SSC_AMP_VALUE_20, G2_TX_SSC_AMP_MASK);
+	usb3_reg_set(reg_base, COMPHY_GEN2_SET2,
+		     GS2_TX_SSC_AMP_VALUE_20, GS2_TX_SSC_AMP_MASK);
 
 	/*
 	 * 7. Unset G3 Spread Spectrum Clock Amplitude
 	 *    set G3 TX and RX Register Master Current Select
 	 */
-	mask = G3_TX_SSC_AMP_MASK | G3_VREG_RXTX_MAS_ISET_MASK |
-		RSVD_PH03FH_6_0_MASK;
-	usb3_reg_set(reg_base, COMPHY_REG_GEN2_SET_3,
-		     G3_VREG_RXTX_MAS_ISET_60U, mask);
+	mask = GS2_TX_SSC_AMP_MASK | GS2_VREG_RXTX_MAS_ISET_MASK |
+		GS2_RSVD_6_0_MASK;
+	usb3_reg_set(reg_base, COMPHY_GEN3_SET2,
+		     GS2_VREG_RXTX_MAS_ISET_60U, mask);
 
 	/*
 	 * 8. Check crystal jumper setting and program the Power and PLL Control
 	 * accordingly Change RX wait
 	 */
 	if (get_ref_clk() == 40) {
-		ref_clk = REF_CLOCK_SPEED_40M;
+		ref_clk = REF_FREF_SEL_PCIE_USB3_40MHZ;
 		cfg = CFG_PM_RXDLOZ_WAIT_12_UNIT;
 
 	} else {
 		/* 25 MHz */
-		ref_clk = USB3_REF_CLOCK_SPEED_25M;
+		ref_clk = REF_FREF_SEL_PCIE_USB3_25MHZ;
 		cfg = CFG_PM_RXDLOZ_WAIT_7_UNIT;
 	}
 
@@ -720,24 +721,24 @@
 	mask = CFG_PM_OSCCLK_WAIT_MASK | CFG_PM_RXDEN_WAIT_MASK |
 		CFG_PM_RXDLOZ_WAIT_MASK;
 	data = CFG_PM_RXDEN_WAIT_1_UNIT  | cfg;
-	usb3_reg_set(reg_base, COMPHY_REG_PWR_MGM_TIM1_ADDR, data, mask);
+	usb3_reg_set(reg_base, COMPHY_PWR_MGM_TIM1, data, mask);
 
 	/*
 	 * 9. Enable idle sync
 	 */
-	data = UNIT_CTRL_DEFAULT_VALUE | IDLE_SYNC_EN;
-	usb3_reg_set(reg_base, COMPHY_REG_UNIT_CTRL_ADDR, data, REG_16_BIT_MASK);
+	data = IDLE_SYNC_EN_DEFAULT_VALUE | IDLE_SYNC_EN;
+	usb3_reg_set(reg_base, COMPHY_IDLE_SYNC_EN, data, REG_16_BIT_MASK);
 
 	/*
 	 * 10. Enable the output of 500M clock
 	 */
-	data = MISC_REG0_DEFAULT_VALUE | CLK500M_EN;
-	usb3_reg_set(reg_base, COMPHY_MISC_REG0_ADDR, data, REG_16_BIT_MASK);
+	data = MISC_CTRL0_DEFAULT_VALUE | CLK500M_EN;
+	usb3_reg_set(reg_base, COMPHY_MISC_CTRL0, data, REG_16_BIT_MASK);
 
 	/*
 	 * 11. Set 20-bit data width
 	 */
-	usb3_reg_set(reg_base, COMPHY_LOOPBACK_REG0, DATA_WIDTH_20BIT,
+	usb3_reg_set(reg_base, COMPHY_DIG_LOOPBACK_EN, DATA_WIDTH_20BIT,
 		     REG_16_BIT_MASK);
 
 	/*
@@ -758,32 +759,31 @@
 		data |= RXD_INVERT_BIT;
 	}
 	mask = TXD_INVERT_BIT | RXD_INVERT_BIT;
-	usb3_reg_set(reg_base, COMPHY_SYNC_PATTERN_REG, data, mask);
+	usb3_reg_set(reg_base, COMPHY_SYNC_PATTERN, data, mask);
 
 	/*
 	 * 14. Set max speed generation to USB3.0 5Gbps
 	 */
-	usb3_reg_set(reg_base, COMPHY_SYNC_MASK_GEN_REG, PHY_GEN_USB3_5G,
+	usb3_reg_set(reg_base, COMPHY_SYNC_MASK_GEN, PHY_GEN_MAX_USB3_5G,
 		     PHY_GEN_MAX_MASK);
 
 	/*
 	 * 15. Set capacitor value for FFE gain peaking to 0xF
 	 */
-	usb3_reg_set(reg_base, COMPHY_REG_GEN3_SETTINGS_3,
-		     COMPHY_GEN_FFE_CAP_SEL_VALUE, COMPHY_GEN_FFE_CAP_SEL_MASK);
+	usb3_reg_set(reg_base, COMPHY_GEN2_SET3,
+		     GS3_FFE_CAP_SEL_VALUE, GS3_FFE_CAP_SEL_MASK);
 
 	/*
 	 * 16. Release SW reset
 	 */
 	data = MODE_CORE_CLK_FREQ_SEL | MODE_PIPE_WIDTH_32 | MODE_REFDIV_BY_4;
-	usb3_reg_set(reg_base, COMPHY_REG_GLOB_PHY_CTRL0_ADDR, data,
-		     REG_16_BIT_MASK);
+	usb3_reg_set(reg_base, COMPHY_RST_CLK_CTRL, data, REG_16_BIT_MASK);
 
 	/* Wait for > 55 us to allow PCLK be enabled */
 	udelay(PLL_SET_DELAY_US);
 
 	if (comphy_index == COMPHY_LANE2) {
-		data = COMPHY_REG_LANE_STATUS1_ADDR + USB3PHY_LANE2_REG_BASE_OFFSET;
+		data = COMPHY_LANE_STAT1 + USB3PHY_LANE2_REG_BASE_OFFSET;
 		mmio_write_32(reg_base + COMPHY_LANE2_INDIR_ADDR_OFFSET,
 			      data);
 
@@ -791,7 +791,7 @@
 		ret = polling_with_timeout(addr, TXDCLK_PCLK_EN, TXDCLK_PCLK_EN,
 					   COMPHY_PLL_TIMEOUT, REG_32BIT);
 	} else {
-		ret = polling_with_timeout(LANE_STATUS1_ADDR(USB3) + reg_base,
+		ret = polling_with_timeout(LANE_STAT1_ADDR(USB3) + reg_base,
 					   TXDCLK_PCLK_EN, TXDCLK_PCLK_EN,
 					   COMPHY_PLL_TIMEOUT, REG_16BIT);
 	}
@@ -826,11 +826,11 @@
 		  USE_MAX_PLL_RATE_EN, USE_MAX_PLL_RATE_EN);
 
 	/* 2. Select 20 bit SERDES interface. */
-	reg_set16(GLOB_CLK_SRC_LO_ADDR(PCIE) + COMPHY_SD_ADDR,
+	reg_set16(CLK_SRC_LO_ADDR(PCIE) + COMPHY_SD_ADDR,
 		  CFG_SEL_20B, CFG_SEL_20B);
 
 	/* 3. Force to use reg setting for PCIe mode */
-	reg_set16(MISC_REG1_ADDR(PCIE) + COMPHY_SD_ADDR,
+	reg_set16(MISC_CTRL1_ADDR(PCIE) + COMPHY_SD_ADDR,
 		  SEL_BITS_PCIE_FORCE, SEL_BITS_PCIE_FORCE);
 
 	/* 4. Change RX wait */
@@ -840,12 +840,12 @@
 		   CFG_PM_RXDLOZ_WAIT_MASK));
 
 	/* 5. Enable idle sync */
-	reg_set16(UNIT_CTRL_ADDR(PCIE) + COMPHY_SD_ADDR,
-		  UNIT_CTRL_DEFAULT_VALUE | IDLE_SYNC_EN, REG_16_BIT_MASK);
+	reg_set16(IDLE_SYNC_EN_ADDR(PCIE) + COMPHY_SD_ADDR,
+		  IDLE_SYNC_EN_DEFAULT_VALUE | IDLE_SYNC_EN, REG_16_BIT_MASK);
 
 	/* 6. Enable the output of 100M/125M/500M clock */
-	reg_set16(MISC_REG0_ADDR(PCIE) + COMPHY_SD_ADDR,
-		  MISC_REG0_DEFAULT_VALUE | CLK500M_EN | TXDCLK_2X_SEL | CLK100M_125M_EN,
+	reg_set16(MISC_CTRL0_ADDR(PCIE) + COMPHY_SD_ADDR,
+		  MISC_CTRL0_DEFAULT_VALUE | CLK500M_EN | TXDCLK_2X_SEL | CLK100M_125M_EN,
 		  REG_16_BIT_MASK);
 
 	/*
@@ -859,9 +859,9 @@
 	 */
 
 	if (get_ref_clk() == 40)
-		ref_clk = REF_CLOCK_SPEED_40M;
+		ref_clk = REF_FREF_SEL_PCIE_USB3_40MHZ;
 	else
-		ref_clk = PCIE_REF_CLOCK_SPEED_25M;
+		ref_clk = REF_FREF_SEL_PCIE_USB3_25MHZ;
 
 	reg_set16(PWR_PLL_CTRL_ADDR(PCIE) + COMPHY_SD_ADDR,
 		  (PU_IVREF_BIT | PU_PLL_BIT | PU_RX_BIT | PU_TX_BIT |
@@ -881,17 +881,17 @@
 		data |= RXD_INVERT_BIT;
 	}
 	mask = TXD_INVERT_BIT | RXD_INVERT_BIT;
-	reg_set16(SYNC_PATTERN_REG_ADDR(PCIE) + COMPHY_SD_ADDR, data, mask);
+	reg_set16(SYNC_PATTERN_ADDR(PCIE) + COMPHY_SD_ADDR, data, mask);
 
 	/* 11. Release SW reset */
-	reg_set16(GLOB_PHY_CTRL0_ADDR(PCIE) + COMPHY_SD_ADDR,
-		  MODE_CORE_CLK_FREQ_SEL | MODE_PIPE_WIDTH_32,
-		  SOFT_RESET | MODE_REFDIV);
+	data = MODE_CORE_CLK_FREQ_SEL | MODE_PIPE_WIDTH_32;
+	mask = data | SOFT_RESET | MODE_REFDIV_MASK;
+	reg_set16(RST_CLK_CTRL_ADDR(PCIE) + COMPHY_SD_ADDR, data, mask);
 
 	/* Wait for > 55 us to allow PCLK be enabled */
 	udelay(PLL_SET_DELAY_US);
 
-	ret = polling_with_timeout(LANE_STATUS1_ADDR(PCIE) + COMPHY_SD_ADDR,
+	ret = polling_with_timeout(LANE_STAT1_ADDR(PCIE) + COMPHY_SD_ADDR,
 				   TXDCLK_PCLK_EN, TXDCLK_PCLK_EN,
 				   COMPHY_PLL_TIMEOUT, REG_16BIT);
 	if (ret) {
@@ -961,7 +961,7 @@
 	debug_enter();
 
 	/* Set phy isolation mode */
-	offset = COMPHY_ISOLATION_CTRL_REG + SATAPHY_LANE2_REG_BASE_OFFSET;
+	offset = COMPHY_ISOLATION_CTRL + SATAPHY_LANE2_REG_BASE_OFFSET;
 	comphy_sata_set_indirect(comphy_indir_regs, offset, PHY_ISOLATE_MODE,
 				 PHY_ISOLATE_MODE);
 
@@ -1025,7 +1025,7 @@
 
 	/* Polling status */
 	mmio_write_32(comphy_indir_regs + COMPHY_LANE2_INDIR_ADDR_OFFSET,
-	       COMPHY_LOOPBACK_REG0 + SATAPHY_LANE2_REG_BASE_OFFSET);
+	       COMPHY_DIG_LOOPBACK_EN + SATAPHY_LANE2_REG_BASE_OFFSET);
 	addr = comphy_indir_regs + COMPHY_LANE2_INDIR_DATA_OFFSET;
 	data = polling_with_timeout(addr, PLL_READY_TX_BIT, PLL_READY_TX_BIT,
 				    COMPHY_PLL_TIMEOUT, REG_32BIT);
diff --git a/drivers/marvell/comphy/phy-comphy-3700.h b/drivers/marvell/comphy/phy-comphy-3700.h
index 94056f1..ed07624 100644
--- a/drivers/marvell/comphy/phy-comphy-3700.h
+++ b/drivers/marvell/comphy/phy-comphy-3700.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2018 Marvell International Ltd.
+ * Copyright (C) 2018-2021 Marvell International Ltd.
  *
  * SPDX-License-Identifier:     BSD-3-Clause
  * https://spdx.org/licenses
@@ -13,11 +13,11 @@
 #define REG_16_BIT_MASK				0xFFFF
 
 #define COMPHY_SELECTOR_PHY_REG			0xFC
-/* bit0: 0: Lane0 is GBE0; 1: Lane1 is PCIE */
+/* bit0: 0: Lane1 is GbE0; 1: Lane1 is PCIE */
 #define COMPHY_SELECTOR_PCIE_GBE0_SEL_BIT	BIT(0)
-/* bit4: 0: Lane1 is GBE1; 1: Lane1 is USB3 */
+/* bit4: 0: Lane0 is GbE1; 1: Lane0 is USB3 */
 #define COMPHY_SELECTOR_USB3_GBE1_SEL_BIT	BIT(4)
-/* bit8: 0: Lane1 is USB, Lane2 is SATA; 1: Lane2 is USB3 */
+/* bit8: 0: Lane0 is USB3 instead of GbE1, Lane2 is SATA; 1: Lane2 is USB3 */
 #define COMPHY_SELECTOR_USB3_PHY_SEL_BIT	BIT(8)
 
 /* SATA PHY register offset */
@@ -53,12 +53,11 @@
 #define PLL_LOCK_BIT			BIT(8)
 #define REF_FREF_SEL_OFFSET		0
 #define REF_FREF_SEL_MASK		(0x1F << REF_FREF_SEL_OFFSET)
-#define REF_CLOCK_SPEED_25M		(0x1 << REF_FREF_SEL_OFFSET)
-#define REF_CLOCK_SPEED_30M		(0x2 << REF_FREF_SEL_OFFSET)
-#define PCIE_REF_CLOCK_SPEED_25M	REF_CLOCK_SPEED_30M
-#define USB3_REF_CLOCK_SPEED_25M	REF_CLOCK_SPEED_30M
-#define REF_CLOCK_SPEED_40M		(0x3 << REF_FREF_SEL_OFFSET)
-#define REF_CLOCK_SPEED_50M		(0x4 << REF_FREF_SEL_OFFSET)
+#define REF_FREF_SEL_SERDES_25MHZ	(0x1 << REF_FREF_SEL_OFFSET)
+#define REF_FREF_SEL_SERDES_40MHZ	(0x3 << REF_FREF_SEL_OFFSET)
+#define REF_FREF_SEL_SERDES_50MHZ	(0x4 << REF_FREF_SEL_OFFSET)
+#define REF_FREF_SEL_PCIE_USB3_25MHZ	(0x2 << REF_FREF_SEL_OFFSET)
+#define REF_FREF_SEL_PCIE_USB3_40MHZ	(0x3 << REF_FREF_SEL_OFFSET)
 #define PHY_MODE_OFFSET			5
 #define PHY_MODE_MASK			(7 << PHY_MODE_OFFSET)
 #define PHY_MODE_SATA			(0x0 << PHY_MODE_OFFSET)
@@ -73,11 +72,9 @@
 #define SPEED_PLL_MASK			(0x3F << SPEED_PLL_OFFSET)
 #define SPEED_PLL_VALUE_16		(0x10 << SPEED_PLL_OFFSET)
 
-#define COMPHY_RESERVED_REG		0x0E
-#define PHYCTRL_FRM_PIN_BIT		BIT(13)
-
-#define COMPHY_LOOPBACK_REG0		0x23
-#define DIG_LB_EN_ADDR(unit)		(COMPHY_LOOPBACK_REG0 * PHY_SHFT(unit))
+#define COMPHY_DIG_LOOPBACK_EN		0x23
+#define DIG_LOOPBACK_EN_ADDR(unit)	(COMPHY_DIG_LOOPBACK_EN * \
+					 PHY_SHFT(unit))
 #define SEL_DATA_WIDTH_OFFSET		10
 #define SEL_DATA_WIDTH_MASK		(0x3 << SEL_DATA_WIDTH_OFFSET)
 #define DATA_WIDTH_10BIT		(0x0 << SEL_DATA_WIDTH_OFFSET)
@@ -85,80 +82,71 @@
 #define DATA_WIDTH_40BIT		(0x2 << SEL_DATA_WIDTH_OFFSET)
 #define PLL_READY_TX_BIT		BIT(4)
 
-#define COMPHY_SYNC_PATTERN_REG		0x24
-#define SYNC_PATTERN_REG_ADDR(unit)	(COMPHY_SYNC_PATTERN_REG * \
-					 PHY_SHFT(unit))
+#define COMPHY_SYNC_PATTERN		0x24
+#define SYNC_PATTERN_ADDR(unit)		(COMPHY_SYNC_PATTERN * PHY_SHFT(unit))
 #define TXD_INVERT_BIT			BIT(10)
 #define RXD_INVERT_BIT			BIT(11)
 
-#define COMPHY_SYNC_MASK_GEN_REG	0x25
+#define COMPHY_SYNC_MASK_GEN		0x25
 #define PHY_GEN_MAX_OFFSET		10
 #define PHY_GEN_MAX_MASK		(3 << PHY_GEN_MAX_OFFSET)
-#define PHY_GEN_USB3_5G			(1 << PHY_GEN_MAX_OFFSET)
+#define PHY_GEN_MAX_USB3_5G		(1 << PHY_GEN_MAX_OFFSET)
 
-#define COMPHY_ISOLATION_CTRL_REG	0x26
-#define ISOLATION_CTRL_REG_ADDR(unit)	(COMPHY_ISOLATION_CTRL_REG * \
-					 PHY_SHFT(unit))
+#define COMPHY_ISOLATION_CTRL		0x26
+#define ISOLATION_CTRL_ADDR(unit)	(COMPHY_ISOLATION_REG * PHY_SHFT(unit))
 #define PHY_ISOLATE_MODE		BIT(15)
 
+#define COMPHY_GEN2_SET2		0x3e
+#define GEN2_SET2_ADDR(unit)		(COMPHY_GEN2_SET2 * PHY_SHFT(unit))
+#define GS2_TX_SSC_AMP_VALUE_20		BIT(14)
+#define GS2_TX_SSC_AMP_OFF		9
+#define GS2_TX_SSC_AMP_LEN		7
+#define GS2_TX_SSC_AMP_MASK		(((1 << GS2_TX_SSC_AMP_LEN) - 1) << \
+					 GS2_TX_SSC_AMP_OFF)
+#define GS2_VREG_RXTX_MAS_ISET_OFF	7
+#define GS2_VREG_RXTX_MAS_ISET_60U	(0 << GS2_VREG_RXTX_MAS_ISET_OFF)
+#define GS2_VREG_RXTX_MAS_ISET_80U	(1 << GS2_VREG_RXTX_MAS_ISET_OFF)
+#define GS2_VREG_RXTX_MAS_ISET_100U	(2 << GS2_VREG_RXTX_MAS_ISET_OFF)
+#define GS2_VREG_RXTX_MAS_ISET_120U	(3 << GS2_VREG_RXTX_MAS_ISET_OFF)
+#define GS2_VREG_RXTX_MAS_ISET_MASK	(BIT(7) | BIT(8))
+#define GS2_RSVD_6_0_OFF		0
+#define GS2_RSVD_6_0_LEN		7
+#define GS2_RSVD_6_0_MASK		(((1 << GS2_RSVD_6_0_LEN) - 1) << \
+					 GS2_RSVD_6_0_OFF)
+
-#define COMPHY_MISC_REG0_ADDR		0x4F
-#define MISC_REG0_ADDR(unit)		(COMPHY_MISC_REG0_ADDR * PHY_SHFT(unit))
+#define COMPHY_GEN3_SET2		0x3f
+#define GEN3_SET2_ADDR(unit)		(COMPHY_GEN3_SET2 * PHY_SHFT(unit))
+
+#define COMPHY_IDLE_SYNC_EN		0x48
+#define IDLE_SYNC_EN_ADDR(unit)		(COMPHY_IDLE_SYNC_EN * PHY_SHFT(unit))
+#define IDLE_SYNC_EN			BIT(12)
+#define IDLE_SYNC_EN_DEFAULT_VALUE	0x60
+
+#define COMPHY_MISC_CTRL0		0x4F
+#define MISC_CTRL0_ADDR(unit)		(COMPHY_MISC_CTRL0 * PHY_SHFT(unit))
 #define CLK100M_125M_EN			BIT(4)
 #define TXDCLK_2X_SEL			BIT(6)
 #define CLK500M_EN			BIT(7)
 #define PHY_REF_CLK_SEL			BIT(10)
-#define MISC_REG0_DEFAULT_VALUE		0xA00D
+#define MISC_CTRL0_DEFAULT_VALUE	0xA00D
 
-#define COMPHY_REG_GEN2_SET_2	0x3e
-#define GEN2_SETTING_2_ADDR(unit)	(COMPHY_REG_GEN2_SET_2 * PHY_SHFT(unit))
-#define G2_TX_SSC_AMP_VALUE_20		BIT(14)
-#define G2_TX_SSC_AMP_OFF		9
-#define G2_TX_SSC_AMP_LEN		7
-#define G2_TX_SSC_AMP_MASK		(((1 << G2_TX_SSC_AMP_LEN) - 1) << \
-					 G2_TX_SSC_AMP_OFF)
-
-#define COMPHY_REG_GEN2_SET_3	0x3f
-#define GEN2_SETTING_3_ADDR(unit)	(COMPHY_REG_GEN2_SET_3 * PHY_SHFT(unit))
-#define G3_TX_SSC_AMP_OFF		9
-#define G3_TX_SSC_AMP_LEN		7
-#define G3_TX_SSC_AMP_MASK		(((1 << G2_TX_SSC_AMP_LEN) - 1) << \
-					 G2_TX_SSC_AMP_OFF)
-#define G3_VREG_RXTX_MAS_ISET_OFF	7
-#define G3_VREG_RXTX_MAS_ISET_60U	(0 << G3_VREG_RXTX_MAS_ISET_OFF)
-#define G3_VREG_RXTX_MAS_ISET_80U	(1 << G3_VREG_RXTX_MAS_ISET_OFF)
-#define G3_VREG_RXTX_MAS_ISET_100U	(2 << G3_VREG_RXTX_MAS_ISET_OFF)
-#define G3_VREG_RXTX_MAS_ISET_120U	(3 << G3_VREG_RXTX_MAS_ISET_OFF)
-#define G3_VREG_RXTX_MAS_ISET_MASK	(BIT(7) | BIT(8))
-#define RSVD_PH03FH_6_0_OFF		0
-#define RSVD_PH03FH_6_0_LEN		7
-#define RSVD_PH03FH_6_0_MASK		(((1 << RSVD_PH03FH_6_0_LEN) - 1) << \
-					 RSVD_PH03FH_6_0_OFF)
-
-#define COMPHY_REG_UNIT_CTRL_ADDR	0x48
-#define UNIT_CTRL_ADDR(unit)		(COMPHY_REG_UNIT_CTRL_ADDR * \
-					 PHY_SHFT(unit))
-#define IDLE_SYNC_EN			BIT(12)
-#define UNIT_CTRL_DEFAULT_VALUE		0x60
-
-#define COMPHY_MISC_REG1_ADDR		0x73
-#define MISC_REG1_ADDR(unit)		(COMPHY_MISC_REG1_ADDR * PHY_SHFT(unit))
+#define COMPHY_MISC_CTRL1		0x73
+#define MISC_CTRL1_ADDR(unit)		(COMPHY_MISC_CTRL1 * PHY_SHFT(unit))
 #define SEL_BITS_PCIE_FORCE		BIT(15)
 
-#define COMPHY_REG_GEN3_SETTINGS_3	0x112
-#define COMPHY_GEN_FFE_CAP_SEL_MASK	0xF
-#define COMPHY_GEN_FFE_CAP_SEL_VALUE	0xF
+#define COMPHY_GEN2_SET3		0x112
+#define GS3_FFE_CAP_SEL_MASK		0xF
+#define GS3_FFE_CAP_SEL_VALUE		0xF
 
-#define COMPHY_REG_LANE_CFG0_ADDR	0x180
-#define LANE_CFG0_ADDR(unit)		(COMPHY_REG_LANE_CFG0_ADDR * \
-					 PHY_SHFT(unit))
+#define COMPHY_LANE_CFG0		0x180
+#define LANE_CFG0_ADDR(unit)		(COMPHY_LANE_CFG0 * PHY_SHFT(unit))
 #define PRD_TXDEEMPH0_MASK		BIT(0)
 #define PRD_TXMARGIN_MASK		(BIT(1) | BIT(2) | BIT(3))
 #define PRD_TXSWING_MASK		BIT(4)
 #define CFG_TX_ALIGN_POS_MASK		(BIT(5) | BIT(6) | BIT(7) | BIT(8))
 
-#define COMPHY_REG_LANE_CFG1_ADDR	0x181
-#define LANE_CFG1_ADDR(unit)		(COMPHY_REG_LANE_CFG1_ADDR * \
-					 PHY_SHFT(unit))
+#define COMPHY_LANE_CFG1		0x181
+#define LANE_CFG1_ADDR(unit)		(COMPHY_LANE_CFG1 * PHY_SHFT(unit))
 #define PRD_TXDEEMPH1_MASK		BIT(15)
 #define USE_MAX_PLL_RATE_EN		BIT(9)
 #define TX_DET_RX_MODE			BIT(6)
@@ -166,21 +154,17 @@
 #define GEN2_TX_DATA_DLY_DEFT		(2 << 3)
 #define TX_ELEC_IDLE_MODE_EN		BIT(0)
 
-#define COMPHY_REG_LANE_STATUS1_ADDR	0x183
-#define LANE_STATUS1_ADDR(unit)		(COMPHY_REG_LANE_STATUS1_ADDR * \
-					 PHY_SHFT(unit))
+#define COMPHY_LANE_STAT1		0x183
+#define LANE_STAT1_ADDR(unit)		(COMPHY_LANE_STAT1 * PHY_SHFT(unit))
 #define TXDCLK_PCLK_EN				BIT(0)
 
-#define COMPHY_REG_LANE_CFG4_ADDR	0x188
-#define LANE_CFG4_ADDR(unit)		(COMPHY_REG_LANE_CFG4_ADDR * \
-					 PHY_SHFT(unit))
+#define COMPHY_LANE_CFG4		0x188
+#define LANE_CFG4_ADDR(unit)		(COMPHY_LANE_CFG4 * PHY_SHFT(unit))
 #define SPREAD_SPECTRUM_CLK_EN		BIT(7)
 
-#define COMPHY_REG_GLOB_PHY_CTRL0_ADDR	0x1C1
-#define GLOB_PHY_CTRL0_ADDR(unit)	(COMPHY_REG_GLOB_PHY_CTRL0_ADDR * \
-					 PHY_SHFT(unit))
+#define COMPHY_RST_CLK_CTRL		0x1C1
+#define RST_CLK_CTRL_ADDR(unit)		(COMPHY_RST_CLK_CTRL * PHY_SHFT(unit))
 #define SOFT_RESET			BIT(0)
-#define MODE_REFDIV			0x30
 #define MODE_CORE_CLK_FREQ_SEL		BIT(9)
 #define MODE_PIPE_WIDTH_32		BIT(3)
 #define MODE_REFDIV_OFFSET		4
@@ -188,24 +172,21 @@
 #define MODE_REFDIV_MASK		(0x3 << MODE_REFDIV_OFFSET)
 #define MODE_REFDIV_BY_4		(0x2 << MODE_REFDIV_OFFSET)
 
-#define COMPHY_REG_TEST_MODE_CTRL_ADDR	0x1C2
-#define TEST_MODE_CTRL_ADDR(unit)	(COMPHY_REG_TEST_MODE_CTRL_ADDR * \
-					 PHY_SHFT(unit))
+#define COMPHY_TEST_MODE_CTRL		0x1C2
+#define TEST_MODE_CTRL_ADDR(unit)	(COMPHY_TEST_MODE_CTRL * PHY_SHFT(unit))
 #define MODE_MARGIN_OVERRIDE		BIT(2)
 
-#define COMPHY_REG_GLOB_CLK_SRC_LO_ADDR	0x1C3
-#define GLOB_CLK_SRC_LO_ADDR(unit)	(COMPHY_REG_GLOB_CLK_SRC_LO_ADDR * \
-					 PHY_SHFT(unit))
+#define COMPHY_CLK_SRC_LO		0x1C3
+#define CLK_SRC_LO_ADDR(unit)		(COMPHY_CLK_SRC_LO * PHY_SHFT(unit))
 #define MODE_CLK_SRC			BIT(0)
 #define BUNDLE_PERIOD_SEL		BIT(1)
-#define BUNDLE_PERIOD_SCALE		(BIT(2) | BIT(3))
+#define BUNDLE_PERIOD_SCALE_MASK	(BIT(2) | BIT(3))
 #define BUNDLE_SAMPLE_CTRL		BIT(4)
-#define PLL_READY_DLY			(BIT(5) | BIT(6) | BIT(7))
+#define PLL_READY_DLY_MASK		(BIT(5) | BIT(6) | BIT(7))
 #define CFG_SEL_20B			BIT(15)
 
-#define COMPHY_REG_PWR_MGM_TIM1_ADDR	0x1D0
-#define PWR_MGM_TIM1_ADDR(unit)		(COMPHY_REG_PWR_MGM_TIM1_ADDR * \
-					 PHY_SHFT(unit))
+#define COMPHY_PWR_MGM_TIM1		0x1D0
+#define PWR_MGM_TIM1_ADDR(unit)		(COMPHY_PWR_MGM_TIM1 * PHY_SHFT(unit))
 #define CFG_PM_OSCCLK_WAIT_OFF		12
 #define CFG_PM_OSCCLK_WAIT_LEN		4
 #define CFG_PM_OSCCLK_WAIT_MASK		(((1 << CFG_PM_OSCCLK_WAIT_LEN) - 1) \
@@ -222,9 +203,18 @@
 #define CFG_PM_RXDLOZ_WAIT_7_UNIT	(7 << CFG_PM_RXDLOZ_WAIT_OFF)
 #define CFG_PM_RXDLOZ_WAIT_12_UNIT	(0xC << CFG_PM_RXDLOZ_WAIT_OFF)
 
+/*
+ * This register is not from PHY lane register space. It only exists in the
+ * indirect register space, before the actual PHY lane 2 registers. So the
+ * offset is absolute, not relative to SATAPHY_LANE2_REG_BASE_OFFSET.
+ * It is used only for SATA PHY initialization.
+ */
+#define COMPHY_RESERVED_REG		0x0E
+#define PHYCTRL_FRM_PIN_BIT		BIT(13)
+
 /* SGMII */
 #define COMPHY_PHY_CFG1_OFFSET(lane)	((1 - (lane)) * 0x28)
-#define PIN_PU_IVEREF_BIT		BIT(1)
+#define PIN_PU_IVREF_BIT		BIT(1)
 #define PIN_RESET_CORE_BIT		BIT(11)
 #define PIN_RESET_COMPHY_BIT		BIT(12)
 #define PIN_PU_PLL_BIT			BIT(16)
@@ -237,11 +227,11 @@
 #define GEN_TX_SEL_MASK			(0xF << GEN_TX_SEL_OFFSET)
 #define PHY_RX_INIT_BIT			BIT(30)
 #define SD_SPEED_1_25_G			0x6
-#define SD_SPEED_2_5_G			0x8
+#define SD_SPEED_3_125_G		0x8
 
 /* COMPHY status reg:
- * lane0: PCIe/GbE0 PHY Status 1
- * lane1: USB3/GbE1 PHY Status 1
+ * lane0: USB3/GbE1 PHY Status 1
+ * lane1: PCIe/GbE0 PHY Status 1
  */
 #define COMPHY_PHY_STATUS_OFFSET(lane)	(0x18 + (1 - (lane)) * 0x28)
 #define PHY_RX_INIT_DONE_BIT		BIT(0)
diff --git a/drivers/marvell/comphy/phy-comphy-common.h b/drivers/marvell/comphy/phy-comphy-common.h
index c599437..ba5d255 100644
--- a/drivers/marvell/comphy/phy-comphy-common.h
+++ b/drivers/marvell/comphy/phy-comphy-common.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2018 Marvell International Ltd.
+ * Copyright (C) 2018-2021 Marvell International Ltd.
  *
  * SPDX-License-Identifier:     BSD-3-Clause
  * https://spdx.org/licenses
@@ -147,7 +147,7 @@
 	debug("<atf>: WR to addr = 0x%lx, data = 0x%x (mask = 0x%x) - ",
 	      addr, data, mask);
 	debug("old value = 0x%x ==> ", mmio_read_32(addr));
-	mmio_clrsetbits_32(addr, mask, data);
+	mmio_clrsetbits_32(addr, mask, data & mask);
 
 	debug("new val 0x%x\n", mmio_read_32(addr));
 }
@@ -159,7 +159,7 @@
 	debug("<atf>: WR to addr = 0x%lx, data = 0x%x (mask = 0x%x) - ",
 	      addr, data, mask);
 	debug("old value = 0x%x ==> ", mmio_read_16(addr));
-	mmio_clrsetbits_16(addr, mask, data);
+	mmio_clrsetbits_16(addr, mask, data & mask);
 
 	debug("new val 0x%x\n", mmio_read_16(addr));
 }
diff --git a/drivers/measured_boot/event_log/event_log.c b/drivers/measured_boot/event_log/event_log.c
index 52ed278..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,7 +254,7 @@
 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;
 
@@ -257,8 +266,8 @@
 	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/drivers/st/gpio/stm32_gpio.c b/drivers/st/gpio/stm32_gpio.c
index 7d63262..51276f4 100644
--- a/drivers/st/gpio/stm32_gpio.c
+++ b/drivers/st/gpio/stm32_gpio.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2020, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2016-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -283,3 +283,10 @@
 
 	stm32mp_clk_disable(clock);
 }
+
+void set_gpio_reset_cfg(uint32_t bank, uint32_t pin)
+{
+	set_gpio(bank, pin, GPIO_MODE_ANALOG, GPIO_SPEED_LOW,
+		 GPIO_NO_PULL, GPIO_ALTERNATE_(0), DT_DISABLED);
+	set_gpio_secure_cfg(bank, pin, stm32_gpio_is_secure_at_reset(bank));
+}
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 78712af..a687d41 100644
--- a/include/drivers/measured_boot/event_log/event_log.h
+++ b/include/drivers/measured_boot/event_log/event_log.h
@@ -38,7 +38,7 @@
 /* Number of hashing algorithms supported */
 #define HASH_ALG_COUNT		1U
 
-#define EVLOG_INVALID_ID	MAX_NUMBER_IDS
+#define EVLOG_INVALID_ID	UINT32_MAX
 
 #define MEMBER_SIZE(type, member) sizeof(((type *)0)->member)
 
diff --git a/include/drivers/st/stm32_gpio.h b/include/drivers/st/stm32_gpio.h
index e241f58..b072345 100644
--- a/include/drivers/st/stm32_gpio.h
+++ b/include/drivers/st/stm32_gpio.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2019, STMicroelectronics - All Rights Reserved
+ * Copyright (c) 2015-2021, STMicroelectronics - All Rights Reserved
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -52,6 +52,7 @@
 void set_gpio(uint32_t bank, uint32_t pin, uint32_t mode, uint32_t speed,
 	      uint32_t pull, uint32_t alternate, uint8_t status);
 void set_gpio_secure_cfg(uint32_t bank, uint32_t pin, bool secure);
+void set_gpio_reset_cfg(uint32_t bank, uint32_t pin);
 #endif /*__ASSEMBLER__*/
 
 #endif /* STM32_GPIO_H */
diff --git a/include/lib/el3_runtime/aarch64/context.h b/include/lib/el3_runtime/aarch64/context.h
index 698e208..512d196 100644
--- a/include/lib/el3_runtime/aarch64/context.h
+++ b/include/lib/el3_runtime/aarch64/context.h
@@ -207,8 +207,8 @@
 #define CTX_MPAMVPMV_EL2	U(0x158)
 
 // Starting with Armv8.6
-#define CTX_HAFGRTR_EL2		U(0x160)
-#define CTX_HDFGRTR_EL2		U(0x168)
+#define CTX_HDFGRTR_EL2		U(0x160)
+#define CTX_HAFGRTR_EL2		U(0x168)
 #define CTX_HDFGWTR_EL2		U(0x170)
 #define CTX_HFGITR_EL2		U(0x178)
 #define CTX_HFGRTR_EL2		U(0x180)
diff --git a/include/lib/fconf/fconf_dyn_cfg_getter.h b/include/lib/fconf/fconf_dyn_cfg_getter.h
index 6f8da0d..ff51c6c 100644
--- a/include/lib/fconf/fconf_dyn_cfg_getter.h
+++ b/include/lib/fconf/fconf_dyn_cfg_getter.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020, Arm Limited. All rights reserved.
+ * Copyright (c) 2019-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,8 @@
 
 #include <lib/fconf/fconf.h>
 
+#define FCONF_INVALID_IDX	0xFFFFFFFFU
+
 /* Dynamic configuration related getter */
 #define dyn_cfg__dtb_getter(id)	dyn_cfg_dtb_info_getter(id)
 
@@ -18,6 +20,7 @@
 	unsigned int config_id;
 };
 
+unsigned int dyn_cfg_dtb_info_get_index(unsigned int config_id);
 struct dyn_cfg_dtb_info_t *dyn_cfg_dtb_info_getter(unsigned int config_id);
 int fconf_populate_dtb_registry(uintptr_t config);
 
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/lib/el3_runtime/aarch64/context.S b/lib/el3_runtime/aarch64/context.S
index e270ad0..c9035e8 100644
--- a/lib/el3_runtime/aarch64/context.S
+++ b/lib/el3_runtime/aarch64/context.S
@@ -145,11 +145,14 @@
 	stp	x11, x12, [x0, #CTX_MPAMVPM7_EL2]
 #endif
 
-#if ARM_ARCH_AT_LEAST(8, 6)
-	mrs	x13, HAFGRTR_EL2
-	mrs	x14, HDFGRTR_EL2
-	stp	x13, x14, [x0, #CTX_HAFGRTR_EL2]
-
+#if ENABLE_FEAT_FGT
+	mrs	x13, HDFGRTR_EL2
+#if ENABLE_FEAT_AMUv1
+   	mrs	x14, HAFGRTR_EL2
+   	stp	x13, x14, [x0, #CTX_HDFGRTR_EL2]
+#else
+   	str	x13, [x0, #CTX_HDFGRTR_EL2]
+#endif
 	mrs	x15, HDFGWTR_EL2
 	mrs	x16, HFGITR_EL2
 	stp	x15, x16, [x0, #CTX_HDFGWTR_EL2]
@@ -157,7 +160,9 @@
 	mrs	x9, HFGRTR_EL2
 	mrs	x10, HFGWTR_EL2
 	stp	x9, x10, [x0, #CTX_HFGRTR_EL2]
+#endif
 
+#if ENABLE_FEAT_ECV
 	mrs	x11, CNTPOFF_EL2
 	str	x11, [x0, #CTX_CNTPOFF_EL2]
 #endif
@@ -319,10 +324,14 @@
 	msr	MPAMVPMV_EL2, x12
 #endif
 
-#if ARM_ARCH_AT_LEAST(8, 6)
-	ldp	x13, x14, [x0, #CTX_HAFGRTR_EL2]
-	msr	HAFGRTR_EL2, x13
-	msr	HDFGRTR_EL2, x14
+#if ENABLE_FEAT_FGT
+#if ENABLE_FEAT_AMUv1
+	ldp	x13, x14, [x0, #CTX_HDFGRTR_EL2]
+	msr	HAFGRTR_EL2, x14
+#else
+	ldr	x13, [x0, #CTX_HDFGRTR_EL2]
+#endif
+	msr	HDFGRTR_EL2, x13
 
 	ldp	x15, x16, [x0, #CTX_HDFGWTR_EL2]
 	msr	HDFGWTR_EL2, x15
@@ -331,7 +340,9 @@
 	ldp	x9, x10, [x0, #CTX_HFGRTR_EL2]
 	msr	HFGRTR_EL2, x9
 	msr	HFGWTR_EL2, x10
+#endif
 
+#if ENABLE_FEAT_ECV
 	ldr	x11, [x0, #CTX_CNTPOFF_EL2]
 	msr	CNTPOFF_EL2, x11
 #endif
diff --git a/lib/fconf/fconf_dyn_cfg_getter.c b/lib/fconf/fconf_dyn_cfg_getter.c
index 25dd7f9..34623fb 100644
--- a/lib/fconf/fconf_dyn_cfg_getter.c
+++ b/lib/fconf/fconf_dyn_cfg_getter.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020, Arm Limited. All rights reserved.
+ * Copyright (c) 2019-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -40,17 +40,30 @@
 	dtb_info->config_id = config_id;
 }
 
-struct dyn_cfg_dtb_info_t *dyn_cfg_dtb_info_getter(unsigned int config_id)
+/* Get index of the config_id image */
+unsigned int dyn_cfg_dtb_info_get_index(unsigned int config_id)
 {
 	unsigned int index;
 
 	/* Positions index to the proper config-id */
 	for (index = 0U; index < MAX_DTB_INFO; index++) {
 		if (dtb_infos[index].config_id == config_id) {
-			return &dtb_infos[index];
+			return index;
 		}
 	}
 
+	return FCONF_INVALID_IDX;
+}
+
+struct dyn_cfg_dtb_info_t *dyn_cfg_dtb_info_getter(unsigned int config_id)
+{
+	/* Positions index to the proper config-id */
+	unsigned int index = dyn_cfg_dtb_info_get_index(config_id);
+
+	if (index < MAX_DTB_INFO) {
+		return &dtb_infos[index];
+	}
+
 	WARN("FCONF: Invalid config id %u\n", config_id);
 
 	return NULL;
diff --git a/make_helpers/defaults.mk b/make_helpers/defaults.mk
index e88148f..72f5e33 100644
--- a/make_helpers/defaults.mk
+++ b/make_helpers/defaults.mk
@@ -136,6 +136,15 @@
 # Flag to enable access to the HCRX_EL2 register by setting SCR_EL3.HXEn.
 ENABLE_FEAT_HCX			:= 0
 
+# Flag to enable access to the HAFGRTR_EL2 register
+ENABLE_FEAT_AMUv1		:= 0
+
+# Flag to enable access to the HDFGRTR_EL2 register
+ENABLE_FEAT_FGT			:= 0
+
+# Flag to enable access to the CNTPOFF_EL2 register
+ENABLE_FEAT_ECV			:= 0
+
 # By default BL31 encryption disabled
 ENCRYPT_BL31			:= 0
 
diff --git a/plat/arm/board/fvp/fvp_bl2_measured_boot.c b/plat/arm/board/fvp/fvp_bl2_measured_boot.c
index decf13d..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;
@@ -25,6 +29,8 @@
 	{ SOC_FW_CONFIG_ID, EVLOG_SOC_FW_CONFIG_STRING, PCR_0 },
 	{ TOS_FW_CONFIG_ID, EVLOG_TOS_FW_CONFIG_STRING, PCR_0 },
 
+	{ CRITICAL_DATA_ID, EVLOG_CRITICAL_DATA_STRING, PCR_1 },
+
 	{ EVLOG_INVALID_ID, NULL, (unsigned int)(-1) }	/* Terminator */
 };
 
@@ -59,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;
@@ -69,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/plat/st/common/include/stm32mp_common.h b/plat/st/common/include/stm32mp_common.h
index 6183964..fbd0dda 100644
--- a/plat/st/common/include/stm32mp_common.h
+++ b/plat/st/common/include/stm32mp_common.h
@@ -48,11 +48,14 @@
 uint32_t stm32_iwdg_shadow_update(uint32_t iwdg_inst, uint32_t flags);
 #endif
 
-#if STM32MP_UART_PROGRAMMER
+#if STM32MP_UART_PROGRAMMER || !defined(IMAGE_BL2)
 /* Get the UART address from its instance number */
 uintptr_t get_uart_address(uint32_t instance_nb);
 #endif
 
+/* Setup the UART console */
+int stm32mp_uart_console_setup(void);
+
 /*
  * Platform util functions for the GPIO driver
  * @bank: Target GPIO bank ID as per DT bindings
@@ -69,6 +72,7 @@
 uintptr_t stm32_get_gpio_bank_base(unsigned int bank);
 unsigned long stm32_get_gpio_bank_clock(unsigned int bank);
 uint32_t stm32_get_gpio_bank_offset(unsigned int bank);
+bool stm32_gpio_is_secure_at_reset(unsigned int bank);
 
 /* Return node offset for target GPIO bank ID @bank or a FDT error code */
 int stm32_get_gpio_bank_pinctrl_node(void *fdt, unsigned int bank);
@@ -114,4 +118,8 @@
 int stm32mp_map_ddr_non_cacheable(void);
 int stm32mp_unmap_ddr(void);
 
+/* Functions to save and get boot peripheral info */
+void stm32_save_boot_interface(uint32_t interface, uint32_t instance);
+void stm32_get_boot_interface(uint32_t *interface, uint32_t *instance);
+
 #endif /* STM32MP_COMMON_H */
diff --git a/plat/st/common/stm32mp_common.c b/plat/st/common/stm32mp_common.c
index 9120408..072155f 100644
--- a/plat/st/common/stm32mp_common.c
+++ b/plat/st/common/stm32mp_common.c
@@ -7,17 +7,23 @@
 #include <assert.h>
 #include <errno.h>
 
-#include <platform_def.h>
-
 #include <arch_helpers.h>
 #include <common/debug.h>
+#include <drivers/delay_timer.h>
+#include <drivers/st/stm32_console.h>
 #include <drivers/st/stm32mp_clkfunc.h>
+#include <drivers/st/stm32mp_reset.h>
 #include <lib/smccc.h>
 #include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 #include <services/arm_arch_svc.h>
 
+#include <platform_def.h>
+
 #define HEADER_VERSION_MAJOR_MASK	GENMASK(23, 16)
+#define RESET_TIMEOUT_US_1MS		1000U
+
+static console_t console;
 
 uintptr_t plat_get_ns_image_entrypoint(void)
 {
@@ -128,6 +134,83 @@
 					   STM32MP_DDR_MAX_SIZE);
 }
 
+#if  defined(IMAGE_BL2)
+static void reset_uart(uint32_t reset)
+{
+	int ret;
+
+	ret = stm32mp_reset_assert(reset, RESET_TIMEOUT_US_1MS);
+	if (ret != 0) {
+		panic();
+	}
+
+	udelay(2);
+
+	ret = stm32mp_reset_deassert(reset, RESET_TIMEOUT_US_1MS);
+	if (ret != 0) {
+		panic();
+	}
+
+	mdelay(1);
+}
+#endif
+
+int stm32mp_uart_console_setup(void)
+{
+	struct dt_node_info dt_uart_info;
+	unsigned int console_flags;
+	uint32_t clk_rate;
+	int result;
+	uint32_t boot_itf __unused;
+	uint32_t boot_instance __unused;
+
+	result = dt_get_stdout_uart_info(&dt_uart_info);
+
+	if ((result <= 0) ||
+	    (dt_uart_info.status == DT_DISABLED) ||
+	    (dt_uart_info.clock < 0) ||
+	    (dt_uart_info.reset < 0)) {
+		return -ENODEV;
+	}
+
+#if STM32MP_UART_PROGRAMMER || !defined(IMAGE_BL2)
+	stm32_get_boot_interface(&boot_itf, &boot_instance);
+
+	if ((boot_itf == BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART) &&
+	    (get_uart_address(boot_instance) == dt_uart_info.base)) {
+		return -EACCES;
+	}
+#endif
+
+#if defined(IMAGE_BL2)
+	if (dt_set_stdout_pinctrl() != 0) {
+		return -ENODEV;
+	}
+#endif
+
+	stm32mp_clk_enable((unsigned long)dt_uart_info.clock);
+
+#if defined(IMAGE_BL2)
+	reset_uart((uint32_t)dt_uart_info.reset);
+#endif
+
+	clk_rate = stm32mp_clk_get_rate((unsigned long)dt_uart_info.clock);
+
+	if (console_stm32_register(dt_uart_info.base, clk_rate,
+				   STM32MP_UART_BAUDRATE, &console) == 0) {
+		panic();
+	}
+
+	console_flags = CONSOLE_FLAG_BOOT | CONSOLE_FLAG_CRASH |
+			CONSOLE_FLAG_TRANSLATE_CRLF;
+#if !defined(IMAGE_BL2) && defined(DEBUG)
+	console_flags |= CONSOLE_FLAG_RUNTIME;
+#endif
+	console_set_scope(&console, console_flags);
+
+	return 0;
+}
+
 /*****************************************************************************
  * plat_is_smccc_feature_available() - This function checks whether SMCCC
  *                                     feature is availabile for platform.
diff --git a/plat/st/stm32mp1/bl2_plat_setup.c b/plat/st/stm32mp1/bl2_plat_setup.c
index 218f28d..0bf8583 100644
--- a/plat/st/stm32mp1/bl2_plat_setup.c
+++ b/plat/st/stm32mp1/bl2_plat_setup.c
@@ -14,14 +14,12 @@
 #include <common/bl_common.h>
 #include <common/debug.h>
 #include <common/desc_image_load.h>
-#include <drivers/delay_timer.h>
 #include <drivers/generic_delay_timer.h>
 #include <drivers/mmc.h>
 #include <drivers/st/bsec.h>
-#include <drivers/st/stm32_console.h>
 #include <drivers/st/stm32_iwdg.h>
+#include <drivers/st/stm32_uart.h>
 #include <drivers/st/stm32mp_pmic.h>
-#include <drivers/st/stm32mp_reset.h>
 #include <drivers/st/stm32mp1_clk.h>
 #include <drivers/st/stm32mp1_pwr.h>
 #include <drivers/st/stm32mp1_ram.h>
@@ -32,12 +30,8 @@
 #include <lib/xlat_tables/xlat_tables_v2.h>
 #include <plat/common/platform.h>
 
-#include <stm32mp1_context.h>
 #include <stm32mp1_dbgmcu.h>
 
-#define RESET_TIMEOUT_US_1MS		1000U
-
-static console_t console;
 static struct stm32mp_auth_ops stm32mp1_auth_ops;
 
 static void print_reset_reason(void)
@@ -167,11 +161,9 @@
 void bl2_el3_plat_arch_setup(void)
 {
 	int32_t result;
-	struct dt_node_info dt_uart_info;
 	const char *board_model;
 	boot_api_context_t *boot_context =
 		(boot_api_context_t *)stm32mp_get_boot_ctx_address();
-	uint32_t clk_rate;
 	uintptr_t pwr_base;
 	uintptr_t rcc_base;
 
@@ -238,6 +230,16 @@
 
 	generic_delay_timer_init();
 
+#if STM32MP_UART_PROGRAMMER
+	/* Disable programmer UART before changing clock tree */
+	if (boot_context->boot_interface_selected ==
+	    BOOT_API_CTX_BOOT_INTERFACE_SEL_SERIAL_UART) {
+		uintptr_t uart_prog_addr =
+			get_uart_address(boot_context->boot_interface_instance);
+
+		stm32_uart_stop(uart_prog_addr);
+	}
+#endif
 	if (stm32mp1_clk_probe() < 0) {
 		panic();
 	}
@@ -248,45 +250,18 @@
 
 	stm32mp1_syscfg_init();
 
-	result = dt_get_stdout_uart_info(&dt_uart_info);
+	stm32_save_boot_interface(boot_context->boot_interface_selected,
+				  boot_context->boot_interface_instance);
 
-	if ((result <= 0) ||
-	    (dt_uart_info.status == 0U) ||
-	    (dt_uart_info.clock < 0) ||
-	    (dt_uart_info.reset < 0)) {
-		goto skip_console_init;
-	}
+#if STM32MP_USB_PROGRAMMER
+	/* Deconfigure all UART RX pins configured by ROM code */
+	stm32mp1_deconfigure_uart_pins();
+#endif
 
-	if (dt_set_stdout_pinctrl() != 0) {
+	if (stm32mp_uart_console_setup() != 0) {
 		goto skip_console_init;
 	}
 
-	stm32mp_clk_enable((unsigned long)dt_uart_info.clock);
-
-	if (stm32mp_reset_assert((uint32_t)dt_uart_info.reset,
-				 RESET_TIMEOUT_US_1MS) != 0) {
-		panic();
-	}
-
-	udelay(2);
-
-	if (stm32mp_reset_deassert((uint32_t)dt_uart_info.reset,
-				   RESET_TIMEOUT_US_1MS) != 0) {
-		panic();
-	}
-
-	mdelay(1);
-
-	clk_rate = stm32mp_clk_get_rate((unsigned long)dt_uart_info.clock);
-
-	if (console_stm32_register(dt_uart_info.base, clk_rate,
-				   STM32MP_UART_BAUDRATE, &console) == 0) {
-		panic();
-	}
-
-	console_set_scope(&console, CONSOLE_FLAG_BOOT |
-			  CONSOLE_FLAG_CRASH | CONSOLE_FLAG_TRANSLATE_CRLF);
-
 	stm32mp_print_cpuinfo();
 
 	board_model = dt_get_board_model();
@@ -314,12 +289,6 @@
 		INFO("IWDG2 freeze error : %i\n", result);
 	}
 
-	if (stm32_save_boot_interface(boot_context->boot_interface_selected,
-				      boot_context->boot_interface_instance) !=
-	    0) {
-		ERROR("Cannot save boot interface\n");
-	}
-
 	stm32mp1_auth_ops.check_key = boot_context->bootrom_ecdsa_check_key;
 	stm32mp1_auth_ops.verify_signature =
 		boot_context->bootrom_ecdsa_verify_signature;
@@ -352,6 +321,7 @@
 	const struct dyn_cfg_dtb_info_t *config_info;
 	bl_mem_params_node_t *tos_fw_mem_params;
 	unsigned int i;
+	unsigned int idx;
 	unsigned long long ddr_top __unused;
 	const unsigned int image_ids[] = {
 		BL32_IMAGE_ID,
@@ -370,8 +340,14 @@
 		set_config_info(STM32MP_FW_CONFIG_BASE, STM32MP_FW_CONFIG_MAX_SIZE, FW_CONFIG_ID);
 		fconf_populate("FW_CONFIG", STM32MP_FW_CONFIG_BASE);
 
+		idx = dyn_cfg_dtb_info_get_index(TOS_FW_CONFIG_ID);
+
 		/* Iterate through all the fw config IDs */
 		for (i = 0U; i < ARRAY_SIZE(image_ids); i++) {
+			if ((image_ids[i] == TOS_FW_CONFIG_ID) && (idx == FCONF_INVALID_IDX)) {
+				continue;
+			}
+
 			bl_mem_params = get_bl_mem_params_node(image_ids[i]);
 			assert(bl_mem_params != NULL);
 
diff --git a/plat/st/stm32mp1/include/stm32mp1_context.h b/plat/st/stm32mp1/include/stm32mp1_context.h
deleted file mode 100644
index 698415a..0000000
--- a/plat/st/stm32mp1/include/stm32mp1_context.h
+++ /dev/null
@@ -1,14 +0,0 @@
-/*
- * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef STM32MP1_CONTEXT_H
-#define STM32MP1_CONTEXT_H
-
-#include <stdint.h>
-
-int stm32_save_boot_interface(uint32_t interface, uint32_t instance);
-
-#endif /* STM32MP1_CONTEXT_H */
diff --git a/plat/st/stm32mp1/include/stm32mp1_private.h b/plat/st/stm32mp1/include/stm32mp1_private.h
index 729d233..2eec16f 100644
--- a/plat/st/stm32mp1/include/stm32mp1_private.h
+++ b/plat/st/stm32mp1/include/stm32mp1_private.h
@@ -21,6 +21,8 @@
 void stm32mp1_syscfg_enable_io_compensation(void);
 void stm32mp1_syscfg_disable_io_compensation(void);
 
+void stm32mp1_deconfigure_uart_pins(void);
+
 #if STM32MP_USE_STM32IMAGE
 uint32_t stm32mp_get_ddr_ns_size(void);
 #endif /* STM32MP_USE_STM32IMAGE */
diff --git a/plat/st/stm32mp1/platform.mk b/plat/st/stm32mp1/platform.mk
index 2a4ab78..3a76d28 100644
--- a/plat/st/stm32mp1/platform.mk
+++ b/plat/st/stm32mp1/platform.mk
@@ -28,6 +28,8 @@
 TF_CFLAGS		+=	-mfloat-abi=soft
 endif
 
+TF_CFLAGS		+=	-Wsign-compare
+
 # Not needed for Cortex-A7
 WORKAROUND_CVE_2017_5715:=	0
 
@@ -124,13 +126,13 @@
 # Enable flags for C files
 $(eval $(call assert_booleans,\
 	$(sort \
+		PLAT_XLAT_TABLES_DYNAMIC \
 		STM32MP_EMMC \
-		STM32MP_SDMMC \
+		STM32MP_EMMC_BOOT \
 		STM32MP_RAW_NAND \
+		STM32MP_SDMMC \
 		STM32MP_SPI_NAND \
 		STM32MP_SPI_NOR \
-		STM32MP_EMMC_BOOT \
-		PLAT_XLAT_TABLES_DYNAMIC \
 		STM32MP_UART_PROGRAMMER \
 		STM32MP_USB_PROGRAMMER \
 		STM32MP_USE_STM32IMAGE \
@@ -138,23 +140,23 @@
 
 $(eval $(call assert_numerics,\
 	$(sort \
-		STM32_TF_A_COPIES \
 		PLAT_PARTITION_MAX_ENTRIES \
+		STM32_TF_A_COPIES \
 		STM32_TF_VERSION \
 )))
 
 $(eval $(call add_defines,\
 	$(sort \
+		PLAT_PARTITION_MAX_ENTRIES \
+		PLAT_XLAT_TABLES_DYNAMIC \
+		STM32_TF_A_COPIES \
 		STM32_TF_VERSION \
 		STM32MP_EMMC \
-		STM32MP_SDMMC \
+		STM32MP_EMMC_BOOT \
 		STM32MP_RAW_NAND \
+		STM32MP_SDMMC \
 		STM32MP_SPI_NAND \
 		STM32MP_SPI_NOR \
-		STM32MP_EMMC_BOOT \
-		PLAT_XLAT_TABLES_DYNAMIC \
-		STM32_TF_A_COPIES \
-		PLAT_PARTITION_MAX_ENTRIES \
 		STM32MP_UART_PROGRAMMER \
 		STM32MP_USB_PROGRAMMER \
 		STM32MP_USE_STM32IMAGE \
@@ -200,7 +202,6 @@
 				drivers/st/pmic/stpmic1.c				\
 				drivers/st/reset/stm32mp1_reset.c			\
 				plat/st/common/stm32mp_dt.c				\
-				plat/st/stm32mp1/stm32mp1_context.c			\
 				plat/st/stm32mp1/stm32mp1_dbgmcu.c			\
 				plat/st/stm32mp1/stm32mp1_helper.S			\
 				plat/st/stm32mp1/stm32mp1_syscfg.c
diff --git a/plat/st/stm32mp1/sp_min/sp_min_setup.c b/plat/st/stm32mp1/sp_min/sp_min_setup.c
index 1495e02..8106795 100644
--- a/plat/st/stm32mp1/sp_min/sp_min_setup.c
+++ b/plat/st/stm32mp1/sp_min/sp_min_setup.c
@@ -18,7 +18,6 @@
 #include <drivers/generic_delay_timer.h>
 #include <drivers/st/bsec.h>
 #include <drivers/st/etzpc.h>
-#include <drivers/st/stm32_console.h>
 #include <drivers/st/stm32_gpio.h>
 #include <drivers/st/stm32_iwdg.h>
 #include <drivers/st/stm32mp1_clk.h>
@@ -36,8 +35,6 @@
  ******************************************************************************/
 static entry_point_info_t bl33_image_ep_info;
 
-static console_t console;
-
 /*******************************************************************************
  * Interrupt handler for FIQ (secure IRQ)
  ******************************************************************************/
@@ -115,8 +112,6 @@
 void sp_min_early_platform_setup2(u_register_t arg0, u_register_t arg1,
 				  u_register_t arg2, u_register_t arg3)
 {
-	struct dt_node_info dt_uart_info;
-	int result;
 	bl_params_t *params_from_bl2 = (bl_params_t *)arg0;
 #if STM32MP_USE_STM32IMAGE
 	uintptr_t dt_addr = STM32MP_DTB_BASE;
@@ -174,24 +169,7 @@
 		panic();
 	}
 
-	result = dt_get_stdout_uart_info(&dt_uart_info);
-
-	if ((result > 0) && (dt_uart_info.status != 0U)) {
-		unsigned int console_flags;
-
-		if (console_stm32_register(dt_uart_info.base, 0,
-					   STM32MP_UART_BAUDRATE, &console) ==
-		    0) {
-			panic();
-		}
-
-		console_flags = CONSOLE_FLAG_BOOT | CONSOLE_FLAG_CRASH |
-			CONSOLE_FLAG_TRANSLATE_CRLF;
-#ifdef DEBUG
-		console_flags |= CONSOLE_FLAG_RUNTIME;
-#endif
-		console_set_scope(&console, console_flags);
-	}
+	(void)stm32mp_uart_console_setup();
 
 	stm32mp1_etzpc_early_setup();
 }
diff --git a/plat/st/stm32mp1/stm32mp1_context.c b/plat/st/stm32mp1/stm32mp1_context.c
deleted file mode 100644
index cf8a91e..0000000
--- a/plat/st/stm32mp1/stm32mp1_context.c
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <errno.h>
-
-#include <platform_def.h>
-
-#include <drivers/st/stm32mp1_clk.h>
-#include <dt-bindings/clock/stm32mp1-clks.h>
-#include <lib/mmio.h>
-
-#include <stm32mp1_context.h>
-
-#define TAMP_BOOT_ITF_BACKUP_REG_ID	U(20)
-#define TAMP_BOOT_ITF_MASK		U(0x0000FF00)
-#define TAMP_BOOT_ITF_SHIFT		8
-
-int stm32_save_boot_interface(uint32_t interface, uint32_t instance)
-{
-	uint32_t bkpr_itf_idx = tamp_bkpr(TAMP_BOOT_ITF_BACKUP_REG_ID);
-
-	stm32mp_clk_enable(RTCAPB);
-
-	mmio_clrsetbits_32(bkpr_itf_idx,
-			   TAMP_BOOT_ITF_MASK,
-			   ((interface << 4) | (instance & 0xFU)) <<
-			   TAMP_BOOT_ITF_SHIFT);
-
-	stm32mp_clk_disable(RTCAPB);
-
-	return 0;
-}
diff --git a/plat/st/stm32mp1/stm32mp1_private.c b/plat/st/stm32mp1/stm32mp1_private.c
index 0165cfe..a8ffecb 100644
--- a/plat/st/stm32mp1/stm32mp1_private.c
+++ b/plat/st/stm32mp1/stm32mp1_private.c
@@ -6,13 +6,14 @@
 
 #include <assert.h>
 
+#include <drivers/st/stm32_gpio.h>
+#include <drivers/st/stm32_iwdg.h>
 #include <libfdt.h>
+#include <lib/mmio.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
 
 #include <platform_def.h>
 
-#include <drivers/st/stm32_iwdg.h>
-#include <lib/xlat_tables/xlat_tables_v2.h>
-
 /* Internal layout of the 32bit OTP word board_id */
 #define BOARD_ID_BOARD_NB_MASK		GENMASK(31, 16)
 #define BOARD_ID_BOARD_NB_SHIFT		16
@@ -34,6 +35,10 @@
 					 BOARD_ID_VARFG_SHIFT)
 #define BOARD_ID2BOM(_id)		((_id) & BOARD_ID_BOM_MASK)
 
+#define TAMP_BOOT_MODE_BACKUP_REG_ID	U(20)
+#define TAMP_BOOT_MODE_ITF_MASK		U(0x0000FF00)
+#define TAMP_BOOT_MODE_ITF_SHIFT	8
+
 #if defined(IMAGE_BL2)
 #define MAP_SEC_SYSRAM	MAP_REGION_FLAT(STM32MP_SYSRAM_BASE, \
 					STM32MP_SYSRAM_SIZE, \
@@ -120,6 +125,15 @@
 	return bank * GPIO_BANK_OFFSET;
 }
 
+bool stm32_gpio_is_secure_at_reset(unsigned int bank)
+{
+	if (bank == GPIO_BANK_Z) {
+		return true;
+	}
+
+	return false;
+}
+
 unsigned long stm32_get_gpio_bank_clock(unsigned int bank)
 {
 	if (bank == GPIO_BANK_Z) {
@@ -153,7 +167,7 @@
 	}
 }
 
-#if STM32MP_UART_PROGRAMMER
+#if STM32MP_UART_PROGRAMMER || !defined(IMAGE_BL2)
 /*
  * UART Management
  */
@@ -179,6 +193,53 @@
 }
 #endif
 
+#if STM32MP_USB_PROGRAMMER
+struct gpio_bank_pin_list {
+	uint32_t bank;
+	uint32_t pin;
+};
+
+static const struct gpio_bank_pin_list gpio_list[] = {
+	{	/* USART2_RX: GPIOA3 */
+		.bank = 0U,
+		.pin = 3U,
+	},
+	{	/* USART3_RX: GPIOB12 */
+		.bank = 1U,
+		.pin = 12U,
+	},
+	{	/* UART4_RX: GPIOB2 */
+		.bank = 1U,
+		.pin = 2U,
+	},
+	{	/* UART5_RX: GPIOB4 */
+		.bank = 1U,
+		.pin = 5U,
+	},
+	{	/* USART6_RX: GPIOC7 */
+		.bank = 2U,
+		.pin = 7U,
+	},
+	{	/* UART7_RX: GPIOF6 */
+		.bank = 5U,
+		.pin = 6U,
+	},
+	{	/* UART8_RX: GPIOE0 */
+		.bank = 4U,
+		.pin = 0U,
+	},
+};
+
+void stm32mp1_deconfigure_uart_pins(void)
+{
+	size_t i;
+
+	for (i = 0U; i < ARRAY_SIZE(gpio_list); i++) {
+		set_gpio_reset_cfg(gpio_list[i].bank, gpio_list[i].pin);
+	}
+}
+#endif
+
 uint32_t stm32mp_get_chip_version(void)
 {
 	uint32_t version = 0U;
@@ -500,3 +561,36 @@
 	return ddr_ns_size;
 }
 #endif /* STM32MP_USE_STM32IMAGE */
+
+void stm32_save_boot_interface(uint32_t interface, uint32_t instance)
+{
+	uint32_t bkpr_itf_idx = tamp_bkpr(TAMP_BOOT_MODE_BACKUP_REG_ID);
+
+	stm32mp_clk_enable(RTCAPB);
+
+	mmio_clrsetbits_32(bkpr_itf_idx,
+			   TAMP_BOOT_MODE_ITF_MASK,
+			   ((interface << 4) | (instance & 0xFU)) <<
+			   TAMP_BOOT_MODE_ITF_SHIFT);
+
+	stm32mp_clk_disable(RTCAPB);
+}
+
+void stm32_get_boot_interface(uint32_t *interface, uint32_t *instance)
+{
+	static uint32_t itf;
+
+	if (itf == 0U) {
+		uint32_t bkpr = tamp_bkpr(TAMP_BOOT_MODE_BACKUP_REG_ID);
+
+		stm32mp_clk_enable(RTCAPB);
+
+		itf = (mmio_read_32(bkpr) & TAMP_BOOT_MODE_ITF_MASK) >>
+			TAMP_BOOT_MODE_ITF_SHIFT;
+
+		stm32mp_clk_disable(RTCAPB);
+	}
+
+	*interface = itf >> 4;
+	*instance = itf & 0xFU;
+}
diff --git a/plat/xilinx/zynqmp/platform.mk b/plat/xilinx/zynqmp/platform.mk
index d075a56..3d98584 100644
--- a/plat/xilinx/zynqmp/platform.mk
+++ b/plat/xilinx/zynqmp/platform.mk
@@ -118,6 +118,7 @@
 endif
 
 BL31_CPPFLAGS		+=	-fno-jump-tables
+TF_CFLAGS_aarch64	+=	-mbranch-protection=none
 
 ifneq (${RESET_TO_BL31},1)
   $(error "Using BL31 as the reset vector is only one option supported on ZynqMP. Please set RESET_TO_BL31 to 1.")
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