Add NS EL0 to S EL1 communication

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

15 files changed, 205 insertions, 81 deletions

diff --git a/aarch64/common/arm_builtins.h b/aarch64/common/arm_builtins.h
index c4cc6d0..1ca14f9 100644
--- a/aarch64/common/arm_builtins.h
+++ b/aarch64/common/arm_builtins.h
@@ -1,14 +1,36 @@
 #ifndef _ARM_BUILTINS_H
 #define _ARM_BUILTINS_H
 
+extern void __smc(void *);
+extern void __svc(void *);
+/*
+#define __smc(_op)                       \
+    asm volatile (                              \
+        "mov x0, %0\n"                       \
+        "smc 0\n"                               \
+        "mov %0, x0\n"                       \
+        : "+r" (_op)   \
+    )
+
 #define __smc(_imm, _desc) \
     asm volatile ("mov x0, %1\n" \
                   "smc #%0\n" \
-                  ::[imm] "I" (_imm), "r" (_desc))
+                  ::[imm] "+I" (_imm), "r" (_desc))
+*/
+
+    /*
+#define __svc(_op)                       \
+    asm volatile (                              \
+        "mov x0, %0\n"                       \
+        "svc 0\n"                               \
+        "mov %0, x0\n"                       \
+        : "+r" (_op)   \
+    )
 #define __svc(_imm, _desc) \
     asm volatile ("mov x0, %1\n" \
                   "svc #%0\n" \
                   ::[imm] "I" (_imm), "r" (_desc))
+    */
 #define __exception_return(_x0) asm volatile ("eret\n")
 #define __set_exception_return(_elr) \
     asm volatile("msr elr_el1, %[elr]\n"::[elr] "r" (_elr))
diff --git a/aarch64/common/builtins.S b/aarch64/common/builtins.S
new file mode 100644
index 0000000..dd0da3b
--- /dev/null
+++ b/aarch64/common/builtins.S
@@ -0,0 +1,13 @@
+#define __ASSEMBLY__
+
+.globl __smc
+__smc:
+    smc #0
+    ret
+
+.globl __svc
+__svc:
+    svc #0
+    ret
+
+#undef __ASSEMBLY__
diff --git a/aarch64/common/init_util.S b/aarch64/common/init_util.S
index 0f9e393..f1dd26b 100644
--- a/aarch64/common/init_util.S
+++ b/aarch64/common/init_util.S
@@ -4,10 +4,12 @@
 /* allocate_pa() - Allocates and returns next pool PA */
 allocate_pa:
     stp x10, x11, [sp, #-16]!
+    str x30, [sp, #-8]!
     ldr x10, =RAM_BASE+0x2000
     ldr x0, [x10]
     add x11, x0, #0x1000
     str x11, [x10]
+    ldr x30, [sp], #8
     ldp x10, x11, [sp], #16
     ret
 
diff --git a/aarch64/common/interop.h b/aarch64/common/interop.h
index 0c55b7c..5cea7e0 100644
--- a/aarch64/common/interop.h
+++ b/aarch64/common/interop.h
@@ -3,6 +3,14 @@
 #include <stdint.h>
 #include <stddef.h>
 
+#define OP_MAP_NSEC_EL0 0
+#define OP_MAP_SEC_EL0  1
+#define OP_MAP_NSEC_EL1 2
+#define OP_MAP_SEC_EL1  4
+#define OP_MAP_EL3      8
+#define OP_MAP_ALL \
+            (OP_MAP_SEC_EL0 | OP_MAP_NSEC_EL1 | OP_MAP_SEC_EL1 | OP_MAP_EL3)
+
 typedef struct {
     uint32_t type;
     size_t len;
diff --git a/aarch64/common/smc.h b/aarch64/common/smc.h
index 39179d4..c0f9bda 100644
--- a/aarch64/common/smc.h
+++ b/aarch64/common/smc.h
@@ -1,23 +1,37 @@
 #ifndef _SMC_H
 #define _SMC_H
 
-#define SMC_NOOP 0
-#define SMC_DISPATCH_MONITOR 1
-#define SMC_YIELD 2
-#define SMC_DISPATCH_SECURE_USR 3
-#define SMC_DISPATCH_SECURE_SVC 4
-#define SMC_ALLOCATE_SECURE_MEMORY 4
-#define SMC_EXIT 5
-#define SMC_MAP 8
+#define SMC_OP_NOOP 0
+#define SMC_OP_DISPATCH_MONITOR 1
+#define SMC_OP_YIELD 2
+#define SMC_OP_DISPATCH_SECURE_USR 3
+#define SMC_OP_DISPATCH_SECURE_SVC 4
+#define SMC_OP_ALLOCATE_SECURE_MEMORY 4
+#define SMC_OP_EXIT 5
+#define SMC_OP_MAP 8
 
 #ifndef __ASSEMBLY__
 #include "interop.h"
 
-typedef union {
-    op_dispatch_t dispatch;
-    op_map_mem_t map;
+typedef struct {
+    int op;
+    union {
+        op_dispatch_t dispatch;
+        op_map_mem_t map;
+    };
 } smc_op_desc_t;
 
+extern smc_op_desc_t *smc_interop_buf;
+
+#define SMC_NO_DESC(_op)                \
+    do {                                \
+        smc_interop_buf->op = (_op);    \
+        __smc(smc_interop_buf);         \
+    } while(0)
+
+#define SMC_EXIT()  SMC_NO_DESC(SMC_OP_EXIT)
+#define SMC_YIELD()  SMC_NO_DESC(SMC_OP_YIELD)
+
 typedef struct {
      void *interop_buf_pa;
      void *interop_buf_va;
diff --git a/aarch64/common/svc.h b/aarch64/common/svc.h
index 0e604ee..8f31ee6 100644
--- a/aarch64/common/svc.h
+++ b/aarch64/common/svc.h
@@ -12,11 +12,15 @@
 #define SVC_EXIT 6
 #define SVC_ALLOC 7
 #define SVC_MAP 8
+#define SVC_YIELD 9
 
 #ifndef __ASSEMBLY__
-typedef union {
-    op_alloc_mem_t alloc;
-    op_map_mem_t map;
+typedef struct {
+    int op;
+    union {
+        op_alloc_mem_t alloc;
+        op_map_mem_t map;
+    };
 } svc_op_desc_t;
 #endif
 
diff --git a/aarch64/el0_ns/Makefile b/aarch64/el0_ns/Makefile
index 2409a1a..f7e92e2 100644
--- a/aarch64/el0_ns/Makefile
+++ b/aarch64/el0_ns/Makefile
@@ -1,7 +1,10 @@
+VPATH	= ../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.o \
+                  builtins.o
 
 -include .*.d
 
diff --git a/aarch64/el0_ns/tztest.c b/aarch64/el0_ns/tztest.c
index 34bedd4..1287c17 100644
--- a/aarch64/el0_ns/tztest.c
+++ b/aarch64/el0_ns/tztest.c
@@ -2,13 +2,22 @@
 #include "arm_builtins.h"
 #include "svc.h"
 
+typedef struct {
+    volatile int fail_count;
+    volatile int test_count;
+    volatile int exception_ec;
+    volatile int exception_iss;
+} test_control_t;
+
+test_control_t *tztest_cntl;
+svc_op_desc_t svc_exit = {.op = SVC_EXIT};
+svc_op_desc_t svc_yield = {.op = SVC_YIELD};
+
 #if 0
 /* 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.
  */
-volatile int *tztest_fail_count;
-volatile int *tztest_test_count;
 
 #define INC_TEST_COUNT()    (*tztest_test_count += 1)
 #define INC_FAIL_COUNT()    (*tztest_fail_count += 1)
@@ -64,36 +73,37 @@ uint32_t P0_nonsecure_check_smc()
 
 void *alloc_mem(int type, size_t len)
 {
-    op_alloc_mem_t alloc;
-    alloc.type = type;
-    alloc.len = len;
-    alloc.addr = NULL;
-    __svc(SVC_ALLOC, &alloc);
-
-    return alloc.addr;
+    svc_op_desc_t op;
+    op.op = SVC_ALLOC;
+    op.alloc.type = type;
+    op.alloc.len = len;
+    op.alloc.addr = NULL;
+    __svc(&op);
+
+    return op.alloc.addr;
 }
 
-void map_va(void *va, size_t len)
+void map_va(void *va, size_t len, int type)
 {
-    op_map_mem_t map;
-    map.va = va;
-    map.len = len;
-    map.type = 0;
+    svc_op_desc_t op;
+    op.op = SVC_MAP;
+    op.map.va = va;
+    op.map.len = len;
+    op.map.type = type;
 
-    __svc(SVC_MAP, &map);
+    __svc(&op);
 }
 
 int main()
 {
     printf("Starting TZ test ...\n");
 
-//    P0_nonsecure_check_smc();
-    void *va = alloc_mem(0, 0x1000);
-    printf("Called alloc_mem: got addr = %x\n", va);
-
-    map_va(va, 0x1000);
+    tztest_cntl = (test_control_t*)alloc_mem(0, 0x1000);
+    map_va(tztest_cntl, 0x1000, OP_MAP_ALL);
+    printf("Called alloc_mem: got addr = %x\n", tztest_cntl);
 
-    __svc(SVC_EXIT, NULL);
+    __svc(&svc_yield);
+    __svc(&svc_exit);
 
     return 0;
 }
diff --git a/aarch64/el1_common/el1.c b/aarch64/el1_common/el1.c
index d69143b..1a240d4 100644
--- a/aarch64/el1_common/el1.c
+++ b/aarch64/el1_common/el1.c
@@ -13,10 +13,11 @@
 
 extern void el1_init_el0();
 
-smc_control_t *el1_smc_cntl;
 smc_op_desc_t *smc_interop_buf;
+smc_control_t *el1_smc_cntl;
 uint64_t el1_next_pa = 0;
 uint64_t el1_heap_pool = 0x40000000;
+
 uint64_t el1_allocate_pa() {
     uint64_t next = el1_next_pa;
     el1_next_pa += 0x1000;
@@ -153,18 +154,17 @@ void *el1_lookup_pa(void *va)
 
 void el1_map_secure(op_map_mem_t *map)
 {
-    op_map_mem_t *op = (op_map_mem_t *)smc_interop_buf;
+    smc_op_desc_t *desc = (smc_op_desc_t *)smc_interop_buf;
 
-    memcpy(op, map, sizeof(op_map_mem_t));
+    memcpy(&desc->map, map, sizeof(op_map_mem_t));
 
-    op->pa = el1_lookup_pa(op->va);
+    desc->op = SMC_OP_MAP;
+    desc->map.pa = el1_lookup_pa(desc->map.va);
 
-    __smc(SMC_MAP, op);
-
-    memset(op, 0, sizeof(op_map_mem_t));
+    __smc(desc);
 }
 
-void el1_handle_exception(uint64_t ec, uint64_t iss, svc_op_desc_t *op)
+void el1_handle_exception(uint64_t ec, uint64_t iss, svc_op_desc_t *desc)
 {
     armv8_data_abort_iss_t dai = {.raw = iss};
 //    armv8_inst_abort_iss_t iai = {.raw = iss};
@@ -176,18 +176,22 @@ void el1_handle_exception(uint64_t ec, uint64_t iss, svc_op_desc_t *op)
     switch (ec) {
     case EC_SVC32:
     case EC_SVC64:
-        switch (iss) {
+        switch (desc->op) {
         case SVC_EXIT:
             DEBUG_MSG("took an svc(SVC_EXIT) \n", iss);
-            __smc(SMC_EXIT, NULL);
+            SMC_EXIT();
+            break;
+        case SVC_YIELD:
+            DEBUG_MSG("took an svc(SVC_YIELD) \n", iss);
+            SMC_YIELD();
             break;
         case SVC_ALLOC:
             DEBUG_MSG("took an svc(SVC_ALLOC) \n", iss);
-            el1_alloc_mem((op_alloc_mem_t *)op);
+            el1_alloc_mem((op_alloc_mem_t *)&desc->alloc);
             break;
         case SVC_MAP:
             DEBUG_MSG("took an svc(SVC_MAP) \n", iss);
-            el1_map_secure((op_map_mem_t *)op);
+            el1_map_secure((op_map_mem_t *)&desc->map);
             break;
         default:
             printf("Unrecognized AArch64 SVC opcode: iss = %d\n", iss);
@@ -232,12 +236,12 @@ void *el1_load_el0(char *elfbase, char *start_va)
         ehdr->e_ident[EI_MAG2] != ELFMAG2 ||
         ehdr->e_ident[EI_MAG3] != ELFMAG3) {
         printf("Invalid ELF header, exiting...\n");
-        __smc(SMC_EXIT, NULL);
+        SMC_EXIT();
     } else if (ehdr->e_type != ET_DYN &&
                (ehdr->e_machine != EM_ARM || ehdr->e_machine != EM_AARCH64)) {
         printf("Incorrect ELF type (type = %d, machine = %d), exiting...\n",
                ehdr->e_type, ehdr->e_machine);
-        __smc(SMC_EXIT, NULL);
+        SMC_EXIT();
     } else {
         printf("Loading %s EL0 test image...\n",
                (ehdr->e_machine == EM_ARM) ?  "aarch32" : "aarch64");
diff --git a/aarch64/el1_ns/Makefile b/aarch64/el1_ns/Makefile
index 9279c03..76bdd26 100644
--- a/aarch64/el1_ns/Makefile
+++ b/aarch64/el1_ns/Makefile
@@ -1,4 +1,4 @@
-VPATH	= ../el1_common
+VPATH	= ../el1_common:../common
 
 EL1_NS_ELF		= el1_nsec.elf
 EL1_NS_IMAGE 	= el1_nsec.bin
@@ -6,7 +6,8 @@ EL1_NS_LOAD		= el1_nsec.lds
 EL1_NS_OBJS		= el1_init.o \
                   el1_exception.o \
                   el1.o \
-                  el1_nsec.o
+                  el1_nsec.o \
+                  builtins.o
 
 -include .*.d
 
diff --git a/aarch64/el1_s/Makefile b/aarch64/el1_s/Makefile
index f519be1..0ae0d5c 100644
--- a/aarch64/el1_s/Makefile
+++ b/aarch64/el1_s/Makefile
@@ -1,4 +1,4 @@
-VPATH	= ../el1_common
+VPATH	= ../el1_common:../common
 
 EL1_S_ELF		= el1_sec.elf
 EL1_S_IMAGE 	= el1_sec.bin
@@ -6,7 +6,8 @@ EL1_S_LOAD		= el1_sec.lds
 EL1_S_OBJS		= el1_init.o \
                   el1_exception.o \
                   el1.o \
-                  el1_sec.o
+                  el1_sec.o \
+				  builtins.o
 
 -include .*.d
 
diff --git a/aarch64/el1_s/el1_sec.c b/aarch64/el1_s/el1_sec.c
index daf94f8..43b828d 100644
--- a/aarch64/el1_s/el1_sec.c
+++ b/aarch64/el1_s/el1_sec.c
@@ -1,9 +1,39 @@
 #include "platform.h"
 #include "arm_builtins.h"
+#include "libcflat.h"
 #include <stdint.h>
 #include "smc.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;
+    printf("EL1_S: In loop\n");
+
+    while (desc->op != SMC_OP_EXIT) {
+        switch (desc->op) {
+        case SMC_OP_MAP:
+            printf("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:
+            printf("EL1_S: Doing a YIELD desc = %p\n", desc);
+            break;
+        default:
+            printf("Unrecognized SMC opcode %d.  Exiting ...\n", desc->op);
+            SMC_EXIT();
+            break;
+        }
+
+        SMC_YIELD();
+    }
+
+    SMC_EXIT();
+}
+
 void el1_init_el0()
 {
-    __smc(SMC_YIELD, NULL);
+    SMC_YIELD();
+
+    el1_sec_smc_loop();
 }
diff --git a/aarch64/el3/el3.c b/aarch64/el3/el3.c
index 13f4342..5a3d1cb 100644
--- a/aarch64/el3/el3.c
+++ b/aarch64/el3/el3.c
@@ -171,10 +171,19 @@ void *el3_lookup_pa(void *va)
 
 void el3_map_mem(op_map_mem_t *map)
 {
-    el3_map_pa((uintptr_t)map->va, (uintptr_t)map->pa);
+    if ((map->type & OP_MAP_EL3) == OP_MAP_EL3) {
+        el3_map_pa((uintptr_t)map->va, (uintptr_t)map->pa);
+        printf("EL3: 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);
+    }
+*/
 }
 
-int el3_handle_exception(uint64_t ec, uint64_t iss, smc_op_desc_t *op)
+int el3_handle_exception(uint64_t ec, uint64_t iss, smc_op_desc_t *desc)
 {
     armv8_data_abort_iss_t dai = {.raw = iss};
     uint64_t elr, far;
@@ -184,23 +193,24 @@ int el3_handle_exception(uint64_t ec, uint64_t iss, smc_op_desc_t *op)
 
     switch (ec) {
     case EC_SMC64:      /* SMC from aarch64 */
-        switch (iss) {
-        case SMC_YIELD:
+        switch (desc->op) {
+        case SMC_OP_YIELD:
             DEBUG_MSG("took an SMC(SMC_YIELD) exception\n");
             return 1;
             break;
-        case SMC_DISPATCH_MONITOR:
-            DEBUG_MSG("took an SMC(SMC_DSPATCH_MONITOR) exception\n");
-            el3_dispatch((op_dispatch_t *)op);
+        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_MAP:
-            DEBUG_MSG("took an SMC(SMC_MAP) exception\n");
-            el3_map_mem((op_map_mem_t *)op);
+        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;
             break;
-        case SMC_NOOP:
-            DEBUG_MSG("took an SMC(SMC_NOOP) exception\n");
+        case SMC_OP_NOOP:
+            DEBUG_MSG("took an smc(SMC_OP_NOOP) exception\n");
             break;
-        case SMC_EXIT:
+        case SMC_OP_EXIT:
             el3_shutdown();
             break;
         default:
diff --git a/aarch64/el3/el3_monitor.h b/aarch64/el3/el3_monitor.h
index 776da41..f5eee11 100644
--- a/aarch64/el3/el3_monitor.h
+++ b/aarch64/el3/el3_monitor.h
@@ -33,7 +33,7 @@ typedef struct state_buf {
     uintptr_t x[27];        /* X4:X30 */
 } state_buf;
 
-extern void monitor_switch(void);
+extern void monitor_switch(int op, smc_op_desc_t *desc);
 extern void monitor_restore_state(state_buf *);
 extern void monitor_save_state(state_buf *);
 
diff --git a/aarch64/el3/el3_monitor_asm.S b/aarch64/el3/el3_monitor_asm.S
index b067f6d..2e1f9b6 100644
--- a/aarch64/el3/el3_monitor_asm.S
+++ b/aarch64/el3/el3_monitor_asm.S
@@ -89,23 +89,25 @@ monitor_restore_state:
 .globl monitor_switch
 monitor_switch:
     stp x0, x1, [sp, #-16]!     /* Save the input regs x0:x3 */
-    mrs x1, scr_el3
-    and x1, x1, #0x1
-    cbnz x1, switch_to_nsec
-switch_to_nsec:
-    adr x0, sec_state
-    MONITOR_SAVE_STATE
-    adr x0, nsec_state
-    MONITOR_RESTORE_STATE
-    b switch_state
+    stp x2, x3, [sp, #-16]!     /* Save the input regs x0:x3 */
+    mrs x3, scr_el3
+    and x3, x3, #0x1
+    cbz x3, switch_to_nsec
 switch_to_sec:
     adr x0, nsec_state
     MONITOR_SAVE_STATE
     adr x0, sec_state
     MONITOR_RESTORE_STATE
+    b switch_state
+switch_to_nsec:
+    adr x0, sec_state
+    MONITOR_SAVE_STATE
+    adr x0, nsec_state
+    MONITOR_RESTORE_STATE
 switch_state:
-    eor x1, x1, #0x1
-    msr scr_el3, x1
+    eor x3, x3, #0x1
+    msr scr_el3, x3
+    ldp x2, x3, [sp], #16
     ldp x0, x1, [sp], #16
     eret
 .end