diff --git a/arm/cstart64.S b/arm/cstart64.S
index 1d98066d0e187..d1860a94fb2d3 100644
--- a/arm/cstart64.S
+++ b/arm/cstart64.S
@@ -7,6 +7,7 @@
  */
 #define __ASSEMBLY__
 #include <asm/asm-offsets.h>
+#include <asm/ptrace.h>
 
 .section .init
 
@@ -26,7 +27,7 @@ start:
 	msr	cpacr_el1, x0
 
 	/* set up exception handling */
-//	bl	exceptions_init
+	bl	exceptions_init
 
 	/* complete setup */
 	ldp	x0, x1, [sp], #16
@@ -40,9 +41,148 @@ start:
 	bl	exit
 	b	halt
 
+exceptions_init:
+	adr	x0, vector_table
+	msr	vbar_el1, x0
+	isb
+	ret
+
 .text
 
 .globl halt
 halt:
 1:	wfi
 	b	1b
+
+/*
+ * Vectors
+ * Adapted from arch/arm64/kernel/entry.S
+ */
+.macro vector_stub, name, vec
+\name:
+	stp	 x0,  x1, [sp, #-S_FRAME_SIZE]!
+	stp	 x2,  x3, [sp,  #16]
+	stp	 x4,  x5, [sp,  #32]
+	stp	 x6,  x7, [sp,  #48]
+	stp	 x8,  x9, [sp,  #64]
+	stp	x10, x11, [sp,  #80]
+	stp	x12, x13, [sp,  #96]
+	stp	x14, x15, [sp, #112]
+	stp	x16, x17, [sp, #128]
+	stp	x18, x19, [sp, #144]
+	stp	x20, x21, [sp, #160]
+	stp	x22, x23, [sp, #176]
+	stp	x24, x25, [sp, #192]
+	stp	x26, x27, [sp, #208]
+	stp	x28, x29, [sp, #224]
+
+	str	x30, [sp, #S_LR]
+
+	.if \vec >= 8
+	mrs	x1, sp_el0
+	.else
+	add	x1, sp, #S_FRAME_SIZE
+	.endif
+	str	x1, [sp, #S_SP]
+
+	mrs	x1, elr_el1
+	mrs	x2, spsr_el1
+	stp	x1, x2, [sp, #S_PC]
+
+	and	x2, x2, #PSR_MODE_MASK
+	cmp	x2, #PSR_MODE_EL0t
+	b.ne	1f
+	adr	x2, user_mode
+	str	xzr, [x2]		/* we're in kernel mode now */
+
+1:	mov	x0, \vec
+	mov	x1, sp
+	mrs	x2, esr_el1
+	bl	do_handle_exception
+
+	ldp	x1, x2, [sp, #S_PC]
+	msr	spsr_el1, x2
+	msr	elr_el1, x1
+
+	and	x2, x2, #PSR_MODE_MASK
+	cmp	x2, #PSR_MODE_EL0t
+	b.ne	1f
+	adr	x2, user_mode
+	mov	x1, #1
+	str	x1, [x2]		/* we're going back to user mode */
+
+1:
+	.if \vec >= 8
+	ldr	x1, [sp, #S_SP]
+	msr	sp_el0, x1
+	.endif
+
+	ldr	x30, [sp, #S_LR]
+
+	ldp	x28, x29, [sp, #224]
+	ldp	x26, x27, [sp, #208]
+	ldp	x24, x25, [sp, #192]
+	ldp	x22, x23, [sp, #176]
+	ldp	x20, x21, [sp, #160]
+	ldp	x18, x19, [sp, #144]
+	ldp	x16, x17, [sp, #128]
+	ldp	x14, x15, [sp, #112]
+	ldp	x12, x13, [sp,  #96]
+	ldp	x10, x11, [sp,  #80]
+	ldp	 x8,  x9, [sp,  #64]
+	ldp	 x6,  x7, [sp,  #48]
+	ldp	 x4,  x5, [sp,  #32]
+	ldp	 x2,  x3, [sp,  #16]
+	ldp	 x0,  x1, [sp], #S_FRAME_SIZE
+
+	eret
+.endm
+
+vector_stub	el1t_sync,     0
+vector_stub	el1t_irq,      1
+vector_stub	el1t_fiq,      2
+vector_stub	el1t_error,    3
+
+vector_stub	el1h_sync,     4
+vector_stub	el1h_irq,      5
+vector_stub	el1h_fiq,      6
+vector_stub	el1h_error,    7
+
+vector_stub	el0_sync_64,   8
+vector_stub	el0_irq_64,    9
+vector_stub	el0_fiq_64,   10
+vector_stub	el0_error_64, 11
+
+vector_stub	el0_sync_32,  12
+vector_stub	el0_irq_32,   13
+vector_stub	el0_fiq_32,   14
+vector_stub	el0_error_32, 15
+
+.section .text.ex
+
+.macro ventry, label
+.align 7
+	b	\label
+.endm
+
+.align 11
+vector_table:
+	ventry	el1t_sync			// Synchronous EL1t
+	ventry	el1t_irq			// IRQ EL1t
+	ventry	el1t_fiq			// FIQ EL1t
+	ventry	el1t_error			// Error EL1t
+
+	ventry	el1h_sync			// Synchronous EL1h
+	ventry	el1h_irq			// IRQ EL1h
+	ventry	el1h_fiq			// FIQ EL1h
+	ventry	el1h_error			// Error EL1h
+
+	ventry	el0_sync_64			// Synchronous 64-bit EL0
+	ventry	el0_irq_64			// IRQ 64-bit EL0
+	ventry	el0_fiq_64			// FIQ 64-bit EL0
+	ventry	el0_error_64			// Error 64-bit EL0
+
+	ventry	el0_sync_32			// Synchronous 32-bit EL0
+	ventry	el0_irq_32			// IRQ 32-bit EL0
+	ventry	el0_fiq_32			// FIQ 32-bit EL0
+	ventry	el0_error_32			// Error 32-bit EL0
diff --git a/arm/selftest.c b/arm/selftest.c
index 30f44261d47db..824af2f3c15af 100644
--- a/arm/selftest.c
+++ b/arm/selftest.c
@@ -8,12 +8,10 @@
 #include <libcflat.h>
 #include <alloc.h>
 #include <asm/setup.h>
-#ifdef __arm__
 #include <asm/ptrace.h>
 #include <asm/asm-offsets.h>
 #include <asm/processor.h>
 #include <asm/page.h>
-#endif
 
 #define TESTGRP "selftest"
 
@@ -80,8 +78,10 @@ static void check_setup(int argc, char **argv)
 	assert_args(nr_tests, 2);
 }
 
-#ifdef __arm__
 static struct pt_regs expected_regs;
+static bool und_works;
+static bool svc_works;
+#if defined(__arm__)
 /*
  * Capture the current register state and execute an instruction
  * that causes an exception. The test handler will check that its
@@ -122,7 +122,6 @@ static bool check_regs(struct pt_regs *regs)
 	return true;
 }
 
-static bool und_works;
 static void und_handler(struct pt_regs *regs)
 {
 	und_works = check_regs(regs);
@@ -140,7 +139,6 @@ static bool check_und(void)
 	return und_works;
 }
 
-static bool svc_works;
 static void svc_handler(struct pt_regs *regs)
 {
 	u32 svc = *(u32 *)(regs->ARM_pc - 4) & 0xffffff;
@@ -181,13 +179,130 @@ static bool check_svc(void)
 
 	return svc_works;
 }
+#elif defined(__aarch64__)
+#include <asm/esr.h>
+
+/*
+ * Capture the current register state and execute an instruction
+ * that causes an exception. The test handler will check that its
+ * capture of the current register state matches the capture done
+ * here.
+ *
+ * NOTE: update clobber list if passed insns needs more than x0,x1
+ */
+#define test_exception(pre_insns, excptn_insn, post_insns)	\
+	asm volatile(						\
+		pre_insns "\n"					\
+		"mov	x1, %0\n"				\
+		"ldr	x0, [x1, #" xstr(S_PSTATE) "]\n"	\
+		"mrs	x1, nzcv\n"				\
+		"orr	w0, w0, w1\n"				\
+		"mov	x1, %0\n"				\
+		"str	w0, [x1, #" xstr(S_PSTATE) "]\n"	\
+		"mov	x0, sp\n"				\
+		"str	x0, [x1, #" xstr(S_SP) "]\n"		\
+		"adr	x0, 1f\n"				\
+		"str	x0, [x1, #" xstr(S_PC) "]\n"		\
+		"stp	 x2,  x3, [x1,  #16]\n"			\
+		"stp	 x4,  x5, [x1,  #32]\n"			\
+		"stp	 x6,  x7, [x1,  #48]\n"			\
+		"stp	 x8,  x9, [x1,  #64]\n"			\
+		"stp	x10, x11, [x1,  #80]\n"			\
+		"stp	x12, x13, [x1,  #96]\n"			\
+		"stp	x14, x15, [x1, #112]\n"			\
+		"stp	x16, x17, [x1, #128]\n"			\
+		"stp	x18, x19, [x1, #144]\n"			\
+		"stp	x20, x21, [x1, #160]\n"			\
+		"stp	x22, x23, [x1, #176]\n"			\
+		"stp	x24, x25, [x1, #192]\n"			\
+		"stp	x26, x27, [x1, #208]\n"			\
+		"stp	x28, x29, [x1, #224]\n"			\
+		"str	x30, [x1, #" xstr(S_LR) "]\n"		\
+		"stp	 x0,  x1, [x1]\n"			\
+	"1:"	excptn_insn "\n"				\
+		post_insns "\n"					\
+	:: "r" (&expected_regs) : "x0", "x1")
+
+static bool check_regs(struct pt_regs *regs)
+{
+	unsigned i;
+
+	/* exception handlers should always run in EL1 */
+	if (current_level() != CurrentEL_EL1)
+		return false;
+
+	for (i = 0; i < ARRAY_SIZE(regs->regs); ++i) {
+		if (regs->regs[i] != expected_regs.regs[i])
+			return false;
+	}
+
+	regs->pstate &= 0xf0000000 /* NZCV */ | 0x3c0 /* DAIF */
+			| PSR_MODE_MASK;
+
+	return regs->sp == expected_regs.sp
+		&& regs->pc == expected_regs.pc
+		&& regs->pstate == expected_regs.pstate;
+}
+
+static enum vector check_vector_prep(void)
+{
+	unsigned long daif;
+
+	if (user_mode)
+		return EL0_SYNC_64;
+
+	asm volatile("mrs %0, daif" : "=r" (daif) ::);
+	expected_regs.pstate = daif | PSR_MODE_EL1h;
+	return EL1H_SYNC;
+}
+
+static void unknown_handler(struct pt_regs *regs, unsigned int esr __unused)
+{
+	und_works = check_regs(regs);
+	regs->pc += 4;
+}
+
+static bool check_und(void)
+{
+	enum vector v = check_vector_prep();
+
+	install_exception_handler(v, ESR_EL1_EC_UNKNOWN, unknown_handler);
+
+	/* try to read an el2 sysreg from el0/1 */
+	test_exception("", "mrs x0, sctlr_el2", "");
+
+	install_exception_handler(v, ESR_EL1_EC_UNKNOWN, NULL);
+
+	return und_works;
+}
+
+static void svc_handler(struct pt_regs *regs, unsigned int esr)
+{
+	u16 svc = esr & 0xffff;
+
+	expected_regs.pc += 4;
+	svc_works = check_regs(regs) && svc == 123;
+}
+
+static bool check_svc(void)
+{
+	enum vector v = check_vector_prep();
+
+	install_exception_handler(v, ESR_EL1_EC_SVC64, svc_handler);
+
+	test_exception("", "svc #123", "");
+
+	install_exception_handler(v, ESR_EL1_EC_SVC64, NULL);
+
+	return svc_works;
+}
+#endif
 
 static void check_vectors(void *arg __unused)
 {
 	report("%s", check_und() && check_svc(), testname);
 	exit(report_summary());
 }
-#endif
 
 int main(int argc, char **argv)
 {
@@ -199,7 +314,6 @@ int main(int argc, char **argv)
 
 		check_setup(argc-1, &argv[1]);
 
-#ifdef __arm__
 	} else if (strcmp(argv[0], "vectors-kernel") == 0) {
 
 		check_vectors(NULL);
@@ -209,7 +323,6 @@ int main(int argc, char **argv)
 		void *sp = memalign(PAGE_SIZE, PAGE_SIZE);
 		memset(sp, 0, PAGE_SIZE);
 		start_usr(check_vectors, NULL, (unsigned long)sp + PAGE_SIZE);
-#endif
 	}
 
 	return report_summary();
diff --git a/arm/unittests.cfg b/arm/unittests.cfg
index 9ac6ecaa55d3b..efcca6bf24af6 100644
--- a/arm/unittests.cfg
+++ b/arm/unittests.cfg
@@ -22,11 +22,9 @@ groups = selftest
 file = selftest.flat
 extra_params = -append 'vectors-kernel'
 groups = selftest
-arch = arm
 
 # Test vector setup and exception handling (user mode).
 [selftest::vectors-user]
 file = selftest.flat
 extra_params = -append 'vectors-user'
 groups = selftest
-arch = arm
diff --git a/config/config-arm64.mak b/config/config-arm64.mak
index 37db3d6026424..91255e7d01432 100644
--- a/config/config-arm64.mak
+++ b/config/config-arm64.mak
@@ -10,6 +10,7 @@ kernel_offset = 0x80000
 CFLAGS += -D__aarch64__
 
 cstart.o = $(TEST_DIR)/cstart64.o
+cflatobjs += lib/arm64/processor.o
 
 # arm64 specific tests
 tests =
diff --git a/lib/arm64/asm-offsets.c b/lib/arm64/asm-offsets.c
index c85b9a1e97e44..d7d33f4d917ab 100644
--- a/lib/arm64/asm-offsets.c
+++ b/lib/arm64/asm-offsets.c
@@ -7,8 +7,24 @@
  */
 #include <libcflat.h>
 #include <kbuild.h>
+#include <asm/ptrace.h>
 
 int main(void)
 {
+	OFFSET(S_X0, pt_regs, regs[0]);
+	OFFSET(S_X1, pt_regs, regs[1]);
+	OFFSET(S_X2, pt_regs, regs[2]);
+	OFFSET(S_X3, pt_regs, regs[3]);
+	OFFSET(S_X4, pt_regs, regs[4]);
+	OFFSET(S_X5, pt_regs, regs[5]);
+	OFFSET(S_X6, pt_regs, regs[6]);
+	OFFSET(S_X7, pt_regs, regs[7]);
+	OFFSET(S_LR, pt_regs, regs[30]);
+	OFFSET(S_SP, pt_regs, sp);
+	OFFSET(S_PC, pt_regs, pc);
+	OFFSET(S_PSTATE, pt_regs, pstate);
+	OFFSET(S_ORIG_X0, pt_regs, orig_x0);
+	OFFSET(S_SYSCALLNO, pt_regs, syscallno);
+	DEFINE(S_FRAME_SIZE, sizeof(struct pt_regs));
 	return 0;
 }
diff --git a/lib/arm64/asm/esr.h b/lib/arm64/asm/esr.h
new file mode 100644
index 0000000000000..8407b003afa7f
--- /dev/null
+++ b/lib/arm64/asm/esr.h
@@ -0,0 +1,43 @@
+/*
+ * From Linux kernel arch/arm64/include/asm/esr.h
+ */
+#ifndef _ASMARM64_ESR_H_
+#define _ASMARM64_ESR_H_
+
+#define ESR_EL1_WRITE		(1 << 6)
+#define ESR_EL1_CM		(1 << 8)
+#define ESR_EL1_IL		(1 << 25)
+
+#define ESR_EL1_EC_SHIFT	(26)
+#define ESR_EL1_EC_UNKNOWN	(0x00)
+#define ESR_EL1_EC_WFI		(0x01)
+#define ESR_EL1_EC_CP15_32	(0x03)
+#define ESR_EL1_EC_CP15_64	(0x04)
+#define ESR_EL1_EC_CP14_MR	(0x05)
+#define ESR_EL1_EC_CP14_LS	(0x06)
+#define ESR_EL1_EC_FP_ASIMD	(0x07)
+#define ESR_EL1_EC_CP10_ID	(0x08)
+#define ESR_EL1_EC_CP14_64	(0x0C)
+#define ESR_EL1_EC_ILL_ISS	(0x0E)
+#define ESR_EL1_EC_SVC32	(0x11)
+#define ESR_EL1_EC_SVC64	(0x15)
+#define ESR_EL1_EC_SYS64	(0x18)
+#define ESR_EL1_EC_IABT_EL0	(0x20)
+#define ESR_EL1_EC_IABT_EL1	(0x21)
+#define ESR_EL1_EC_PC_ALIGN	(0x22)
+#define ESR_EL1_EC_DABT_EL0	(0x24)
+#define ESR_EL1_EC_DABT_EL1	(0x25)
+#define ESR_EL1_EC_SP_ALIGN	(0x26)
+#define ESR_EL1_EC_FP_EXC32	(0x28)
+#define ESR_EL1_EC_FP_EXC64	(0x2C)
+#define ESR_EL1_EC_SERROR	(0x2F)
+#define ESR_EL1_EC_BREAKPT_EL0	(0x30)
+#define ESR_EL1_EC_BREAKPT_EL1	(0x31)
+#define ESR_EL1_EC_SOFTSTP_EL0	(0x32)
+#define ESR_EL1_EC_SOFTSTP_EL1	(0x33)
+#define ESR_EL1_EC_WATCHPT_EL0	(0x34)
+#define ESR_EL1_EC_WATCHPT_EL1	(0x35)
+#define ESR_EL1_EC_BKPT32	(0x38)
+#define ESR_EL1_EC_BRK64	(0x3C)
+
+#endif /* _ASMARM64_ESR_H_ */
diff --git a/lib/arm64/asm/processor.h b/lib/arm64/asm/processor.h
new file mode 100644
index 0000000000000..66296f549f87e
--- /dev/null
+++ b/lib/arm64/asm/processor.h
@@ -0,0 +1,52 @@
+#ifndef _ASMARM64_PROCESSOR_H_
+#define _ASMARM64_PROCESSOR_H_
+/*
+ * Copyright (C) 2014, Red Hat Inc, Andrew Jones <drjones@redhat.com>
+ *
+ * This work is licensed under the terms of the GNU LGPL, version 2.
+ */
+#include <asm/ptrace.h>
+
+enum vector {
+	EL1T_SYNC,
+	EL1T_IRQ,
+	EL1T_FIQ,
+	EL1T_ERROR,
+	EL1H_SYNC,
+	EL1H_IRQ,
+	EL1H_FIQ,
+	EL1H_ERROR,
+	EL0_SYNC_64,
+	EL0_IRQ_64,
+	EL0_FIQ_64,
+	EL0_ERROR_64,
+	EL0_SYNC_32,
+	EL0_IRQ_32,
+	EL0_FIQ_32,
+	EL0_ERROR_32,
+	VECTOR_MAX,
+};
+
+#define EC_MAX 64
+
+typedef void (*vector_fn)(enum vector v, struct pt_regs *regs,
+			  unsigned int esr);
+typedef void (*exception_fn)(struct pt_regs *regs, unsigned int esr);
+extern void install_vector_handler(enum vector v, vector_fn fn);
+extern void install_exception_handler(enum vector v, unsigned int ec,
+				      exception_fn fn);
+
+extern void show_regs(struct pt_regs *regs);
+extern void *get_sp(void);
+
+static inline unsigned long current_level(void)
+{
+	unsigned long el;
+	asm volatile("mrs %0, CurrentEL" : "=r" (el));
+	return el & 0xc;
+}
+
+extern bool user_mode;
+extern void start_usr(void (*func)(void *arg), void *arg, unsigned long sp_usr);
+
+#endif /* _ASMARM64_PROCESSOR_H_ */
diff --git a/lib/arm64/asm/ptrace.h b/lib/arm64/asm/ptrace.h
new file mode 100644
index 0000000000000..dd89d82063b1d
--- /dev/null
+++ b/lib/arm64/asm/ptrace.h
@@ -0,0 +1,95 @@
+#ifndef _ASMARM64_PTRACE_H_
+#define _ASMARM64_PTRACE_H_
+/*
+ * Adapted from Linux kernel headers
+ * arch/arm64/include/asm/ptrace.h
+ * arch/arm64/include/uapi/asm/ptrace.h
+ */
+
+/* Current Exception Level values, as contained in CurrentEL */
+#define CurrentEL_EL1	(1 << 2)
+#define CurrentEL_EL2	(2 << 2)
+
+/*
+ * PSR bits
+ */
+#define PSR_MODE_EL0t	0x00000000
+#define PSR_MODE_EL1t	0x00000004
+#define PSR_MODE_EL1h	0x00000005
+#define PSR_MODE_EL2t	0x00000008
+#define PSR_MODE_EL2h	0x00000009
+#define PSR_MODE_EL3t	0x0000000c
+#define PSR_MODE_EL3h	0x0000000d
+#define PSR_MODE_MASK	0x0000000f
+
+/* AArch32 CPSR bits */
+#define PSR_MODE32_BIT	0x00000010
+
+/* AArch64 SPSR bits */
+#define PSR_F_BIT	0x00000040
+#define PSR_I_BIT	0x00000080
+#define PSR_A_BIT	0x00000100
+#define PSR_D_BIT	0x00000200
+#define PSR_Q_BIT	0x08000000
+#define PSR_V_BIT	0x10000000
+#define PSR_C_BIT	0x20000000
+#define PSR_Z_BIT	0x40000000
+#define PSR_N_BIT	0x80000000
+
+/*
+ * Groups of PSR bits
+ */
+#define PSR_f		0xff000000	/* Flags                */
+#define PSR_s		0x00ff0000	/* Status               */
+#define PSR_x		0x0000ff00	/* Extension            */
+#define PSR_c		0x000000ff	/* Control              */
+
+#ifndef __ASSEMBLY__
+#include <libcflat.h>
+
+struct user_pt_regs {
+	u64		regs[31];
+	u64		sp;
+	u64		pc;
+	u64		pstate;
+};
+
+struct user_fpsimd_state {
+	__uint128_t	vregs[32];
+	u32		fpsr;
+	u32		fpcr;
+};
+
+/*
+ * This struct defines the way the registers are stored on the stack during an
+ * exception. Note that sizeof(struct pt_regs) has to be a multiple of 16 (for
+ * stack alignment). struct user_pt_regs must form a prefix of struct pt_regs.
+ */
+struct pt_regs {
+	union {
+		struct user_pt_regs user_regs;
+		struct {
+			u64 regs[31];
+			u64 sp;
+			u64 pc;
+			u64 pstate;
+		};
+	};
+	u64 orig_x0;
+	u64 syscallno;
+};
+
+#define user_mode(regs) \
+	(((regs)->pstate & PSR_MODE_MASK) == PSR_MODE_EL0t)
+
+#define processor_mode(regs) \
+	((regs)->pstate & PSR_MODE_MASK)
+
+#define interrupts_enabled(regs) \
+	(!((regs)->pstate & PSR_I_BIT))
+
+#define fast_interrupts_enabled(regs) \
+	(!((regs)->pstate & PSR_F_BIT))
+
+#endif /* !__ASSEMBLY__ */
+#endif /* _ASMARM64_PTRACE_H_ */
diff --git a/lib/arm64/processor.c b/lib/arm64/processor.c
new file mode 100644
index 0000000000000..7230a8ab3f702
--- /dev/null
+++ b/lib/arm64/processor.c
@@ -0,0 +1,168 @@
+/*
+ * processor control and status functions
+ *
+ * Copyright (C) 2014, Red Hat Inc, Andrew Jones <drjones@redhat.com>
+ *
+ * This work is licensed under the terms of the GNU LGPL, version 2.
+ */
+#include <libcflat.h>
+#include <asm/ptrace.h>
+#include <asm/processor.h>
+#include <asm/esr.h>
+
+static char *vector_names[] = {
+	"el1t_sync",
+	"el1t_irq",
+	"el1t_fiq",
+	"el1t_error",
+	"el1h_sync",
+	"el1h_irq",
+	"el1h_fiq",
+	"el1h_error",
+	"el0_sync_64",
+	"el0_irq_64",
+	"el0_fiq_64",
+	"el0_error_64",
+	"el0_sync_32",
+	"el0_irq_32",
+	"el0_fiq_32",
+	"el0_error_32",
+};
+
+static char *ec_names[EC_MAX] = {
+	[ESR_EL1_EC_UNKNOWN]		= "UNKNOWN",
+	[ESR_EL1_EC_WFI]		= "WFI",
+	[ESR_EL1_EC_CP15_32]		= "CP15_32",
+	[ESR_EL1_EC_CP15_64]		= "CP15_64",
+	[ESR_EL1_EC_CP14_MR]		= "CP14_MR",
+	[ESR_EL1_EC_CP14_LS]		= "CP14_LS",
+	[ESR_EL1_EC_FP_ASIMD]		= "FP_ASMID",
+	[ESR_EL1_EC_CP10_ID]		= "CP10_ID",
+	[ESR_EL1_EC_CP14_64]		= "CP14_64",
+	[ESR_EL1_EC_ILL_ISS]		= "ILL_ISS",
+	[ESR_EL1_EC_SVC32]		= "SVC32",
+	[ESR_EL1_EC_SVC64]		= "SVC64",
+	[ESR_EL1_EC_SYS64]		= "SYS64",
+	[ESR_EL1_EC_IABT_EL0]		= "IABT_EL0",
+	[ESR_EL1_EC_IABT_EL1]		= "IABT_EL1",
+	[ESR_EL1_EC_PC_ALIGN]		= "PC_ALIGN",
+	[ESR_EL1_EC_DABT_EL0]		= "DABT_EL0",
+	[ESR_EL1_EC_DABT_EL1]		= "DABT_EL1",
+	[ESR_EL1_EC_SP_ALIGN]		= "SP_ALIGN",
+	[ESR_EL1_EC_FP_EXC32]		= "FP_EXC32",
+	[ESR_EL1_EC_FP_EXC64]		= "FP_EXC64",
+	[ESR_EL1_EC_SERROR]		= "SERROR",
+	[ESR_EL1_EC_BREAKPT_EL0]	= "BREAKPT_EL0",
+	[ESR_EL1_EC_BREAKPT_EL1]	= "BREAKPT_EL1",
+	[ESR_EL1_EC_SOFTSTP_EL0]	= "SOFTSTP_EL0",
+	[ESR_EL1_EC_SOFTSTP_EL1]	= "SOFTSTP_EL1",
+	[ESR_EL1_EC_WATCHPT_EL0]	= "WATCHPT_EL0",
+	[ESR_EL1_EC_WATCHPT_EL1]	= "WATCHPT_EL1",
+	[ESR_EL1_EC_BKPT32]		= "BKPT32",
+	[ESR_EL1_EC_BRK64]		= "BRK64",
+};
+
+void show_regs(struct pt_regs *regs)
+{
+	int i;
+
+	printf("pc : [<%016llx>] lr : [<%016llx>] pstate: %08llx\n",
+			regs->pc, regs->regs[30], regs->pstate);
+	printf("sp : %016llx\n", regs->sp);
+
+	for (i = 29; i >= 0; --i) {
+		printf("x%-2d: %016llx ", i, regs->regs[i]);
+		if (i % 2 == 0)
+			printf("\n");
+	}
+	printf("\n");
+}
+
+void *get_sp(void)
+{
+	register unsigned long sp asm("sp");
+	return (void *)sp;
+}
+
+static void bad_exception(enum vector v, struct pt_regs *regs,
+			  unsigned int esr, bool bad_vector)
+{
+	unsigned int ec = esr >> ESR_EL1_EC_SHIFT;
+
+	if (bad_vector) {
+		if (v < VECTOR_MAX)
+			printf("Unhandled vector %d (%s)\n", v,
+					vector_names[v]);
+		else
+			printf("Got bad vector=%d\n", v);
+	} else {
+		if (ec_names[ec])
+			printf("Unhandled exception ec=0x%x (%s)\n", ec,
+					ec_names[ec]);
+		else
+			printf("Got bad ec=0x%x\n", ec);
+	}
+
+	printf("Vector: %d (%s)\n", v, vector_names[v]);
+	printf("ESR_EL1: %08lx, ec=0x%x (%s)\n", esr, ec, ec_names[ec]);
+	printf("Exception frame registers:\n");
+	show_regs(regs);
+	abort();
+}
+
+static exception_fn exception_handlers[VECTOR_MAX][EC_MAX];
+
+void install_exception_handler(enum vector v, unsigned int ec, exception_fn fn)
+{
+	if (v < VECTOR_MAX && ec < EC_MAX)
+		exception_handlers[v][ec] = fn;
+}
+
+static void default_vector_handler(enum vector v, struct pt_regs *regs,
+				   unsigned int esr)
+{
+	unsigned int ec = esr >> ESR_EL1_EC_SHIFT;
+
+	if (ec < EC_MAX && exception_handlers[v][ec])
+		exception_handlers[v][ec](regs, esr);
+	else
+		bad_exception(v, regs, esr, false);
+}
+
+static vector_fn vector_handlers[VECTOR_MAX] = {
+	[EL1H_SYNC]	= default_vector_handler,
+	[EL1H_IRQ]	= default_vector_handler,
+	[EL0_SYNC_64]	= default_vector_handler,
+	[EL0_IRQ_64]	= default_vector_handler,
+};
+
+void do_handle_exception(enum vector v, struct pt_regs *regs, unsigned int esr)
+{
+	if (v < VECTOR_MAX && vector_handlers[v])
+		vector_handlers[v](v, regs, esr);
+	else
+		bad_exception(v, regs, esr, true);
+}
+
+void install_vector_handler(enum vector v, vector_fn fn)
+{
+	if (v < VECTOR_MAX)
+		vector_handlers[v] = fn;
+}
+
+bool user_mode;
+void start_usr(void (*func)(void *arg), void *arg, unsigned long sp_usr)
+{
+	sp_usr &= (~15UL); /* stack ptr needs 16-byte alignment */
+
+	user_mode = true;
+
+	asm volatile(
+		"mov	x0, %0\n"
+		"msr	sp_el0, %1\n"
+		"msr	elr_el1, %2\n"
+		"mov	x3, xzr\n"	/* clear and "set" PSR_MODE_EL0t */
+		"msr	spsr_el1, x3\n"
+		"eret\n"
+	:: "r" (arg), "r" (sp_usr), "r" (func) : "x0", "x3");
+}
