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>

6 files changed, 36 insertions, 15 deletions

diff --git a/el1/aarch64/el1_exception.S b/el1/aarch64/el1_exception.S
index a7d3e80..68f66bc 100644
--- a/el1/aarch64/el1_exception.S
+++ b/el1/aarch64/el1_exception.S
@@ -27,9 +27,11 @@ el1_sync_exception_current:
 el1_sync_exception_lower64:
     str x30, [sp, #-8]!
     stp x2, x3, [sp, #-16]!
-    mrs x2, spsr_el1    /* Get the lower SPSR */
-    str x2, [sp, #-8]!  /* Save the SPSR in case a nested interrupt occurs */
+    mrs x2, spsr_el1    /* Nested EL1 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_el1
+    mrs x3, elr_el1     /* The handlers deal with saving this */
     mrs x0, esr_el1
     mov x1, #0xffffff
     and x1, x1, x0
@@ -38,8 +40,6 @@ el1_sync_exception_lower64:
     b.eq el1_sync_exception_lower64_svc
     cmp x0, #EC_SVC32
     b.eq el1_sync_exception_lower64_svc
-    mrs x2, far_el1
-    mrs x3, elr_el1     /* The handler deals with this getting destroyed */
     bl el1_handle_exception
     b el1_sync_exception_lower64_done
 el1_sync_exception_lower64_svc:
diff --git a/el1/arm/el1_exception.S b/el1/arm/el1_exception.S
index 5a5128b..9106b0b 100644
--- a/el1/arm/el1_exception.S
+++ b/el1/arm/el1_exception.S
@@ -19,6 +19,8 @@ el1_vectors:
 el1_svc_vect:
 	srsdb sp!, #CPSR_M_SVC
     push {r1-r3}
+	mov r2, #0                      /* There is no FAR on an SVC */
+	ldr r3, [sp, #16]               /* Jump back over the saved args for LR */
     bl el1_handle_svc
     pop {r1-r3}
     rfefd sp!
diff --git a/el1/el1.c b/el1/el1.c
index 348c1b8..f455a0e 100644
--- a/el1/el1.c
+++ b/el1/el1.c
@@ -62,7 +62,8 @@ void el1_interop_test(op_test_t *desc)
     memcpy(desc, smc_interop_buf, sizeof(smc_op_desc_t));
 }
 
-int el1_handle_svc(uint32_t op, svc_op_desc_t *desc)
+int el1_handle_svc(uint32_t op, svc_op_desc_t *desc,
+                   uintptr_t __attribute((unused))far, uintptr_t elr)
 {
     uint32_t ret = 0;
 
@@ -162,6 +163,11 @@ int el1_handle_svc(uint32_t op, svc_op_desc_t *desc)
         break;
     }
 
+    /* We always restore the elr just in case we hit a nested EL1 exception in
+     * the handler.  This can happen as we run exception tests.
+     */
+    __set_exception_return(elr);
+
     return ret;
 }
 
@@ -191,12 +197,10 @@ void el1_handle_exception(uintptr_t ec, uintptr_t iss, uintptr_t far,
 #if AARCH32
         if (syscntl->excp.action != EXCP_ACTION_SKIP) {
             elr += 4;
-            __set_exception_return(elr);
         }
 #else
         if (syscntl->excp.action == EXCP_ACTION_SKIP) {
             elr +=4;
-            __set_exception_return(elr);
         }
 #endif
         break;
@@ -223,7 +227,6 @@ void el1_handle_exception(uintptr_t ec, uintptr_t iss, uintptr_t far,
 
         if (syscntl->excp.action == EXCP_ACTION_SKIP) {
             elr +=4;
-            __set_exception_return(elr);
         }
         break;
     case EC_SIMD:
@@ -234,7 +237,6 @@ void el1_handle_exception(uintptr_t ec, uintptr_t iss, uintptr_t far,
          */
         if (syscntl->excp.action == EXCP_ACTION_SKIP) {
             elr += 4;
-            __set_exception_return(elr);
         }
         break;
     default:
@@ -242,6 +244,11 @@ void el1_handle_exception(uintptr_t ec, uintptr_t iss, uintptr_t far,
         SMC_EXIT();
         break;
     }
+
+    /* We always restore the elr just in case there was a nested EL1 exception.
+     * In some cases up above, the elr has been adjusted.
+     */
+    __set_exception_return(elr);
 }
 
 void el1_start(uintptr_t base, uintptr_t size)
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
diff --git a/el3/arm/el3_exception.S b/el3/arm/el3_exception.S
index 448ab06..6f5b9fb 100644
--- a/el3/arm/el3_exception.S
+++ b/el3/arm/el3_exception.S
@@ -18,6 +18,8 @@ el3_vectors:
 
 el3_smc_vect:
 	srsdb sp!, #CPSR_M_MON
+	mov r2, #0                      /* There is no FAR on an SVC */
+	ldr r3, [sp, #16]               /* Jump back over the saved args for LR */
     bl el3_handle_smc
     cmp r0, #0
     beq el3_smc_vect_done
diff --git a/el3/el3.c b/el3/el3.c
index 5ca4759..325c03c 100644
--- a/el3/el3.c
+++ b/el3/el3.c
@@ -82,7 +82,8 @@ uint32_t el3_map_mem(op_map_mem_t *map)
     return 0;
 }
 
-int el3_handle_smc(uintptr_t op, smc_op_desc_t *desc)
+int el3_handle_smc(uintptr_t op, smc_op_desc_t *desc,
+                   uintptr_t __attribute((unused))far, uintptr_t elr)
 {
     op_test_t *test = (op_test_t*)desc;
 
@@ -183,6 +184,7 @@ int el3_handle_smc(uintptr_t op, smc_op_desc_t *desc)
         break;
     }
 
+    __set_exception_return(elr);
     return 0;
 }
 
@@ -229,7 +231,6 @@ int el3_handle_exception(uintptr_t ec, uintptr_t iss, uintptr_t far,
          */
         if (syscntl->excp.action == EXCP_ACTION_SKIP) {
             elr +=4;
-            __set_exception_return(elr);
         }
         break;
     case EC_WFI_WFE:
@@ -241,7 +242,6 @@ int el3_handle_exception(uintptr_t ec, uintptr_t iss, uintptr_t far,
          */
         if (syscntl->excp.action == EXCP_ACTION_SKIP) {
             elr +=4;
-            __set_exception_return(elr);
         }
         break;
     case EC_SIMD:
@@ -252,7 +252,6 @@ int el3_handle_exception(uintptr_t ec, uintptr_t iss, uintptr_t far,
          */
         if (syscntl->excp.action == EXCP_ACTION_SKIP) {
             elr +=4;
-            __set_exception_return(elr);
         }
         break;
     default:
@@ -261,6 +260,11 @@ int el3_handle_exception(uintptr_t ec, uintptr_t iss, uintptr_t far,
         break;
     }
 
+    /* We always restore the elr just in case there was a nested EL3 exception.
+     * In some cases up above, the elr has been adjusted.
+     */
+    __set_exception_return(elr);
+
     return 0;
 }