diff --git a/lib/psci/aarch64/psci_entry.S b/lib/psci/aarch64/psci_entry.S
deleted file mode 100644
index 646ebcf..0000000
--- a/lib/psci/aarch64/psci_entry.S
+++ /dev/null
@@ -1,111 +0,0 @@
-/*
- * Copyright (c) 2013-2016, ARM Limited and Contributors. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * Redistributions of source code must retain the above copyright notice, this
- * list of conditions and the following disclaimer.
- *
- * Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * Neither the name of ARM nor the names of its contributors may be used
- * to endorse or promote products derived from this software without specific
- * prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include <arch.h>
-#include <asm_macros.S>
-#include <el3_common_macros.S>
-#include <psci.h>
-#include <xlat_tables.h>
-
-	.globl	psci_entrypoint
-	.globl	psci_power_down_wfi
-
-	/* --------------------------------------------------------------------
-	 * This CPU has been physically powered up. It is either resuming from
-	 * suspend or has simply been turned on. In both cases, call the power
-	 * on finisher.
-	 * --------------------------------------------------------------------
-	 */
-func psci_entrypoint
-	/*
-	 * On the warm boot path, most of the EL3 initialisations performed by
-	 * 'el3_entrypoint_common' must be skipped:
-	 *
-	 *  - Only when the platform bypasses the BL1/BL31 entrypoint by
-	 *    programming the reset address do we need to set the CPU endianness.
-	 *    In other cases, we assume this has been taken care by the
-	 *    entrypoint code.
-	 *
-	 *  - No need to determine the type of boot, we know it is a warm boot.
-	 *
-	 *  - Do not try to distinguish between primary and secondary CPUs, this
-	 *    notion only exists for a cold boot.
-	 *
-	 *  - No need to initialise the memory or the C runtime environment,
-	 *    it has been done once and for all on the cold boot path.
-	 */
-	el3_entrypoint_common					\
-		_set_endian=PROGRAMMABLE_RESET_ADDRESS		\
-		_warm_boot_mailbox=0				\
-		_secondary_cold_boot=0				\
-		_init_memory=0					\
-		_init_c_runtime=0				\
-		_exception_vectors=runtime_exceptions
-
-	/* --------------------------------------------
-	 * Enable the MMU with the DCache disabled. It
-	 * is safe to use stacks allocated in normal
-	 * memory as a result. All memory accesses are
-	 * marked nGnRnE when the MMU is disabled. So
-	 * all the stack writes will make it to memory.
-	 * All memory accesses are marked Non-cacheable
-	 * when the MMU is enabled but D$ is disabled.
-	 * So used stack memory is guaranteed to be
-	 * visible immediately after the MMU is enabled
-	 * Enabling the DCache at the same time as the
-	 * MMU can lead to speculatively fetched and
-	 * possibly stale stack memory being read from
-	 * other caches. This can lead to coherency
-	 * issues.
-	 * --------------------------------------------
-	 */
-	mov	x0, #DISABLE_DCACHE
-	bl	bl31_plat_enable_mmu
-
-	bl	psci_power_up_finish
-
-	b	el3_exit
-endfunc psci_entrypoint
-
-	/* --------------------------------------------
-	 * This function is called to indicate to the
-	 * power controller that it is safe to power
-	 * down this cpu. It should not exit the wfi
-	 * and will be released from reset upon power
-	 * up. 'wfi_spill' is used to catch erroneous
-	 * exits from wfi.
-	 * --------------------------------------------
-	 */
-func psci_power_down_wfi
-	dsb	sy		// ensure write buffer empty
-	wfi
-	bl	plat_panic_handler
-endfunc psci_power_down_wfi
-
diff --git a/lib/psci/aarch64/psci_helpers.S b/lib/psci/aarch64/psci_helpers.S
index 87144dd..ff250a0 100644
--- a/lib/psci/aarch64/psci_helpers.S
+++ b/lib/psci/aarch64/psci_helpers.S
@@ -35,6 +35,10 @@
 
 	.globl	psci_do_pwrdown_cache_maintenance
 	.globl	psci_do_pwrup_cache_maintenance
+	.globl	psci_power_down_wfi
+#if !ERROR_DEPRECATED
+	.globl psci_entrypoint
+#endif
 
 /* -----------------------------------------------------------------------
  * void psci_do_pwrdown_cache_maintenance(unsigned int power level);
@@ -152,3 +156,25 @@
 	ldp	x29, x30, [sp], #16
 	ret
 endfunc psci_do_pwrup_cache_maintenance
+
+/* -----------------------------------------------------------------------
+ * void psci_power_down_wfi(void);
+ * This function is called to indicate to the power controller that it
+ * is safe to power down this cpu. It should not exit the wfi and will
+ * be released from reset upon power up.
+ * -----------------------------------------------------------------------
+ */
+func psci_power_down_wfi
+	dsb	sy		// ensure write buffer empty
+	wfi
+	bl	plat_panic_handler
+endfunc psci_power_down_wfi
+
+/* -----------------------------------------------------------------------
+ * void psci_entrypoint(void);
+ * The deprecated entry point for PSCI on warm boot for AArch64.
+ * -----------------------------------------------------------------------
+ */
+func_deprecated psci_entrypoint
+	b	bl31_warm_entrypoint
+endfunc_deprecated psci_entrypoint
diff --git a/lib/psci/psci_common.c b/lib/psci/psci_common.c
index 2a0afb4..c6fea5b 100644
--- a/lib/psci/psci_common.c
+++ b/lib/psci/psci_common.c
@@ -683,7 +683,7 @@
  * code to enable the gic cpu interface and for a cluster it will enable
  * coherency at the interconnect level in addition to gic cpu interface.
  ******************************************************************************/
-void psci_power_up_finish(void)
+void psci_warmboot_entrypoint(void)
 {
 	unsigned int end_pwrlvl, cpu_idx = plat_my_core_pos();
 	psci_power_state_t state_info = { {PSCI_LOCAL_STATE_RUN} };
diff --git a/lib/psci/psci_lib.mk b/lib/psci/psci_lib.mk
index 93c7832..662e14a 100644
--- a/lib/psci/psci_lib.mk
+++ b/lib/psci/psci_lib.mk
@@ -28,21 +28,20 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 
-PSCI_LIB_SOURCES		:=	lib/el3_runtime/cpu_data_array.c	\
-				lib/el3_runtime/aarch64/context.S		\
-				lib/el3_runtime/aarch64/cpu_data.S		\
-				lib/el3_runtime/aarch64/context_mgmt.c		\
-				lib/cpus/aarch64/cpu_helpers.S			\
-				lib/locks/exclusive/spinlock.S			\
-				lib/psci/psci_off.c				\
-				lib/psci/psci_on.c				\
-				lib/psci/psci_suspend.c				\
-				lib/psci/psci_common.c				\
-				lib/psci/psci_main.c				\
-				lib/psci/psci_setup.c				\
-				lib/psci/psci_system_off.c			\
-				lib/psci/aarch64/psci_entry.S			\
-				lib/psci/aarch64/psci_helpers.S			\
+PSCI_LIB_SOURCES	:=	lib/el3_runtime/cpu_data_array.c	\
+				lib/el3_runtime/aarch64/context.S	\
+				lib/el3_runtime/aarch64/cpu_data.S	\
+				lib/el3_runtime/aarch64/context_mgmt.c	\
+				lib/cpus/aarch64/cpu_helpers.S		\
+				lib/locks/exclusive/spinlock.S		\
+				lib/psci/psci_off.c			\
+				lib/psci/psci_on.c			\
+				lib/psci/psci_suspend.c			\
+				lib/psci/psci_common.c			\
+				lib/psci/psci_main.c			\
+				lib/psci/psci_setup.c			\
+				lib/psci/psci_system_off.c		\
+				lib/psci/aarch64/psci_helpers.S
 
 ifeq (${USE_COHERENT_MEM}, 1)
 PSCI_LIB_SOURCES		+=	lib/locks/bakery/bakery_lock_coherent.c
diff --git a/lib/psci/psci_main.c b/lib/psci/psci_main.c
index 04ef10e..d412be3 100644
--- a/lib/psci/psci_main.c
+++ b/lib/psci/psci_main.c
@@ -33,8 +33,7 @@
 #include <assert.h>
 #include <debug.h>
 #include <platform.h>
-#include <runtime_svc.h>
-#include <std_svc.h>
+#include <smcc.h>
 #include <string.h>
 #include "psci_private.h"
 
@@ -327,7 +326,7 @@
 /*******************************************************************************
  * PSCI top level handler for servicing SMCs.
  ******************************************************************************/
-uintptr_t psci_smc_handler(uint32_t smc_fid,
+u_register_t psci_smc_handler(uint32_t smc_fid,
 			  u_register_t x1,
 			  u_register_t x2,
 			  u_register_t x3,
@@ -337,11 +336,11 @@
 			  u_register_t flags)
 {
 	if (is_caller_secure(flags))
-		SMC_RET1(handle, SMC_UNK);
+		return SMC_UNK;
 
 	/* Check the fid against the capabilities */
 	if (!(psci_caps & define_psci_cap(smc_fid)))
-		SMC_RET1(handle, SMC_UNK);
+		return SMC_UNK;
 
 	if (((smc_fid >> FUNCID_CC_SHIFT) & FUNCID_CC_MASK) == SMC_32) {
 		/* 32-bit PSCI function, clear top parameter bits */
@@ -352,31 +351,31 @@
 
 		switch (smc_fid) {
 		case PSCI_VERSION:
-			SMC_RET1(handle, psci_version());
+			return psci_version();
 
 		case PSCI_CPU_OFF:
-			SMC_RET1(handle, psci_cpu_off());
+			return psci_cpu_off();
 
 		case PSCI_CPU_SUSPEND_AARCH32:
-			SMC_RET1(handle, psci_cpu_suspend(x1, x2, x3));
+			return psci_cpu_suspend(x1, x2, x3);
 
 		case PSCI_CPU_ON_AARCH32:
-			SMC_RET1(handle, psci_cpu_on(x1, x2, x3));
+			return psci_cpu_on(x1, x2, x3);
 
 		case PSCI_AFFINITY_INFO_AARCH32:
-			SMC_RET1(handle, psci_affinity_info(x1, x2));
+			return psci_affinity_info(x1, x2);
 
 		case PSCI_MIG_AARCH32:
-			SMC_RET1(handle, psci_migrate(x1));
+			return psci_migrate(x1);
 
 		case PSCI_MIG_INFO_TYPE:
-			SMC_RET1(handle, psci_migrate_info_type());
+			return psci_migrate_info_type();
 
 		case PSCI_MIG_INFO_UP_CPU_AARCH32:
-			SMC_RET1(handle, psci_migrate_info_up_cpu());
+			return psci_migrate_info_up_cpu();
 
 		case PSCI_SYSTEM_SUSPEND_AARCH32:
-			SMC_RET1(handle, psci_system_suspend(x1, x2));
+			return psci_system_suspend(x1, x2);
 
 		case PSCI_SYSTEM_OFF:
 			psci_system_off();
@@ -387,14 +386,14 @@
 			/* We should never return from psci_system_reset() */
 
 		case PSCI_FEATURES:
-			SMC_RET1(handle, psci_features(x1));
+			return psci_features(x1);
 
 #if ENABLE_PSCI_STAT
 		case PSCI_STAT_RESIDENCY_AARCH32:
-			SMC_RET1(handle, psci_stat_residency(x1, x2));
+			return psci_stat_residency(x1, x2);
 
 		case PSCI_STAT_COUNT_AARCH32:
-			SMC_RET1(handle, psci_stat_count(x1, x2));
+			return psci_stat_count(x1, x2);
 #endif
 
 		default:
@@ -405,29 +404,29 @@
 
 		switch (smc_fid) {
 		case PSCI_CPU_SUSPEND_AARCH64:
-			SMC_RET1(handle, psci_cpu_suspend(x1, x2, x3));
+			return psci_cpu_suspend(x1, x2, x3);
 
 		case PSCI_CPU_ON_AARCH64:
-			SMC_RET1(handle, psci_cpu_on(x1, x2, x3));
+			return psci_cpu_on(x1, x2, x3);
 
 		case PSCI_AFFINITY_INFO_AARCH64:
-			SMC_RET1(handle, psci_affinity_info(x1, x2));
+			return psci_affinity_info(x1, x2);
 
 		case PSCI_MIG_AARCH64:
-			SMC_RET1(handle, psci_migrate(x1));
+			return psci_migrate(x1);
 
 		case PSCI_MIG_INFO_UP_CPU_AARCH64:
-			SMC_RET1(handle, psci_migrate_info_up_cpu());
+			return psci_migrate_info_up_cpu();
 
 		case PSCI_SYSTEM_SUSPEND_AARCH64:
-			SMC_RET1(handle, psci_system_suspend(x1, x2));
+			return psci_system_suspend(x1, x2);
 
 #if ENABLE_PSCI_STAT
 		case PSCI_STAT_RESIDENCY_AARCH64:
-			SMC_RET1(handle, psci_stat_residency(x1, x2));
+			return psci_stat_residency(x1, x2);
 
 		case PSCI_STAT_COUNT_AARCH64:
-			SMC_RET1(handle, psci_stat_count(x1, x2));
+			return psci_stat_count(x1, x2);
 #endif
 
 		default:
@@ -436,5 +435,5 @@
 	}
 
 	WARN("Unimplemented PSCI Call: 0x%x \n", smc_fid);
-	SMC_RET1(handle, SMC_UNK);
+	return SMC_UNK;
 }
diff --git a/lib/psci/psci_on.c b/lib/psci/psci_on.c
index d4826ed..f4bb797 100644
--- a/lib/psci/psci_on.c
+++ b/lib/psci/psci_on.c
@@ -32,11 +32,9 @@
 #include <arch_helpers.h>
 #include <assert.h>
 #include <bl_common.h>
-#include <bl31.h>
 #include <debug.h>
 #include <context_mgmt.h>
 #include <platform.h>
-#include <runtime_svc.h>
 #include <stddef.h>
 #include "psci_private.h"
 
@@ -177,7 +175,7 @@
 	 * on have completed. Perform enough arch.initialization
 	 * to run in the non-secure address space.
 	 */
-	bl31_arch_setup();
+	psci_arch_setup();
 
 	/*
 	 * Lock the CPU spin lock to make sure that the context initialization
diff --git a/lib/psci/psci_private.h b/lib/psci/psci_private.h
index f42ce55..4935214 100644
--- a/lib/psci/psci_private.h
+++ b/lib/psci/psci_private.h
@@ -192,7 +192,6 @@
 void psci_query_sys_suspend_pwrstate(psci_power_state_t *state_info);
 int psci_validate_mpidr(u_register_t mpidr);
 void psci_init_req_local_pwr_states(void);
-void psci_power_up_finish(void);
 int psci_validate_entry_point(entry_point_info_t *ep,
 			uintptr_t entrypoint, u_register_t context_id);
 void psci_get_parent_pwr_domain_nodes(unsigned int cpu_idx,
diff --git a/lib/psci/psci_setup.c b/lib/psci/psci_setup.c
index fac0ede..d35e000 100644
--- a/lib/psci/psci_setup.c
+++ b/lib/psci/psci_setup.c
@@ -184,15 +184,17 @@
 }
 
 /*******************************************************************************
- * This function initializes the power domain topology tree by querying the
- * platform. The power domain nodes higher than the CPU are populated in the
- * array psci_non_cpu_pd_nodes[] and the CPU power domains are populated in
- * psci_cpu_pd_nodes[]. The platform exports its static topology map through the
+ * This function does the architectural setup and takes the warm boot
+ * entry-point `mailbox_ep` as an argument. The function also initializes the
+ * power domain topology tree by querying the platform. The power domain nodes
+ * higher than the CPU are populated in the array psci_non_cpu_pd_nodes[] and
+ * the CPU power domains are populated in psci_cpu_pd_nodes[]. The platform
+ * exports its static topology map through the
  * populate_power_domain_topology_tree() API. The algorithm populates the
  * psci_non_cpu_pd_nodes and psci_cpu_pd_nodes iteratively by using this
- * topology map.  On a platform that implements two clusters of 2 cpus each, and
- * supporting 3 domain levels, the populated psci_non_cpu_pd_nodes would look
- * like this:
+ * topology map.  On a platform that implements two clusters of 2 cpus each,
+ * and supporting 3 domain levels, the populated psci_non_cpu_pd_nodes would
+ * look like this:
  *
  * ---------------------------------------------------
  * | system node | cluster 0 node  | cluster 1 node  |
@@ -204,10 +206,13 @@
  * |   CPU 0   |   CPU 1   |   CPU 2   |   CPU 3  |
  * ------------------------------------------------
  ******************************************************************************/
-int psci_setup(void)
+int psci_setup(uintptr_t mailbox_ep)
 {
 	const unsigned char *topology_tree;
 
+	/* Do the Architectural initialization */
+	psci_arch_setup();
+
 	/* Query the topology map from the platform */
 	topology_tree = plat_get_power_domain_tree_desc();
 
@@ -229,8 +234,8 @@
 	 */
 	psci_set_pwr_domains_to_run(PLAT_MAX_PWR_LVL);
 
-	plat_setup_psci_ops((uintptr_t)psci_entrypoint,
-					&psci_plat_pm_ops);
+	assert(mailbox_ep);
+	plat_setup_psci_ops(mailbox_ep, &psci_plat_pm_ops);
 	assert(psci_plat_pm_ops);
 
 	/* Initialize the psci capability */
@@ -259,3 +264,17 @@
 
 	return 0;
 }
+
+/*******************************************************************************
+ * This duplicates what the primary cpu did after a cold boot in BL1. The same
+ * needs to be done when a cpu is hotplugged in. This function could also over-
+ * ride any EL3 setup done by BL1 as this code resides in rw memory.
+ ******************************************************************************/
+void psci_arch_setup(void)
+{
+	/* Program the counter frequency */
+	write_cntfrq_el0(plat_get_syscnt_freq2());
+
+	/* Initialize the cpu_ops pointer. */
+	init_cpu_ops();
+}
diff --git a/lib/psci/psci_suspend.c b/lib/psci/psci_suspend.c
index ee1ccef..904a4e7 100644
--- a/lib/psci/psci_suspend.c
+++ b/lib/psci/psci_suspend.c
@@ -37,7 +37,6 @@
 #include <cpu_data.h>
 #include <debug.h>
 #include <platform.h>
-#include <runtime_svc.h>
 #include <stddef.h>
 #include "psci_private.h"