Add EL1 SVC Memory alloc op

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

8 files changed, 179 insertions, 43 deletions

diff --git a/aarch64/common/arm_builtins.h b/aarch64/common/arm_builtins.h
index 99d8749..93ed47b 100644
--- a/aarch64/common/arm_builtins.h
+++ b/aarch64/common/arm_builtins.h
@@ -2,9 +2,16 @@
 #define _ARM_BUILTINS_H
 
 #define __smc(_imm) asm volatile ("smc # %[imm] \n"::[imm] "I" (_imm))
-#define __svc(_imm) asm volatile ("svc # %[imm] \n"::[imm] "I" (_imm))
+#define __svc(_imm, _desc) \
+    asm volatile ("mov x0, %1\n" \
+                  "svc #%0\n" \
+                  ::[imm] "I" (_imm), "r" (_desc))
 #define __exception_return() asm volatile ("eret \n")
 #define __set_exception_return(_elr) \
     asm volatile("msr elr_el1, %[elr]\n"::[elr] "r" (_elr))
+#define __get_exception_return(_addr) \
+    asm volatile("mrs %0, elr_el1\n": "=r" (_addr))
+#define __get_exception_address(_addr) \
+    asm volatile("mrs %0, far_el1\n": "=r" (_addr))
 
 #endif
diff --git a/aarch64/common/smc.h b/aarch64/common/smc.h
index d4a293c..59e6a7d 100644
--- a/aarch64/common/smc.h
+++ b/aarch64/common/smc.h
@@ -11,16 +11,14 @@
 
 #ifndef __ASSEMBLY__
 typedef struct {
-    uint32_t (*func)(uint32_t);
-    uint32_t arg;
-    uint32_t ret;
-} tztest_dispatch_t;
+    uintptr_t (*func)(uintptr_t);
+    uintptr_t arg;
+    uintptr_t ret;
+} dispatch_t;
 
-typedef struct {
-    union {
-        tztest_dispatch_t dispatch;
-    };
-} tztest_smc_desc_t;
+typedef union {
+    dispatch_t dispatch;
+} smc_op_desc_t;
 #endif
 
 #endif
diff --git a/aarch64/common/svc.h b/aarch64/common/svc.h
index 838785c..ed870c7 100644
--- a/aarch64/common/svc.h
+++ b/aarch64/common/svc.h
@@ -8,6 +8,19 @@
 #define SVC_DISPATCH_NONSECURE_SVC 4
 #define SVC_GET_SECURE_STATE 5
 #define SVC_EXIT 6
+#define SVC_ALLOC 7
+
+#ifndef __ASSEMBLY__
+typedef struct {
+    uint32_t type;
+    size_t len;
+    void *addr;
+} alloc_mem_t;
+
+typedef union {
+    alloc_mem_t alloc;
+} svc_op_desc_t;
+#endif
 
 /*
 typedef struct {
diff --git a/aarch64/el0_ns/tztest.c b/aarch64/el0_ns/tztest.c
index 9004475..dfc2b5d 100644
--- a/aarch64/el0_ns/tztest.c
+++ b/aarch64/el0_ns/tztest.c
@@ -1,23 +1,90 @@
-
 #include "libcflat.h"
 #include "arm_builtins.h"
 #include "svc.h"
 
-int y = 0;
+#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)
+
+#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)
 
-int foo()
+#define TEST_EXCEPTION(_fn, _excp)                      \
+    do {                                                \
+        TEST_FUNCTION(_fn, *tztest_exception == (_excp));   \
+        *tztest_exception = 0;                          \
+    } while (0)
+#ifdef DEBUG
+void validate_state(uint32_t mode, uint32_t state)
 {
-    printf("Hello from foo\n");
+    tztest_svc_desc_t desc;
 
-    return 4;
+    assert((_read_cpsr() & CPSR_MODE_MASK) == mode);
+
+    CLEAR_SVC_DESC(desc);
+    __svc(SVC_GET_SECURE_STATE, &desc);
+    assert(desc.secure_state.state == state);
+}
+#else
+void validate_state(__attribute__((unused)) uint32_t mode,
+                    __attribute__((unused)) uint32_t state) {}
+#endif
+
+uint32_t P0_nonsecure_check_smc()
+{
+    validate_state(CPSR_MODE_USR, TZTEST_STATE_NONSECURE);
+    printf("\nValidating non-secure P0 smc behavior:\n");
+    printf("\tUnprivileged P0 smc call ... ");
+    TEST_EXCEPTION(smc_noop(), CPSR_MODE_UND);
+
+    return 0;
+}
+#endif
+
+void *alloc_mem(int type, size_t len)
+{
+    alloc_mem_t alloc;
+    alloc.type = type;
+    alloc.len = len;
+    alloc.addr = NULL;
+    __svc(SVC_ALLOC, &alloc);
+
+    return alloc.addr;
 }
 
 int main()
 {
-    int x = foo();
+    printf("Starting TZ test ...\n");
+
+//    P0_nonsecure_check_smc();
+    void *va = alloc_mem(0, 0x2000);
+    printf("Called alloc_mem: got addr = %x\n", va);
+
+    va = alloc_mem(0, 0x1000);
+    printf("Called alloc_mem: got addr = %x\n", va);
 
-    y = x;
+    __svc(SVC_EXIT, NULL);
 
-    __svc(SVC_EXIT);
-    return y;
+    return 0;
 }
diff --git a/aarch64/el1_common/el1.c b/aarch64/el1_common/el1.c
index ab42040..32f3cec 100644
--- a/aarch64/el1_common/el1.c
+++ b/aarch64/el1_common/el1.c
@@ -14,6 +14,7 @@
 extern void el1_init_el0();
 
 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;
@@ -105,11 +106,34 @@ int el1_unmap_va(uint64_t addr)
     return 0;
 }
 
-void el1_handle_exception(uint64_t ec, uint64_t iss, uint64_t far,
-                          uint64_t elr)
+void *el1_heap_allocate(size_t len)
+{
+    void *addr = (void *)el1_heap_pool;
+    size_t off;
+
+    for (off = 0; off < len; off += 0x1000) {
+        el1_map_va(el1_heap_pool + off);
+    }
+
+    el1_heap_pool += off;
+
+    return addr;
+}
+
+void el1_alloc_mem(alloc_mem_t *alloc)
+{
+    alloc->addr = el1_heap_allocate(alloc->len);
+}
+
+void el1_handle_exception(uint64_t ec, uint64_t iss, svc_op_desc_t *op)
 {
     armv8_data_abort_iss_t dai = {.raw = iss};
 //    armv8_inst_abort_iss_t iai = {.raw = iss};
+    uint64_t elr, far;
+
+    __get_exception_address(far);
+    __get_exception_return(elr);
+
     switch (ec) {
     case EC_SVC32:
     case EC_SVC64:
@@ -118,15 +142,18 @@ void el1_handle_exception(uint64_t ec, uint64_t iss, uint64_t far,
         case SVC_EXIT:
             __smc(SMC_EXIT);
             break;
+        case SVC_ALLOC:
+            el1_alloc_mem((alloc_mem_t *)op);
+            break;
         default:
+            printf("Unrecognized AArch64 SVC opcode: iss = %d\n", iss);
             break;
         }
-        break;
     case EC_IABORT_LOWER:
-        printf("Instruction abort at lower level: address = %0lx\n", far);
+        printf("Instruction abort at lower level: far = %0lx\n", far);
         break;
     case EC_IABORT:
-        printf("Instruction abort at current level: address = %0lx\n", far);
+        printf("Instruction abort at EL3: far = %0lx\n", far);
         break;
     case EC_DABORT_LOWER:
         printf("Data abort (%s) at lower level: far = %0lx elr = %0lx\n",
@@ -134,12 +161,12 @@ void el1_handle_exception(uint64_t ec, uint64_t iss, uint64_t far,
         el1_map_va(far);
         break;
     case EC_DABORT:
-        printf("Data abort (%s) at current level: far = %0lx elr = %0lx\n",
+        printf("Data abort (%s) at EL1: far = %0lx elr = %0lx\n",
                dai.wnr ? "write" : "read", far, elr);
         el1_map_va(far);
         break;
     default:
-        printf("Unhandled EL3 exception: EC = %d  ISS = %d\n", ec, iss);
+        printf("Unhandled EL1 exception: EC = %d  ISS = %d\n", ec, iss);
         break;
     }
 }
@@ -160,12 +187,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");
-        __svc(SVC_EXIT);
+        __smc(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);
-        __svc(SVC_EXIT);
+        __smc(SMC_EXIT);
     } else {
         printf("Loading %s EL0 test image...\n",
                (ehdr->e_machine == EM_ARM) ?  "aarch32" : "aarch64");
diff --git a/aarch64/el1_common/el1_exception.S b/aarch64/el1_common/el1_exception.S
index 81acf66..482c371 100644
--- a/aarch64/el1_common/el1_exception.S
+++ b/aarch64/el1_common/el1_exception.S
@@ -5,23 +5,29 @@ el1_vectors:
 .word 0                     // Add padding to force the below alignment
 .align 9                    // Force these vectors to 0x400 alignment
 el1_sync_exception_current:
+    stp x30, x2, [sp, #-16]!
+    stp x0, x1, [sp, #-16]!
+    mov x2, x0;
     mrs x0, esr_el1
     mov x1, #0xffffff
     and x1, x1, x0
     lsr x0, x0, #26
-    mrs x2, far_el1
-    mrs x3, elr_el1
     bl el1_handle_exception
+    ldp x0, x1, [sp], #16
+    ldp x30, x2, [sp], #16
     eret
 .align 10                   // Force these vectors to 0x400 alignment
 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
     lsr x0, x0, #26
-    mrs x2, far_el1
-    mrs x3, elr_el1
     bl el1_handle_exception
+    ldp x0, x1, [sp], #16
+    ldp x30, x2, [sp], #16
     eret
 .align 7
 el1_serr_exception:
diff --git a/aarch64/el3/el3.c b/aarch64/el3/el3.c
index 9f0d061..9ce97f8 100644
--- a/aarch64/el3/el3.c
+++ b/aarch64/el3/el3.c
@@ -15,11 +15,11 @@
 state_buf sec_state;
 state_buf nsec_state;
 
-void el3_dispatch(tztest_smc_desc_t *desc)
+void el3_dispatch(dispatch_t *disp)
 {
-    uint32_t (*func)(uint32_t) = desc->dispatch.func;
+    uintptr_t (*func)(uintptr_t) = disp->func;
     DEBUG_MSG("Entered\n");
-    desc->dispatch.ret = func(desc->dispatch.arg);
+    disp->ret = func(disp->arg);
     DEBUG_MSG("Exiting\n");
 }
 
@@ -94,9 +94,14 @@ int el3_unmap_va(uint64_t addr)
     return 0;
 }
 
-void el3_handle_exception(uint64_t ec, uint64_t iss, uint64_t addr)
+void el3_handle_exception(uint64_t ec, uint64_t iss, smc_op_desc_t *op)
 {
     armv8_data_abort_iss_t dai = {.raw = iss};
+    uint64_t elr, far;
+
+    __get_exception_address(far);
+    __get_exception_return(elr);
+
     switch (ec) {
     case EC_SMC64:      /* SMC from aarch64 */
         switch (iss) {
@@ -106,7 +111,7 @@ void el3_handle_exception(uint64_t ec, uint64_t iss, uint64_t addr)
             break;
         case SMC_DISPATCH_MONITOR:
             DEBUG_MSG("took an SMC(SMC_DSPATCH_MONITOR) exception\n");
-            el3_dispatch(NULL);
+            el3_dispatch((dispatch_t *)op);
             break;
         case SMC_NOOP:
             DEBUG_MSG("took an SMC(SMC_NOOP) exception\n");
@@ -116,16 +121,22 @@ void el3_handle_exception(uint64_t ec, uint64_t iss, uint64_t addr)
             break;
         default:
             printf("Unrecognized AArch64 SMC opcode: iss = %d\n", iss);
+            break;
         }
+    case EC_IABORT_LOWER:
+        printf("Instruction abort at lower level: far = %0lx\n", far);
+        break;
+    case EC_IABORT:
+        printf("Instruction abort at EL3: far = %0lx\n", far);
         break;
     case EC_DABORT_LOWER:
-        printf("Data abort (%s) at lower level: address = %0lx\n",
-               dai.wnr ? "write" : "read", addr);
+        printf("Data abort (%s) at lower level: far = %0lx elr = %0lx\n",
+               dai.wnr ? "write" : "read", far, elr);
         break;
     case EC_DABORT:
-        printf("Data abort (%s) at current level (EL3): address = %0lx\n",
-               dai.wnr ? "write" : "read", addr);
-        el3_map_va(addr);
+        printf("Data abort (%s) at EL3: far = %0lx elr = %0lx\n",
+               dai.wnr ? "write" : "read", far, elr);
+        el3_map_va(far);
         break;
 
     default:
diff --git a/aarch64/el3/el3_exception.S b/aarch64/el3/el3_exception.S
index 80388c0..07eb25d 100644
--- a/aarch64/el3/el3_exception.S
+++ b/aarch64/el3/el3_exception.S
@@ -5,20 +5,27 @@ el3_vectors:
 .word 0                     // Add padding to force the below alignment
 .align 9                    // Force these vectors to 0x400 alignment
 el3_sync_exception_current:
+    stp x30, x2, [sp, #-16]!
+    stp x0, x1, [sp, #-16]!
     mrs x0, esr_el3
     mov x1, #0xffffff
     and x1, x1, x0
     lsr x0, x0, #26
-    mrs x2, far_el3
     bl el3_handle_exception
+    ldp x0, x1, [sp], #16
+    ldp x30, x2, [sp], #16
     eret
 .align 10                   // Force these vectors to 0x400 alignment
 el3_sync_exception_lower64:
+    stp x30, x2, [sp, #-16]!
+    stp x0, x1, [sp, #-16]!
     mrs x0, esr_el3
     mov x1, #0xffffff
     and x1, x1, x0
     lsr x0, x0, #26
     bl el3_handle_exception
+    ldp x0, x1, [sp], #16
+    ldp x30, x2, [sp], #16
     eret
 .align 7
 el3_serr_exception: