Add secure EL0 along with supporting changes

Added a separate secure EL0 image and loading.  Changed exception model to have
commands driven from this level.  Fixed exception handler issues and cleaned up
debug messages.

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

25 files changed, 551 insertions, 188 deletions

diff --git a/.gdbinit64 b/.gdbinit64
index 8eb2761..0dbffe2 100644
--- a/.gdbinit64
+++ b/.gdbinit64
@@ -21,3 +21,7 @@ define load_el0ns
 add-symbol-file aarch64/el0_ns/tztest.elf 0x10000
 end
 
+define load_el0s
+add-symbol-file aarch64/el0_s/tztest.elf 0x10000
+end
+
diff --git a/aarch64/Makefile b/aarch64/Makefile
index 2223c6b..12cf91f 100644
--- a/aarch64/Makefile
+++ b/aarch64/Makefile
@@ -2,7 +2,8 @@ BIOS_IMAGE 		= tztest.img
 EL3_IMAGE		= el3/el3.bin
 EL1_S_IMAGE		= el1_s/el1_sec.bin
 EL1_NS_IMAGE	= el1_ns/el1_nsec.bin
-TZTEST_IMAGE 	= el0_ns/tztest.elf
+TZTEST_NS_IMAGE = el0_ns/tztest.elf
+TZTEST_S_IMAGE 	= el0_s/tztest.elf
 
 CFLAGS += -I../../platform/$(PLAT) -I../../libcflat/include -I../common
 
@@ -10,11 +11,12 @@ CFLAGS += -I../../platform/$(PLAT) -I../../libcflat/include -I../common
 
 ##################################################################
 
-$(BIOS_IMAGE): $(EL3_IMAGE) $(EL1_S_IMAGE) $(EL1_NS_IMAGE) $(TZTEST_IMAGE)
+$(BIOS_IMAGE): $(EL3_IMAGE) $(EL1_S_IMAGE) $(EL1_NS_IMAGE) $(TZTEST_NS_IMAGE) $(TZTEST_S_IMAGE)
 	dd if=$(EL3_IMAGE) of=$@ 2> /dev/null
 	dd oflag=seek_bytes seek=65536 if=$(EL1_S_IMAGE) of=$@ 2> /dev/null
 	dd oflag=seek_bytes seek=131072 if=$(EL1_NS_IMAGE) of=$@ 2> /dev/null
-	dd oflag=seek_bytes seek=196608 if=$(TZTEST_IMAGE) of=$@ 2> /dev/null
+	dd oflag=seek_bytes seek=196608 if=$(TZTEST_NS_IMAGE) of=$@ 2> /dev/null
+	dd oflag=seek_bytes seek=327680 if=$(TZTEST_S_IMAGE) of=$@ 2> /dev/null
 	chmod +x $(BIOS_IMAGE)
 
 $(EL3_IMAGE):
@@ -26,14 +28,18 @@ $(EL1_S_IMAGE):
 $(EL1_NS_IMAGE):
 	$(MAKE) -C el1_ns all
 
-$(TZTEST_IMAGE):
+$(TZTEST_NS_IMAGE):
 	$(MAKE) -C el0_ns all
 
+$(TZTEST_S_IMAGE):
+	$(MAKE) -C el0_s all
+
 all: $(BIOS_IMAGE)
 
 clean:
 	$(MAKE) -C el3 clean
 	$(MAKE) -C el1_s clean
 	$(MAKE) -C el1_ns clean
+	$(MAKE) -C el0_s clean
 	$(MAKE) -C el0_ns clean
 	$(RM) $(BIOS_IMAGE) .*.d
diff --git a/aarch64/common/debug.h b/aarch64/common/debug.h
index 3d74f34..673f6dd 100644
--- a/aarch64/common/debug.h
+++ b/aarch64/common/debug.h
@@ -3,7 +3,7 @@
 
 #ifdef DEBUG
 #define DEBUG_MSG(_str, ...) \
-    printf("\n[DEBUG] %s: " _str, __FUNCTION__, ##__VA_ARGS__)
+    printf("\n[DEBUG] %s (%s): " _str, __FUNCTION__, SEC_STATE_STR, ##__VA_ARGS__)
 #define DEBUG_ARG
 #else
 #define DEBUG_MSG(_str, ...)
diff --git a/aarch64/common/interop.h b/aarch64/common/interop.h
index 32f0f7a..4e14a60 100644
--- a/aarch64/common/interop.h
+++ b/aarch64/common/interop.h
@@ -33,4 +33,11 @@ typedef struct {
 typedef struct {
     uint64_t data;
 } op_get_data_t;
+
+typedef struct {
+    uint64_t orig;
+    uint64_t val;
+    uint64_t count;
+    uint64_t fail;
+} op_test_t;
 #endif
diff --git a/aarch64/common/smc.h b/aarch64/common/smc.h
index a36e7a3..14fc393 100644
--- a/aarch64/common/smc.h
+++ b/aarch64/common/smc.h
@@ -9,23 +9,32 @@
 #define SMC_OP_ALLOCATE_SECURE_MEMORY 4
 #define SMC_OP_EXIT 5
 #define SMC_OP_MAP 8
+#define SMC_OP_TEST 13
 
 #ifndef __ASSEMBLY__
 #include "interop.h"
 
 extern uint32_t __smc(uint32_t, void *);
 
+const char *smc_op_name[] = {
+    [SMC_OP_NOOP] = "SMC_OP_NOOP",
+    [SMC_OP_DISPATCH_MONITOR] = "SMC_DISPATCH_MONITOR",
+    [SMC_OP_YIELD] = "SMC_OP_YIELD",
+    [SMC_OP_EXIT] = "SMC_OP_EXIT",
+    [SMC_OP_MAP] = "SMC_OP_MAP",
+    [SMC_OP_TEST] = "SMC_OP_TEST"
+};
+
 typedef union {
     op_dispatch_t dispatch;
     op_map_mem_t map;
+    op_test_t test;
 } smc_op_desc_t;
 
 extern smc_op_desc_t *smc_interop_buf;
 
-#define SMC_NO_DESC(_op) __smc((_op), NULL)
-
-#define SMC_EXIT()  SMC_NO_DESC(SMC_OP_EXIT)
-#define SMC_YIELD()  SMC_NO_DESC(SMC_OP_YIELD)
+#define SMC_EXIT()  __smc(SMC_OP_EXIT, NULL)
+#define SMC_YIELD()  __smc(SMC_OP_YIELD, smc_interop_buf);
 
 #endif
 
diff --git a/aarch64/common/svc.h b/aarch64/common/svc.h
index 8c4c43a..7ec72ef 100644
--- a/aarch64/common/svc.h
+++ b/aarch64/common/svc.h
@@ -1,30 +1,36 @@
 #ifndef _SVC_H
 #define _SVC_H
 
-#define SVC_RETURN_FROM_SECURE_USR 0
-#define SVC_DISPATCH_MONITOR 1
-#define SVC_DISPATCH_SECURE_USR 2
-#define SVC_DISPATCH_SECURE_SVC 3
-#define SVC_DISPATCH_NONSECURE_SVC 4
-#define SVC_GET_SECURE_STATE 5
-#define SVC_EXIT 6
-#define SVC_ALLOC 7
-#define SVC_MAP 8
-#define SVC_YIELD 9
-#define SVC_GET_SYSCNTL 10
-#define SVC_GET_MODE 11
+#define SVC_OP_EXIT 6
+#define SVC_OP_ALLOC 7
+#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
 
 #ifndef __ASSEMBLY__
 #include "interop.h"
 
-extern void __svc(uint32_t, void *);
+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_MODE] = "SVC_OP_GET_MODE",
+    [SVC_OP_TEST] = "SVC_OP_TEST"
+};
 
 typedef union {
     op_alloc_mem_t alloc;
     op_map_mem_t map;
     op_get_data_t get;
+    op_test_t test;
 } svc_op_desc_t;
 
+extern uint32_t __svc(uint32_t, const svc_op_desc_t *);
+
 #endif
 
 #endif
diff --git a/aarch64/common/syscntl.h b/aarch64/common/syscntl.h
index 0b78ef5..75d2637 100644
--- a/aarch64/common/syscntl.h
+++ b/aarch64/common/syscntl.h
@@ -21,11 +21,17 @@ typedef struct {
 #define NSEC    1
 
 typedef struct {
+    volatile int fail_count;
+    volatile int test_count;
+} test_control_t;
+
+typedef struct {
     smc_interop_t smc_interop;
     sys_exception_t el3_excp;
     sys_exception_t el1_excp[2];
     uint32_t excp_action;
     bool excp_log;
+    test_control_t *test_cntl;
 } sys_control_t;
 
 #endif
diff --git a/aarch64/el0_ns/el0.h b/aarch64/el0_ns/el0.h
new file mode 100644
index 0000000..28a34d8
--- /dev/null
+++ b/aarch64/el0_ns/el0.h
@@ -0,0 +1,11 @@
+#ifndef _EL0_H
+#define _EL0_H
+
+#include "platform.h"
+
+#define EL0_STACK_BASE EL0_NS_STACK_BASE
+
+#define SEC_STATE_STR "non-secure"
+#define SEC_STATE 1
+
+#endif
diff --git a/aarch64/el0_ns/tztest.c b/aarch64/el0_ns/tztest.c
index d2ea4e4..bbc4f4f 100644
--- a/aarch64/el0_ns/tztest.c
+++ b/aarch64/el0_ns/tztest.c
@@ -3,13 +3,9 @@
 #include "syscntl.h"
 #include "armv8_exception.h"
 #include "arm_builtins.h"
+#include "el0.h"
+#include "debug.h"
 
-typedef struct {
-    volatile int fail_count;
-    volatile int test_count;
-} test_control_t;
-
-test_control_t *tztest_cntl;
 sys_control_t *syscntl = NULL;
 
 /* Make the below globals volatile as  found that the compiler uses the
@@ -17,8 +13,8 @@ sys_control_t *syscntl = NULL;
  * actually happened.
  */
 
-#define INC_TEST_COUNT()    (tztest_cntl->test_count += 1)
-#define INC_FAIL_COUNT()    (tztest_cntl->fail_count += 1)
+#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 {                                                \
@@ -132,7 +128,7 @@ void *alloc_mem(int type, size_t len)
     op.alloc.type = type;
     op.alloc.len = len;
     op.alloc.addr = NULL;
-    __svc(SVC_ALLOC, &op);
+    __svc(SVC_OP_ALLOC, &op);
 
     return op.alloc.addr;
 }
@@ -144,42 +140,50 @@ void map_va(void *va, size_t len, int type)
     op.map.len = len;
     op.map.type = type;
 
-    __svc(SVC_MAP, &op);
+    __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("Testing interop communication between ELs... ");
+    TEST_CONDITION(!test.fail && test.val == (test.orig >> test.count));
 }
 
 int main()
 {
-    op_get_data_t get_data;
+    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 */
-    tztest_cntl = (test_control_t*)alloc_mem(0, 0x1000);
-    map_va(tztest_cntl, 0x1000, OP_MAP_ALL);
+    syscntl->test_cntl = (test_control_t*)alloc_mem(0, 0x1000);
+    map_va(syscntl->test_cntl, 0x1000, OP_MAP_ALL);
 
-    /* Fetch the system-wide control structure */
-    __svc(SVC_GET_SYSCNTL, &get_data);
-    syscntl = (sys_control_t *)get_data.data;
+    /* 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) {
-        printf("Failed to acquire system control structure\n");
-        __svc(SVC_EXIT, NULL);
+        DEBUG_MSG("Failed to acquire system control structure\n");
+        __svc(SVC_OP_EXIT, &desc);
     }
 
-    __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);
+    __svc(SVC_OP_EXIT, NULL);
 
     return 0;
 }
diff --git a/aarch64/el0_s/Makefile b/aarch64/el0_s/Makefile
new file mode 100644
index 0000000..7cd6c17
--- /dev/null
+++ b/aarch64/el0_s/Makefile
@@ -0,0 +1,28 @@
+VPATH	= ../common
+
+EL0_S_ELF		= tztest.elf
+EL0_S_IMAGE 	= tztest.bin
+EL0_S_LOAD		= tztest.lds
+EL0_S_OBJS		= tztest.o \
+                  builtins.o
+
+-include .*.d
+
+##################################################################
+
+$(EL0_S_ELF): $(EL0_S_OBJS) $(EL0_S_LOAD)
+	$(LD) -fpic -pie -o $@ $(EL0_S_OBJS) $(FLATLIBS) --script=$(EL0_S_LOAD)
+
+$(EL0_S_IMAGE): $(EL0_S_ELF)
+	$(OBJCOPY) -O binary $< $@
+
+$(EL0_S_LOAD): tztest.lds.S Makefile ../../platform/$(PLAT)/
+	$(CC) $(CFLAGS) -fpic -pie -E -P -C -o $@ $<
+
+%.o: %.S
+	$(CC) $(CFLAGS) -mcmodel=large -c -nostdlib -o $(notdir $@) $<
+
+all: $(EL0_S_IMAGE)
+
+clean:
+	$(RM) $(EL0_S_OBJS) $(EL0_S_LOAD) $(EL0_S_ELF) $(EL0_S_IMAGE) .*.d
diff --git a/aarch64/el0_s/el0.h b/aarch64/el0_s/el0.h
new file mode 100644
index 0000000..290f3d5
--- /dev/null
+++ b/aarch64/el0_s/el0.h
@@ -0,0 +1,11 @@
+#ifndef _EL0_H
+#define _EL0_H
+
+#include "platform.h"
+
+#define EL0_STACK_BASE EL0_S_STACK_BASE
+
+#define SEC_STATE_STR "secure"
+#define SEC_STATE 0
+
+#endif
diff --git a/aarch64/el0_s/tztest.c b/aarch64/el0_s/tztest.c
new file mode 100644
index 0000000..c9a0506
--- /dev/null
+++ b/aarch64/el0_s/tztest.c
@@ -0,0 +1,219 @@
+#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.lds.S b/aarch64/el0_s/tztest.lds.S
new file mode 100644
index 0000000..35eeb67
--- /dev/null
+++ b/aarch64/el0_s/tztest.lds.S
@@ -0,0 +1,22 @@
+OUTPUT_FORMAT("elf64-littleaarch64")
+OUTPUT_ARCH(aarch64)
+TARGET(binary)
+ENTRY(main)
+
+#include "platform.h"
+
+SECTIONS
+{
+    . = 0x0;
+    .text . : {
+        *(.text);
+        _EL0_NS_TEXT_SIZE = SIZEOF(.text);
+    }
+
+    . = ALIGN(4k);
+    .data . : {
+        *(.*data);
+        *(.*bss);
+        _EL0_NS_DATA_SIZE = SIZEOF(.data);
+    }
+}
diff --git a/aarch64/el1_common/el1.c b/aarch64/el1_common/el1.c
index b6f3a6c..d59e635 100644
--- a/aarch64/el1_common/el1.c
+++ b/aarch64/el1_common/el1.c
@@ -155,46 +155,72 @@ void *el1_lookup_pa(void *va)
 
 void el1_map_secure(op_map_mem_t *map)
 {
-    smc_op_desc_t *desc = (smc_op_desc_t *)smc_interop_buf;
+    if ((map->type & OP_MAP_EL3) == OP_MAP_EL3) {
+        smc_op_desc_t *desc = (smc_op_desc_t *)smc_interop_buf;
+        memcpy(desc, map, sizeof(op_map_mem_t));
+        if (desc->map.pa) {
+            desc->map.pa = el1_lookup_pa(desc->map.va);
+        }
+        __smc(SMC_OP_MAP, desc);
+    } else {
+        el1_map_pa((uint64_t)map->va, (uint64_t)map->pa);
+    }
+}
 
-    memcpy(desc, map, sizeof(op_map_mem_t));
+void el1_interop_test(op_test_t *desc)
+{
+    op_test_t *test = (op_test_t*)smc_interop_buf;
 
-    desc->map.pa = el1_lookup_pa(desc->map.va);
+    memcpy(smc_interop_buf, desc, sizeof(smc_op_desc_t));
+    if (test->val != test->orig >> test->count) {
+        test->fail++;
+    }
+    test->val >>= 1;
+    test->count++;
 
-    __smc(SMC_OP_MAP, desc);
+    __smc(SMC_OP_TEST, smc_interop_buf);
+    if (test->val != test->orig >> test->count) {
+        test->fail++;
+    }
+    test->val >>= 1;
+    test->count++;
+    memcpy(desc, smc_interop_buf, sizeof(smc_op_desc_t));
 }
 
-void el1_handle_svc(uint64_t ec, uint32_t op, svc_op_desc_t *desc)
+int el1_handle_svc(uint32_t op, svc_op_desc_t *desc)
 {
-    assert(ec == EC_SVC64);
+    uint32_t ret = 0;
 
+    DEBUG_MSG("Took an svc(%s) - desc = %p\n", svc_op_name[op], desc);
     switch (op) {
-    case SVC_EXIT:
-        DEBUG_MSG("took an svc(SVC_EXIT) \n", op);
+    case SVC_OP_EXIT:
         SMC_EXIT();
         break;
-    case SVC_YIELD:
-        DEBUG_MSG("took an svc(SVC_YIELD) \n", op);
-        SMC_YIELD();
+    case SVC_OP_YIELD:
+        ret = SMC_YIELD();
+        memcpy(desc, smc_interop_buf, sizeof(smc_op_desc_t));
         break;
-    case SVC_ALLOC:
-        DEBUG_MSG("took an svc(SVC_ALLOC) \n", op);
-        el1_alloc_mem((op_alloc_mem_t *)&desc->alloc);
+    case SVC_OP_ALLOC:
+        el1_alloc_mem((op_alloc_mem_t *)desc);
         break;
-    case SVC_MAP:
-        DEBUG_MSG("took an svc(SVC_MAP) \n", op);
-        el1_map_secure((op_map_mem_t *)&desc->map);
+    case SVC_OP_MAP:
+        el1_map_secure((op_map_mem_t *)desc);
         break;
-    case SVC_GET_SYSCNTL:
+    case SVC_OP_GET_SYSCNTL:
         desc->get.data = (uint64_t)syscntl;
         break;
-    case SVC_GET_MODE:
+    case SVC_OP_GET_MODE:
         desc->get.data = read_currentel();
         break;
+    case SVC_OP_TEST:
+        el1_interop_test((op_test_t *)desc);
+        break;
     default:
-        printf("Unrecognized AArch64 SVC opcode: op = %d\n", op);
+        DEBUG_MSG("Unrecognized AArch64 SVC opcode: op = %d\n", op);
         break;
     }
+
+    return ret;
 }
 
 void el1_handle_exception(uint64_t ec, uint64_t iss)
@@ -217,8 +243,7 @@ void el1_handle_exception(uint64_t ec, uint64_t iss)
 
     switch (ec) {
     case EC_UNKNOWN:
-        DEBUG_MSG("EL1_%s: Unknown exception far = 0x%lx  elr = 0x%lx\n",
-                  SEC_STATE ? "NS" : "S", far, elr);
+        DEBUG_MSG("Unknown 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) {
@@ -228,26 +253,26 @@ void el1_handle_exception(uint64_t ec, uint64_t iss)
         break;
 
     case EC_IABORT_LOWER:
-        DEBUG_MSG("EL1_%s: Instruction abort at lower level: far = %0lx\n",
-                  SEC_STATE ? "NS" : "S", far);
+        DEBUG_MSG("Instruction abort at lower level: far = %0lx\n", far);
+        SMC_EXIT();
         break;
     case EC_IABORT:
-        DEBUG_MSG("EL1_%s: Instruction abort at EL1: far = %0lx\n",
-                  SEC_STATE ? "NS" : "S", far);
+        DEBUG_MSG("Instruction abort at EL1: far = %0lx\n", far);
+        SMC_EXIT();
         break;
     case EC_DABORT_LOWER:
-        DEBUG_MSG("EL1_%s: Data abort (%s) at lower level: "
-                  "far = %0lx elr = %0lx\n",
-                  SEC_STATE ? "NS" : "S", dai.wnr ? "write" : "read",
-                  far, elr);
+        DEBUG_MSG("Data abort (%s) at lower level: far = %0lx elr = %0lx\n",
+                  dai.wnr ? "write" : "read", far, elr);
+        SMC_EXIT();
         break;
     case EC_DABORT:
-        DEBUG_MSG("EL1_%s: Data abort (%s) at EL1: far = %0lx elr = %0lx\n",
-                  SEC_STATE ? "NS" : "S", dai.wnr ? "write" : "read", far, elr);
+        DEBUG_MSG("Data abort (%s) at EL1: far = %0lx elr = %0lx\n",
+                  dai.wnr ? "write" : "read", far, elr);
+        SMC_EXIT();
         break;
     default:
-        DEBUG_MSG("EL1_%s: Unhandled EL1 exception: EC = %d  ISS = %d\n",
-                  SEC_STATE ? "NS" : "S", ec, iss);
+        DEBUG_MSG("Unhandled EL1 exception: EC = %d  ISS = %d\n", ec, iss);
+        SMC_EXIT();
         break;
     }
 }
diff --git a/aarch64/el1_common/el1_exception.S b/aarch64/el1_common/el1_exception.S
index 058c222..15c90d7 100644
--- a/aarch64/el1_common/el1_exception.S
+++ b/aarch64/el1_common/el1_exception.S
@@ -24,7 +24,6 @@ el1_sync_exception_current:
 el1_sync_exception_lower64:
     stp x30, x2, [sp, #-16]!
     stp x0, x1, [sp, #-16]!
-    mov x2, x0;
     mrs x0, esr_el1
     mov x1, #0xffffff
     and x1, x1, x0
@@ -35,14 +34,13 @@ el1_sync_exception_lower64:
     b.eq el1_sync_exception_lower64_svc
     bl el1_handle_exception
     ldp x0, x1, [sp], #16
-    ldp x30, x2, [sp], #16
     b el1_sync_exception_lower64_done
 el1_sync_exception_lower64_svc:
-    ldp x1, x2, [sp]        /* Fetch our inputs as SVC args */
+    ldp x0, x1, [sp]       /* Fetch our inputs as SVC args */
     bl el1_handle_svc
-    ldp x0, x1, [sp], #16
-    ldp x30, x2, [sp], #16
+    ldp x2, x1, [sp], #16
 el1_sync_exception_lower64_done:
+    ldp x30, x2, [sp], #16
     eret
 .align 7
 el1_serr_exception:
diff --git a/aarch64/el1_common/el1_init.S b/aarch64/el1_common/el1_init.S
index d32dcdc..ebb2866 100644
--- a/aarch64/el1_common/el1_init.S
+++ b/aarch64/el1_common/el1_init.S
@@ -1,5 +1,6 @@
 #define __ASSEMBLY__
 #include "el1.h"
+#include "el0.h"
 #include "armv8_vmsa.h"
 #undef __ASSEMBLY__
 
diff --git a/aarch64/el1_ns/Makefile b/aarch64/el1_ns/Makefile
index 76bdd26..a24b271 100644
--- a/aarch64/el1_ns/Makefile
+++ b/aarch64/el1_ns/Makefile
@@ -11,6 +11,8 @@ EL1_NS_OBJS		= el1_init.o \
 
 -include .*.d
 
+CFLAGS += -I../el0_ns/
+
 ##################################################################
 
 $(EL1_NS_ELF): $(EL1_NS_OBJS) $(EL1_NS_LOAD)
diff --git a/aarch64/el1_ns/el1_nsec.c b/aarch64/el1_ns/el1_nsec.c
index de4560a..bbaaee2 100644
--- a/aarch64/el1_ns/el1_nsec.c
+++ b/aarch64/el1_ns/el1_nsec.c
@@ -1,5 +1,7 @@
 #include "platform.h"
 #include "arm_builtins.h"
+#include "el1.h"
+#include "debug.h"
 
 extern void *el1_load_el0(char *base, char *start_va);
 
diff --git a/aarch64/el1_s/Makefile b/aarch64/el1_s/Makefile
index 0ae0d5c..189174d 100644
--- a/aarch64/el1_s/Makefile
+++ b/aarch64/el1_s/Makefile
@@ -11,6 +11,8 @@ EL1_S_OBJS		= el1_init.o \
 
 -include .*.d
 
+CFLAGS += -I../el0_s/
+
 ##################################################################
 
 $(EL1_S_ELF): $(EL1_S_OBJS) $(EL1_S_LOAD)
diff --git a/aarch64/el1_s/el1_sec.c b/aarch64/el1_s/el1_sec.c
index cde517f..fd9b3ab 100644
--- a/aarch64/el1_s/el1_sec.c
+++ b/aarch64/el1_s/el1_sec.c
@@ -2,39 +2,17 @@
 #include "arm_builtins.h"
 #include "libcflat.h"
 #include <stdint.h>
-#include "smc.h"
+#include "el1.h"
 #include "debug.h"
 
-extern void el1_map_pa(uintptr_t vaddr, uintptr_t paddr);
-void el1_sec_smc_loop()
-{
-    smc_op_desc_t *desc = smc_interop_buf;
-    uint32_t op = SMC_OP_YIELD;
-
-    DEBUG_MSG("EL1_S: In loop\n");
-
-    while (op != SMC_OP_EXIT) {
-        switch (op) {
-        case SMC_OP_MAP:
-            DEBUG_MSG("EL1_S: Doing a MAP desc = %p\n", desc);
-            el1_map_pa((uintptr_t)(desc->map.va), (uintptr_t)(desc->map.pa));
-            break;
-        case SMC_OP_YIELD:
-            DEBUG_MSG("EL1_S: Doing a YIELD desc = %p\n", desc);
-            break;
-        default:
-            DEBUG_MSG("Unrecognized SMC opcode %d.  Exiting ...\n", op);
-            SMC_EXIT();
-            break;
-        }
-
-        op = __smc(op, desc);
-    }
-
-    SMC_EXIT();
-}
+extern void *el1_load_el0(char *base, char *start_va);
 
 void el1_init_el0()
 {
-    el1_sec_smc_loop();
+    int (*main)(void);
+
+    main = el1_load_el0((char *)EL0_S_FLASH_BASE, (char *)EL0_NS_BASE_VA);
+
+    __set_exception_return(main);
+    __exception_return();
 }
diff --git a/aarch64/el3/el3.c b/aarch64/el3/el3.c
index 46a2642..f8189b1 100644
--- a/aarch64/el3/el3.c
+++ b/aarch64/el3/el3.c
@@ -6,13 +6,15 @@
 #include "libcflat.h"
 #include "smc.h"
 #include "svc.h"
-#include "debug.h"
 #include "armv8_exception.h"
 #include "armv8_vmsa.h"
 #include "el3_monitor.h"
 #include "arm_builtins.h"
 #include "syscntl.h"
 
+#define SEC_STATE_STR "EL3"
+#include "debug.h"
+
 state_buf sec_state;
 state_buf nsec_state;
 
@@ -170,46 +172,52 @@ void *el3_lookup_pa(void *va)
     return (void *)pa;
 }
 
-void el3_map_mem(op_map_mem_t *map)
+uint32_t el3_map_mem(op_map_mem_t *map)
 {
     if ((map->type & OP_MAP_EL3) == OP_MAP_EL3) {
         el3_map_pa((uintptr_t)map->va, (uintptr_t)map->pa);
-        DEBUG_MSG("EL3: Mapped VA:0x%lx to PA:0x%lx\n", map->va, map->pa);
+        DEBUG_MSG("Mapped VA:0x%lx to PA:0x%lx\n", map->va, map->pa);
     }
 
-/*
     if ((map->type & OP_MAP_SEC_EL1) == OP_MAP_SEC_EL1) {
-        monitor_switch(SMC_OP_MAP, (smc_op_desc_t *)map);
+        map->type &= ~OP_MAP_EL3;
+        DEBUG_MSG("Initiating SVC_OP_MAP\n");
+        return SVC_OP_MAP;
     }
-*/
+
+    return 0;
 }
 
-int el3_handle_smc(uint64_t ec, uint64_t op, smc_op_desc_t *desc)
+int el3_handle_smc(uint64_t op, smc_op_desc_t *desc)
 {
-    assert(ec == EC_SMC64);
+    op_test_t *test = (op_test_t*)desc;
 
+    DEBUG_MSG("Took an smc(%s) - desc = %p\n", smc_op_name[op], desc);
     switch (op) {
     case SMC_OP_YIELD:
-        DEBUG_MSG("took an SMC(SMC_YIELD) exception\n");
-        return 1;
+        return SVC_OP_YIELD;
         break;
     case SMC_OP_DISPATCH_MONITOR:
-        DEBUG_MSG("took an smc(SMC_OP_DSPATCH_MONITOR) exception\n");
         el3_dispatch((op_dispatch_t *)&desc->dispatch);
         break;
     case SMC_OP_MAP:
-        DEBUG_MSG("took an smc(SMC_OP_MAP) exception\n");
-        el3_map_mem((op_map_mem_t *)&desc->map);
-        return 1;
+        return el3_map_mem((op_map_mem_t *)&desc->map);
         break;
     case SMC_OP_NOOP:
-        DEBUG_MSG("took an smc(SMC_OP_NOOP) exception\n");
         break;
     case SMC_OP_EXIT:
         el3_shutdown();
         break;
+    case SMC_OP_TEST:
+        if (test->val != test->orig >> test->count) {
+            test->fail++;
+        }
+        test->val >>= 1;
+        test->count++;
+        return SVC_OP_TEST;
     default:
-        printf("Unrecognized AArch64 SMC opcode: iss = %d\n", op);
+        printf("Unrecognized AArch64 SMC opcode: op = %d\n", op);
+        el3_shutdown();
         break;
     }
 
@@ -238,20 +246,25 @@ int el3_handle_exception(uint64_t ec, uint64_t iss)
         break;
     case EC_IABORT_LOWER:
         printf("Instruction abort at lower level: far = %0lx\n", far);
+        el3_shutdown();
         break;
     case EC_IABORT:
         printf("Instruction abort at EL3: far = %0lx\n", far);
+        el3_shutdown();
         break;
     case EC_DABORT_LOWER:
         printf("Data abort (%s) at lower level: far = %0lx elr = %0lx\n",
                dai.wnr ? "write" : "read", far, elr);
+        el3_shutdown();
         break;
     case EC_DABORT:
         printf("Data abort (%s) at EL3: far = %0lx elr = %0lx\n",
                dai.wnr ? "write" : "read", far, elr);
+        el3_shutdown();
         break;
     default:
         printf("Unhandled EL3 exception: EC = %d  ISS = %d\n", ec, iss);
+        el3_shutdown();
         break;
     }
 
@@ -270,6 +283,7 @@ void el3_monitor_init()
      */
     sec_state.elr_el3 = EL1_S_FLASH_BASE;
     sec_state.spsr_el3 = 0x5;
+    sec_state.spsel = 0x1;
     sec_state.x[0] = (uint64_t)el3_lookup_pa(syscntl);
 
     /* Set-up the nonsecure state buffer to return to the non-secure
@@ -278,6 +292,7 @@ void el3_monitor_init()
      */
     nsec_state.elr_el3 = EL1_NS_FLASH_BASE;
     nsec_state.spsr_el3 = 0x5;
+    nsec_state.spsel = 0x1;
     nsec_state.x[0] = (uint64_t)el3_lookup_pa(syscntl);
 }
 
@@ -295,6 +310,7 @@ void el3_start(uint64_t base, uint64_t size)
     }
 
     syscntl = el3_heap_allocate(0x1000);
+
     smc_interop_buf = el3_heap_allocate(0x1000);
     syscntl->smc_interop.buf_va = smc_interop_buf;
     syscntl->smc_interop.buf_pa = el3_lookup_pa(smc_interop_buf);
diff --git a/aarch64/el3/el3_exception.S b/aarch64/el3/el3_exception.S
index cbed1a7..89578dd 100644
--- a/aarch64/el3/el3_exception.S
+++ b/aarch64/el3/el3_exception.S
@@ -24,7 +24,6 @@ el3_sync_exception_current:
 el3_sync_exception_lower64:
     stp x30, x2, [sp, #-16]!
     stp x0, x1, [sp, #-16]!
-    mov x2, x0
     mrs x0, esr_el3
     mov x1, #0xffffff
     and x1, x1, x0
@@ -38,11 +37,11 @@ el3_sync_exception_lower64:
     ldp x30, x2, [sp], #16
     b el3_sync_exception_lower64_done
 el3_sync_exception_lower64_smc:
-    ldp x1, x2, [sp]        /* Fetch our inputs as SMC args */
+    ldp x0, x1, [sp]        /* Fetch our inputs as SMC args */
     bl el3_handle_smc
-    cbz x0, el3_sync_exception_lower64_done
-    ldp x0, x1, [sp], #16
+    ldp x2, x1, [sp], #16
     ldp x30, x2, [sp], #16
+    cbz x0, el3_sync_exception_lower64_done
     b monitor_switch        /* This never returns, erets */
 el3_sync_exception_lower64_done:
     eret
diff --git a/aarch64/el3/el3_monitor.h b/aarch64/el3/el3_monitor.h
index f5eee11..ec10022 100644
--- a/aarch64/el3/el3_monitor.h
+++ b/aarch64/el3/el3_monitor.h
@@ -8,6 +8,8 @@ typedef struct state_buf {
     uintptr_t spsr_el1;
     uintptr_t esr_el1;
     uintptr_t sp_el1;
+    uintptr_t spsel;
+    uintptr_t sp_el0;
     uintptr_t spsr_abt;
     uintptr_t spsr_und;
     uintptr_t spsr_irq;
diff --git a/aarch64/el3/el3_monitor_asm.S b/aarch64/el3/el3_monitor_asm.S
index 2e1f9b6..1bf5959 100644
--- a/aarch64/el3/el3_monitor_asm.S
+++ b/aarch64/el3/el3_monitor_asm.S
@@ -19,64 +19,66 @@
     SAVE_SYS_PAIR elr_el3, spsr_el3, 0*16
     SAVE_SYS_PAIR elr_el1, spsr_el1, 1*16
     SAVE_SYS_PAIR esr_el1, sp_el1, 2*16
-//    SAVE_SYS_PAIR spsr_abt, spsr_und, 3*16
-//    SAVE_SYS_PAIR spsr_irq, spsr_fiq, 4*16
-    SAVE_SYS_PAIR sctlr_el1, actlr_el1, 5*16
-    SAVE_SYS_PAIR cpacr_el1, csselr_el1, 6*16
-    SAVE_SYS_PAIR ttbr0_el1, ttbr1_el1, 7*16
-    SAVE_SYS_PAIR tcr_el1, vbar_el1, 8*16
-    SAVE_SYS_PAIR mair_el1, amair_el1, 9*16
-    SAVE_SYS_PAIR tpidr_el0, tpidr_el1, 10*16
-    SAVE_SYS_PAIR tpidrro_el0, contextidr_el1, 11*16
-//    SAVE_SYS_PAIR par_el1, far_el1, 12*16
-//    SAVE_SYS_PAIR afsr0_el1, afsr1_el1, 13*16
+    SAVE_SYS_PAIR spsel, sp_el0, 3*16
+//    SAVE_SYS_PAIR spsr_abt, spsr_und, 4*16
+//    SAVE_SYS_PAIR spsr_irq, spsr_fiq, 5*16
+    SAVE_SYS_PAIR sctlr_el1, actlr_el1, 6*16
+    SAVE_SYS_PAIR cpacr_el1, csselr_el1, 7*16
+    SAVE_SYS_PAIR ttbr0_el1, ttbr1_el1, 8*16
+    SAVE_SYS_PAIR tcr_el1, vbar_el1, 9*16
+    SAVE_SYS_PAIR mair_el1, amair_el1, 10*16
+    SAVE_SYS_PAIR tpidr_el0, tpidr_el1, 11*16
+    SAVE_SYS_PAIR tpidrro_el0, contextidr_el1, 12*16
+//    SAVE_SYS_PAIR par_el1, far_el1, 13*16
+//    SAVE_SYS_PAIR afsr0_el1, afsr1_el1, 14*16
 
-    stp x4, x5, [x0, #14*16]
-    stp x6, x7, [x0, #15*16]
-    stp x8, x9, [x0, #16*16]
-    stp x10, x11, [x0, #17*16]
-    stp x12, x13, [x0, #18*16]
-    stp x14, x15, [x0, #19*16]
-    stp x16, x17, [x0, #20*16]
-    stp x18, x19, [x0, #21*16]
-    stp x20, x21, [x0, #22*16]
-    stp x22, x23, [x0, #23*16]
-    stp x24, x25, [x0, #24*16]
-    stp x26, x27, [x0, #25*16]
-    stp x28, x29, [x0, #26*16]
-    str x30, [x0, #27*16]
+    stp x4, x5, [x0, #15*16]
+    stp x6, x7, [x0, #16*16]
+    stp x8, x9, [x0, #17*16]
+    stp x10, x11, [x0, #18*16]
+    stp x12, x13, [x0, #19*16]
+    stp x14, x15, [x0, #20*16]
+    stp x16, x17, [x0, #21*16]
+    stp x18, x19, [x0, #22*16]
+    stp x20, x21, [x0, #23*16]
+    stp x22, x23, [x0, #24*16]
+    stp x24, x25, [x0, #25*16]
+    stp x26, x27, [x0, #26*16]
+    stp x28, x29, [x0, #27*16]
+    str x30, [x0, #28*16]
 .endm
 
 .macro MONITOR_RESTORE_STATE
     RESTORE_SYS_PAIR elr_el3, spsr_el3, 0*16
     RESTORE_SYS_PAIR elr_el1, spsr_el1, 1*16
     RESTORE_SYS_PAIR esr_el1, sp_el1, 2*16
-//    RESTORE_SYS_PAIR spsr_abt, spsr_und, 3*16
-//    RESTORE_SYS_PAIR spsr_irq, spsr_fiq, 4*16
-    RESTORE_SYS_PAIR sctlr_el1, actlr_el1, 5*16
-    RESTORE_SYS_PAIR cpacr_el1, csselr_el1, 6*16
-    RESTORE_SYS_PAIR ttbr0_el1, ttbr1_el1, 7*16
-    RESTORE_SYS_PAIR tcr_el1, vbar_el1, 8*16
-    RESTORE_SYS_PAIR mair_el1, amair_el1, 9*16
-    RESTORE_SYS_PAIR tpidr_el0, tpidr_el1, 10*16
-    RESTORE_SYS_PAIR tpidrro_el0, contextidr_el1, 11*16
-//    RESTORE_SYS_PAIR par_el1, far_el1, 12*16
-//    RESTORE_SYS_PAIR afsr0_el1, afsr1_el1, 13*16
+    RESTORE_SYS_PAIR spsel, sp_el0, 3*16
+//    RESTORE_SYS_PAIR spsr_abt, spsr_und, 4*16
+//    RESTORE_SYS_PAIR spsr_irq, spsr_fiq, 5*16
+    RESTORE_SYS_PAIR sctlr_el1, actlr_el1, 6*16
+    RESTORE_SYS_PAIR cpacr_el1, csselr_el1, 7*16
+    RESTORE_SYS_PAIR ttbr0_el1, ttbr1_el1, 8*16
+    RESTORE_SYS_PAIR tcr_el1, vbar_el1, 9*16
+    RESTORE_SYS_PAIR mair_el1, amair_el1, 10*16
+    RESTORE_SYS_PAIR tpidr_el0, tpidr_el1, 11*16
+    RESTORE_SYS_PAIR tpidrro_el0, contextidr_el1, 12*16
+//    RESTORE_SYS_PAIR par_el1, far_el1, 13*16
+//    RESTORE_SYS_PAIR afsr0_el1, afsr1_el1, 14*16
 
-    ldp x4, x5, [x0, #14*16]
-    ldp x6, x7, [x0, #15*16]
-    ldp x8, x9, [x0, #16*16]
-    ldp x10, x11, [x0, #17*16]
-    ldp x12, x13, [x0, #18*16]
-    ldp x14, x15, [x0, #19*16]
-    ldp x16, x17, [x0, #20*16]
-    ldp x18, x19, [x0, #21*16]
-    ldp x20, x21, [x0, #22*16]
-    ldp x22, x23, [x0, #23*16]
-    ldp x24, x25, [x0, #24*16]
-    ldp x26, x27, [x0, #25*16]
-    ldp x28, x29, [x0, #26*16]
-    ldr x30, [x0, #27*16]
+    ldp x4, x5, [x0, #15*16]
+    ldp x6, x7, [x0, #16*16]
+    ldp x8, x9, [x0, #17*16]
+    ldp x10, x11, [x0, #18*16]
+    ldp x12, x13, [x0, #19*16]
+    ldp x14, x15, [x0, #20*16]
+    ldp x16, x17, [x0, #21*16]
+    ldp x18, x19, [x0, #22*16]
+    ldp x20, x21, [x0, #23*16]
+    ldp x22, x23, [x0, #24*16]
+    ldp x24, x25, [x0, #25*16]
+    ldp x26, x27, [x0, #26*16]
+    ldp x28, x29, [x0, #27*16]
+    ldr x30, [x0, #28*16]
 .endm
 
 .globl monitor_restore_state
diff --git a/platform/virt/platform.h b/platform/virt/platform.h
index 97b90a4..665c5f8 100644
--- a/platform/virt/platform.h
+++ b/platform/virt/platform.h
@@ -11,12 +11,14 @@
 #define EL1_S_FLASH_BASE (FLASH_BASE+0x10000)
 #define EL1_NS_FLASH_BASE (FLASH_BASE+0x20000)
 #define EL0_NS_FLASH_BASE (FLASH_BASE+0x30000)
+#define EL0_S_FLASH_BASE (FLASH_BASE+0x50000)
 
 #define RAM_BASE 0x40000000                 // From QEMU virt.c
 #define RAM_SIZE (2*1024*1024*1024)         // Only 2GB needed
 #define EL3_BASE_VA 0xF000000000
 #define EL1_S_BASE_VA 0xC000000000
 #define EL1_NS_BASE_VA 0x80000000
+#define EL0_S_BASE_VA 0x00080000
 #define EL0_NS_BASE_VA 0x00010000
 #define EL3_RAM_BASE RAM_BASE
 #define EL3_RAM_SIZE (512*1024)
@@ -42,7 +44,8 @@
 #define EL1_NS_STACK_SIZE 0x40000
 #define EL1_NS_STACK_BASE 0xFFFFF000
 #define EL0_STACK_SIZE 0x40000
-#define EL0_STACK_BASE 0x80000000-0x1000
+#define EL0_S_STACK_BASE 0x80000000-0x1000
+#define EL0_NS_STACK_BASE 0x70000000-0x1000
 
 #define UART0_BASE 0x09000000