--- locore.S.orig	2016-12-01 15:19:41.385734000 -0500
+++ locore.S	2016-12-01 16:49:00.715847000 -0500
@@ -46,6 +46,17 @@
 #define	NORMAL_UNCACHED	1
 #define	NORMAL_MEM	2
 
+/* ZZZ ? Are these already defined */
+#define PSCI_VERSION		0x84000000
+#define PSCI_CPU_ON		0xC4000003
+#define PSCI_SYSTEM_OFF		0x84000008
+#define PSCI_SYSTEM_RESET	0x84000009
+
+#define WDOG_ADDRESS		0x3f100000
+#define WDOG_PASSWORD		0x5a000000
+
+#define SPIN_TABLE		0xD8
+	
 /*
  * We assume:
  *  MMU      on with an identity map, or off
@@ -57,6 +68,16 @@
 	.text
 	.globl _start
 _start:
+	mrs	w0, currentel
+	asr	w0, #2
+	and	w0, #3
+	cmp	w0, 3
+	b.ne	_in_el2
+	adr	x0, el3_exception_vectors
+	msr	vbar_el3, x0
+	/* ZZZ should just drop to el1 in one step */
+	bl	drop_to_el2
+	
 	/* Drop to EL1 */
 	bl	drop_to_el1
 
@@ -192,6 +213,18 @@
 #endif
 
 /*
+ * If we are started in EL3, drop to EL2
+*/
+drop_to_el2:	
+	mov	x0, #SPSR_EL3_VAL
+	msr	spsr_el3, x0
+	adr	x0, _in_el2
+	msr	elr_el3, x0
+	eret
+_in_el2:
+	ret
+	
+/*
  * If we are started in EL2, configure the required hypervisor
  * registers and drop to EL1.
  */
@@ -697,3 +730,81 @@
 	.global	szsigcode
 szsigcode:
 	.quad	esigcode - sigcode
+
+
+		
+.align  11
+.globl  el3_exception_vectors
+el3_exception_vectors:
+	eret     /* Sync, Current EL using SP0 */
+	.align  7
+	eret     /* IRQ, Current EL using SP0 */
+	.align  7
+	eret     /* FIQ, Current EL using SP0 */
+	.align  7
+	eret     /* SError, Current EL using SP0 */
+	.align  7
+	eret     /* Sync, Current EL using SPx */
+	.align  7
+	eret     /* IRQ, Current EL using SPx */
+	.align  7
+	eret     /* FIQ, Current EL using SPx */
+	.align  7
+	eret     /* SError, Current EL using SPx */
+	.align  7
+	b	handle_sync	/* Sync, Lower EL using AArch64 */
+	.align  7
+	eret     /* IRQ, Lower EL using AArch64 */
+	.align  7
+	eret     /* FIQ, Lower EL using AArch64 */
+	.align  7
+	eret     /* SError, Lower EL using AArch64 */
+	.align  7
+	eret     /* Sync, Lower EL using AArch32 */
+	.align  7
+	eret     /* IRQ, Lower EL using AArch32 */
+	.align  7
+	eret     /* FIQ, Lower EL using AArch32 */
+	.align  7
+	eret     /* SError, Lower EL using AArch32 */
+
+handle_sync:
+	ldr     x9, =PSCI_VERSION
+	cmp     x0, x9
+	b.eq    psci_version
+	ldr     x9, =PSCI_CPU_ON
+	cmp     x0, x9
+	b.eq    psci_cpu_on
+	ldr     x9, =PSCI_SYSTEM_OFF
+	cmp     x0, x9
+	b.eq    psci_reset
+	ldr     x9, =PSCI_SYSTEM_RESET
+	cmp     x0, x9
+	b.eq    psci_reset
+	/* fall through to invalid ID case */
+invalid_smc_id:
+	ldr     x0, =0xFFFFFFFF
+	eret
+psci_version:
+	/* Return v0.2 */
+	mov     x0, #0x00000002
+	eret
+psci_cpu_on:
+	mov	x5, #SPIN_TABLE
+	lsl	x1, x1, 4
+	str	x2, [x5, x1]
+	add	x1, x1, 8
+	str	x3, [x5, x1]
+	sev
+	mov	x0, #0 /* x1 = 0 */
+	eret
+psci_reset:
+	ldr	x0, =WDOG_ADDRESS
+	ldr	w1, =WDOG_PASSWORD
+	orr	w2, w1, #0x8
+	orr	w2, w2, #0x2 /* 10 ticks (~150ms) */
+	str	w2, [x0, #0x24]
+	orr	w2, w1, #0x20 /* full reset */
+	str	w2, [x0, #0x1c]
+loop:
+	b loop