Commonized EL0 code

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

12 files changed, 325 insertions, 462 deletions

diff --git a/aarch64/common/svc.h b/aarch64/common/svc.h
index 9837205..19379d8 100644
--- a/aarch64/common/svc.h
+++ b/aarch64/common/svc.h
@@ -13,16 +13,7 @@
 #ifndef __ASSEMBLY__
 #include "interop.h"
 
-const char *svc_op_name[] = {
-    [SVC_OP_EXIT] = "SVC_OP_EXIT",
-    [SVC_OP_ALLOC] = "SVC_OP_ALLOC",
-    [SVC_OP_MAP] = "SVC_OP_MAP",
-    [SVC_OP_YIELD] = "SVC_OP_YIELD",
-    [SVC_OP_GET_SYSCNTL] = "SVC_OP_GET_SYSCNTL",
-    [SVC_OP_GET_REG] = "SVC_OP_GET_REG",
-    [SVC_OP_SET_REG] = "SVC_OP_SET_REG",
-    [SVC_OP_TEST] = "SVC_OP_TEST"
-};
+extern const char *svc_op_name[];
 
 typedef union {
     op_alloc_mem_t alloc;
diff --git a/aarch64/el0_common/el0_common.h b/aarch64/el0_common/el0_common.h
new file mode 100644
index 0000000..58ad537
--- /dev/null
+++ b/aarch64/el0_common/el0_common.h
@@ -0,0 +1,65 @@
+#ifndef __EL0_COMMON_H
+#define __EL0_COMMON_H
+
+#include "libcflat.h"
+#include "svc.h"
+#include "syscntl.h"
+#include "armv8_exception.h"
+#include "arm_builtins.h"
+#include "el0.h"
+#include "debug.h"
+
+/* Make the below globals volatile as  found that the compiler uses the
+ * register value ratherh than the memory value making it look like the writes
+ * actually happened.
+ */
+
+extern sys_control_t *syscntl;
+
+#define INC_TEST_COUNT()    (syscntl->test_cntl->test_count += 1)
+#define INC_FAIL_COUNT()    (syscntl->test_cntl->fail_count += 1)
+
+#define TEST_CONDITION(_cond)                           \
+    do {                                                \
+        if (!(_cond)) {                                 \
+            printf("FAILED\n");                         \
+            INC_FAIL_COUNT();                           \
+        } else {                                        \
+            printf("PASSED\n");                         \
+        }                                               \
+        INC_TEST_COUNT();                               \
+    } while(0)
+
+#define TEST_FUNCTION(_fn, _cond)                       \
+    do {                                                \
+        _fn;                                            \
+        TEST_CONDITION(_cond);                          \
+    } while(0)
+
+#define TEST_EXCEPTION(_fn, _excp, _el)                 \
+    do {                                                \
+        syscntl->_el.ec = 0;                            \
+        syscntl->excp_action = EXCP_ACTION_SKIP;        \
+        syscntl->excp_log = true;                       \
+        _fn;                                            \
+        TEST_CONDITION(syscntl->_el.taken &&            \
+                       syscntl->_el.ec == (_excp));     \
+        syscntl->_el.taken = 0;                         \
+        syscntl->excp_action = 0;                       \
+        syscntl->_el.ec = 0;                            \
+    } while (0)
+
+#define TEST_EL1S_EXCEPTION(_fn, _excp) \
+        TEST_EXCEPTION(_fn, _excp, el1_excp[SEC])
+#define TEST_EL1NS_EXCEPTION(_fn, _excp) \
+        TEST_EXCEPTION(_fn, _excp, el1_excp[NSEC])
+#define TEST_EL3_EXCEPTION(_fn, _excp) \
+        TEST_EXCEPTION(_fn, _excp, el3_excp)
+
+extern uint32_t P0_nonsecure_check_smc();
+extern uint32_t P0_check_register_access();
+extern uint32_t P0_check_trap_to_EL3();
+extern void *alloc_mem(int type, size_t len);
+extern void map_va(void *va, size_t len, int type);
+
+#endif
diff --git a/aarch64/el0_common/tztest.c b/aarch64/el0_common/tztest.c
new file mode 100644
index 0000000..51a4466
--- /dev/null
+++ b/aarch64/el0_common/tztest.c
@@ -0,0 +1,117 @@
+#include "libcflat.h"
+#include "svc.h"
+#include "syscntl.h"
+#include "armv8_exception.h"
+#include "arm_builtins.h"
+#include "el0.h"
+#include "debug.h"
+#include "el0_common.h"
+
+sys_control_t *syscntl = NULL;
+
+uint32_t P0_nonsecure_check_smc()
+{
+    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);
+
+    return 0;
+}
+
+uint32_t P0_check_register_access()
+{
+    /* Set things to non-secure P1 and attempt accesses */
+    printf("\nValidating %s P0 restricted register access:\n", SEC_STATE_STR);
+
+    printf("\t%s P0 SCR read ... ", SEC_STATE_STR);
+    TEST_EL1NS_EXCEPTION(read_scr_el3(), EC_UNKNOWN);
+
+    printf("\t%s P0 SCR write ... ", SEC_STATE_STR);
+    TEST_EL1NS_EXCEPTION(write_scr_el3(0), EC_UNKNOWN);
+
+    printf("\t%s P0 SDER read ... ", SEC_STATE_STR);
+    TEST_EL1NS_EXCEPTION(read_sder32_el3(), EC_UNKNOWN);
+
+    printf("\t%s P0 SDER write ... ", SEC_STATE_STR);
+    TEST_EL1NS_EXCEPTION(write_sder32_el3(0), EC_UNKNOWN);
+
+/*
+    printf("\t%s P0 MVBAR read ... ", SEC_STATE_STR);
+    TEST_EL1NS_EXCEPTION(read_mvbar(), EC_UNKNOWN);
+
+    printf("\t%s P0 MVBAR write ... ", SEC_STATE_STR);
+    TEST_EL1NS_EXCEPTION(write_mvbar(0), EC_UNKNOWN);
+
+    printf("\t%s P0 NSACR write ... ", SEC_STATE_STR);
+    TEST_EL1NS_EXCEPTION(write_nsacr(0), EC_UNKNOWN);
+*/
+
+    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 ... ", SEC_STATE_STR);
+    TEST_EL1NS_EXCEPTION(write_cptr_el3(0), EC_UNKNOWN);
+
+    return 0;
+}
+
+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;
+    op.alloc.type = type;
+    op.alloc.len = len;
+    op.alloc.addr = NULL;
+    __svc(SVC_OP_ALLOC, &op);
+
+    return op.alloc.addr;
+}
+
+void map_va(void *va, size_t len, int type)
+{
+    svc_op_desc_t op;
+    op.map.va = va;
+    op.map.len = len;
+    op.map.type = type;
+
+    __svc(SVC_OP_MAP, &op);
+}
+
diff --git a/aarch64/el0_ns/Makefile b/aarch64/el0_ns/Makefile
index f7e92e2..2f144ac 100644
--- a/aarch64/el0_ns/Makefile
+++ b/aarch64/el0_ns/Makefile
@@ -1,13 +1,16 @@
-VPATH	= ../common
+VPATH	= ../el0_common:../common
 
 EL0_NS_ELF		= tztest.elf
 EL0_NS_IMAGE 	= tztest.bin
 EL0_NS_LOAD		= tztest.lds
-EL0_NS_OBJS		= tztest.o \
+EL0_NS_OBJS		= tztest_nsec.o \
+				  tztest.o \
                   builtins.o
 
 -include .*.d
 
+CFLAGS += -I../el0_common
+
 ##################################################################
 
 $(EL0_NS_ELF): $(EL0_NS_OBJS) $(EL0_NS_LOAD)
diff --git a/aarch64/el0_ns/tztest.c b/aarch64/el0_ns/tztest.c
deleted file mode 100644
index 8e6ce57..0000000
--- a/aarch64/el0_ns/tztest.c
+++ /dev/null
@@ -1,229 +0,0 @@
-#include "libcflat.h"
-#include "svc.h"
-#include "syscntl.h"
-#include "armv8_exception.h"
-#include "arm_builtins.h"
-#include "el0.h"
-#include "debug.h"
-
-sys_control_t *syscntl = NULL;
-
-/* Make the below globals volatile as  found that the compiler uses the
- * register value ratherh than the memory value making it look like the writes
- * actually happened.
- */
-
-#define INC_TEST_COUNT()    (syscntl->test_cntl->test_count += 1)
-#define INC_FAIL_COUNT()    (syscntl->test_cntl->fail_count += 1)
-
-#define TEST_CONDITION(_cond)                           \
-    do {                                                \
-        if (!(_cond)) {                                 \
-            printf("FAILED\n");                         \
-            INC_FAIL_COUNT();                           \
-        } else {                                        \
-            printf("PASSED\n");                         \
-        }                                               \
-        INC_TEST_COUNT();                               \
-    } while(0)
-
-#define TEST_FUNCTION(_fn, _cond)                       \
-    do {                                                \
-        _fn;                                            \
-        TEST_CONDITION(_cond);                          \
-    } while(0)
-
-#define TEST_EXCEPTION(_fn, _excp, _el)                 \
-    do {                                                \
-        syscntl->_el.ec = 0;                            \
-        syscntl->excp_action = EXCP_ACTION_SKIP;        \
-        syscntl->excp_log = true;                       \
-        _fn;                                            \
-        TEST_CONDITION(syscntl->_el.taken &&            \
-                       syscntl->_el.ec == (_excp));     \
-        syscntl->_el.taken = 0;                         \
-        syscntl->excp_action = 0;                       \
-        syscntl->_el.ec = 0;                            \
-    } while (0)
-
-#define TEST_EL1S_EXCEPTION(_fn, _excp) \
-        TEST_EXCEPTION(_fn, _excp, el1_excp[SEC])
-#define TEST_EL1NS_EXCEPTION(_fn, _excp) \
-        TEST_EXCEPTION(_fn, _excp, el1_excp[NSEC])
-#define TEST_EL3_EXCEPTION(_fn, _excp) \
-        TEST_EXCEPTION(_fn, _excp, el3_excp)
-
-uint32_t P0_nonsecure_check_smc()
-{
-    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);
-
-    return 0;
-}
-
-uint32_t P0_check_register_access()
-{
-    /* Set things to non-secure P1 and attempt accesses */
-    printf("\nValidating %s P0 restricted register access:\n", SEC_STATE_STR);
-
-    printf("\t%s P0 SCR read ... ", SEC_STATE_STR);
-    TEST_EL1NS_EXCEPTION(read_scr_el3(), EC_UNKNOWN);
-
-    printf("\t%s P0 SCR write ... ", SEC_STATE_STR);
-    TEST_EL1NS_EXCEPTION(write_scr_el3(0), EC_UNKNOWN);
-
-    printf("\t%s P0 SDER read ... ", SEC_STATE_STR);
-    TEST_EL1NS_EXCEPTION(read_sder32_el3(), EC_UNKNOWN);
-
-    printf("\t%s P0 SDER write ... ", SEC_STATE_STR);
-    TEST_EL1NS_EXCEPTION(write_sder32_el3(0), EC_UNKNOWN);
-
-/*
-    printf("\t%s P0 MVBAR read ... ", SEC_STATE_STR);
-    TEST_EL1NS_EXCEPTION(read_mvbar(), EC_UNKNOWN);
-
-    printf("\t%s P0 MVBAR write ... ", SEC_STATE_STR);
-    TEST_EL1NS_EXCEPTION(write_mvbar(0), EC_UNKNOWN);
-
-    printf("\t%s P0 NSACR write ... ", SEC_STATE_STR);
-    TEST_EL1NS_EXCEPTION(write_nsacr(0), EC_UNKNOWN);
-*/
-
-    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 ... ", SEC_STATE_STR);
-    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();
-
-    return 0;
-}
-
-uint32_t P0_secure_check_register_access()
-{
-//    validate_state(CPSR_MODE_USR, TZTEST_STATE_SECURE);
-
-    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;
-    op.alloc.type = type;
-    op.alloc.len = len;
-    op.alloc.addr = NULL;
-    __svc(SVC_OP_ALLOC, &op);
-
-    return op.alloc.addr;
-}
-
-void map_va(void *va, size_t len, int type)
-{
-    svc_op_desc_t op;
-    op.map.va = va;
-    op.map.len = len;
-    op.map.type = type;
-
-    __svc(SVC_OP_MAP, &op);
-}
-
-void interop_test()
-{
-    op_test_t test;
-
-    test.orig = test.val = 1024;
-    test.fail = test.count = 0;
-    __svc(SVC_OP_TEST, (svc_op_desc_t *)&test);
-
-    printf("\nValidating interop communication between ELs... ");
-    TEST_CONDITION(!test.fail && test.val == (test.orig >> test.count));
-}
-
-int main()
-{
-    svc_op_desc_t desc;
-
-    printf("Starting TZ test ...\n");
-
-    /* Fetch the system-wide control structure */
-    __svc(SVC_OP_GET_SYSCNTL, &desc);
-    syscntl = ((sys_control_t *)desc.get.data);
-
-    /* Allocate and globally map test control descriptor */
-    syscntl->test_cntl = (test_control_t*)alloc_mem(0, 0x1000);
-    map_va(syscntl->test_cntl, 0x1000, OP_MAP_ALL);
-
-    /* Test EL to EL communication */
-    interop_test();
-
-    /* If we didn't get a valid control structure then something has already
-     * gone drastically wrong.
-     */
-    if (!syscntl) {
-        DEBUG_MSG("Failed to acquire system control structure\n");
-        __svc(SVC_OP_EXIT, &desc);
-    }
-
-    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);
-
-    return 0;
-}
diff --git a/aarch64/el0_ns/tztest_nsec.c b/aarch64/el0_ns/tztest_nsec.c
new file mode 100644
index 0000000..37b2439
--- /dev/null
+++ b/aarch64/el0_ns/tztest_nsec.c
@@ -0,0 +1,51 @@
+#include "el0_common.h"
+
+void interop_test()
+{
+    op_test_t test;
+
+    test.orig = test.val = 1024;
+    test.fail = test.count = 0;
+    __svc(SVC_OP_TEST, (svc_op_desc_t *)&test);
+
+    printf("\nValidating interop communication between ELs... ");
+    TEST_CONDITION(!test.fail && test.val == (test.orig >> test.count));
+}
+
+int main()
+{
+    svc_op_desc_t desc;
+
+    printf("Starting TZ test ...\n");
+
+    /* Fetch the system-wide control structure */
+    __svc(SVC_OP_GET_SYSCNTL, &desc);
+    syscntl = ((sys_control_t *)desc.get.data);
+
+    /* Allocate and globally map test control descriptor */
+    syscntl->test_cntl = (test_control_t*)alloc_mem(0, 0x1000);
+    map_va(syscntl->test_cntl, 0x1000, OP_MAP_ALL);
+
+    /* Test EL to EL communication */
+    interop_test();
+
+    /* If we didn't get a valid control structure then something has already
+     * gone drastically wrong.
+     */
+    if (!syscntl) {
+        DEBUG_MSG("Failed to acquire system control structure\n");
+        __svc(SVC_OP_EXIT, &desc);
+    }
+
+    P0_nonsecure_check_smc();
+    P0_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);
+
+    return 0;
+}
diff --git a/aarch64/el0_s/Makefile b/aarch64/el0_s/Makefile
index 7cd6c17..5a82239 100644
--- a/aarch64/el0_s/Makefile
+++ b/aarch64/el0_s/Makefile
@@ -1,13 +1,16 @@
-VPATH	= ../common
+VPATH	= ../el0_common:../common
 
 EL0_S_ELF		= tztest.elf
 EL0_S_IMAGE 	= tztest.bin
 EL0_S_LOAD		= tztest.lds
-EL0_S_OBJS		= tztest.o \
+EL0_S_OBJS		= tztest_sec.o \
+				  tztest.o \
                   builtins.o
 
 -include .*.d
 
+CFLAGS += -I../el0_common
+
 ##################################################################
 
 $(EL0_S_ELF): $(EL0_S_OBJS) $(EL0_S_LOAD)
diff --git a/aarch64/el0_s/tztest.c b/aarch64/el0_s/tztest.c
deleted file mode 100644
index c9a0506..0000000
--- a/aarch64/el0_s/tztest.c
+++ /dev/null
@@ -1,219 +0,0 @@
-#include "libcflat.h"
-#include "svc.h"
-#include "syscntl.h"
-#include "armv8_exception.h"
-#include "arm_builtins.h"
-#include "el0.h"
-#include "debug.h"
-
-sys_control_t *syscntl = NULL;
-
-/* Make the below globals volatile as  found that the compiler uses the
- * register value ratherh than the memory value making it look like the writes
- * actually happened.
- */
-
-#define INC_TEST_COUNT()    (syscntl->test_cntl->test_count += 1)
-#define INC_FAIL_COUNT()    (syscntl->test_cntl->fail_count += 1)
-
-#define TEST_CONDITION(_cond)                           \
-    do {                                                \
-        if (!(_cond)) {                                 \
-            printf("FAILED\n");                         \
-            INC_FAIL_COUNT();                           \
-        } else {                                        \
-            printf("PASSED\n");                         \
-        }                                               \
-        INC_TEST_COUNT();                               \
-    } while(0)
-
-#define TEST_FUNCTION(_fn, _cond)                       \
-    do {                                                \
-        _fn;                                            \
-        TEST_CONDITION(_cond);                          \
-    } while(0)
-
-#define TEST_EXCEPTION(_fn, _excp, _el)                 \
-    do {                                                \
-        syscntl->_el.ec = 0;                            \
-        syscntl->excp_action = EXCP_ACTION_SKIP;        \
-        syscntl->excp_log = true;                       \
-        _fn;                                            \
-        TEST_CONDITION(syscntl->_el.taken &&            \
-                       syscntl->_el.ec == (_excp));     \
-        syscntl->_el.taken = 0;                         \
-        syscntl->excp_action = 0;                       \
-        syscntl->_el.ec = 0;                            \
-    } while (0)
-
-#define TEST_EL1S_EXCEPTION(_fn, _excp) \
-        TEST_EXCEPTION(_fn, _excp, el1_excp[SEC])
-#define TEST_EL1NS_EXCEPTION(_fn, _excp) \
-        TEST_EXCEPTION(_fn, _excp, el1_excp[NSEC])
-#define TEST_EL3_EXCEPTION(_fn, _excp) \
-        TEST_EXCEPTION(_fn, _excp, el3_excp)
-
-uint32_t P0_nonsecure_check_smc()
-{
-    printf("\nValidating non-secure P0 smc behavior:\n");
-    printf("\tUnprivileged P0 smc call ... ");
-
-    TEST_EL1NS_EXCEPTION(asm volatile("smc #0\n"), EC_UNKNOWN);
-
-    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;
-    op.alloc.type = type;
-    op.alloc.len = len;
-    op.alloc.addr = NULL;
-    __svc(SVC_OP_ALLOC, &op);
-
-    return op.alloc.addr;
-}
-
-void map_va(void *va, size_t len, int type)
-{
-    svc_op_desc_t op;
-    op.map.va = va;
-    op.map.len = len;
-    op.map.type = type;
-
-    __svc(SVC_OP_MAP, &op);
-}
-
-void el0_sec_loop()
-{
-    svc_op_desc_t desc;
-    op_test_t *test = (op_test_t *)&desc;
-    uint32_t op = SVC_OP_YIELD;
-
-    DEBUG_MSG("In loop desc = %p  test = %p\n", &desc, test);
-
-    while (op != SVC_OP_EXIT) {
-        switch (op) {
-        case SVC_OP_MAP:
-            DEBUG_MSG("Doing a MAP desc = %p\n", &desc);
-            op = SVC_OP_MAP;
-            break;
-        case SVC_OP_YIELD:
-            DEBUG_MSG("Doing a YIELD desc = %p\n", &desc);
-            break;
-        case SVC_OP_TEST:
-            DEBUG_MSG("Handling an svc(SVC_OP_TEST) = %p\n", &desc);
-            if (test->val != test->orig >> test->count) {
-                test->fail++;
-            }
-            test->val >>= 1;
-            test->count++;
-            break;
-        case 0:
-            op = SVC_OP_YIELD;
-            break;
-        default:
-            DEBUG_MSG("Unrecognized SVC opcode %d.  Exiting ...\n", op);
-            op = SVC_OP_EXIT;
-            break;
-        }
-
-        DEBUG_MSG("Calling svc(%d, %p)\n", op, &desc);
-        op = __svc(op, &desc);
-        DEBUG_MSG("Back from svc - op = %d &desc = %p\n", op, &desc);
-    }
-
-    __svc(SVC_OP_EXIT, NULL);
-}
-
-int main()
-{
-    svc_op_desc_t desc;
-
-    printf("Starting secure-side EL0  ...\n");
-
-    /* Fetch the system-wide control structure */
-    __svc(SVC_OP_GET_SYSCNTL, &desc);
-    syscntl = (sys_control_t *)desc.get.data;
-
-    /* If we didn't get a valid control structure then something has already
-     * gone drastically wrong.
-     */
-    if (!syscntl) {
-        printf("Failed to acquire system control structure\n");
-        __svc(SVC_OP_EXIT, NULL);
-    }
-
-    el0_sec_loop();
-
-//    P0_nonsecure_check_smc();
-//    P0_nonsecure_check_register_access();
-
-    /* Fetch the system-wide control structure */
-//    __svc(SVC_OP_GET_MODE, &get_data);
-//    uint64_t el = get_data.data;
-//    printf("EL = 0x%lx\n", el);
-
-    __svc(SVC_OP_EXIT, NULL);
-
-    return 0;
-}
diff --git a/aarch64/el0_s/tztest_sec.c b/aarch64/el0_s/tztest_sec.c
new file mode 100644
index 0000000..a7ee3b7
--- /dev/null
+++ b/aarch64/el0_s/tztest_sec.c
@@ -0,0 +1,66 @@
+#include "el0_common.h"
+
+void el0_sec_loop()
+{
+    svc_op_desc_t desc;
+    op_test_t *test = (op_test_t *)&desc;
+    uint32_t op = SVC_OP_YIELD;
+
+    DEBUG_MSG("Starting loop - desc = %p  test = %p\n", &desc, test);
+
+    while (op != SVC_OP_EXIT) {
+        switch (op) {
+        case SVC_OP_MAP:
+            DEBUG_MSG("Handling a SVC_OP_MAP - desc = %p\n", &desc);
+            op = SVC_OP_MAP;
+            break;
+        case SVC_OP_YIELD:
+            DEBUG_MSG("Handling a SVC_OP_YIELD - desc = %p\n", &desc);
+            break;
+        case SVC_OP_TEST:
+            DEBUG_MSG("Handling a SVC_OP_TEST - desc =  %p\n", &desc);
+            if (test->val != test->orig >> test->count) {
+                test->fail++;
+            }
+            test->val >>= 1;
+            test->count++;
+            break;
+        case 0:
+            op = SVC_OP_YIELD;
+            break;
+        default:
+            DEBUG_MSG("Unrecognized SVC opcode %d.  Exiting ...\n", op);
+            op = SVC_OP_EXIT;
+            break;
+        }
+
+        DEBUG_MSG("Calling svc(%d, %p)\n", op, &desc);
+        op = __svc(op, &desc);
+        DEBUG_MSG("Returned from svc - op = %d &desc = %p\n", op, &desc);
+    }
+
+    __svc(SVC_OP_EXIT, NULL);
+}
+
+int main()
+{
+    svc_op_desc_t desc;
+
+    printf("Starting secure-side EL0  ...\n");
+
+    /* Fetch the system-wide control structure */
+    __svc(SVC_OP_GET_SYSCNTL, &desc);
+    syscntl = (sys_control_t *)desc.get.data;
+
+    /* If we didn't get a valid control structure then something has already
+     * gone drastically wrong.
+     */
+    if (!syscntl) {
+        printf("Failed to acquire system control structure\n");
+        __svc(SVC_OP_EXIT, NULL);
+    }
+
+    el0_sec_loop();
+
+    return 0;
+}
diff --git a/aarch64/el1_common/svc.c b/aarch64/el1_common/svc.c
new file mode 100644
index 0000000..cbe2488
--- /dev/null
+++ b/aarch64/el1_common/svc.c
@@ -0,0 +1,13 @@
+#include "svc.h"
+
+const char *svc_op_name[] = {
+    [SVC_OP_EXIT] = "SVC_OP_EXIT",
+    [SVC_OP_ALLOC] = "SVC_OP_ALLOC",
+    [SVC_OP_MAP] = "SVC_OP_MAP",
+    [SVC_OP_YIELD] = "SVC_OP_YIELD",
+    [SVC_OP_GET_SYSCNTL] = "SVC_OP_GET_SYSCNTL",
+    [SVC_OP_GET_REG] = "SVC_OP_GET_REG",
+    [SVC_OP_SET_REG] = "SVC_OP_SET_REG",
+    [SVC_OP_TEST] = "SVC_OP_TEST"
+};
+
diff --git a/aarch64/el1_ns/Makefile b/aarch64/el1_ns/Makefile
index a24b271..6a6b97c 100644
--- a/aarch64/el1_ns/Makefile
+++ b/aarch64/el1_ns/Makefile
@@ -7,6 +7,7 @@ EL1_NS_OBJS		= el1_init.o \
                   el1_exception.o \
                   el1.o \
                   el1_nsec.o \
+				  svc.o \
                   builtins.o
 
 -include .*.d
diff --git a/aarch64/el1_s/Makefile b/aarch64/el1_s/Makefile
index 189174d..0396603 100644
--- a/aarch64/el1_s/Makefile
+++ b/aarch64/el1_s/Makefile
@@ -7,6 +7,7 @@ EL1_S_OBJS		= el1_init.o \
                   el1_exception.o \
                   el1.o \
                   el1_sec.o \
+				  svc.o \
 				  builtins.o
 
 -include .*.d