Add EL1 test support and initial tests

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

11 files changed, 314 insertions, 6 deletions

diff --git a/common/aarch64/arm_builtins.h b/common/aarch64/arm_builtins.h
index 9d0f1b9..b31cbfb 100644
--- a/common/aarch64/arm_builtins.h
+++ b/common/aarch64/arm_builtins.h
@@ -50,6 +50,8 @@ 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 uint64_t read_sctlr_el3();
+extern void write_sctlr_el3(uint64_t);
 extern void __set_exception_return(uint64_t);
 extern void __exception_return(uintptr_t, uint32_t);
 
@@ -63,5 +65,9 @@ extern void __exception_return(uintptr_t, uint32_t);
 #define WRITE_CPACR(_val) write_cpacr_el1(_val)
 #define READ_CURRENTEL() read_currentel()
 #define WRITE_CURRENTEL(_val) read_currentel(_val)
+#define READ_SCTLR() read_sctlr_el1()
+#define WRITE_SCTLR(_val) write_sctlr_el1(_val)
+#define READ_SCTLR_EL3() read_sctlr_el3()
+#define WRITE_SCTLR_EL3(_val) write_sctlr_el3(_val)
 
 #endif
diff --git a/common/aarch64/builtins.S b/common/aarch64/builtins.S
index cfc8a41..d1065bd 100644
--- a/common/aarch64/builtins.S
+++ b/common/aarch64/builtins.S
@@ -43,6 +43,9 @@ WRITE_REG cpacr_el1
 READ_REG sctlr_el1
 WRITE_REG sctlr_el1
 
+READ_REG sctlr_el3
+WRITE_REG sctlr_el3
+
 .globl __set_exception_return
 __set_exception_return:
     str x30, [sp, #-8]!
diff --git a/common/smc.h b/common/smc.h
index b54991e..0373efe 100644
--- a/common/smc.h
+++ b/common/smc.h
@@ -32,6 +32,24 @@ extern smc_op_desc_t *smc_interop_buf;
 #define SMC_EXIT()  __smc(SMC_OP_EXIT, NULL)
 #define SMC_YIELD()  __smc(SMC_OP_YIELD, smc_interop_buf);
 
+#define SMC_GET_REG(__reg, __el, __val)     \
+    do {                                \
+        smc_op_desc_t *desc = smc_interop_buf;             \
+        desc->get.key = (__reg);         \
+        desc->get.el = (__el);           \
+        __smc(SMC_OP_GET_REG, desc);   \
+        (__val) = desc->get.data;        \
+    } while (0)
+
+#define SMC_SET_REG(__reg, __el, __val)     \
+    do {                                \
+        smc_op_desc_t *desc = smc_interop_buf;             \
+        desc->get.key = (__reg);         \
+        desc->get.el = (__el);           \
+        desc->get.data = (__val);        \
+        __smc(SMC_OP_SET_REG, desc);   \
+    } while (0)
+
 #endif
 
 #endif
diff --git a/el1/el1.c b/el1/el1.c
index c042a75..746f7a1 100644
--- a/el1/el1.c
+++ b/el1/el1.c
@@ -2,6 +2,7 @@
 #include "exception.h"
 #include "vmsa.h"
 #include "mem_util.h"
+#include "tztest.h"
 
 uintptr_t mem_pgtbl_base = EL1_PGTBL_BASE;
 smc_op_desc_t *smc_interop_buf;
@@ -136,6 +137,8 @@ int el1_handle_svc(uint32_t op, svc_op_desc_t *desc)
     case SVC_OP_DISPATCH:
         if (desc->disp.el == exception_level &&
             desc->disp.state == secure_state)  {
+            run_test(desc->disp.fid, desc->disp.el,
+                     desc->disp.state, desc->disp.arg);
         } else {
             memcpy(smc_interop_buf, desc, sizeof(smc_op_desc_t));
             __smc(SMC_OP_DISPATCH, smc_interop_buf);
@@ -212,7 +215,7 @@ void el1_handle_exception(uintptr_t ec, uintptr_t iss, uintptr_t far,
         }
         break;
     default:
-        DEBUG_MSG("Unhandled EL1 exception: ec = %d iss = %d\n", ec, iss);
+        DEBUG_MSG("Unhandled EL1 exception: ec = 0x%x iss = 0x%x\n", ec, iss);
         SMC_EXIT();
         break;
     }
diff --git a/el1/nonsecure/Makefile b/el1/nonsecure/Makefile
index b7a3e20..2a2efc2 100644
--- a/el1/nonsecure/Makefile
+++ b/el1/nonsecure/Makefile
@@ -1,4 +1,5 @@
-VPATH	= $(ARCH):../$(ARCH):../../common/$(ARCH):../
+VPATH	= $(ARCH):../$(ARCH):../../common/$(ARCH):
+VPATH   += ../:../../tztest/el1/nonsecure:../../tztest/el1:../../tztest
 
 EL1_NS_ELF		= el1_nsec.elf
 EL1_NS_IMAGE 	= el1_nsec.bin
@@ -9,7 +10,10 @@ EL1_NS_OBJS		= el1_init.o \
                   el1_loader.o \
                   el1_nsec.o \
                   mem_util.o \
-                  builtins.o
+				  builtins.o \
+				  tztest.o \
+				  tztest_el1.o \
+				  tztest_el1_nsec.o
 
 libgcc := $(shell $(CC) $(CFLAGS) --print-libgcc-file-name)
 FLATLIBS = ../../libcflat/libcflat.a $(libgcc) ../../libcflat/$(ARCH)/libeabi.a
@@ -18,6 +22,7 @@ FLATLIBS = ../../libcflat/libcflat.a $(libgcc) ../../libcflat/$(ARCH)/libeabi.a
 
 CFLAGS += -I$(ARCH) -I../$(ARCH) -I../ -I../$(ARCH)
 CFLAGS += -I../../common/$(ARCH) -I../../common/
+CFLAGS += -I../../tztest/el1 -I../../tztest
 
 ##################################################################
 
diff --git a/el1/secure/Makefile b/el1/secure/Makefile
index a8057df..70e10dc 100644
--- a/el1/secure/Makefile
+++ b/el1/secure/Makefile
@@ -1,4 +1,5 @@
-VPATH	= $(ARCH):../$(ARCH):../../common/$(ARCH):../
+VPATH	= $(ARCH):../$(ARCH):../../common/$(ARCH):
+VPATH   += ../:../../tztest/el1/secure:../../tztest/el1:../../tztest
 
 EL1_S_ELF		= el1_sec.elf
 EL1_S_IMAGE 	= el1_sec.bin
@@ -9,7 +10,10 @@ EL1_S_OBJS		= el1_init.o \
                   el1_loader.o \
                   el1_sec.o \
                   mem_util.o \
-				  builtins.o
+				  builtins.o \
+				  tztest.o \
+				  tztest_el1.o \
+				  tztest_el1_sec.o
 
 libgcc := $(shell $(CC) $(CFLAGS) --print-libgcc-file-name)
 FLATLIBS = ../../libcflat/libcflat.a $(libgcc) ../../libcflat/$(ARCH)/libeabi.a
@@ -18,6 +22,7 @@ FLATLIBS = ../../libcflat/libcflat.a $(libgcc) ../../libcflat/$(ARCH)/libeabi.a
 
 CFLAGS += -I$(ARCH) -I../$(ARCH) -I../ -I../$(ARCH)
 CFLAGS += -I../../common/$(ARCH) -I../../common/
+CFLAGS += -I../../tztest/el1 -I../../tztest
 
 ##################################################################
 
diff --git a/tztest/el1/nonsecure/tztest_el1_nsec.c b/tztest/el1/nonsecure/tztest_el1_nsec.c
new file mode 100644
index 0000000..59c4613
--- /dev/null
+++ b/tztest/el1/nonsecure/tztest_el1_nsec.c
@@ -0,0 +1,69 @@
+#include "libcflat.h"
+#include "svc.h"
+#include "smc.h"
+#include "syscntl.h"
+#include "arm_builtins.h"
+#include "exception.h"
+#include "state.h"
+#include "debug.h"
+#include "tztest_internal.h"
+#include "tztest_el1.h"
+
+uint32_t el1_check_register_access(uint32_t __attribute__((unused))arg)
+{
+    uintptr_t val;
+
+    /* Set things to non-secure P1 and attempt accesses */
+    TEST_HEAD("restricted register access");
+
+    /* We capture the read and use it in the write of the below to insure we do
+     * not destroy the register setting.  This can only go wrong if read fails
+     * but write succeeds which is unlikely.
+     * Note: If the above scenario occurs, we will likely hang on the way out
+     * of the test.
+     */
+    TEST_MSG("SCR read");
+    TEST_EL1_EXCEPTION(val = READ_SCR(), EC_UNKNOWN);
+
+    TEST_MSG("SCR write");
+    TEST_EL1_EXCEPTION(WRITE_SCR(val), EC_UNKNOWN);
+
+    TEST_MSG("SDER read");
+    TEST_EL1_EXCEPTION(val = READ_SDER(), EC_UNKNOWN);
+
+    TEST_MSG("SDER write");
+    TEST_EL1_EXCEPTION(WRITE_SDER(val), EC_UNKNOWN);
+
+#ifdef AARCH32
+    TEST_MSG("MVBAR read");
+    TEST_EL1_EXCEPTION(val = READ_MVBAR(), EC_UNKNOWN);
+
+    TEST_MSG("MVBAR write");
+    TEST_EL1_EXCEPTION(WRITE_MVBAR(val), EC_UNKNOWN);
+
+    /* It should be safe to write a zero to NSACR as we have not disabled any
+     * non-secure access at this point.
+     */
+    TEST_MSG("NSACR write");
+    TEST_EL1_EXCEPTION(WRITE_NSACR(0), EC_UNKNOWN);
+#endif
+
+#ifdef AARCH64
+    TEST_MSG("CPTR_EL3 read");
+    TEST_EL1_EXCEPTION(val = READ_CPTR_EL3(), EC_UNKNOWN);
+
+    TEST_MSG("CPTR_EL3 write");
+    TEST_EL1_EXCEPTION(WRITE_CPTR_EL3(val), EC_UNKNOWN);
+#endif
+
+    return 0;
+}
+
+tztest_t test_func[] = {
+    [TZTEST_REG_ACCESS] = el1_check_register_access,
+#ifdef AARCH64
+    [TZTEST_CPACR_TRAP] = el1_check_cpacr_trap,
+    [TZTEST_WFX_TRAP] = el1_check_wfx_trap
+#endif
+};
+
diff --git a/tztest/el1/secure/tztest_el1_sec.c b/tztest/el1/secure/tztest_el1_sec.c
new file mode 100644
index 0000000..60f70b9
--- /dev/null
+++ b/tztest/el1/secure/tztest_el1_sec.c
@@ -0,0 +1,79 @@
+#include "libcflat.h"
+#include "svc.h"
+#include "smc.h"
+#include "syscntl.h"
+#include "arm_builtins.h"
+#include "exception.h"
+#include "state.h"
+#include "debug.h"
+#include "tztest_internal.h"
+#include "tztest_el1.h"
+
+uint32_t el1_check_register_access(uint32_t __attribute__((unused))arg)
+{
+    uintptr_t val;
+
+    /* Set things to non-secure P1 and attempt accesses */
+    TEST_HEAD("unrestricted register access");
+
+    /* We capture the read and use it in the write of the below to insure we do
+     * not destroy the register setting.  This can only go wrong if read fails
+     * but write succeeds which is unlikely.
+     * Note: If the above scenario occurs, we will likely hang on the way out
+     * of the test.
+     */
+#ifdef AARCH32
+    TEST_MSG("SCR read");
+    TEST_NO_EXCEPTION(val = READ_SCR());
+
+    TEST_MSG("SCR write");
+    TEST_NO_EXCEPTION(WRITE_SCR(val));
+
+    TEST_MSG("SDER read");
+    TEST_NO_EXCEPTION(val = READ_SDER());
+
+    TEST_MSG("SDER write");
+    TEST_NO_EXCEPTION(WRITE_SDER(val));
+
+    TEST_MSG("MVBAR read");
+    TEST_NO_EXCEPTION(val = READ_MVBAR());
+
+    TEST_MSG("MVBAR write");
+    TEST_NO_EXCEPTION(WRITE_MVBAR(val));
+
+    /* It should be safe to write a zero to NSACR as we have not disabled any
+     * non-secure access at this point.
+     */
+    TEST_MSG("NSACR write");
+    TEST_NO_EXCEPTION(WRITE_NSACR(0));
+#else
+    TEST_MSG("SCR read");
+    TEST_EL1_EXCEPTION(val = READ_SCR(), EC_UNKNOWN);
+
+    TEST_MSG("SCR write");
+    TEST_EL1_EXCEPTION(WRITE_SCR(val), EC_UNKNOWN);
+
+    TEST_MSG("SDER read");
+    TEST_EL1_EXCEPTION(val = READ_SDER(), EC_UNKNOWN);
+
+    TEST_MSG("SDER write");
+    TEST_EL1_EXCEPTION(WRITE_SDER(val), EC_UNKNOWN);
+
+    TEST_MSG("CPTR_EL3 read");
+    TEST_EL1_EXCEPTION(val = READ_CPTR_EL3(), EC_UNKNOWN);
+
+    TEST_MSG("CPTR_EL3 write");
+    TEST_EL1_EXCEPTION(WRITE_CPTR_EL3(val), EC_UNKNOWN);
+#endif
+
+    return 0;
+}
+
+tztest_t test_func[] = {
+    [TZTEST_REG_ACCESS] = el1_check_register_access,
+#ifdef AARCH64
+    [TZTEST_CPACR_TRAP] = el1_check_cpacr_trap,
+    [TZTEST_WFX_TRAP] = el1_check_wfx_trap
+#endif
+};
+
diff --git a/tztest/el1/tztest_el1.c b/tztest/el1/tztest_el1.c
new file mode 100644
index 0000000..e3f9856
--- /dev/null
+++ b/tztest/el1/tztest_el1.c
@@ -0,0 +1,100 @@
+#include "libcflat.h"
+#include "svc.h"
+#include "smc.h"
+#include "syscntl.h"
+#include "arm_builtins.h"
+#include "exception.h"
+#include "state.h"
+#include "debug.h"
+#include "tztest_internal.h"
+
+#ifdef AARCH64
+uint32_t el1_check_cpacr_trap(uint32_t __attribute__((unused))arg)
+{
+    uint64_t cptr_el3, cpacr;
+
+    TEST_HEAD("CPACR trapping");
+
+    /* Get the current CPTR so we can restore it later */
+    SMC_GET_REG(CPTR_EL3, 3, cptr_el3);
+
+    /* Disable CPACR access */
+    SMC_SET_REG(CPTR_EL3, 3, cptr_el3 | CPTR_TCPAC);
+
+    /* Try to read CPACR */
+    TEST_MSG("Read of disabled CPACR");
+    TEST_EL3_EXCEPTION(cpacr = READ_CPACR(), EC_SYSINSN);
+
+    /* Try to write CPACR */
+    TEST_MSG("Write of disabled CPACR");
+    TEST_EL3_EXCEPTION(WRITE_CPACR(cpacr), EC_SYSINSN);
+
+#ifdef FP_TEST
+    /* Disable FP access */
+    TEST_MSG("Read of disabled FP reg");
+    SMC_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 */
+    SMC_SET_REG(CPTR_EL3, 3, cptr_el3);
+
+    return 0;
+}
+
+uint32_t el1_check_wfx_trap(uint32_t __attribute__((unused))arg)
+{
+    uint64_t sctlr, scr;
+
+    TEST_HEAD("WFx traps");
+
+    /* Get the current SCR so we can restore it later */
+    SMC_GET_REG(SCR, 3, scr);
+
+    /* Get the current SCTLR so we can restore it later */
+    sctlr = READ_SCTLR();
+
+    /* Clearing SCTLR.nTWE normally traps WFE to EL1 but we are already there */
+    WRITE_SCTLR(sctlr & ~SCTLR_nTWE);
+    TEST_MSG("WFE (SCTLR.nTWE clear, SCR.WFE clear)");
+    TEST_NO_EXCEPTION(asm volatile("wfe\n"));
+
+    /* SCTLR.nTWE left as trapping to check precedence */
+
+    /* Trap WFE instructions to EL3.  This should work even though SCTLR.nTWE
+     * is clear
+     */
+    SMC_SET_REG(SCR, 3, scr | SCR_WFE);
+    TEST_MSG("WFE (SCTLR.nTWE clear, SCR.WFE set)");
+    TEST_EL3_EXCEPTION(asm volatile("wfe\n"), EC_WFI_WFE);
+
+    /* Restore SCTLR */
+    WRITE_SCTLR(sctlr);
+
+    /* This should trap to EL3 with SCTLR.nTWE set */
+    TEST_MSG("WFE (SCTLR.nTWE set, SCR.WFE set)");
+    TEST_EL3_EXCEPTION(asm volatile("wfe\n"), EC_WFI_WFE);
+
+    /* Restore SCR */
+    SMC_SET_REG(SCR, 3, scr);
+
+    /* We cannot test the effect of WFI in EL1 mode like we did with WFE as it
+     * causes a hang.  It is assumed that since the exception is not trapped we
+     * actually execute the instruction.
+     * For this reason we don't bother to test the SCTLR bit effect or
+     * precedence.
+     */
+
+    /* Trap WFI instructions to EL3  */
+    SMC_SET_REG(SCR, 3, scr | SCR_WFI);
+
+    TEST_MSG("WFI (SCR.WFI set)");
+    TEST_EL3_EXCEPTION(asm volatile("wfi\n"), EC_WFI_WFE);
+
+    /* Restore SCR */
+    SMC_SET_REG(SCR, 3, scr);
+
+    return 0;
+}
+#endif
+
diff --git a/tztest/el1/tztest_el1.h b/tztest/el1/tztest_el1.h
new file mode 100644
index 0000000..8545a26
--- /dev/null
+++ b/tztest/el1/tztest_el1.h
@@ -0,0 +1,8 @@
+#ifndef _TZTEST_EL1_H
+#define _TZTEST_EL1_H
+
+uint32_t el1_check_cpacr_trap(uint32_t __attribute__((unused))arg);
+uint32_t el1_check_wfx_trap(uint32_t __attribute__((unused))arg);
+uint32_t el1_check_register_access(uint32_t __attribute__((unused))arg);
+
+#endif
diff --git a/tztest/tztest.c b/tztest/tztest.c
index d1340aa..ad64e0b 100644
--- a/tztest/tztest.c
+++ b/tztest/tztest.c
@@ -23,6 +23,16 @@ tztest_case_t tztest[] = {
     TEST(TZTEST_WFX_TRAP, EL0, NONSECURE, 0),
     TEST(TZTEST_WFX_TRAP, EL0, SECURE, 0),
 #endif
+    TEST(TZTEST_SMC, EL1, NONSECURE, 0),
+    TEST(TZTEST_SMC, EL1, SECURE, 0),
+    TEST(TZTEST_REG_ACCESS, EL1, NONSECURE, 0),
+    TEST(TZTEST_REG_ACCESS, EL1, SECURE, 0),
+#ifdef AARCH64
+    TEST(TZTEST_CPACR_TRAP, EL1, NONSECURE, 0),
+    TEST(TZTEST_CPACR_TRAP, EL1, SECURE, 0),
+    TEST(TZTEST_WFX_TRAP, EL1, NONSECURE, 0),
+    TEST(TZTEST_WFX_TRAP, EL1, SECURE, 0),
+#endif
     TEST_END
 };
 
@@ -42,7 +52,9 @@ void run_test(uint32_t fid, uint32_t el, uint32_t state, uint32_t arg)
     op_dispatch_t disp;
 
     if (el == exception_level && state == secure_state) {
-        test_func[fid](arg);
+        if (test_func[fid]) {
+            test_func[fid](arg);
+        }
     } else {
         disp.fid = fid;
         disp.el = el;