diff --git a/include/bl31/runtime_svc.h b/include/bl31/runtime_svc.h
index f112418..30ba29f 100644
--- a/include/bl31/runtime_svc.h
+++ b/include/bl31/runtime_svc.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2015, 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:
@@ -31,74 +31,9 @@
 #ifndef __RUNTIME_SVC_H__
 #define __RUNTIME_SVC_H__
 
-/*******************************************************************************
- * Bit definitions inside the function id as per the SMC calling convention
- ******************************************************************************/
-#define FUNCID_TYPE_SHIFT		31
-#define FUNCID_CC_SHIFT			30
-#define FUNCID_OEN_SHIFT		24
-#define FUNCID_NUM_SHIFT		0
+#include <bl_common.h>		/* to include exception types */
+#include <smcc_helpers.h>	/* to include SMCC definitions */
 
-#define FUNCID_TYPE_MASK		0x1
-#define FUNCID_CC_MASK			0x1
-#define FUNCID_OEN_MASK			0x3f
-#define FUNCID_NUM_MASK			0xffff
-
-#define FUNCID_TYPE_WIDTH		1
-#define FUNCID_CC_WIDTH			1
-#define FUNCID_OEN_WIDTH		6
-#define FUNCID_NUM_WIDTH		16
-
-#define GET_SMC_CC(id)			((id >> FUNCID_CC_SHIFT) & \
-					 FUNCID_CC_MASK)
-#define GET_SMC_TYPE(id)		((id >> FUNCID_TYPE_SHIFT) & \
-					 FUNCID_TYPE_MASK)
-
-#define SMC_64				1
-#define SMC_32				0
-#define SMC_UNK				0xffffffff
-#define SMC_TYPE_FAST			1
-#define SMC_TYPE_STD			0
-#define SMC_PREEMPTED		0xfffffffe
-/*******************************************************************************
- * Owning entity number definitions inside the function id as per the SMC
- * calling convention
- ******************************************************************************/
-#define OEN_ARM_START			0
-#define OEN_ARM_END			0
-#define OEN_CPU_START			1
-#define OEN_CPU_END			1
-#define OEN_SIP_START			2
-#define OEN_SIP_END			2
-#define OEN_OEM_START			3
-#define OEN_OEM_END			3
-#define OEN_STD_START			4	/* Standard Calls */
-#define OEN_STD_END			4
-#define OEN_TAP_START			48	/* Trusted Applications */
-#define OEN_TAP_END			49
-#define OEN_TOS_START			50	/* Trusted OS */
-#define OEN_TOS_END			63
-#define OEN_LIMIT			64
-
-/*******************************************************************************
- * Constants to indicate type of exception to the common exception handler.
- ******************************************************************************/
-#define SYNC_EXCEPTION_SP_EL0		0x0
-#define IRQ_SP_EL0			0x1
-#define FIQ_SP_EL0			0x2
-#define SERROR_SP_EL0			0x3
-#define SYNC_EXCEPTION_SP_ELX		0x4
-#define IRQ_SP_ELX			0x5
-#define FIQ_SP_ELX			0x6
-#define SERROR_SP_ELX			0x7
-#define SYNC_EXCEPTION_AARCH64		0x8
-#define IRQ_AARCH64			0x9
-#define FIQ_AARCH64			0xa
-#define SERROR_AARCH64			0xb
-#define SYNC_EXCEPTION_AARCH32		0xc
-#define IRQ_AARCH32			0xd
-#define FIQ_AARCH32			0xe
-#define SERROR_AARCH32			0xf
 
 /*******************************************************************************
  * Structure definition, typedefs & constants for the runtime service framework
@@ -122,68 +57,9 @@
 
 #ifndef __ASSEMBLY__
 
-#include <cassert.h>
-#include <context.h>
-#include <stdint.h>
-
-/* Various flags passed to SMC handlers */
-#define SMC_FROM_SECURE		(0 << 0)
-#define SMC_FROM_NON_SECURE	(1 << 0)
-
-#define is_caller_non_secure(_f)	(!!(_f & SMC_FROM_NON_SECURE))
-#define is_caller_secure(_f)		(!(is_caller_non_secure(_f)))
-
 /* Prototype for runtime service initializing function */
 typedef int32_t (*rt_svc_init_t)(void);
 
-/* Convenience macros to return from SMC handler */
-#define SMC_RET0(_h)	{ \
-	return (uint64_t) (_h);		\
-}
-#define SMC_RET1(_h, _x0)	{ \
-	write_ctx_reg(get_gpregs_ctx(_h), CTX_GPREG_X0, (_x0)); \
-	SMC_RET0(_h);						\
-}
-#define SMC_RET2(_h, _x0, _x1)	{ \
-	write_ctx_reg(get_gpregs_ctx(_h), CTX_GPREG_X1, (_x1)); \
-	SMC_RET1(_h, (_x0)); \
-}
-#define SMC_RET3(_h, _x0, _x1, _x2)	{ \
-	write_ctx_reg(get_gpregs_ctx(_h), CTX_GPREG_X2, (_x2)); \
-	SMC_RET2(_h, (_x0), (_x1)); \
-}
-#define SMC_RET4(_h, _x0, _x1, _x2, _x3)	{ \
-	write_ctx_reg(get_gpregs_ctx(_h), CTX_GPREG_X3, (_x3)); \
-	SMC_RET3(_h, (_x0), (_x1), (_x2)); \
-}
-
-
-/*
- * Convenience macros to access general purpose registers using handle provided
- * to SMC handler. These takes the offset values defined in context.h
- */
-#define SMC_GET_GP(_h, _g) \
-	read_ctx_reg(get_gpregs_ctx(_h), (_g));
-#define SMC_SET_GP(_h, _g, _v) \
-	write_ctx_reg(get_gpregs_ctx(_h), (_g), (_v));
-
-/*
- * Convenience macros to access EL3 context registers using handle provided to
- * SMC handler. These takes the offset values defined in context.h
- */
-#define SMC_GET_EL3(_h, _e) \
-	read_ctx_reg(get_el3state_ctx(_h), (_e));
-#define SMC_SET_EL3(_h, _e, _v) \
-	write_ctx_reg(get_el3state_ctx(_h), (_e), (_v));
-
-/* The macro below is used to identify a Standard Service SMC call */
-#define is_std_svc_call(_fid)		((((_fid) >> FUNCID_OEN_SHIFT) & \
-					   FUNCID_OEN_MASK) == OEN_STD_START)
-
-/* The macro below is used to identify a valid Fast SMC call */
-#define is_valid_fast_smc(_fid)		((!(((_fid) >> 16) & 0xff)) && \
-					   (GET_SMC_TYPE(_fid) == SMC_TYPE_FAST))
-
 /*
  * Prototype for runtime service SMC handler function. x0 (SMC Function ID) to
  * x4 are as passed by the caller. Rest of the arguments to SMC and the context
@@ -247,28 +123,6 @@
 					((call_type & FUNCID_TYPE_MASK) \
 					 << FUNCID_OEN_WIDTH))
 
-
-/*
- * Macro to define UUID for services. Apart from defining and initializing a
- * uuid_t structure, this macro verifies that the first word of the defined UUID
- * does not equal SMC_UNK. This is to ensure that the caller won't mistake the
- * returned UUID in x0 for an invalid SMC error return
- */
-#define DEFINE_SVC_UUID(_name, _tl, _tm, _th, _cl, _ch, \
-		_n0, _n1, _n2, _n3, _n4, _n5) \
-	CASSERT(_tl != SMC_UNK, invalid_svc_uuid);\
-	static const uuid_t _name = { \
-		_tl, _tm, _th, _cl, _ch, \
-		{ _n0, _n1, _n2, _n3, _n4, _n5 } \
-	}
-
-/* Return a UUID in the SMC return registers */
-#define SMC_UUID_RET(_h, _uuid) \
-	SMC_RET4(handle, ((const uint32_t *) &(_uuid))[0], \
-			 ((const uint32_t *) &(_uuid))[1], \
-			 ((const uint32_t *) &(_uuid))[2], \
-			 ((const uint32_t *) &(_uuid))[3])
-
 /*******************************************************************************
  * Function & variable prototypes
  ******************************************************************************/
diff --git a/include/common/bl_common.h b/include/common/bl_common.h
index 0eec989..d8741c9 100644
--- a/include/common/bl_common.h
+++ b/include/common/bl_common.h
@@ -89,6 +89,26 @@
 	(_p)->h.attr = (uint32_t)(_attr) ; \
 	} while (0)
 
+/*******************************************************************************
+ * Constants to indicate type of exception to the common exception handler.
+ ******************************************************************************/
+#define SYNC_EXCEPTION_SP_EL0		0x0
+#define IRQ_SP_EL0			0x1
+#define FIQ_SP_EL0			0x2
+#define SERROR_SP_EL0			0x3
+#define SYNC_EXCEPTION_SP_ELX		0x4
+#define IRQ_SP_ELX			0x5
+#define FIQ_SP_ELX			0x6
+#define SERROR_SP_ELX			0x7
+#define SYNC_EXCEPTION_AARCH64		0x8
+#define IRQ_AARCH64			0x9
+#define FIQ_AARCH64			0xa
+#define SERROR_AARCH64			0xb
+#define SYNC_EXCEPTION_AARCH32		0xc
+#define IRQ_AARCH32			0xd
+#define FIQ_AARCH32			0xe
+#define SERROR_AARCH32			0xf
+
 #ifndef __ASSEMBLY__
 #include <cdefs.h> /* For __dead2 */
 #include <cassert.h>
diff --git a/include/bl31/context.h b/include/common/context.h
similarity index 100%
rename from include/bl31/context.h
rename to include/common/context.h
diff --git a/include/bl31/context_mgmt.h b/include/common/context_mgmt.h
similarity index 77%
rename from include/bl31/context_mgmt.h
rename to include/common/context_mgmt.h
index 1ef4076..141b348 100644
--- a/include/bl31/context_mgmt.h
+++ b/include/common/context_mgmt.h
@@ -31,9 +31,9 @@
 #ifndef __CM_H__
 #define __CM_H__
 
+#include <arch.h>
+#include <bl_common.h>
 #include <common_def.h>
-#include <cpu_data.h>
-#include <stdint.h>
 
 /*******************************************************************************
  * Forward declarations
@@ -46,7 +46,6 @@
 void cm_init(void);
 void *cm_get_context_by_mpidr(uint64_t mpidr,
 			      uint32_t security_state) __warn_deprecated;
-static inline void *cm_get_context(uint32_t security_state);
 void cm_set_context_by_mpidr(uint64_t mpidr,
 			     void *context,
 			     uint32_t security_state) __warn_deprecated;
@@ -55,7 +54,9 @@
 void cm_set_context_by_index(unsigned int cpu_idx,
 			     void *context,
 			     unsigned int security_state);
-static inline void cm_set_context(void *context, uint32_t security_state);
+void *cm_get_context(uint32_t security_state);
+void cm_set_context(void *context, uint32_t security_state);
+inline void cm_set_next_context(void *context);
 void cm_init_context(uint64_t mpidr,
 		     const struct entry_point_info *ep) __warn_deprecated;
 void cm_init_my_context(const struct entry_point_info *ep);
@@ -76,27 +77,28 @@
 /* Inline definitions */
 
 /*******************************************************************************
- * This function returns a pointer to the most recent 'cpu_context' structure
- * for the calling CPU that was set as the context for the specified security
- * state. NULL is returned if no such structure has been specified.
+ * This function is used to program the context that's used for exception
+ * return. This initializes the SP_EL3 to a pointer to a 'cpu_context' set for
+ * the required security state
  ******************************************************************************/
-void *cm_get_context(uint32_t security_state)
+inline void cm_set_next_context(void *context)
 {
-	assert(security_state <= NON_SECURE);
+#if DEBUG
+	uint64_t sp_mode;
 
-	return get_cpu_data(cpu_context[security_state]);
+	/*
+	 * Check that this function is called with SP_EL0 as the stack
+	 * pointer
+	 */
+	__asm__ volatile("mrs	%0, SPSel\n"
+			 : "=r" (sp_mode));
+
+	assert(sp_mode == MODE_SP_EL0);
+#endif
+
+	__asm__ volatile("msr	spsel, #1\n"
+			 "mov	sp, %0\n"
+			 "msr	spsel, #0\n"
+			 : : "r" (context));
 }
-
-/*******************************************************************************
- * This function sets the pointer to the current 'cpu_context' structure for the
- * specified security state for the calling CPU
- ******************************************************************************/
-void cm_set_context(void *context, uint32_t security_state)
-{
-	assert(security_state <= NON_SECURE);
-
-	set_cpu_data(cpu_context[security_state], context);
-}
-
-
 #endif /* __CM_H__ */
diff --git a/include/common/smcc_helpers.h b/include/common/smcc_helpers.h
new file mode 100644
index 0000000..6a07b01
--- /dev/null
+++ b/include/common/smcc_helpers.h
@@ -0,0 +1,165 @@
+/*
+ * Copyright (c) 2015, 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.
+ */
+
+#ifndef __SMCC_HELPERS_H__
+#define __SMCC_HELPERS_H__
+
+/*******************************************************************************
+ * Bit definitions inside the function id as per the SMC calling convention
+ ******************************************************************************/
+#define FUNCID_TYPE_SHIFT		31
+#define FUNCID_CC_SHIFT			30
+#define FUNCID_OEN_SHIFT		24
+#define FUNCID_NUM_SHIFT		0
+
+#define FUNCID_TYPE_MASK		0x1
+#define FUNCID_CC_MASK			0x1
+#define FUNCID_OEN_MASK			0x3f
+#define FUNCID_NUM_MASK			0xffff
+
+#define FUNCID_TYPE_WIDTH		1
+#define FUNCID_CC_WIDTH			1
+#define FUNCID_OEN_WIDTH		6
+#define FUNCID_NUM_WIDTH		16
+
+#define GET_SMC_CC(id)			((id >> FUNCID_CC_SHIFT) & \
+					 FUNCID_CC_MASK)
+#define GET_SMC_TYPE(id)		((id >> FUNCID_TYPE_SHIFT) & \
+					 FUNCID_TYPE_MASK)
+
+#define SMC_64				1
+#define SMC_32				0
+#define SMC_UNK				0xffffffff
+#define SMC_TYPE_FAST			1
+#define SMC_TYPE_STD			0
+#define SMC_PREEMPTED		0xfffffffe
+/*******************************************************************************
+ * Owning entity number definitions inside the function id as per the SMC
+ * calling convention
+ ******************************************************************************/
+#define OEN_ARM_START			0
+#define OEN_ARM_END			0
+#define OEN_CPU_START			1
+#define OEN_CPU_END			1
+#define OEN_SIP_START			2
+#define OEN_SIP_END			2
+#define OEN_OEM_START			3
+#define OEN_OEM_END			3
+#define OEN_STD_START			4	/* Standard Calls */
+#define OEN_STD_END			4
+#define OEN_TAP_START			48	/* Trusted Applications */
+#define OEN_TAP_END			49
+#define OEN_TOS_START			50	/* Trusted OS */
+#define OEN_TOS_END			63
+#define OEN_LIMIT			64
+
+#ifndef __ASSEMBLY__
+
+#include <cassert.h>
+#include <context.h>
+#include <stdint.h>
+
+/* Various flags passed to SMC handlers */
+#define SMC_FROM_SECURE		(0 << 0)
+#define SMC_FROM_NON_SECURE	(1 << 0)
+
+#define is_caller_non_secure(_f)	(!!(_f & SMC_FROM_NON_SECURE))
+#define is_caller_secure(_f)		(!(is_caller_non_secure(_f)))
+
+/* Convenience macros to return from SMC handler */
+#define SMC_RET0(_h)	{ \
+	return (uint64_t) (_h);		\
+}
+#define SMC_RET1(_h, _x0)	{ \
+	write_ctx_reg(get_gpregs_ctx(_h), CTX_GPREG_X0, (_x0)); \
+	SMC_RET0(_h);						\
+}
+#define SMC_RET2(_h, _x0, _x1)	{ \
+	write_ctx_reg(get_gpregs_ctx(_h), CTX_GPREG_X1, (_x1)); \
+	SMC_RET1(_h, (_x0)); \
+}
+#define SMC_RET3(_h, _x0, _x1, _x2)	{ \
+	write_ctx_reg(get_gpregs_ctx(_h), CTX_GPREG_X2, (_x2)); \
+	SMC_RET2(_h, (_x0), (_x1)); \
+}
+#define SMC_RET4(_h, _x0, _x1, _x2, _x3)	{ \
+	write_ctx_reg(get_gpregs_ctx(_h), CTX_GPREG_X3, (_x3)); \
+	SMC_RET3(_h, (_x0), (_x1), (_x2)); \
+}
+
+/*
+ * Convenience macros to access general purpose registers using handle provided
+ * to SMC handler. These takes the offset values defined in context.h
+ */
+#define SMC_GET_GP(_h, _g) \
+	read_ctx_reg(get_gpregs_ctx(_h), (_g));
+#define SMC_SET_GP(_h, _g, _v) \
+	write_ctx_reg(get_gpregs_ctx(_h), (_g), (_v));
+
+/*
+ * Convenience macros to access EL3 context registers using handle provided to
+ * SMC handler. These takes the offset values defined in context.h
+ */
+#define SMC_GET_EL3(_h, _e) \
+	read_ctx_reg(get_el3state_ctx(_h), (_e));
+#define SMC_SET_EL3(_h, _e, _v) \
+	write_ctx_reg(get_el3state_ctx(_h), (_e), (_v));
+
+/* The macro below is used to identify a Standard Service SMC call */
+#define is_std_svc_call(_fid)		((((_fid) >> FUNCID_OEN_SHIFT) & \
+					   FUNCID_OEN_MASK) == OEN_STD_START)
+
+/* The macro below is used to identify a valid Fast SMC call */
+#define is_valid_fast_smc(_fid)		((!(((_fid) >> 16) & 0xff)) && \
+					   (GET_SMC_TYPE(_fid) == SMC_TYPE_FAST))
+
+/*
+ * Macro to define UUID for services. Apart from defining and initializing a
+ * uuid_t structure, this macro verifies that the first word of the defined UUID
+ * does not equal SMC_UNK. This is to ensure that the caller won't mistake the
+ * returned UUID in x0 for an invalid SMC error return
+ */
+#define DEFINE_SVC_UUID(_name, _tl, _tm, _th, _cl, _ch, \
+		_n0, _n1, _n2, _n3, _n4, _n5) \
+	CASSERT(_tl != SMC_UNK, invalid_svc_uuid);\
+	static const uuid_t _name = { \
+		_tl, _tm, _th, _cl, _ch, \
+		{ _n0, _n1, _n2, _n3, _n4, _n5 } \
+	}
+
+/* Return a UUID in the SMC return registers */
+#define SMC_UUID_RET(_h, _uuid) \
+	SMC_RET4(handle, ((const uint32_t *) &(_uuid))[0], \
+			 ((const uint32_t *) &(_uuid))[1], \
+			 ((const uint32_t *) &(_uuid))[2], \
+			 ((const uint32_t *) &(_uuid))[3])
+
+#endif /*__ASSEMBLY__*/
+#endif /* __SMCC_HELPERS_H__ */