diff --git a/bl31/bl31_main.c b/bl31/bl31_main.c
index f79a122..6765e60 100644
--- a/bl31/bl31_main.c
+++ b/bl31/bl31_main.c
@@ -71,9 +71,6 @@
  ******************************************************************************/
 void bl31_main(void)
 {
-#if DEBUG
-	unsigned long mpidr = read_mpidr();
-#endif
 
 	/* Perform remaining generic architectural setup from EL3 */
 	bl31_arch_setup();
@@ -98,7 +95,7 @@
 	 * structure which has an exception stack allocated.  The PSCI
 	 * service should have set the context.
 	 */
-	assert(cm_get_context(mpidr, NON_SECURE));
+	assert(cm_get_context(NON_SECURE));
 	cm_set_next_eret_context(NON_SECURE);
 	cm_init_pcpu_ptr_cache();
 	write_vbar_el3((uint64_t) runtime_exceptions);
@@ -195,7 +192,7 @@
 	 * Save the args generated in BL2 for the image in the right context
 	 * used on its entry
 	 */
-	ctx = cm_get_context(read_mpidr(), image_type);
+	ctx = cm_get_context(image_type);
 	gp_regs = get_gpregs_ctx(ctx);
 	memcpy(gp_regs, (void *)&next_image_info->args, sizeof(aapcs64_params_t));
 
diff --git a/bl31/context_mgmt.c b/bl31/context_mgmt.c
index b3dcf2d..b602840 100644
--- a/bl31/context_mgmt.c
+++ b/bl31/context_mgmt.c
@@ -77,10 +77,10 @@
 
 /*******************************************************************************
  * This function returns a pointer to the most recent 'cpu_context' structure
- * that was set as the context for the specified security state. NULL is
- * returned if no such structure has been specified.
+ * for the CPU identified by MPIDR that was set as the context for the specified
+ * security state. NULL is returned if no such structure has been specified.
  ******************************************************************************/
-void *cm_get_context(uint64_t mpidr, uint32_t security_state)
+void *cm_get_context_by_mpidr(uint64_t mpidr, uint32_t security_state)
 {
 	uint32_t linear_id = platform_get_core_pos(mpidr);
 
@@ -90,10 +90,24 @@
 }
 
 /*******************************************************************************
- * This function sets the pointer to the current 'cpu_context' structure for the
- * specified security state.
+ * 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.
  ******************************************************************************/
-void cm_set_context(uint64_t mpidr, void *context, uint32_t security_state)
+void *cm_get_context(uint32_t security_state)
+{
+	uint32_t linear_id = platform_get_core_pos(read_mpidr());
+
+	assert(security_state <= NON_SECURE);
+
+	return cm_context_info[linear_id].ptr[security_state];
+}
+
+/*******************************************************************************
+ * This function sets the pointer to the current 'cpu_context' structure for the
+ * specified security state for the CPU identified by MPIDR
+ ******************************************************************************/
+void cm_set_context_by_mpidr(uint64_t mpidr, void *context, uint32_t security_state)
 {
 	uint32_t linear_id = platform_get_core_pos(mpidr);
 
@@ -103,6 +117,15 @@
 }
 
 /*******************************************************************************
+ * 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)
+{
+	cm_set_context_by_mpidr(read_mpidr(), context, security_state);
+}
+
+/*******************************************************************************
  * The next four functions are used by runtime services to save and restore EL3
  * and EL1 contexts on the 'cpu_context' structure for the specified security
  * state.
@@ -111,7 +134,7 @@
 {
 	cpu_context_t *ctx;
 
-	ctx = cm_get_context(read_mpidr(), security_state);
+	ctx = cm_get_context(security_state);
 	assert(ctx);
 
 	el3_sysregs_context_save(get_el3state_ctx(ctx));
@@ -121,7 +144,7 @@
 {
 	cpu_context_t *ctx;
 
-	ctx = cm_get_context(read_mpidr(), security_state);
+	ctx = cm_get_context(security_state);
 	assert(ctx);
 
 	el3_sysregs_context_restore(get_el3state_ctx(ctx));
@@ -131,7 +154,7 @@
 {
 	cpu_context_t *ctx;
 
-	ctx = cm_get_context(read_mpidr(), security_state);
+	ctx = cm_get_context(security_state);
 	assert(ctx);
 
 	el1_sysregs_context_save(get_sysregs_ctx(ctx));
@@ -141,7 +164,7 @@
 {
 	cpu_context_t *ctx;
 
-	ctx = cm_get_context(read_mpidr(), security_state);
+	ctx = cm_get_context(security_state);
 	assert(ctx);
 
 	el1_sysregs_context_restore(get_sysregs_ctx(ctx));
@@ -159,7 +182,7 @@
 	cpu_context_t *ctx;
 	el3_state_t *state;
 
-	ctx = cm_get_context(read_mpidr(), security_state);
+	ctx = cm_get_context(security_state);
 	assert(ctx);
 
 	/* Program the interrupt routing model for this security state */
@@ -183,7 +206,7 @@
 	cpu_context_t *ctx;
 	el3_state_t *state;
 
-	ctx = cm_get_context(read_mpidr(), security_state);
+	ctx = cm_get_context(security_state);
 	assert(ctx);
 
 	/* Populate EL3 state so that ERET jumps to the correct entry */
@@ -204,7 +227,7 @@
 	el3_state_t *state;
 	uint32_t scr_el3;
 
-	ctx = cm_get_context(read_mpidr(), security_state);
+	ctx = cm_get_context(security_state);
 	assert(ctx);
 
 	/* Ensure that the bit position is a valid one */
@@ -233,7 +256,7 @@
 	cpu_context_t *ctx;
 	el3_state_t *state;
 
-	ctx = cm_get_context(read_mpidr(), security_state);
+	ctx = cm_get_context(security_state);
 	assert(ctx);
 
 	/* Populate EL3 state so that ERET jumps to the correct entry */
@@ -253,7 +276,7 @@
 	uint64_t sp_mode;
 #endif
 
-	ctx = cm_get_context(read_mpidr(), security_state);
+	ctx = cm_get_context(security_state);
 	assert(ctx);
 
 #if DEBUG