Add aarch64 NS P0 register access test

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

6 files changed, 112 insertions, 3 deletions

diff --git a/aarch64/common/arm_builtins.h b/aarch64/common/arm_builtins.h
index 40326ef..4828365 100644
--- a/aarch64/common/arm_builtins.h
+++ b/aarch64/common/arm_builtins.h
@@ -1,6 +1,8 @@
 #ifndef _ARM_BUILTINS_H
 #define _ARM_BUILTINS_H
 
+#include "stdint.h"
+
 #define __exception_return(_x0) asm volatile ("eret\n")
 #define __set_exception_return(_elr) \
     asm volatile("mrs x7, currentel\n"  \
@@ -41,4 +43,12 @@
                  "farel1: mrs %0, far_el1\n"   \
                  "fardone:\n" : "=r" (_addr))
 
+extern uint64_t read_currentel();
+extern void write_currentel(uint64_t);
+extern uint64_t read_scr_el3();
+extern void write_scr_el3(uint64_t);
+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);
 #endif
diff --git a/aarch64/common/builtins.S b/aarch64/common/builtins.S
index dd0da3b..d924f75 100644
--- a/aarch64/common/builtins.S
+++ b/aarch64/common/builtins.S
@@ -1,5 +1,6 @@
 #define __ASSEMBLY__
 
+.section .text
 .globl __smc
 __smc:
     smc #0
@@ -10,4 +11,31 @@ __svc:
     svc #0
     ret
 
+.macro READ_REG reg
+.globl read_\reg
+read_\reg:
+    mrs x0, \reg
+    ret
+.endm
+
+.macro WRITE_REG reg
+.globl write_\reg
+write_\reg:
+    msr \reg, x0
+    ret
+.endm
+
+READ_REG currentel
+WRITE_REG currentel
+
+READ_REG scr_el3
+WRITE_REG scr_el3
+
+READ_REG sder32_el3
+WRITE_REG sder32_el3
+
+READ_REG cptr_el3
+WRITE_REG cptr_el3
+
+
 #undef __ASSEMBLY__
diff --git a/aarch64/common/interop.h b/aarch64/common/interop.h
index d0579d1..32f0f7a 100644
--- a/aarch64/common/interop.h
+++ b/aarch64/common/interop.h
@@ -31,6 +31,6 @@ typedef struct {
 } op_dispatch_t;
 
 typedef struct {
-    void *datap;
+    uint64_t data;
 } op_get_data_t;
 #endif
diff --git a/aarch64/common/svc.h b/aarch64/common/svc.h
index 7e10e69..8c4c43a 100644
--- a/aarch64/common/svc.h
+++ b/aarch64/common/svc.h
@@ -12,6 +12,7 @@
 #define SVC_MAP 8
 #define SVC_YIELD 9
 #define SVC_GET_SYSCNTL 10
+#define SVC_GET_MODE 11
 
 #ifndef __ASSEMBLY__
 #include "interop.h"
diff --git a/aarch64/el0_ns/tztest.c b/aarch64/el0_ns/tztest.c
index 6150d8c..d2ea4e4 100644
--- a/aarch64/el0_ns/tztest.c
+++ b/aarch64/el0_ns/tztest.c
@@ -2,6 +2,7 @@
 #include "svc.h"
 #include "syscntl.h"
 #include "armv8_exception.h"
+#include "arm_builtins.h"
 
 typedef struct {
     volatile int fail_count;
@@ -66,6 +67,65 @@ uint32_t P0_nonsecure_check_smc()
     return 0;
 }
 
+uint32_t P0_check_register_access(int state)
+{
+    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("\t%s P0 SCR read ... ", state_str[state]);
+    TEST_EL1NS_EXCEPTION(read_scr_el3(), EC_UNKNOWN);
+
+    printf("\t%s P0 SCR write ... ", state_str[state]);
+    TEST_EL1NS_EXCEPTION(write_scr_el3(0), EC_UNKNOWN);
+
+    printf("\t%s P0 SDER read ... ", state_str[state]);
+    TEST_EL1NS_EXCEPTION(read_sder32_el3(), EC_UNKNOWN);
+
+    printf("\t%s P0 SDER write ... ", state_str[state]);
+    TEST_EL1NS_EXCEPTION(write_sder32_el3(0), EC_UNKNOWN);
+
+/*
+    printf("\t%s P0 MVBAR read ... ", state_str[state]);
+    TEST_EL1NS_EXCEPTION(read_mvbar(), EC_UNKNOWN);
+
+    printf("\t%s P0 MVBAR write ... ", state_str[state]);
+    TEST_EL1NS_EXCEPTION(write_mvbar(0), EC_UNKNOWN);
+
+    printf("\t%s P0 NSACR write ... ", state_str[state]);
+    TEST_EL1NS_EXCEPTION(write_nsacr(0), EC_UNKNOWN);
+*/
+
+    printf("\t%s P0 CPTR_EL3 read ... ", state_str[state]);
+    TEST_EL1NS_EXCEPTION(read_cptr_el3(), EC_UNKNOWN);
+
+    printf("\t%s P0 CPTR_EL3 write ... ", state_str[state]);
+    TEST_EL1NS_EXCEPTION(write_cptr_el3(0), EC_UNKNOWN);
+
+    return 0;
+}
+
+uint32_t P0_nonsecure_check_register_access()
+{
+//    validate_state(CPSR_MODE_USR, TZTEST_STATE_NONSECURE);
+
+    P0_check_register_access(NSEC);
+
+    return 0;
+}
+
+uint32_t P0_secure_check_register_access()
+{
+//    validate_state(CPSR_MODE_USR, TZTEST_STATE_SECURE);
+
+    P0_check_register_access(SEC);
+
+    return 0;
+}
+//SECURE_USR_FUNC(P0_secure_check_register_access);
+
 void *alloc_mem(int type, size_t len)
 {
     svc_op_desc_t op;
@@ -99,7 +159,8 @@ int main()
 
     /* Fetch the system-wide control structure */
     __svc(SVC_GET_SYSCNTL, &get_data);
-    syscntl = get_data.datap;
+    syscntl = (sys_control_t *)get_data.data;
+
     /* If we didn't get a valid control structure then something has already
      * gone drastically wrong.
      */
@@ -111,6 +172,12 @@ int main()
     __svc(SVC_YIELD, NULL);
 
     P0_nonsecure_check_smc();
+    P0_nonsecure_check_register_access();
+
+    /* Fetch the system-wide control structure */
+//    __svc(SVC_GET_MODE, &get_data);
+//    uint64_t el = get_data.data;
+//    printf("EL = 0x%lx\n", el);
 
     __svc(SVC_EXIT, NULL);
 
diff --git a/aarch64/el1_common/el1.c b/aarch64/el1_common/el1.c
index 9179c5f..b6f3a6c 100644
--- a/aarch64/el1_common/el1.c
+++ b/aarch64/el1_common/el1.c
@@ -186,7 +186,10 @@ void el1_handle_svc(uint64_t ec, uint32_t op, svc_op_desc_t *desc)
         el1_map_secure((op_map_mem_t *)&desc->map);
         break;
     case SVC_GET_SYSCNTL:
-        desc->get.datap = syscntl;
+        desc->get.data = (uint64_t)syscntl;
+        break;
+    case SVC_GET_MODE:
+        desc->get.data = read_currentel();
         break;
     default:
         printf("Unrecognized AArch64 SVC opcode: op = %d\n", op);