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 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/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/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/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/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