Fix EL1/3 nested exception support

There was still an issue with taking nested EL1 exceptions where the ELR was
not preserved when combining SVC and other non-SVC exceptions.

Added nested EL3 exception support.

Signed-off-by: Greg Bellows <greg.bellows@linaro.org>

1 files changed, 8 insertions, 2 deletions

diff --git a/el3/aarch64/el3_exception.S b/el3/aarch64/el3_exception.S
index dc5d1e7..f3bdd17 100644
--- a/el3/aarch64/el3_exception.S
+++ b/el3/aarch64/el3_exception.S
@@ -28,7 +28,11 @@ el3_sync_exception_current:
 el3_sync_exception_lower64:
     str x30, [sp, #-8]!
     stp x2, x3, [sp, #-16]!
+    mrs x2, spsr_el3    /* Nested EL3 exceptions will overwrite the SPSR */
+    str x2, [sp, #-8]!  /* Save the SPSR so we can restore it later */
     stp x0, x1, [sp, #-16]!
+    mrs x2, far_el3
+    mrs x3, elr_el3
     mrs x0, esr_el3
     mov x1, #0xffffff
     and x1, x1, x0
@@ -37,10 +41,10 @@ el3_sync_exception_lower64:
     b.eq el3_sync_exception_lower64_smc
     cmp x0, #EC_SMC32
     b.eq el3_sync_exception_lower64_smc
-    mrs x2, far_el3
-    mrs x3, elr_el3
     bl el3_handle_exception
     ldp x0, x1, [sp], #16
+    ldr x2, [sp], #8
+    msr spsr_el3, x2        /* Restore the SPSR in case it was destroyed */
     ldp x2, x3, [sp], #16
     ldr x30, [sp], #8
     b el3_sync_exception_lower64_done
@@ -48,6 +52,8 @@ el3_sync_exception_lower64_smc:
     ldp x0, x1, [sp]        /* Fetch our inputs as SMC args */
     bl el3_handle_smc
     ldp x2, x1, [sp], #16   /* We don't want to overwrite x0, so use x2 */
+    ldr x2, [sp], #8
+    msr spsr_el3, x2        /* Restore the SPSR in case it was destroyed */
     ldp x2, x3, [sp], #16   /* We can throw away the old x0, and restore x2 */
     ldr x30, [sp], #8
     cbz x0, el3_sync_exception_lower64_done