diff --git a/common/psci/psci_common.c b/common/psci/psci_common.c
index 193655d..214db78 100644
--- a/common/psci/psci_common.c
+++ b/common/psci/psci_common.c
@@ -36,6 +36,7 @@
 #include <platform.h>
 #include <psci.h>
 #include <psci_private.h>
+#include <context_mgmt.h>
 #include <runtime_svc.h>
 #include "debug.h"
 
@@ -86,7 +87,8 @@
 	unsigned int state;
 
 	/* Retrieve our node from the topology tree */
-	node = psci_get_aff_map_node(mpidr & MPIDR_AFFINITY_MASK, MPIDR_AFFLVL0);
+	node = psci_get_aff_map_node(mpidr & MPIDR_AFFINITY_MASK,
+			MPIDR_AFFLVL0);
 	assert(node);
 
 	/*
@@ -222,13 +224,12 @@
 void psci_get_ns_entry_info(unsigned int index)
 {
 	unsigned long sctlr = 0, scr, el_status, id_aa64pfr0;
-	gp_regs *ns_gp_regs;
+	uint64_t mpidr = read_mpidr();
+	cpu_context *ns_entry_context;
+	gp_regs *ns_entry_gpregs;
 
 	scr = read_scr();
 
-	/* Switch to the non-secure view of the registers */
-	write_scr(scr | SCR_NS_BIT);
-
 	/* Find out which EL we are going to */
 	id_aa64pfr0 = read_id_aa64pfr0_el1();
 	el_status = (id_aa64pfr0 >> ID_AA64PFR0_EL2_SHIFT) &
@@ -257,23 +258,29 @@
 		write_sctlr_el1(sctlr);
 
 	/* Fulfill the cpu_on entry reqs. as per the psci spec */
-	write_scr(scr);
-	write_elr(psci_ns_entry_info[index].eret_info.entrypoint);
+	ns_entry_context = (cpu_context *) cm_get_context(mpidr, NON_SECURE);
+	assert(ns_entry_context);
 
 	/*
-	 * Set the general purpose registers to ~0 upon entry into the
-	 * non-secure world except for x0 which should contain the
-	 * context id & spsr. This is done directly on the "would be"
-	 * stack pointer. Prior to entry into the non-secure world, an
-	 * offset equivalent to the size of the 'gp_regs' structure is
-	 * added to the sp. This general purpose register context is
-	 * retrieved then.
+	 * Setup general purpose registers to return the context id and
+	 * prevent leakage of secure information into the normal world.
 	 */
-	ns_gp_regs = (gp_regs *) platform_get_stack(read_mpidr());
-	ns_gp_regs--;
-	memset(ns_gp_regs, ~0, sizeof(*ns_gp_regs));
-	ns_gp_regs->x0 = psci_ns_entry_info[index].context_id;
-	ns_gp_regs->spsr = psci_ns_entry_info[index].eret_info.spsr;
+	ns_entry_gpregs = get_gpregs_ctx(ns_entry_context);
+	write_ctx_reg(ns_entry_gpregs,
+		      CTX_GPREG_X0,
+		      psci_ns_entry_info[index].context_id);
+
+	/*
+	 * Tell the context management library to setup EL3 system registers to
+	 * be able to ERET into the ns state, and SP_EL3 points to the right
+	 * context to exit from EL3 correctly.
+	 */
+	cm_set_el3_eret_context(NON_SECURE,
+			psci_ns_entry_info[index].eret_info.entrypoint,
+			psci_ns_entry_info[index].eret_info.spsr,
+			scr);
+
+	cm_set_next_eret_context(NON_SECURE);
 }
 
 /*******************************************************************************
@@ -344,7 +351,7 @@
 		 */
 		spsr |= DAIF_ABT_BIT | DAIF_IRQ_BIT | DAIF_FIQ_BIT;
 		spsr <<= PSR_DAIF_SHIFT;
-		if(ee)
+		if (ee)
 			spsr |= SPSR32_EE_BIT;
 		spsr |= mode;
 
@@ -500,7 +507,7 @@
 	mpidr_aff_map_nodes mpidr_nodes;
 	int rc;
 
-	mpidr &= MPIDR_AFFINITY_MASK;;
+	mpidr &= MPIDR_AFFINITY_MASK;
 
 	/*
 	 * Collect the pointers to the nodes in the topology tree for
diff --git a/common/psci/psci_entry.S b/common/psci/psci_entry.S
index 28a4143..15e074c 100644
--- a/common/psci/psci_entry.S
+++ b/common/psci/psci_entry.S
@@ -34,6 +34,7 @@
 #include <psci_private.h>
 #include <runtime_svc.h>
 #include <asm_macros.S>
+#include <cm_macros.S>
 
 	.globl	psci_aff_on_finish_entry
 	.globl	psci_aff_suspend_finish_entry
@@ -74,6 +75,13 @@
 	msr	vbar_el3, x0
 	isb
 
+	/* ---------------------------------------------
+	 * Use SP_EL0 for the C runtime stack.
+	 * ---------------------------------------------
+	 */
+	msr	spsel, #0
+	isb
+
 	bl	read_mpidr
 	mov	x19, x0
 	bl	platform_set_coherent_stack
@@ -102,31 +110,19 @@
 	bl	platform_set_stack
 
 	/* ---------------------------------------------
-	 * Now that the execution stack has been set
+	 * Now that the context management has been set
 	 * up, enable full runtime exception handling.
-	 * Since we're just about to leave this EL with
-	 * ERET, we don't need an ISB here
+	 * SP_EL3 is pointing to a 'cpu_context'
+	 * structure which has an exception stack
+	 * allocated. Since we're just about to leave
+	 * this EL with ERET, we don't need an ISB here
 	 * ---------------------------------------------
 	 */
 	adr	x0, runtime_exceptions
 	msr	vbar_el3, x0
 
-	/* --------------------------------------------
-	 * Use the size of the general purpose register
-	 * context to restore the register state
-	 * stashed by earlier code
-	 * --------------------------------------------
-	 */
-	sub	sp, sp, #SIZEOF_GPREGS
-	exception_exit	restore_regs
-
-	/* --------------------------------------------
-	 * Jump back to the non-secure world assuming
-	 * that the elr and spsr setup has been done
-	 * by the finishers
-	 * --------------------------------------------
-	 */
-	eret
+	zero_callee_saved_regs
+	b	el3_exit
 _panic:
 	b	_panic
 
diff --git a/common/psci/psci_main.c b/common/psci/psci_main.c
index a70a21a..67f189d 100644
--- a/common/psci/psci_main.c
+++ b/common/psci/psci_main.c
@@ -35,6 +35,9 @@
 #include <console.h>
 #include <platform.h>
 #include <psci_private.h>
+#include <runtime_svc.h>
+#include <debug.h>
+#include <context_mgmt.h>
 
 /*******************************************************************************
  * PSCI frontend api for servicing SMCs. Described in the PSCI spec.
@@ -199,3 +202,70 @@
 	assert(0);
 }
 
+/*******************************************************************************
+ * PSCI top level handler for servicing SMCs.
+ ******************************************************************************/
+uint64_t psci_smc_handler(uint32_t smc_fid,
+			  uint64_t x1,
+			  uint64_t x2,
+			  uint64_t x3,
+			  uint64_t x4,
+			  void *cookie,
+			  void *handle,
+			  uint64_t flags)
+{
+	uint64_t rc;
+
+	switch (smc_fid) {
+	case PSCI_VERSION:
+		rc = psci_version();
+		break;
+
+	case PSCI_CPU_OFF:
+		rc = __psci_cpu_off();
+		break;
+
+	case PSCI_CPU_SUSPEND_AARCH64:
+	case PSCI_CPU_SUSPEND_AARCH32:
+		rc = __psci_cpu_suspend(x1, x2, x3);
+		break;
+
+	case PSCI_CPU_ON_AARCH64:
+	case PSCI_CPU_ON_AARCH32:
+		rc = psci_cpu_on(x1, x2, x3);
+		break;
+
+	case PSCI_AFFINITY_INFO_AARCH32:
+	case PSCI_AFFINITY_INFO_AARCH64:
+		rc = psci_affinity_info(x1, x2);
+		break;
+
+	case PSCI_MIG_AARCH32:
+	case PSCI_MIG_AARCH64:
+		rc = psci_migrate(x1);
+		break;
+
+	case PSCI_MIG_INFO_TYPE:
+		rc = psci_migrate_info_type();
+		break;
+
+	case PSCI_MIG_INFO_UP_CPU_AARCH32:
+	case PSCI_MIG_INFO_UP_CPU_AARCH64:
+		rc = psci_migrate_info_up_cpu();
+		break;
+
+	case PSCI_SYSTEM_OFF:
+		psci_system_off();
+		assert(0);
+
+	case PSCI_SYSTEM_RESET:
+		psci_system_reset();
+		assert(0);
+
+	default:
+		rc = SMC_UNK;
+		WARN("Unimplemented psci call -> 0x%x \n", smc_fid);
+	}
+
+	SMC_RET1(handle, rc);
+}
diff --git a/common/psci/psci_private.h b/common/psci/psci_private.h
index de9c291..3d7ae74 100644
--- a/common/psci/psci_private.h
+++ b/common/psci/psci_private.h
@@ -151,6 +151,16 @@
 			       int,
 			       int);
 extern unsigned int psci_afflvl_suspend_finish(unsigned long, int, int);
+
+/* Private exported functions from psci_main.c */
+extern uint64_t psci_smc_handler(uint32_t smc_fid,
+				 uint64_t x1,
+				 uint64_t x2,
+				 uint64_t x3,
+				 uint64_t x4,
+				 void *cookie,
+				 void *handle,
+				 uint64_t flags);
 #endif /*__ASSEMBLY__*/
 
 #endif /* __PSCI_PRIVATE_H__ */
diff --git a/common/psci/psci_setup.c b/common/psci/psci_setup.c
index c471d1f..8d7903c 100644
--- a/common/psci/psci_setup.c
+++ b/common/psci/psci_setup.c
@@ -195,6 +195,9 @@
 		cm_set_context(mpidr,
 				(void *) &psci_ns_context[linear_id],
 				NON_SECURE);
+
+		/* Initialize exception stack in the context */
+		cm_init_exception_stack(mpidr, NON_SECURE);
 	}
 
 	return;
@@ -348,5 +351,5 @@
 		OEN_STD_END,
 		SMC_TYPE_FAST,
 		psci_setup,
-		NULL
+		psci_smc_handler
 );