Add WFx tests and general cleanupaarch64

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

13 files changed, 228 insertions, 63 deletions

diff --git a/aarch64/common/arm_builtins.h b/aarch64/common/arm_builtins.h
index 4744531..940b2fd 100644
--- a/aarch64/common/arm_builtins.h
+++ b/aarch64/common/arm_builtins.h
@@ -3,19 +3,14 @@
 
 #include "stdint.h"
 
+#define CPTR_TFP    (1 << 10)
+#define CPTR_TCPAC  (1 << 31)
+#define SCTLR_nTWI  (1 << 16)
+#define SCTLR_nTWE  (1 << 18)
+#define SCR_WFI     (1 << 12)
+#define SCR_WFE     (1 << 13)
+
 #define __exception_return(_x0) asm volatile ("eret\n")
-#define __set_exception_return(_elr) \
-    asm volatile("mrs x7, currentel\n"  \
-                 "cmp x7, #0x4\n"   \
-                 "b.eq setelrel1\n"   \
-                 "cmp x7, #0x8\n"   \
-                 "b.eq setelrel2\n"   \
-                 "setelrel3: msr elr_el3, %[elr]\n"   \
-                 "b setelrdone\n"   \
-                 "setelrel2: msr elr_el2, %[elr]\n"   \
-                 "b setelrdone\n"   \
-                 "setelrel1: msr elr_el1, %[elr]\n"   \
-                 "setelrdone:\n":: [elr] "r" (_elr) : "r7")
 
 #define __get_exception_return(_addr) \
     asm volatile("mrs x0, currentel\n"  \
@@ -53,4 +48,9 @@ extern uint64_t read_cptr_el3();
 extern void write_cptr_el3(uint64_t);
 extern uint64_t read_cpacr_el1();
 extern void write_cpacr_el1(uint64_t);
+extern uint64_t read_cpacr_el1();
+extern void write_cpacr_el1(uint64_t);
+extern uint64_t read_sctlr_el1();
+extern void write_sctlr_el1(uint64_t);
+extern void __set_exception_return(uint64_t);
 #endif
diff --git a/aarch64/common/armv8_exception.h b/aarch64/common/armv8_exception.h
index 7f51257..056e5e4 100644
--- a/aarch64/common/armv8_exception.h
+++ b/aarch64/common/armv8_exception.h
@@ -2,6 +2,7 @@
 #define _EXCEPTION_H
 
 #define EC_UNKNOWN 0x00
+#define EC_WFI_WFE 0x01
 #define EC_SIMD 0x07
 #define EC_ILLEGAL_STATE 0x0E
 #define EC_SVC32 0x11
diff --git a/aarch64/common/builtins.S b/aarch64/common/builtins.S
index 597f076..a3b69ac 100644
--- a/aarch64/common/builtins.S
+++ b/aarch64/common/builtins.S
@@ -40,4 +40,27 @@ WRITE_REG cptr_el3
 READ_REG cpacr_el1
 WRITE_REG cpacr_el1
 
+READ_REG sctlr_el1
+WRITE_REG sctlr_el1
+
+.globl __set_exception_return
+__set_exception_return:
+    str x30, [sp, #-8]!
+    mrs x7, currentel
+    cmp x7, #0x4
+    b.eq __set_exception_return_el1
+    cmp x7, #0x8
+    b.eq __set_exception_return_el2
+__set_exception_return_el3:
+    msr elr_el3, x0
+    b __set_exception_return_done
+__set_exception_return_el2:
+    msr elr_el2, x0
+    b __set_exception_return_done
+__set_exception_return_el1:
+    msr elr_el1, x0
+__set_exception_return_done:
+    ldr x30, [sp], #8
+    ret
+
 #undef __ASSEMBLY__
diff --git a/aarch64/common/interop.h b/aarch64/common/interop.h
index 2fe8285..fcd7448 100644
--- a/aarch64/common/interop.h
+++ b/aarch64/common/interop.h
@@ -33,7 +33,9 @@ typedef struct {
 typedef enum {
     CURRENTEL = 1,
     CPTR_EL3,
-    CPACR_EL1
+    CPACR_EL1,
+    SCR_EL3,
+    SCTLR_EL1,
 } op_reg_key_t;
 
 typedef struct {
diff --git a/aarch64/common/svc.h b/aarch64/common/svc.h
index 670b0cb..0bfaff0 100644
--- a/aarch64/common/svc.h
+++ b/aarch64/common/svc.h
@@ -27,6 +27,24 @@ typedef union {
 
 extern uint32_t __svc(uint32_t, const svc_op_desc_t *);
 
+#define SVC_GET_REG(__reg, __el, __val)     \
+    do {                                \
+        svc_op_desc_t desc;             \
+        desc.get.key = (__reg);         \
+        desc.get.el = (__el);           \
+        __svc(SVC_OP_GET_REG, &desc);   \
+        (__val) = desc.get.data;        \
+    } while (0)
+
+#define SVC_SET_REG(__reg, __el, __val)     \
+    do {                                \
+        svc_op_desc_t desc;             \
+        desc.get.key = (__reg);         \
+        desc.get.el = (__el);           \
+        desc.get.data = (__val);        \
+        __svc(SVC_OP_SET_REG, &desc);   \
+    } while (0)
+
 #endif
 
 #endif
diff --git a/aarch64/el0_common/tztest.c b/aarch64/el0_common/tztest.c
index a270959..e0e71ae 100644
--- a/aarch64/el0_common/tztest.c
+++ b/aarch64/el0_common/tztest.c
@@ -9,22 +9,28 @@
 
 sys_control_t *syscntl = NULL;
 
-uint32_t P0_check_smc()
+#define TEST_HEAD(_str, ...) \
+    printf("Validating %s EL%d " _str ":\n", SEC_STATE_STR, el, ##__VA_ARGS__)
+
+#define TEST_MSG(_str, ...) \
+    printf("\tEL%d(%s): " _str "... ", el, SEC_STATE_STR, ##__VA_ARGS__)
+
+uint32_t check_smc(uint32_t el)
 {
-    printf("\nValidating %s P0 smc behavior:\n", SEC_STATE_STR);
-    printf("\tUnprivileged P0 smc call ... ");
+    TEST_HEAD("smc behavior");
 
+    TEST_MSG("smc call");
     TEST_EL1_EXCEPTION(asm volatile("smc #0\n"), EC_UNKNOWN);
 
     return 0;
 }
 
-uint32_t P0_check_register_access()
+uint32_t check_register_access(uint32_t __attribute__((unused))el)
 {
     /* Set things to non-secure P1 and attempt accesses */
-    printf("\nValidating %s P0 restricted register access:\n", SEC_STATE_STR);
+    printf("restricted register access");
 
-    printf("\t%s P0 SCR read ... ", SEC_STATE_STR);
+    printf("SCR read");
     TEST_EL1_EXCEPTION(read_scr_el3(), EC_UNKNOWN);
 
     printf("\t%s P0 SCR write ... ", SEC_STATE_STR);
@@ -56,48 +62,111 @@ uint32_t P0_check_register_access()
     return 0;
 }
 
-uint32_t P0_check_trap_to_EL3()
+uint32_t check_cpacr_trap(uint32_t __attribute__((unused))el)
 {
-    uint32_t cptr_el3;
-    svc_op_desc_t desc;
+    uint64_t cptr_el3, cpacr;
 
-    printf("\nValidating %s P1 trap to EL3:\n", SEC_STATE_STR);
+    printf("\nValidating %s CPACR trapping:\n", SEC_STATE_STR);
 
     /* Get the current CPTR so we can restore it later */
-    desc.get.key = CPTR_EL3;
-    desc.get.el = 3;
-    __svc(SVC_OP_GET_REG, &desc);
+    SVC_GET_REG(CPTR_EL3, 3, cptr_el3);
 
     /* Disable CPACR access */
-    cptr_el3 = desc.get.data;
-    desc.set.data =  desc.get.data | (1 << 31);
-    __svc(SVC_OP_SET_REG, &desc);
+    SVC_SET_REG(CPTR_EL3, 3, cptr_el3 | CPTR_TCPAC);
 
     /* Try to read CPACR */
-    desc.get.key = CPACR_EL1;
-    desc.get.el = 1;
-    printf("\t%s P1 read of disabled CPACR... ", SEC_STATE_STR);
-    TEST_EL3_EXCEPTION(__svc(SVC_OP_GET_REG, &desc), EC_SYSINSN);
+    printf("\t%s EL1 read of disabled CPACR... ", SEC_STATE_STR);
+    TEST_EL3_EXCEPTION(SVC_GET_REG(CPACR_EL1, 1, cpacr), EC_SYSINSN);
 
-    /* Try to write CPACR
-     * Note: data still set to above get value in case we succeeded.
-     */
-    printf("\t%s P1 write of disabled CPACR... ", SEC_STATE_STR);
-    TEST_EL3_EXCEPTION(__svc(SVC_OP_SET_REG, &desc), EC_SYSINSN);
+    /* Try to write CPACR */
+    printf("\t%s EL1 write of disabled CPACR... ", SEC_STATE_STR);
+    TEST_EL3_EXCEPTION(SVC_SET_REG(CPACR_EL1, 1, cpacr), EC_SYSINSN);
+
+#ifdef FP_TEST
+    /* Disable FP access */
+    printf("\t%s EL1 read of disabled FP reg... ", SEC_STATE_STR);
+    SVC_SET_REG(CPTR_EL3, 3, cptr_el3 | CPTR_TFP);
+    TEST_EL3_EXCEPTION(asm volatile("fcmp s0, #0.0\n"), EC_SIMD);
+#endif
 
     /* Restore the original CPTR */
-    desc.get.key = CPTR_EL3;
-    desc.get.el = 3;
-    desc.set.data = cptr_el3;
-    __svc(SVC_OP_SET_REG, &desc);
+    SVC_SET_REG(CPTR_EL3, 3, cptr_el3);
+
+    return 0;
+}
+
+uint32_t check_wfx_trap(uint32_t __attribute__((unused))el)
+{
+    uint64_t sctlr, scr_el3;
+
+    printf("\nValidating %s WFx traps:\n", SEC_STATE_STR);
+
+    /* Get the current SCR so we can restore it later */
+    SVC_GET_REG(SCR_EL3, 3, scr_el3);
+
+    /* Get the current SCTLR so we can restore it later */
+    SVC_GET_REG(SCTLR_EL1, 1, sctlr);
+
+    /* Clear SCTLR.nTWE to cause WFE instructions to trap to EL1 */
+    SVC_SET_REG(SCTLR_EL1, 1, sctlr & ~SCTLR_nTWE);
+    printf("\t%s EL0 execution of WFE trapped to EL1... ", SEC_STATE_STR);
+    TEST_EL1_EXCEPTION(asm volatile("wfe\n"), EC_WFI_WFE);
+
+    /* SCTLR.nTWE left as trapping to check precedence */
+
+    /* Trap WFE instructions to EL3.  This should work even though SCTLR.nTWE
+     * is clear
+     */
+    SVC_SET_REG(SCR_EL3, 3, scr_el3 | SCR_WFE);
+    printf("\t%s EL0 execution of trapped WFE (SCTLR.nTWE clear)... ",
+           SEC_STATE_STR);
+    TEST_EL3_EXCEPTION(asm volatile("wfe\n"), EC_WFI_WFE);
+
+    /* Restore SCTLR */
+    SVC_SET_REG(SCTLR_EL1, 1, sctlr);
+
+    /* This should trap to EL3 with SCTLR.nTWE set */
+    printf("\t%s EL0 execution of trapped WFE (SCTLR.nTWE set)... ",
+           SEC_STATE_STR);
+    TEST_EL3_EXCEPTION(asm volatile("wfe\n"), EC_WFI_WFE);
+
+    /* Restore SCR */
+    SVC_SET_REG(SCR_EL3, 3, scr_el3);
+
+    /* Clear SCTLR.nTWI to cause WFI instructions to trap to EL1 */
+    SVC_SET_REG(SCTLR_EL1, 1, sctlr & ~SCTLR_nTWI);
+    printf("\t%s EL0 execution of WFI trapped to EL1... ", SEC_STATE_STR);
+    TEST_EL1_EXCEPTION(asm volatile("wfi\n"), EC_WFI_WFE);
+
+    /* SCTLR.nTWI left as trapping to check precedence */
+
+    /* Trap WFI instructions to EL3.  This should work even though SCTLR.nTWE
+     * is clear
+     */
+    SVC_SET_REG(SCR_EL3, 3, scr_el3 | SCR_WFI);
+
+    printf("\t%s EL0 execution of trapped WFI (SCTLR.nTWI clear)... ",
+           SEC_STATE_STR);
+    TEST_EL3_EXCEPTION(asm volatile("wfi\n"), EC_WFI_WFE);
+
+    /* Restore SCTLR */
+    SVC_SET_REG(SCTLR_EL1, 1, sctlr);
+
+    printf("\t%s EL0 execution of trapped WFI (SCTLR.nTWI set)... ",
+           SEC_STATE_STR);
+    TEST_EL3_EXCEPTION(asm volatile("wfi\n"), EC_WFI_WFE);
+
+    /* Restore SCR */
+    SVC_SET_REG(SCR_EL3, 3, scr_el3);
 
     return 0;
 }
 
 void tztest_init()
 {
-    tztest[TZTEST_P0_SMC] = P0_check_smc;
-    tztest[TZTEST_REG_ACCESS] = P0_check_register_access;
-    tztest[TZTEST_TRAP_TO_EL3] = P0_check_trap_to_EL3;
+    tztest[TZTEST_SMC] = check_smc;
+    tztest[TZTEST_REG_ACCESS] = check_register_access;
+    tztest[TZTEST_CPACR_TRAP] = check_cpacr_trap;
+    tztest[TZTEST_WFX_TRAP] = check_wfx_trap;
 }
 
diff --git a/aarch64/el0_common/tztest.h b/aarch64/el0_common/tztest.h
index f2e2af7..5f8bf73 100644
--- a/aarch64/el0_common/tztest.h
+++ b/aarch64/el0_common/tztest.h
@@ -1,18 +1,20 @@
 #ifndef _TZTEST_H
 #define _TZTEST_H
 
-typedef uint32_t (*tztest_t)();
+typedef uint32_t (*tztest_t)(uint32_t el);
 extern tztest_t tztest[];
 
 extern void tztest_init();
-extern uint32_t P0_check_smc();
-extern uint32_t P0_check_register_access();
-extern uint32_t P0_check_trap_to_EL3();
+extern uint32_t check_smc(uint32_t el);
+extern uint32_t check_register_access(uint32_t el);
+extern uint32_t check_cpacr_trap(uint32_t el);
+extern uint32_t check_wfx_trap(uint32_t el);
 
 typedef enum {
-    TZTEST_P0_SMC = 0,
+    TZTEST_SMC = 0,
     TZTEST_REG_ACCESS,
-    TZTEST_TRAP_TO_EL3,
+    TZTEST_CPACR_TRAP,
+    TZTEST_WFX_TRAP,
     TZTEST_COUNT
 } tztest_func_id_t;
 
diff --git a/aarch64/el0_ns/tztest_nsec.c b/aarch64/el0_ns/tztest_nsec.c
index b263cfc..ffe0f66 100644
--- a/aarch64/el0_ns/tztest_nsec.c
+++ b/aarch64/el0_ns/tztest_nsec.c
@@ -14,11 +14,11 @@ void interop_test()
     TEST_CONDITION(!test.fail && test.val == (test.orig >> test.count));
 }
 
-void run_test(tztest_func_id_t fid)
+void run_test(tztest_func_id_t fid, uint32_t el)
 {
     op_dispatch_t disp;
 
-    tztest[fid]();
+    tztest[fid](el);
 
     disp.func_id = fid;
     __svc(SVC_OP_DISPATCH, (svc_op_desc_t *)&disp);
@@ -51,9 +51,10 @@ int main()
         __svc(SVC_OP_EXIT, &desc);
     }
 
-    run_test(TZTEST_P0_SMC);
-    run_test(TZTEST_REG_ACCESS);
-    run_test(TZTEST_TRAP_TO_EL3);
+    run_test(TZTEST_SMC, 0);
+    run_test(TZTEST_REG_ACCESS, 0);
+    run_test(TZTEST_CPACR_TRAP, 0);
+    run_test(TZTEST_WFX_TRAP, 0);
 
     printf("\nValidation complete.  Passed %d of %d tests\n",
            syscntl->test_cntl->test_count - syscntl->test_cntl->fail_count,
diff --git a/aarch64/el0_s/tztest_sec.c b/aarch64/el0_s/tztest_sec.c
index 42e202e..35b4972 100644
--- a/aarch64/el0_s/tztest_sec.c
+++ b/aarch64/el0_s/tztest_sec.c
@@ -28,7 +28,7 @@ void el0_sec_loop()
             op = SVC_OP_YIELD;
             break;
         case SVC_OP_DISPATCH:
-            tztest[desc->disp.func_id]();
+            tztest[desc->disp.func_id](0);
             op = SVC_OP_YIELD;
             break;
         case 0:
diff --git a/aarch64/el1_common/el1.c b/aarch64/el1_common/el1.c
index e9dc31a..29551eb 100644
--- a/aarch64/el1_common/el1.c
+++ b/aarch64/el1_common/el1.c
@@ -221,6 +221,9 @@ int el1_handle_svc(uint32_t op, svc_op_desc_t *desc)
             case CPACR_EL1:
                 desc->get.data = read_cpacr_el1();
                 break;
+            case SCR_EL3:
+                desc->get.data = read_scr_el3();
+                break;
             }
         } else if (desc->get.el == 3) {
             memcpy(smc_interop_buf, desc, sizeof(smc_op_desc_t));
@@ -232,13 +235,16 @@ int el1_handle_svc(uint32_t op, svc_op_desc_t *desc)
         if (desc->set.el == 1) {
             switch (desc->set.key) {
             case CURRENTEL:
-                read_currentel(desc->set.data);
+                write_currentel(desc->set.data);
                 break;
             case CPTR_EL3:
-                read_cptr_el3(desc->set.data);
+                write_cptr_el3(desc->set.data);
                 break;
             case CPACR_EL1:
-                read_cpacr_el1(desc->set.data);
+                write_cpacr_el1(desc->set.data);
+                break;
+            case SCR_EL3:
+                write_scr_el3(desc->set.data);
                 break;
             }
         } else if (desc->set.el == 3) {
@@ -290,7 +296,6 @@ void el1_handle_exception(uint64_t ec, uint64_t iss)
             __set_exception_return(elr);
         }
         break;
-
     case EC_IABORT_LOWER:
         DEBUG_MSG("Instruction abort at lower level: far = %0lx\n", far);
         SMC_EXIT();
@@ -309,6 +314,16 @@ void el1_handle_exception(uint64_t ec, uint64_t iss)
                   dai.wnr ? "write" : "read", far, elr);
         SMC_EXIT();
         break;
+    case EC_WFI_WFE:
+        DEBUG_MSG("WFI/WFE instruction exception far = 0x%lx  elr = 0x%lx\n",
+                  far, elr);
+
+        if (syscntl->el1_excp[SEC_STATE].action == EXCP_ACTION_SKIP ||
+            syscntl->excp_action == EXCP_ACTION_SKIP) {
+            elr +=4;
+            __set_exception_return(elr);
+        }
+        break;
     default:
         DEBUG_MSG("Unhandled EL1 exception: EC = %d  ISS = %d\n", ec, iss);
         SMC_EXIT();
diff --git a/aarch64/el1_ns/el1_nsec.c b/aarch64/el1_ns/el1_nsec.c
index bbaaee2..79dc4cb 100644
--- a/aarch64/el1_ns/el1_nsec.c
+++ b/aarch64/el1_ns/el1_nsec.c
@@ -11,6 +11,6 @@ void el1_init_el0()
 
     main = el1_load_el0((char *)EL0_NS_FLASH_BASE, (char *)EL0_NS_BASE_VA);
 
-    __set_exception_return(main);
+    __set_exception_return((uint64_t)main);
     __exception_return();
 }
diff --git a/aarch64/el1_s/el1_sec.c b/aarch64/el1_s/el1_sec.c
index fd9b3ab..2aea580 100644
--- a/aarch64/el1_s/el1_sec.c
+++ b/aarch64/el1_s/el1_sec.c
@@ -13,6 +13,6 @@ void el1_init_el0()
 
     main = el1_load_el0((char *)EL0_S_FLASH_BASE, (char *)EL0_NS_BASE_VA);
 
-    __set_exception_return(main);
+    __set_exception_return((uint64_t)main);
     __exception_return();
 }
diff --git a/aarch64/el3/el3.c b/aarch64/el3/el3.c
index 57a42c2..ac4919e 100644
--- a/aarch64/el3/el3.c
+++ b/aarch64/el3/el3.c
@@ -219,6 +219,9 @@ int el3_handle_smc(uint64_t op, smc_op_desc_t *desc)
             case CPACR_EL1:
                 desc->get.data = read_cpacr_el1();
                 break;
+            case SCR_EL3:
+                desc->get.data = read_scr_el3();
+                break;
             }
         }
         break;
@@ -234,6 +237,9 @@ int el3_handle_smc(uint64_t op, smc_op_desc_t *desc)
             case CPACR_EL1:
                 write_cpacr_el1(desc->set.data);
                 break;
+            case SCR_EL3:
+                write_scr_el3(desc->set.data);
+                break;
             }
         }
         break;
@@ -288,6 +294,34 @@ int el3_handle_exception(uint64_t ec, uint64_t iss)
         DEBUG_MSG("System instruction exception far = 0x%lx  elr = 0x%lx\n",
                   far, elr);
 
+        /* Other than system calls, synchronous exceptions return to the
+         * offending instruction.  The user should have issued a SKIP.
+         */
+        if (syscntl->el3_excp.action == EXCP_ACTION_SKIP ||
+            syscntl->excp_action == EXCP_ACTION_SKIP) {
+            elr +=4;
+            __set_exception_return(elr);
+        }
+        break;
+    case EC_WFI_WFE:
+        DEBUG_MSG("WFI/WFE instruction exception far = 0x%lx  elr = 0x%lx\n",
+                  far, elr);
+
+        /* Other than system calls, synchronous exceptions return to the
+         * offending instruction.  The user should have issued a SKIP.
+         */
+        if (syscntl->el3_excp.action == EXCP_ACTION_SKIP ||
+            syscntl->excp_action == EXCP_ACTION_SKIP) {
+            elr +=4;
+            __set_exception_return(elr);
+        }
+        break;
+    case EC_SIMD:
+        DEBUG_MSG("Adv SIMD or FP access exception - far = 0x%lx elr = 0x%lx\n",
+                  far, elr);
+        /* Other than system calls, synchronous exceptions return to the
+         * offending instruction.  The user should have issued a SKIP.
+         */
         if (syscntl->el3_excp.action == EXCP_ACTION_SKIP ||
             syscntl->excp_action == EXCP_ACTION_SKIP) {
             elr +=4;