diff --git a/Makefile.am b/Makefile.am
index a5e8e8b..40bc5d6 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -48,6 +48,7 @@
 endif
 
 if PSCI
+DEFINES		+= -DPSCI
 ARCH_OBJ	+= psci.o
 COMMON_OBJ	+= psci.o
 PSCI_NODE	:= psci {				\
diff --git a/arch/aarch32/include/asm/cpu.h b/arch/aarch32/include/asm/cpu.h
index c1bce9a..3426075 100644
--- a/arch/aarch32/include/asm/cpu.h
+++ b/arch/aarch32/include/asm/cpu.h
@@ -63,6 +63,7 @@
 #define SCR		"p15, 0, %0, c1, c1, 0"
 #define NSACR		"p15, 0, %0, c1, c1, 2"
 #define ICIALLU		"p15, 0, %0, c7, c5, 0"
+#define MVBAR		"p15, 0, %0, c12, c0, 1"
 
 #define ICC_SRE		"p15, 6, %0, c12, c12, 5"
 #define ICC_CTLR	"p15, 6, %0, c12, c12, 4"
diff --git a/arch/aarch32/init.c b/arch/aarch32/init.c
index 7143c66..e25f0c7 100644
--- a/arch/aarch32/init.c
+++ b/arch/aarch32/init.c
@@ -8,6 +8,7 @@
  */
 #include <cpu.h>
 #include <platform.h>
+#include <stdbool.h>
 
 static const char *mode_string(void)
 {
@@ -39,3 +40,18 @@
 
 	mcr(CNTFRQ, COUNTER_FREQ);
 }
+
+#ifdef PSCI
+extern char psci_vectors[];
+
+bool cpu_init_psci_arch(void)
+{
+	if (read_cpsr_mode() != PSR_MON)
+		return false;
+
+	mcr(MVBAR, (unsigned long)psci_vectors);
+	isb();
+
+	return true;
+}
+#endif
diff --git a/arch/aarch32/psci.S b/arch/aarch32/psci.S
index e0d2972..cdc36b0 100644
--- a/arch/aarch32/psci.S
+++ b/arch/aarch32/psci.S
@@ -15,7 +15,7 @@
 
 	.section .vectors
 	.align 6
-smc_vectors:
+ASM_DATA(psci_vectors)
 	b	err_exception			@ Reset
 	b	err_exception			@ Undef
 	b	handle_smc			@ SMC
@@ -39,11 +39,5 @@
 	movs	pc, lr
 
 ASM_FUNC(start_el3)
-	ldr	r0, =smc_vectors
-	blx	setup_vector
-	/* pass through */
-
 ASM_FUNC(start_no_el3)
-	cpuid	r0, r1
-	blx	find_logical_id
 	b	psci_first_spin
diff --git a/arch/aarch32/utils.S b/arch/aarch32/utils.S
index 5809f48..58279aa 100644
--- a/arch/aarch32/utils.S
+++ b/arch/aarch32/utils.S
@@ -35,12 +35,3 @@
 	bx	lr
 3:	mov	r0, #MPIDR_INVALID
 	bx	lr
-
-/*
- * Setup EL3 vectors.
- * r0: vector address
- */
-ASM_FUNC(setup_vector)
-	mcr	p15, 0, r0, c12, c0, 1	@ MVBAR
-	isb
-	bx	lr
diff --git a/arch/aarch64/init.c b/arch/aarch64/init.c
index 74190be..6677f2b 100644
--- a/arch/aarch64/init.c
+++ b/arch/aarch64/init.c
@@ -89,3 +89,18 @@
 
 	msr(CNTFRQ_EL0, COUNTER_FREQ);
 }
+
+#ifdef PSCI
+extern char psci_vectors[];
+
+bool cpu_init_psci_arch(void)
+{
+	if (mrs(CurrentEL) != CURRENTEL_EL3)
+		return false;
+
+	msr(VBAR_EL3, (unsigned long)psci_vectors);
+	isb();
+
+	return true;
+}
+#endif
diff --git a/arch/aarch64/psci.S b/arch/aarch64/psci.S
index 8bd224b..d6ca2eb 100644
--- a/arch/aarch64/psci.S
+++ b/arch/aarch64/psci.S
@@ -19,7 +19,7 @@
 	.section .vectors, "w"
 
 	.align 11
-vector:
+ASM_DATA(psci_vectors)
 	// current EL, SP_EL0
 	ventry	err_exception	// synchronous
 	ventry	err_exception	// IRQ
@@ -80,22 +80,5 @@
 	eret
 
 ASM_FUNC(start_el3)
-	ldr	x0, =vector
-	bl	setup_vector
-
-	/* only boot the primary cpu (entry 0 in the table) */
-	cpuid	x0, x1
-	bl	find_logical_id
-	b	psci_first_spin
-
-/*
- * This PSCI implementation requires EL3. Without EL3 we'll only boot the
- * primary cpu, all others will be trapped in an infinite loop.
- */
 ASM_FUNC(start_no_el3)
-	cpuid	x0, x1
-	bl	find_logical_id
-	cbz	x0, psci_first_spin
-spin_dead:
-	wfe
-	b	spin_dead
+	b	psci_first_spin
diff --git a/arch/aarch64/spin.S b/arch/aarch64/spin.S
index 1ea1c0b..764c532 100644
--- a/arch/aarch64/spin.S
+++ b/arch/aarch64/spin.S
@@ -12,6 +12,9 @@
 
 	.text
 
+ASM_FUNC(cpu_init_bootmethod)
+	ret
+
 ASM_FUNC(start_el3)
 ASM_FUNC(start_no_el3)
 	cpuid	x0, x1
diff --git a/arch/aarch64/utils.S b/arch/aarch64/utils.S
index 85c7f8a..32393cc 100644
--- a/arch/aarch64/utils.S
+++ b/arch/aarch64/utils.S
@@ -32,12 +32,3 @@
 	ret
 3:	mov	x0, #MPIDR_INVALID
 	ret
-
-/*
- * Setup EL3 vectors
- * x0: vector address
- */
-ASM_FUNC(setup_vector)
-	msr	VBAR_EL3, x0
-	isb
-	ret
diff --git a/common/init.c b/common/init.c
index fc74b9e..3c05ac3 100644
--- a/common/init.c
+++ b/common/init.c
@@ -6,6 +6,7 @@
  * Use of this source code is governed by a BSD-style license that can be
  * found in the LICENSE.txt file.
  */
+#include <boot.h>
 #include <cpu.h>
 #include <platform.h>
 
@@ -44,7 +45,9 @@
 
 void cpu_init_bootwrapper(void)
 {
-	if (this_cpu_logical_id() == 0) {
+	unsigned int cpu = this_cpu_logical_id();
+
+	if (cpu == 0) {
 		init_uart();
 		announce_bootwrapper();
 		announce_arch();
@@ -52,4 +55,6 @@
 		print_string("\r\n");
 		init_platform();
 	}
+
+	cpu_init_bootmethod(cpu);
 }
diff --git a/common/psci.c b/common/psci.c
index a0e8700..5ae4255 100644
--- a/common/psci.c
+++ b/common/psci.c
@@ -12,6 +12,7 @@
 #include <bakery_lock.h>
 #include <boot.h>
 #include <cpu.h>
+#include <platform.h>
 #include <psci.h>
 
 #ifndef CPU_IDS
@@ -78,12 +79,25 @@
 	}
 }
 
-void __noreturn psci_first_spin(unsigned int cpu)
+void __noreturn psci_first_spin(void)
 {
-	if (cpu == MPIDR_INVALID)
-		while (1);
+	unsigned int cpu = this_cpu_logical_id();
 
 	first_spin(cpu, branch_table + cpu, PSCI_ADDR_INVALID);
 
 	unreachable();
 }
+
+void cpu_init_bootmethod(unsigned int cpu)
+{
+	if (cpu_init_psci_arch())
+		return;
+
+	if (cpu == 0) {
+		print_string("WARNING: PSCI could not be initialized. Boot may fail\r\n\r\n");
+		return;
+	}
+
+	while (1)
+		wfe();
+}
diff --git a/include/boot.h b/include/boot.h
index d75e013..459d1d5 100644
--- a/include/boot.h
+++ b/include/boot.h
@@ -10,10 +10,17 @@
 #define __BOOT_H
 
 #include <compiler.h>
+#include <stdbool.h>
 
 void __noreturn spin(unsigned long *mbox, unsigned long invalid, int is_entry);
 
 void __noreturn first_spin(unsigned int cpu, unsigned long *mbox,
 			   unsigned long invalid_addr);
 
+void cpu_init_bootmethod(unsigned int cpu);
+
+#ifdef PSCI
+bool cpu_init_psci_arch(void);
+#endif
+
 #endif