From 4c2cc7c4e92094efa562a872cef515fdf6d6f915 Mon Sep 17 00:00:00 2001
From: Alexander Graf <agraf@suse.de>
Date: Fri, 4 Mar 2016 01:10:05 +0100
Subject: [PATCH] arm64: Allow exceptions to return

Our current arm64 exception handlers all panic and never return to the
exception triggering code.

But if any handler wanted to continue execution after fixups, it would
need help from the exception handling code to restore all registers.

This patch implements that help. With this code, exception handlers on
aarch64 can successfully return to the place the exception happened (or
somewhere else if they modify elr).

Signed-off-by: Alexander Graf <agraf@suse.de>
---
 arch/arm/cpu/armv8/exceptions.S | 34 +++++++++++++++++++++++++++++++++
 1 file changed, 34 insertions(+)

diff --git a/arch/arm/cpu/armv8/exceptions.S b/arch/arm/cpu/armv8/exceptions.S
index baf9401e64..4f4f526f93 100644
--- a/arch/arm/cpu/armv8/exceptions.S
+++ b/arch/arm/cpu/armv8/exceptions.S
@@ -82,31 +82,65 @@ vectors:
 _do_bad_sync:
 	exception_entry
 	bl	do_bad_sync
+	b	exception_exit
 
 _do_bad_irq:
 	exception_entry
 	bl	do_bad_irq
+	b	exception_exit
 
 _do_bad_fiq:
 	exception_entry
 	bl	do_bad_fiq
+	b	exception_exit
 
 _do_bad_error:
 	exception_entry
 	bl	do_bad_error
+	b	exception_exit
 
 _do_sync:
 	exception_entry
 	bl	do_sync
+	b	exception_exit
 
 _do_irq:
 	exception_entry
 	bl	do_irq
+	b	exception_exit
 
 _do_fiq:
 	exception_entry
 	bl	do_fiq
+	b	exception_exit
 
 _do_error:
 	exception_entry
 	bl	do_error
+	b	exception_exit
+
+exception_exit:
+	ldp	x2, x0, [sp],#16
+	switch_el x11, 3f, 2f, 1f
+3:	msr	elr_el3, x2
+	b	0f
+2:	msr	elr_el2, x2
+	b	0f
+1:	msr	elr_el1, x2
+0:
+	ldp	x1, x2, [sp],#16
+	ldp	x3, x4, [sp],#16
+	ldp	x5, x6, [sp],#16
+	ldp	x7, x8, [sp],#16
+	ldp	x9, x10, [sp],#16
+	ldp	x11, x12, [sp],#16
+	ldp	x13, x14, [sp],#16
+	ldp	x15, x16, [sp],#16
+	ldp	x17, x18, [sp],#16
+	ldp	x19, x20, [sp],#16
+	ldp	x21, x22, [sp],#16
+	ldp	x23, x24, [sp],#16
+	ldp	x25, x26, [sp],#16
+	ldp	x27, x28, [sp],#16
+	ldp	x29, x30, [sp],#16
+	eret