Complete the AArch32 integration

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

5 files changed, 28 insertions, 11 deletions

diff --git a/arm/common/mem_util.c b/arm/common/mem_util.c
index a111950..de5bd21 100644
--- a/arm/common/mem_util.c
+++ b/arm/common/mem_util.c
@@ -22,7 +22,7 @@ uintptr_t mem_allocate_pa()
 uintptr_t mem_allocate_l1_page()
 {
     uintptr_t next = mem_next_l1_page;
-    mem_next_pa += PAGE_SIZE;
+    mem_next_l1_page += PAGE_SIZE;
     return next;
 }
 
diff --git a/arm/el0_ns/tztest_nsec.c b/arm/el0_ns/tztest_nsec.c
index 5fa0afb..b1f3946 100644
--- a/arm/el0_ns/tztest_nsec.c
+++ b/arm/el0_ns/tztest_nsec.c
@@ -64,8 +64,10 @@ int main()
 
     run_test(TZTEST_SMC, 0);
     run_test(TZTEST_REG_ACCESS, 0);
+#if AARCH64
     run_test(TZTEST_CPACR_TRAP, 0);
     run_test(TZTEST_WFX_TRAP, 0);
+#endif
 
     printf("\nValidation complete.  Passed %d of %d tests.\n",
               syscntl->test_cntl->test_count - syscntl->test_cntl->fail_count,
diff --git a/arm/el1_common/el1.c b/arm/el1_common/el1.c
index 88fe160..54681ce 100644
--- a/arm/el1_common/el1.c
+++ b/arm/el1_common/el1.c
@@ -146,11 +146,26 @@ void el1_handle_exception(uintptr_t ec, uintptr_t iss, uintptr_t far,
     case EC_UNKNOWN:
         DEBUG_MSG("Unknown exception far = 0x%lx  elr = 0x%lx\n", far, elr);
 
+        /* The preferred return address in the case of an undefined instruction
+         * is the actual offending instruction.  In the case of ARMv7, we need
+         * to decrement the address by 4 to get this behavior as the PC has
+         * already been moved past this instruction.
+         * If SKIP is enabled then we can just do nothing and get the correct
+         * behavior.
+         */
+#if AARCH32
+        if (syscntl->el1_excp[SEC_STATE].action != EXCP_ACTION_SKIP &&
+            syscntl->excp_action != EXCP_ACTION_SKIP) {
+            elr += 4;
+            __set_exception_return(elr);
+        }
+#else
         if (syscntl->el1_excp[SEC_STATE].action == EXCP_ACTION_SKIP ||
             syscntl->excp_action == EXCP_ACTION_SKIP) {
             elr +=4;
             __set_exception_return(elr);
         }
+#endif
         break;
     case EC_IABORT_LOWER:
         DEBUG_MSG("Instruction abort at lower level: far = %0lx\n", far);
diff --git a/arm/el1_common/el1_exception.S b/arm/el1_common/el1_exception.S
index 1b871d7..ba7115e 100644
--- a/arm/el1_common/el1_exception.S
+++ b/arm/el1_common/el1_exception.S
@@ -19,9 +19,9 @@ el1_vectors:
 
 el1_svc_vect:
 	srsdb sp!, #CPSR_MODE_SVC
-    push {r0-r3}
+    push {r1-r3}
     bl el1_handle_svc
-    pop {r0-r3}
+    pop {r1-r3}
     rfefd sp!
 
 el1_iabort_vect:
diff --git a/arm/el3/el3.c b/arm/el3/el3.c
index 170b3b1..1af72ca 100644
--- a/arm/el3/el3.c
+++ b/arm/el3/el3.c
@@ -199,7 +199,7 @@ int el3_handle_exception(uintptr_t ec, uintptr_t iss, uintptr_t far,
         if (syscntl->el3_excp.action == EXCP_ACTION_SKIP ||
             syscntl->excp_action == EXCP_ACTION_SKIP) {
             elr +=4;
-//            __set_exception_return(elr);
+            __set_exception_return(elr);
         }
         break;
     case EC_WFI_WFE:
@@ -212,7 +212,7 @@ int el3_handle_exception(uintptr_t ec, uintptr_t iss, uintptr_t far,
         if (syscntl->el3_excp.action == EXCP_ACTION_SKIP ||
             syscntl->excp_action == EXCP_ACTION_SKIP) {
             elr +=4;
-//            __set_exception_return(elr);
+            __set_exception_return(elr);
         }
         break;
     case EC_SIMD:
@@ -224,7 +224,7 @@ int el3_handle_exception(uintptr_t ec, uintptr_t iss, uintptr_t far,
         if (syscntl->el3_excp.action == EXCP_ACTION_SKIP ||
             syscntl->excp_action == EXCP_ACTION_SKIP) {
             elr +=4;
-//            __set_exception_return(elr);
+            __set_exception_return(elr);
         }
         break;
     default:
@@ -238,6 +238,8 @@ int el3_handle_exception(uintptr_t ec, uintptr_t iss, uintptr_t far,
 
 void el3_monitor_init()
 {
+    uintptr_t syscntl_pa = (uintptr_t)mem_lookup_pa(syscntl);
+
     /* Clear out our secure and non-secure state buffers */
     memset(&sec_state, 0, sizeof(sec_state));
     memset(&nsec_state, 0, sizeof(nsec_state));
@@ -246,9 +248,7 @@ void el3_monitor_init()
      * sequence. This will occur when we return from exception after monitor
      * initialization.
      */
-    sec_state.lr_mon = EL1_S_FLASH_BASE;
-    sec_state.spsr_mon = CPSR_MODE_SVC | CPSR_I;
-    sec_state.r[0] = (uintptr_t)mem_lookup_pa(syscntl);
+    sec_state.r[0] = syscntl_pa;
 
     /* Set-up the nonsecure state buffer to return to the non-secure
      * initialization sequence. This will occur on the first monitor context
@@ -256,7 +256,7 @@ void el3_monitor_init()
      */
     nsec_state.lr_mon = EL1_NS_FLASH_BASE;
     nsec_state.spsr_mon = CPSR_MODE_SVC | CPSR_I;
-    nsec_state.r[0] = (uintptr_t)mem_lookup_pa(syscntl);
+    nsec_state.r[0] = syscntl_pa;
 }
 
 void el3_start(uintptr_t base, uintptr_t size)
@@ -284,5 +284,5 @@ void el3_start(uintptr_t base, uintptr_t size)
      * return.
      */
     monitor_restore_state(&sec_state);
-    __exception_return(sec_state.lr_mon, sec_state.spsr_mon);
+    __exception_return(EL1_S_FLASH_BASE, CPSR_MODE_SVC | CPSR_I);
 }