diff --git a/bl31/aarch64/cpu_data.S b/bl31/aarch64/cpu_data.S
index a53bd6d..0842825 100644
--- a/bl31/aarch64/cpu_data.S
+++ b/bl31/aarch64/cpu_data.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014-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:
@@ -32,7 +32,6 @@
 #include <cpu_data.h>
 
 .globl	init_cpu_data_ptr
-.globl	_cpu_data_by_mpidr
 .globl	_cpu_data_by_index
 
 /* -----------------------------------------------------------------
@@ -41,37 +40,19 @@
  * Initialise the TPIDR_EL3 register to refer to the cpu_data_t
  * for the calling CPU. This must be called before cm_get_cpu_data()
  *
- * This can be called without a valid stack.
- * clobbers: x0, x1, x9, x10
+ * This can be called without a valid stack. It assumes that
+ * plat_my_core_pos() does not clobber register x10.
+ * clobbers: x0, x1, x10
  * -----------------------------------------------------------------
  */
 func init_cpu_data_ptr
 	mov	x10, x30
-	mrs	x0, mpidr_el1
-	bl	_cpu_data_by_mpidr
+	bl	plat_my_core_pos
+	bl	_cpu_data_by_index
 	msr	tpidr_el3, x0
 	ret	x10
 endfunc init_cpu_data_ptr
 
-
-/* -----------------------------------------------------------------
- * cpu_data_t *_cpu_data_by_mpidr(uint64_t mpidr)
- *
- * Return the cpu_data structure for the CPU with given MPIDR
- *
- * This can be called without a valid stack. It assumes that
- * platform_get_core_pos() does not clobber register x9.
- * clobbers: x0, x1, x9
- * -----------------------------------------------------------------
- */
-func _cpu_data_by_mpidr
-	mov	x9, x30
-	bl	platform_get_core_pos
-	mov	x30, x9
-	b	_cpu_data_by_index
-endfunc _cpu_data_by_mpidr
-
-
 /* -----------------------------------------------------------------
  * cpu_data_t *_cpu_data_by_index(uint32_t cpu_index)
  *
diff --git a/bl31/bl31_main.c b/bl31/bl31_main.c
index 19f3774..a1a3710 100644
--- a/bl31/bl31_main.c
+++ b/bl31/bl31_main.c
@@ -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:
@@ -155,7 +155,7 @@
 	INFO("BL3-1: Next image address = 0x%llx\n",
 		(unsigned long long) next_image_info->pc);
 	INFO("BL3-1: Next image spsr = 0x%x\n", next_image_info->spsr);
-	cm_init_context(read_mpidr_el1(), next_image_info);
+	cm_init_my_context(next_image_info);
 	cm_prepare_el3_exit(image_type);
 }