diff --git a/bl1/aarch64/bl1_entrypoint.S b/bl1/aarch64/bl1_entrypoint.S
index e7f92c71dd572160717d24b7b0d9d396e5a3bb83..82330c11eb5a13acc9e25112975ecdb33cbfcbc2 100644
--- a/bl1/aarch64/bl1_entrypoint.S
+++ b/bl1/aarch64/bl1_entrypoint.S
@@ -57,7 +57,7 @@ func bl1_entrypoint
 	 * reset e.g. cache, tlb invalidations etc.
 	 * ---------------------------------------------
 	 */
-	bl	cpu_reset_handler
+	bl	reset_handler
 
 	/* ---------------------------------------------
 	 * Enable the instruction cache, stack pointer
diff --git a/bl1/bl1.ld.S b/bl1/bl1.ld.S
index 0ca4a63094043f1f92f7191a724548bebcd3c571..8092396e593a78449f9bf8560b7c8f48b723abde 100644
--- a/bl1/bl1.ld.S
+++ b/bl1/bl1.ld.S
@@ -50,10 +50,23 @@ SECTIONS
         *bl1_entrypoint.o(.text*)
         *(.text*)
         *(.rodata*)
+
+        /*
+         * Ensure 8-byte alignment for cpu_ops so that its fields are also
+         * aligned. Also ensure cpu_ops inclusion.
+         */
+        . = ALIGN(8);
+        __CPU_OPS_START__ = .;
+        KEEP(*(cpu_ops))
+        __CPU_OPS_END__ = .;
+
         *(.vectors)
         __RO_END__ = .;
     } >ROM
 
+    ASSERT(__CPU_OPS_END__ > __CPU_OPS_START__,
+           "cpu_ops not defined for this platform.")
+
     /*
      * The .data section gets copied from ROM to RAM at runtime.
      * Its LMA must be 16-byte aligned.
diff --git a/bl1/bl1.mk b/bl1/bl1.mk
index 032dc5eba7bec83030e1b75ece45f14295a0140b..8e73bef43b1cca06d72134ad147b91f12d0fa329 100644
--- a/bl1/bl1.mk
+++ b/bl1/bl1.mk
@@ -32,6 +32,6 @@ BL1_SOURCES		+=	bl1/bl1_main.c				\
 				bl1/aarch64/bl1_arch_setup.c		\
 				bl1/aarch64/bl1_entrypoint.S		\
 				bl1/aarch64/bl1_exceptions.S		\
-				lib/aarch64/cpu_helpers.S
+				lib/cpus/aarch64/cpu_helpers.S
 
 BL1_LINKERFILE		:=	bl1/bl1.ld.S
diff --git a/bl31/aarch64/bl31_entrypoint.S b/bl31/aarch64/bl31_entrypoint.S
index a088c2e03c24354cf4f9f17723aef1be10ef267c..a9238dcb99e61206d14d83415d012445bc7aab72 100644
--- a/bl31/aarch64/bl31_entrypoint.S
+++ b/bl31/aarch64/bl31_entrypoint.S
@@ -68,7 +68,7 @@ func bl31_entrypoint
 	 * Boot ROM(BL0) programming sequence
 	 * -----------------------------------------------------
 	 */
-	bl	cpu_reset_handler
+	bl	reset_handler
 #endif
 	/* ---------------------------------------------
 	 * Enable the instruction cache, stack pointer
diff --git a/bl31/bl31.ld.S b/bl31/bl31.ld.S
index 83ef7e7bf8ae446cf5f8193d56e162ada206cccd..add65b8d6a10a6fe2fdcd724fd44e725117388e1 100644
--- a/bl31/bl31.ld.S
+++ b/bl31/bl31.ld.S
@@ -58,6 +58,15 @@ SECTIONS
         KEEP(*(rt_svc_descs))
         __RT_SVC_DESCS_END__ = .;
 
+        /*
+         * Ensure 8-byte alignment for cpu_ops so that its fields are also
+         * aligned. Also ensure cpu_ops inclusion.
+         */
+        . = ALIGN(8);
+        __CPU_OPS_START__ = .;
+        KEEP(*(cpu_ops))
+        __CPU_OPS_END__ = .;
+
         *(.vectors)
         __RO_END_UNALIGNED__ = .;
         /*
@@ -69,6 +78,9 @@ SECTIONS
         __RO_END__ = .;
     } >RAM
 
+    ASSERT(__CPU_OPS_END__ > __CPU_OPS_START__,
+           "cpu_ops not defined for this platform.")
+
     .data . : {
         __DATA_START__ = .;
         *(.data*)
diff --git a/bl31/bl31.mk b/bl31/bl31.mk
index 53b58b3af572aab2f3fb90d131c1bdc9a235b76f..f53a41ff7f49c4fec1a4e8fd47a29d737291824c 100644
--- a/bl31/bl31.mk
+++ b/bl31/bl31.mk
@@ -39,7 +39,7 @@ BL31_SOURCES		+=	bl31/bl31_main.c				\
 				bl31/aarch64/cpu_data.S				\
 				bl31/aarch64/runtime_exceptions.S		\
 				bl31/aarch64/crash_reporting.S			\
-				lib/aarch64/cpu_helpers.S			\
+				lib/cpus/aarch64/cpu_helpers.S			\
 				lib/locks/bakery/bakery_lock.c			\
 				lib/locks/exclusive/spinlock.S			\
 				services/std_svc/std_svc_setup.c		\
diff --git a/include/bl31/cpu_data.h b/include/bl31/cpu_data.h
index 355160b9537ac50b6cc80bba5d8772eb33c822ed..ba7ae0633597a83f8625e7d470910f86e2047081 100644
--- a/include/bl31/cpu_data.h
+++ b/include/bl31/cpu_data.h
@@ -32,14 +32,16 @@
 #define __CPU_DATA_H__
 
 /* Offsets for the cpu_data structure */
-#define CPU_DATA_CRASH_BUF_OFFSET	0x10
+#define CPU_DATA_CRASH_BUF_OFFSET	0x20
 #if CRASH_REPORTING
 #define CPU_DATA_LOG2SIZE		7
 #else
 #define CPU_DATA_LOG2SIZE		6
 #endif
 /* need enough space in crash buffer to save 8 registers */
-#define CPU_DATA_CRASH_BUF_SIZE	64
+#define CPU_DATA_CRASH_BUF_SIZE		64
+#define CPU_DATA_CPU_OPS_PTR		0x10
+
 #ifndef __ASSEMBLY__
 
 #include <arch_helpers.h>
@@ -66,10 +68,11 @@
  ******************************************************************************/
 typedef struct cpu_data {
 	void *cpu_context[2];
+	uint64_t cpu_ops_ptr;
+	struct psci_cpu_data psci_svc_cpu_data;
 #if CRASH_REPORTING
 	uint64_t crash_buf[CPU_DATA_CRASH_BUF_SIZE >> 3];
 #endif
-	struct psci_cpu_data psci_svc_cpu_data;
 } __aligned(CACHE_WRITEBACK_GRANULE) cpu_data_t;
 
 #if CRASH_REPORTING
@@ -82,6 +85,10 @@ CASSERT(CPU_DATA_CRASH_BUF_OFFSET == __builtin_offsetof
 CASSERT((1 << CPU_DATA_LOG2SIZE) == sizeof(cpu_data_t),
 	assert_cpu_data_log2size_mismatch);
 
+CASSERT(CPU_DATA_CPU_OPS_PTR == __builtin_offsetof
+		(cpu_data_t, cpu_ops_ptr),
+		assert_cpu_data_cpu_ops_ptr_offset_mismatch);
+
 struct cpu_data *_cpu_data_by_index(uint32_t cpu_index);
 struct cpu_data *_cpu_data_by_mpidr(uint64_t mpidr);
 
diff --git a/include/lib/aarch64/arch.h b/include/lib/aarch64/arch.h
index 042720823a10a5e3b9031a8b3a7de163098a00c1..bb33acbada7235f8846a10e0577f1e9e705b43ee 100644
--- a/include/lib/aarch64/arch.h
+++ b/include/lib/aarch64/arch.h
@@ -35,6 +35,8 @@
 /*******************************************************************************
  * MIDR bit definitions
  ******************************************************************************/
+#define MIDR_IMPL_MASK		0xff
+#define MIDR_IMPL_SHIFT		0x18
 #define MIDR_PN_MASK		0xfff
 #define MIDR_PN_SHIFT		0x4
 #define MIDR_PN_AEM		0xd0f
diff --git a/include/lib/aarch64/cpu_macros.S b/include/lib/aarch64/cpu_macros.S
new file mode 100644
index 0000000000000000000000000000000000000000..51c56e8d7ee98f40e6bdda6cd4d73bffbbc5ab4c
--- /dev/null
+++ b/include/lib/aarch64/cpu_macros.S
@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2014, 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>
+
+#define CPU_IMPL_PN_MASK	(MIDR_IMPL_MASK << MIDR_IMPL_SHIFT) | \
+				(MIDR_PN_MASK << MIDR_PN_SHIFT)
+
+	/*
+	 * Define the offsets to the fields in cpu_ops structure.
+	 */
+	.struct 0
+CPU_MIDR: /* cpu_ops midr */
+	.space  8
+/* Reset fn is needed in BL at reset vector */
+#if IMAGE_BL1 || (IMAGE_BL31 && RESET_TO_BL31)
+CPU_RESET_FUNC: /* cpu_ops reset_func */
+	.space  8
+#endif
+CPU_OPS_SIZE = .
+
+	/*
+	 * Convenience macro to declare cpu_ops structure.
+	 * Make sure the structure fields are as per the offsets
+	 * defined above.
+	 */
+	.macro declare_cpu_ops _name:req, _midr:req, _noresetfunc = 0
+	.section cpu_ops, "a"; .align 3
+	.type cpu_ops_\_name, %object
+	.quad \_midr
+#if IMAGE_BL1 || (IMAGE_BL31 && RESET_TO_BL31)
+	.if \_noresetfunc
+	.quad 0
+	.else
+	.quad \_name\()_reset_func
+	.endif
+#endif
+	.endm
diff --git a/lib/cpus/aarch64/aem_generic.S b/lib/cpus/aarch64/aem_generic.S
new file mode 100644
index 0000000000000000000000000000000000000000..a8dbf1a19b03d8d05637ca6457bd2713a8bdd239
--- /dev/null
+++ b/lib/cpus/aarch64/aem_generic.S
@@ -0,0 +1,41 @@
+/*
+ * Copyright (c) 2014, 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 <cpu_macros.S>
+
+#define BASE_AEM_MIDR 0x410FD0F0
+
+#define FOUNDATION_AEM_MIDR 0x410FD000
+
+
+declare_cpu_ops aem_generic, BASE_AEM_MIDR, 1
+
+declare_cpu_ops aem_generic, FOUNDATION_AEM_MIDR, 1
diff --git a/lib/aarch64/cpu_helpers.S b/lib/cpus/aarch64/cortex_a53.S
similarity index 82%
rename from lib/aarch64/cpu_helpers.S
rename to lib/cpus/aarch64/cortex_a53.S
index abb996d69b0ac632f535a086788fe3ac72c03ebf..2d28dd979d319b3730cef5e641a5a105c336aaa5 100644
--- a/lib/aarch64/cpu_helpers.S
+++ b/lib/cpus/aarch64/cortex_a53.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2014, 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:
@@ -27,29 +27,22 @@
  * 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 <cpu_macros.S>
+#include <plat_macros.S>
 
-	.weak	cpu_reset_handler
-
+#define CORTEX_A53_MIDR 0x410FD030
 
-func cpu_reset_handler
+func cortex_a53_reset_func
 	/* ---------------------------------------------
-	 * As a bare minimal enable the SMP bit.
+	 * As a bare minimum enable the SMP bit.
 	 * ---------------------------------------------
 	 */
-	mrs	x0, midr_el1
-	lsr	x0, x0, #MIDR_PN_SHIFT
-	and	x0, x0, #MIDR_PN_MASK
-	cmp	x0, #MIDR_PN_A57
-	b.eq	smp_setup_begin
-	cmp	x0, #MIDR_PN_A53
-	b.ne	smp_setup_end
-smp_setup_begin:
 	mrs	x0, CPUECTLR_EL1
 	orr	x0, x0, #CPUECTLR_SMP_BIT
 	msr	CPUECTLR_EL1, x0
 	isb
-smp_setup_end:
 	ret
+
+declare_cpu_ops cortex_a53, CORTEX_A53_MIDR
diff --git a/lib/cpus/aarch64/cortex_a57.S b/lib/cpus/aarch64/cortex_a57.S
new file mode 100644
index 0000000000000000000000000000000000000000..df3a8987ec10e1c9bbc6b6e4e8785050939a3797
--- /dev/null
+++ b/lib/cpus/aarch64/cortex_a57.S
@@ -0,0 +1,48 @@
+/*
+ * Copyright (c) 2014, 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 <cpu_macros.S>
+#include <plat_macros.S>
+
+#define CORTEX_A57_MIDR 0x410FD070
+
+func cortex_a57_reset_func
+	/* ---------------------------------------------
+	 * As a bare minimum enable the SMP bit.
+	 * ---------------------------------------------
+	 */
+	mrs	x0, CPUECTLR_EL1
+	orr	x0, x0, #CPUECTLR_SMP_BIT
+	msr	CPUECTLR_EL1, x0
+	isb
+	ret
+
+declare_cpu_ops cortex_a57, CORTEX_A57_MIDR
diff --git a/lib/cpus/aarch64/cpu_helpers.S b/lib/cpus/aarch64/cpu_helpers.S
new file mode 100644
index 0000000000000000000000000000000000000000..d25d1a30ea972c633ddd09e2fb8390271b107c27
--- /dev/null
+++ b/lib/cpus/aarch64/cpu_helpers.S
@@ -0,0 +1,106 @@
+/*
+ * Copyright (c) 2014, 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 <assert_macros.S>
+#include <cpu_macros.S>
+#if IMAGE_BL31
+#include <cpu_data.h>
+#endif
+
+ /* Reset fn is needed in BL at reset vector */
+#if IMAGE_BL1 || (IMAGE_BL31 && RESET_TO_BL31)
+	/*
+	 * The reset handler common to all platforms.  After a matching
+	 * cpu_ops structure entry is found, the correponding reset_handler
+	 * in the cpu_ops is invoked.
+	 */
+	.globl	reset_handler
+func reset_handler
+	mov	x10, x30
+
+	/* Get the matching cpu_ops pointer */
+	bl	get_cpu_ops_ptr
+#if ASM_ASSERTION
+	cmp	x0, #0
+	ASM_ASSERT(ne)
+#endif
+
+	/* Get the cpu_ops reset handler */
+	ldr	x2, [x0, #CPU_RESET_FUNC]
+	cbz	x2, 1f
+	blr	x2
+1:
+	ret	x10
+#endif /* IMAGE_BL1 || (IMAGE_BL31 && RESET_TO_BL31) */
+
+	/*
+	 * The below function returns the cpu_ops structure matching the
+	 * midr of the core. It reads the MIDR_EL1 and finds the matching
+	 * entry in cpu_ops entries. Only the implementation and part number
+	 * are used to match the entries.
+	 * Return :
+	 *     x0 - The matching cpu_ops pointer on Success
+	 *     x0 - 0 on failure.
+	 * Clobbers : x0 - x5
+	 */
+	.globl	get_cpu_ops_ptr
+func get_cpu_ops_ptr
+	/* Get the cpu_ops start and end locations */
+	adr	x4, (__CPU_OPS_START__ + CPU_MIDR)
+	adr	x5, (__CPU_OPS_END__ + CPU_MIDR)
+
+	/* Initialize the return parameter */
+	mov	x0, #0
+
+	/* Read the MIDR_EL1 */
+	mrs	x2, midr_el1
+	mov_imm	x3, CPU_IMPL_PN_MASK
+
+	/* Retain only the implementation and part number using mask */
+	and	w2, w2, w3
+1:
+	/* Check if we have reached end of list */
+	cmp	x4, x5
+	b.eq	error_exit
+
+	/* load the midr from the cpu_ops */
+	ldr	x1, [x4], #CPU_OPS_SIZE
+	and	w1, w1, w3
+
+	/* Check if midr matches to midr of this core */
+	cmp	w1, w2
+	b.ne	1b
+
+	/* Subtract the increment and offset to get the cpu-ops pointer */
+	sub	x0, x4, #(CPU_OPS_SIZE + CPU_MIDR)
+error_exit:
+	ret
diff --git a/plat/fvp/platform.mk b/plat/fvp/platform.mk
index ffed7e4e9ab2e55318a050da6d8256e4bd37aa9f..e7c06a80439cd60c786f0d705ce51fec8696cac7 100644
--- a/plat/fvp/platform.mk
+++ b/plat/fvp/platform.mk
@@ -74,6 +74,9 @@ PLAT_BL_COMMON_SOURCES	:=	drivers/arm/pl011/pl011_console.S		\
 				plat/fvp/fvp_io_storage.c
 
 BL1_SOURCES		+=	drivers/arm/cci400/cci400.c			\
+				lib/cpus/aarch64/aem_generic.S			\
+				lib/cpus/aarch64/cortex_a53.S			\
+				lib/cpus/aarch64/cortex_a57.S			\
 				plat/common/aarch64/platform_up_stack.S		\
 				plat/fvp/bl1_fvp_setup.c			\
 				plat/fvp/aarch64/fvp_common.c			\
@@ -90,6 +93,9 @@ BL31_SOURCES		+=	drivers/arm/cci400/cci400.c			\
 				drivers/arm/gic/gic_v2.c			\
 				drivers/arm/gic/gic_v3.c			\
 				drivers/arm/tzc400/tzc400.c			\
+				lib/cpus/aarch64/aem_generic.S			\
+				lib/cpus/aarch64/cortex_a53.S			\
+				lib/cpus/aarch64/cortex_a57.S			\
 				plat/common/plat_gic.c				\
 				plat/common/aarch64/platform_mp_stack.S		\
 				plat/fvp/bl31_fvp_setup.c			\