Add trap to EL3 test

Added test that validates reads and writes to disabled CPACR from EL1 trap to
EL3.

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

10 files changed, 169 insertions, 30 deletions

diff --git a/aarch64/common/arm_builtins.h b/aarch64/common/arm_builtins.h
index 4828365..4744531 100644
--- a/aarch64/common/arm_builtins.h
+++ b/aarch64/common/arm_builtins.h
@@ -51,4 +51,6 @@ extern uint64_t read_sder32_el3();
 extern void write_sder32_el3(uint64_t);
 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);
 #endif
diff --git a/aarch64/common/armv8_exception.h b/aarch64/common/armv8_exception.h
index 85b2604..7f51257 100644
--- a/aarch64/common/armv8_exception.h
+++ b/aarch64/common/armv8_exception.h
@@ -8,6 +8,7 @@
 #define EC_SMC32 0x13
 #define EC_SVC64 0x15
 #define EC_SMC64 0x17
+#define EC_SYSINSN 0x18
 #define EC_IABORT_LOWER 0x20
 #define EC_IABORT 0x21
 #define EC_DABORT_LOWER 0x24
diff --git a/aarch64/common/builtins.S b/aarch64/common/builtins.S
index d924f75..597f076 100644
--- a/aarch64/common/builtins.S
+++ b/aarch64/common/builtins.S
@@ -37,5 +37,7 @@ WRITE_REG sder32_el3
 READ_REG cptr_el3
 WRITE_REG cptr_el3
 
+READ_REG cpacr_el1
+WRITE_REG cpacr_el1
 
 #undef __ASSEMBLY__
diff --git a/aarch64/common/interop.h b/aarch64/common/interop.h
index 4e14a60..7bb1484 100644
--- a/aarch64/common/interop.h
+++ b/aarch64/common/interop.h
@@ -30,9 +30,17 @@ typedef struct {
     uintptr_t ret;
 } op_dispatch_t;
 
+typedef enum {
+    CURRENTEL = 1,
+    CPTR_EL3,
+    CPACR_EL1
+} op_reg_key_t;
+
 typedef struct {
+    uint64_t key;
     uint64_t data;
-} op_get_data_t;
+    uint64_t el;
+} op_data_t;
 
 typedef struct {
     uint64_t orig;
diff --git a/aarch64/common/smc.h b/aarch64/common/smc.h
index 14fc393..3896cf2 100644
--- a/aarch64/common/smc.h
+++ b/aarch64/common/smc.h
@@ -9,6 +9,8 @@
 #define SMC_OP_ALLOCATE_SECURE_MEMORY 4
 #define SMC_OP_EXIT 5
 #define SMC_OP_MAP 8
+#define SMC_OP_GET_REG  11
+#define SMC_OP_SET_REG  12
 #define SMC_OP_TEST 13
 
 #ifndef __ASSEMBLY__
@@ -22,6 +24,8 @@ const char *smc_op_name[] = {
     [SMC_OP_YIELD] = "SMC_OP_YIELD",
     [SMC_OP_EXIT] = "SMC_OP_EXIT",
     [SMC_OP_MAP] = "SMC_OP_MAP",
+    [SMC_OP_GET_REG] = "SMC_OP_GET_REG",
+    [SMC_OP_SET_REG] = "SMC_OP_SET_REG",
     [SMC_OP_TEST] = "SMC_OP_TEST"
 };
 
@@ -29,6 +33,8 @@ typedef union {
     op_dispatch_t dispatch;
     op_map_mem_t map;
     op_test_t test;
+    op_data_t get;
+    op_data_t set;
 } smc_op_desc_t;
 
 extern smc_op_desc_t *smc_interop_buf;
diff --git a/aarch64/common/svc.h b/aarch64/common/svc.h
index 7ec72ef..9837205 100644
--- a/aarch64/common/svc.h
+++ b/aarch64/common/svc.h
@@ -6,8 +6,9 @@
 #define SVC_OP_MAP 8
 #define SVC_OP_YIELD 9
 #define SVC_OP_GET_SYSCNTL 10
-#define SVC_OP_GET_MODE 11
-#define SVC_OP_TEST 12
+#define SVC_OP_GET_REG 11
+#define SVC_OP_SET_REG 12
+#define SVC_OP_TEST 13
 
 #ifndef __ASSEMBLY__
 #include "interop.h"
@@ -18,14 +19,16 @@ const char *svc_op_name[] = {
     [SVC_OP_MAP] = "SVC_OP_MAP",
     [SVC_OP_YIELD] = "SVC_OP_YIELD",
     [SVC_OP_GET_SYSCNTL] = "SVC_OP_GET_SYSCNTL",
-    [SVC_OP_GET_MODE] = "SVC_OP_GET_MODE",
+    [SVC_OP_GET_REG] = "SVC_OP_GET_REG",
+    [SVC_OP_SET_REG] = "SVC_OP_SET_REG",
     [SVC_OP_TEST] = "SVC_OP_TEST"
 };
 
 typedef union {
     op_alloc_mem_t alloc;
     op_map_mem_t map;
-    op_get_data_t get;
+    op_data_t get;
+    op_data_t set;
     op_test_t test;
 } svc_op_desc_t;
 
diff --git a/aarch64/el0_ns/tztest.c b/aarch64/el0_ns/tztest.c
index bbc4f4f..8e6ce57 100644
--- a/aarch64/el0_ns/tztest.c
+++ b/aarch64/el0_ns/tztest.c
@@ -55,7 +55,7 @@ sys_control_t *syscntl = NULL;
 
 uint32_t P0_nonsecure_check_smc()
 {
-    printf("\nValidating non-secure P0 smc behavior:\n");
+    printf("\nValidating %s P0 smc behavior:\n", SEC_STATE_STR);
     printf("\tUnprivileged P0 smc call ... ");
 
     TEST_EL1NS_EXCEPTION(asm volatile("smc #0\n"), EC_UNKNOWN);
@@ -63,41 +63,38 @@ uint32_t P0_nonsecure_check_smc()
     return 0;
 }
 
-uint32_t P0_check_register_access(int state)
+uint32_t P0_check_register_access()
 {
-    char *state_str[2] = {"Secure", "Nonsecure"};
-
     /* Set things to non-secure P1 and attempt accesses */
-    printf("\nValidating %s P0 restricted register access:\n",
-           (state == NSEC) ? "nonsecure" : "secure");
+    printf("\nValidating %s P0 restricted register access:\n", SEC_STATE_STR);
 
-    printf("\t%s P0 SCR read ... ", state_str[state]);
+    printf("\t%s P0 SCR read ... ", SEC_STATE_STR);
     TEST_EL1NS_EXCEPTION(read_scr_el3(), EC_UNKNOWN);
 
-    printf("\t%s P0 SCR write ... ", state_str[state]);
+    printf("\t%s P0 SCR write ... ", SEC_STATE_STR);
     TEST_EL1NS_EXCEPTION(write_scr_el3(0), EC_UNKNOWN);
 
-    printf("\t%s P0 SDER read ... ", state_str[state]);
+    printf("\t%s P0 SDER read ... ", SEC_STATE_STR);
     TEST_EL1NS_EXCEPTION(read_sder32_el3(), EC_UNKNOWN);
 
-    printf("\t%s P0 SDER write ... ", state_str[state]);
+    printf("\t%s P0 SDER write ... ", SEC_STATE_STR);
     TEST_EL1NS_EXCEPTION(write_sder32_el3(0), EC_UNKNOWN);
 
 /*
-    printf("\t%s P0 MVBAR read ... ", state_str[state]);
+    printf("\t%s P0 MVBAR read ... ", SEC_STATE_STR);
     TEST_EL1NS_EXCEPTION(read_mvbar(), EC_UNKNOWN);
 
-    printf("\t%s P0 MVBAR write ... ", state_str[state]);
+    printf("\t%s P0 MVBAR write ... ", SEC_STATE_STR);
     TEST_EL1NS_EXCEPTION(write_mvbar(0), EC_UNKNOWN);
 
-    printf("\t%s P0 NSACR write ... ", state_str[state]);
+    printf("\t%s P0 NSACR write ... ", SEC_STATE_STR);
     TEST_EL1NS_EXCEPTION(write_nsacr(0), EC_UNKNOWN);
 */
 
-    printf("\t%s P0 CPTR_EL3 read ... ", state_str[state]);
+    printf("\t%s P0 CPTR_EL3 read ... ", SEC_STATE_STR);
     TEST_EL1NS_EXCEPTION(read_cptr_el3(), EC_UNKNOWN);
 
-    printf("\t%s P0 CPTR_EL3 write ... ", state_str[state]);
+    printf("\t%s P0 CPTR_EL3 write ... ", SEC_STATE_STR);
     TEST_EL1NS_EXCEPTION(write_cptr_el3(0), EC_UNKNOWN);
 
     return 0;
@@ -107,7 +104,7 @@ uint32_t P0_nonsecure_check_register_access()
 {
 //    validate_state(CPSR_MODE_USR, TZTEST_STATE_NONSECURE);
 
-    P0_check_register_access(NSEC);
+    P0_check_register_access();
 
     return 0;
 }
@@ -116,12 +113,50 @@ uint32_t P0_secure_check_register_access()
 {
 //    validate_state(CPSR_MODE_USR, TZTEST_STATE_SECURE);
 
-    P0_check_register_access(SEC);
+    P0_check_register_access();
 
     return 0;
 }
 //SECURE_USR_FUNC(P0_secure_check_register_access);
 
+uint32_t P0_check_trap_to_EL3()
+{
+    uint32_t cptr_el3;
+    svc_op_desc_t desc;
+
+    printf("\nValidating %s P1 trap to EL3:\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);
+
+    /* Disable CPACR access */
+    cptr_el3 = desc.get.data;
+    desc.set.data =  desc.get.data | (1 << 31);
+    __svc(SVC_OP_SET_REG, &desc);
+
+    /* 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);
+
+    /* 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);
+
+    /* Restore the original CPTR */
+    desc.get.key = CPTR_EL3;
+    desc.get.el = 3;
+    desc.set.data = cptr_el3;
+    __svc(SVC_OP_SET_REG, &desc);
+
+    return 0;
+}
+
 void *alloc_mem(int type, size_t len)
 {
     svc_op_desc_t op;
@@ -151,7 +186,7 @@ void interop_test()
     test.fail = test.count = 0;
     __svc(SVC_OP_TEST, (svc_op_desc_t *)&test);
 
-    printf("Testing interop communication between ELs... ");
+    printf("\nValidating interop communication between ELs... ");
     TEST_CONDITION(!test.fail && test.val == (test.orig >> test.count));
 }
 
@@ -182,6 +217,11 @@ int main()
 
     P0_nonsecure_check_smc();
     P0_nonsecure_check_register_access();
+    P0_check_trap_to_EL3();
+
+    printf("\nValidation complete.  Passed %d of %d tests\n",
+           syscntl->test_cntl->test_count - syscntl->test_cntl->fail_count,
+           syscntl->test_cntl->test_count);
 
     __svc(SVC_OP_EXIT, NULL);
 
diff --git a/aarch64/el1_common/el1.c b/aarch64/el1_common/el1.c
index d59e635..5d5fe4a 100644
--- a/aarch64/el1_common/el1.c
+++ b/aarch64/el1_common/el1.c
@@ -209,8 +209,42 @@ int el1_handle_svc(uint32_t op, svc_op_desc_t *desc)
     case SVC_OP_GET_SYSCNTL:
         desc->get.data = (uint64_t)syscntl;
         break;
-    case SVC_OP_GET_MODE:
-        desc->get.data = read_currentel();
+    case SVC_OP_GET_REG:
+        if (desc->get.el == 1) {
+            switch (desc->get.key) {
+            case CURRENTEL:
+                desc->get.data = read_currentel();
+                break;
+            case CPTR_EL3:
+                desc->get.data = read_cptr_el3();
+                break;
+            case CPACR_EL1:
+                desc->get.data = read_cpacr_el1();
+                break;
+            }
+        } else if (desc->get.el == 3) {
+            memcpy(smc_interop_buf, desc, sizeof(smc_op_desc_t));
+            __smc(SMC_OP_GET_REG, smc_interop_buf);
+            memcpy(desc, smc_interop_buf, sizeof(smc_op_desc_t));
+        }
+        break;
+    case SVC_OP_SET_REG:
+        if (desc->set.el == 1) {
+            switch (desc->set.key) {
+            case CURRENTEL:
+                read_currentel(desc->set.data);
+                break;
+            case CPTR_EL3:
+                read_cptr_el3(desc->set.data);
+                break;
+            case CPACR_EL1:
+                read_cpacr_el1(desc->set.data);
+                break;
+            }
+        } else if (desc->set.el == 3) {
+            memcpy(smc_interop_buf, desc, sizeof(smc_op_desc_t));
+            __smc(SMC_OP_SET_REG, smc_interop_buf);
+        }
         break;
     case SVC_OP_TEST:
         el1_interop_test((op_test_t *)desc);
diff --git a/aarch64/el3/Makefile b/aarch64/el3/Makefile
index 349febb..f049cb6 100644
--- a/aarch64/el3/Makefile
+++ b/aarch64/el3/Makefile
@@ -1,7 +1,10 @@
+VPATH	= ../common
+
 EL3_OBJS		= el3_init.o \
                   el3_exception.o \
                   el3.o \
-                  el3_monitor_asm.o
+                  el3_monitor_asm.o \
+                  builtins.o
 EL3_ELF			= el3.elf
 EL3_IMAGE		= el3.bin
 EL3_LOAD		= el3.lds
diff --git a/aarch64/el3/el3.c b/aarch64/el3/el3.c
index f8189b1..23112f9 100644
--- a/aarch64/el3/el3.c
+++ b/aarch64/el3/el3.c
@@ -35,7 +35,7 @@ void el3_dispatch(op_dispatch_t *disp)
 void el3_shutdown() {
     uintptr_t *sysreg_cfgctrl = (uintptr_t *)(SYSREG_BASE + SYSREG_CFGCTRL);
 
-    printf("Shutting down\n");
+    printf("\nTest complete\n");
 
     *sysreg_cfgctrl = SYS_SHUTDOWN;
 
@@ -198,10 +198,10 @@ int el3_handle_smc(uint64_t op, smc_op_desc_t *desc)
         return SVC_OP_YIELD;
         break;
     case SMC_OP_DISPATCH_MONITOR:
-        el3_dispatch((op_dispatch_t *)&desc->dispatch);
+        el3_dispatch((op_dispatch_t *)desc);
         break;
     case SMC_OP_MAP:
-        return el3_map_mem((op_map_mem_t *)&desc->map);
+        return el3_map_mem((op_map_mem_t *)desc);
         break;
     case SMC_OP_NOOP:
         break;
@@ -215,6 +215,36 @@ int el3_handle_smc(uint64_t op, smc_op_desc_t *desc)
         test->val >>= 1;
         test->count++;
         return SVC_OP_TEST;
+    case SMC_OP_GET_REG:
+        if (desc->get.el == 3) {
+            switch (desc->get.key) {
+            case CURRENTEL:
+                desc->get.data = read_currentel();
+                break;
+            case CPTR_EL3:
+                desc->get.data = read_cptr_el3();
+                break;
+            case CPACR_EL1:
+                desc->get.data = read_cpacr_el1();
+                break;
+            }
+        }
+        break;
+    case SMC_OP_SET_REG:
+        if (desc->set.el == 3) {
+            switch (desc->set.key) {
+            case CURRENTEL:
+                write_currentel(desc->set.data);
+                break;
+            case CPTR_EL3:
+                write_cptr_el3(desc->set.data);
+                break;
+            case CPACR_EL1:
+                write_cpacr_el1(desc->set.data);
+                break;
+            }
+        }
+        break;
     default:
         printf("Unrecognized AArch64 SMC opcode: op = %d\n", op);
         el3_shutdown();
@@ -262,8 +292,18 @@ int el3_handle_exception(uint64_t ec, uint64_t iss)
                dai.wnr ? "write" : "read", far, elr);
         el3_shutdown();
         break;
+    case EC_SYSINSN:
+        DEBUG_MSG("System instruction exception far = 0x%lx  elr = 0x%lx\n",
+                  far, elr);
+
+        if (syscntl->el3_excp.action == EXCP_ACTION_SKIP ||
+            syscntl->excp_action == EXCP_ACTION_SKIP) {
+            elr +=4;
+            __set_exception_return(elr);
+        }
+        break;
     default:
-        printf("Unhandled EL3 exception: EC = %d  ISS = %d\n", ec, iss);
+        printf("Unhandled EL3 exception: EC = 0x%lx  ISS = 0x%lx\n", ec, iss);
         el3_shutdown();
         break;
     }