Add exception logging and controls along with test

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

11 files changed, 142 insertions, 71 deletions

diff --git a/aarch64/common/arm_builtins.h b/aarch64/common/arm_builtins.h
index 4e4417e..40326ef 100644
--- a/aarch64/common/arm_builtins.h
+++ b/aarch64/common/arm_builtins.h
@@ -3,7 +3,18 @@
 
 #define __exception_return(_x0) asm volatile ("eret\n")
 #define __set_exception_return(_elr) \
-    asm volatile("msr elr_el1, %[elr]\n"::[elr] "r" (_elr))
+    asm volatile("mrs x7, currentel\n"  \
+                 "cmp x7, #0x4\n"   \
+                 "b.eq setelrel1\n"   \
+                 "cmp x7, #0x8\n"   \
+                 "b.eq setelrel2\n"   \
+                 "setelrel3: msr elr_el3, %[elr]\n"   \
+                 "b setelrdone\n"   \
+                 "setelrel2: msr elr_el2, %[elr]\n"   \
+                 "b setelrdone\n"   \
+                 "setelrel1: msr elr_el1, %[elr]\n"   \
+                 "setelrdone:\n":: [elr] "r" (_elr) : "r7")
+
 #define __get_exception_return(_addr) \
     asm volatile("mrs x0, currentel\n"  \
                  "cmp x0, #0x4\n"   \
@@ -16,6 +27,7 @@
                  "b elrdone\n"   \
                  "elrel1: mrs %0, elr_el1\n"   \
                  "elrdone:\n" : "=r" (_addr))
+
 #define __get_exception_address(_addr) \
     asm volatile("mrs x0, currentel\n"  \
                  "cmp x0, #0x4\n"   \
diff --git a/aarch64/common/interop.h b/aarch64/common/interop.h
index 5cea7e0..d0579d1 100644
--- a/aarch64/common/interop.h
+++ b/aarch64/common/interop.h
@@ -30,4 +30,7 @@ typedef struct {
     uintptr_t ret;
 } op_dispatch_t;
 
+typedef struct {
+    void *datap;
+} op_get_data_t;
 #endif
diff --git a/aarch64/common/svc.h b/aarch64/common/svc.h
index 2594298..7e10e69 100644
--- a/aarch64/common/svc.h
+++ b/aarch64/common/svc.h
@@ -21,6 +21,7 @@ extern void __svc(uint32_t, void *);
 typedef union {
     op_alloc_mem_t alloc;
     op_map_mem_t map;
+    op_get_data_t get;
 } svc_op_desc_t;
 
 #endif
diff --git a/aarch64/common/syscntl.h b/aarch64/common/syscntl.h
index 657ab30..0b78ef5 100644
--- a/aarch64/common/syscntl.h
+++ b/aarch64/common/syscntl.h
@@ -10,13 +10,22 @@ typedef struct {
     uint32_t ec;
     uint32_t iss;
     uint32_t far;
-    bool ignore;
+    bool log;
+    uint32_t action;
+    bool taken;
 } sys_exception_t;
 
+#define EXCP_ACTION_SKIP 1
+
+#define SEC     0
+#define NSEC    1
+
 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;
 } sys_control_t;
 
 #endif
diff --git a/aarch64/el0_ns/tztest.c b/aarch64/el0_ns/tztest.c
index f7f6e70..6150d8c 100644
--- a/aarch64/el0_ns/tztest.c
+++ b/aarch64/el0_ns/tztest.c
@@ -1,23 +1,23 @@
 #include "libcflat.h"
 #include "svc.h"
+#include "syscntl.h"
+#include "armv8_exception.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;
+sys_control_t *syscntl = NULL;
 
-#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.
  */
 
-#define INC_TEST_COUNT()    (*tztest_test_count += 1)
-#define INC_FAIL_COUNT()    (*tztest_fail_count += 1)
+#define INC_TEST_COUNT()    (tztest_cntl->test_count += 1)
+#define INC_FAIL_COUNT()    (tztest_cntl->fail_count += 1)
 
 #define TEST_CONDITION(_cond)                           \
     do {                                                \
@@ -36,37 +36,35 @@ test_control_t *tztest_cntl;
         TEST_CONDITION(_cond);                          \
     } while(0)
 
-#define TEST_EXCEPTION(_fn, _excp)                      \
+#define TEST_EXCEPTION(_fn, _excp, _el)                 \
     do {                                                \
-        TEST_FUNCTION(_fn, *tztest_exception == (_excp));   \
-        *tztest_exception = 0;                          \
+        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)
-#ifdef DEBUG
-void validate_state(uint32_t mode, uint32_t state)
-{
-    tztest_svc_desc_t desc;
 
-    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
+#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()
 {
-    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);
+
+    TEST_EL1NS_EXCEPTION(asm volatile("smc #0\n"), EC_UNKNOWN);
 
     return 0;
 }
-#endif
 
 void *alloc_mem(int type, size_t len)
 {
@@ -91,13 +89,29 @@ void map_va(void *va, size_t len, int type)
 
 int main()
 {
+    op_get_data_t get_data;
+
     printf("Starting TZ test ...\n");
 
+    /* Allocate and globally map test control descriptor */
     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);
+
+    /* Fetch the system-wide control structure */
+    __svc(SVC_GET_SYSCNTL, &get_data);
+    syscntl = get_data.datap;
+    /* 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);
+    }
 
     __svc(SVC_YIELD, NULL);
+
+    P0_nonsecure_check_smc();
+
     __svc(SVC_EXIT, NULL);
 
     return 0;
diff --git a/aarch64/el1_common/el1.c b/aarch64/el1_common/el1.c
index e7e8be7..9179c5f 100644
--- a/aarch64/el1_common/el1.c
+++ b/aarch64/el1_common/el1.c
@@ -15,7 +15,7 @@
 extern void el1_init_el0();
 
 smc_op_desc_t *smc_interop_buf;
-sys_control_t *el1_sys_cntl;
+sys_control_t *syscntl;
 uint64_t el1_next_pa = 0;
 uint64_t el1_heap_pool = 0x40000000;
 
@@ -50,7 +50,7 @@ void el1_map_pa(uintptr_t vaddr, uintptr_t paddr)
     off = ((vaddr >> (39-(i*9))) & 0x1FF) << 3;
     pte = (uint64_t *)(pa | off);
     *pte = paddr & ~0xFFF;
-    *pte |= (PTE_PAGE | PTE_ACCESS);
+    *pte |= (PTE_PAGE | PTE_ACCESS | PTE_USER_RW);
     DEBUG_MSG("mapped VA:0x%lx to PA:0x%x (PTE:0x%lx)",
               vaddr, paddr, pte);
 }
@@ -186,6 +186,7 @@ void el1_handle_svc(uint64_t ec, uint32_t op, svc_op_desc_t *desc)
         el1_map_secure((op_map_mem_t *)&desc->map);
         break;
     case SVC_GET_SYSCNTL:
+        desc->get.datap = syscntl;
         break;
     default:
         printf("Unrecognized AArch64 SVC opcode: op = %d\n", op);
@@ -195,37 +196,55 @@ void el1_handle_svc(uint64_t ec, uint32_t op, svc_op_desc_t *desc)
 
 void el1_handle_exception(uint64_t ec, uint64_t iss)
 {
+#ifdef DEBUG
     armv8_data_abort_iss_t dai = {.raw = iss};
 //    armv8_inst_abort_iss_t iai = {.raw = iss};
+#endif
     uint64_t elr, far;
 
     __get_exception_address(far);
     __get_exception_return(elr);
 
-    el1_sys_cntl->el1_excp[is_secure].ec = ec;
-    el1_sys_cntl->el1_excp[is_secure].iss = iss;
-    el1_sys_cntl->el1_excp[is_secure].far = far;
+    if (syscntl->excp_log || syscntl->el1_excp[SEC_STATE].log) {
+        syscntl->el1_excp[SEC_STATE].taken = true;
+        syscntl->el1_excp[SEC_STATE].ec = ec;
+        syscntl->el1_excp[SEC_STATE].iss = iss;
+        syscntl->el1_excp[SEC_STATE].far = far;
+    }
 
     switch (ec) {
+    case EC_UNKNOWN:
+        DEBUG_MSG("EL1_%s: Unknown exception far = 0x%lx  elr = 0x%lx\n",
+                  SEC_STATE ? "NS" : "S", far, elr);
+
+        if (syscntl->el1_excp[SEC_STATE].action == EXCP_ACTION_SKIP ||
+            syscntl->excp_action == EXCP_ACTION_SKIP) {
+            elr +=4;
+            __set_exception_return(elr);
+        }
         break;
+
     case EC_IABORT_LOWER:
-        printf("Instruction abort at lower level: far = %0lx\n", far);
+        DEBUG_MSG("EL1_%s: Instruction abort at lower level: far = %0lx\n",
+                  SEC_STATE ? "NS" : "S", far);
         break;
     case EC_IABORT:
-        printf("Instruction abort at EL1: far = %0lx\n", far);
+        DEBUG_MSG("EL1_%s: Instruction abort at EL1: far = %0lx\n",
+                  SEC_STATE ? "NS" : "S", far);
         break;
     case EC_DABORT_LOWER:
-        printf("Data abort (%s) at lower level: far = %0lx elr = %0lx\n",
-               dai.wnr ? "write" : "read", far, elr);
-        el1_map_va(far);
+        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);
         break;
     case EC_DABORT:
-        printf("Data abort (%s) at EL1: far = %0lx elr = %0lx\n",
-               dai.wnr ? "write" : "read", far, elr);
-        el1_map_va(far);
+        DEBUG_MSG("EL1_%s: Data abort (%s) at EL1: far = %0lx elr = %0lx\n",
+                  SEC_STATE ? "NS" : "S", dai.wnr ? "write" : "read", far, elr);
         break;
     default:
-        printf("Unhandled EL1 exception: EC = %d  ISS = %d\n", ec, iss);
+        DEBUG_MSG("EL1_%s: Unhandled EL1 exception: EC = %d  ISS = %d\n",
+                  SEC_STATE ? "NS" : "S", ec, iss);
         break;
     }
 }
@@ -274,8 +293,8 @@ void *el1_load_el0(char *elfbase, char *start_va)
         if (!strcmp(secname, ".text") || !strcmp(secname, ".data")) {
             uint64_t sect = (uint64_t)((char *)elfbase + shdr[i].sh_offset);
             char *base_va = start_va + shdr[i].sh_addr;
-            printf("\tloading %s section: 0x%x bytes @ 0x%lx\n",
-                  secname, shdr[i].sh_size, base_va);
+            DEBUG_MSG("\tloading %s section: 0x%x bytes @ 0x%lx\n",
+                      secname, shdr[i].sh_size, base_va);
             for (off = 0; off < shdr[i].sh_size; off += 0x1000) {
                 el1_map_va((uintptr_t)(base_va + off));
                 memcpy((void *)(base_va + off), (void *)(sect + off), 0x1000);
@@ -296,7 +315,7 @@ void el1_start(uint64_t base, uint64_t size)
     uint64_t addr = base;
     size_t len;
 
-    printf("EL1 (%s) started...\n", SECURE_STATE);
+    printf("EL1 (%s) started...\n", SEC_STATE_STR);
 
     /* Unmap the init segement so we don't accidentally use it */
     for (len = 0; len < ((size + 0xFFF) & ~0xFFF);
@@ -304,12 +323,12 @@ void el1_start(uint64_t base, uint64_t size)
         el1_unmap_va(addr);
     }
 
-    void *pa = el1_sys_cntl;
-    el1_sys_cntl = (sys_control_t *)el1_heap_allocate(0x1000);
-    el1_map_pa((uintptr_t)el1_sys_cntl, (uintptr_t)pa);
-    el1_map_pa((uintptr_t)el1_sys_cntl->smc_interop.buf_va,
-               (uintptr_t)el1_sys_cntl->smc_interop.buf_pa);
-    smc_interop_buf = el1_sys_cntl->smc_interop.buf_va;
+    void *pa = syscntl;
+    syscntl = (sys_control_t *)el1_heap_allocate(0x1000);
+    el1_map_pa((uintptr_t)syscntl, (uintptr_t)pa);
+    el1_map_pa((uintptr_t)syscntl->smc_interop.buf_va,
+               (uintptr_t)syscntl->smc_interop.buf_pa);
+    smc_interop_buf = syscntl->smc_interop.buf_va;
 
     el1_init_el0();
 
diff --git a/aarch64/el1_common/el1_init.S b/aarch64/el1_common/el1_init.S
index 4e9c37e..d32dcdc 100644
--- a/aarch64/el1_common/el1_init.S
+++ b/aarch64/el1_common/el1_init.S
@@ -1,5 +1,7 @@
+#define __ASSEMBLY__
 #include "el1.h"
 #include "armv8_vmsa.h"
+#undef __ASSEMBLY__
 
 .section .init
 .align 12
@@ -165,7 +167,7 @@ el1_init_start:
 /* Migrate the next PA to the non-init code */
     ldr x10, =el1_next_pa
     str x17, [x10]
-    ldr x10, =el1_sys_cntl
+    ldr x10, =syscntl
     str x16, [x10]
 
     /* Pass the address and size of the init section to start so it
diff --git a/aarch64/el1_ns/el1.h b/aarch64/el1_ns/el1.h
index 9bf0947..5e641f9 100644
--- a/aarch64/el1_ns/el1.h
+++ b/aarch64/el1_ns/el1.h
@@ -15,6 +15,7 @@
 #define EL1_PGTBL_BASE EL1_NS_PGTBL_BASE
 #define EL1_PGTBL_SIZE EL1_NS_PGTBL_SIZE
 
-#define SECURE_STATE "non-secure"
-#define is_secure 0
+#define SEC_STATE_STR "non-secure"
+#define SEC_STATE 1
+
 #endif
diff --git a/aarch64/el1_s/el1.h b/aarch64/el1_s/el1.h
index 56fbaf4..a8fc6ad 100644
--- a/aarch64/el1_s/el1.h
+++ b/aarch64/el1_s/el1.h
@@ -15,6 +15,7 @@
 #define EL1_PGTBL_BASE EL1_S_PGTBL_BASE
 #define EL1_PGTBL_SIZE EL1_S_PGTBL_SIZE
 
-#define SECURE_STATE "secure"
-#define is_secure 1
+#define SEC_STATE_STR "secure"
+#define SEC_STATE 0
+
 #endif
diff --git a/aarch64/el1_s/el1_sec.c b/aarch64/el1_s/el1_sec.c
index 09cc9f9..cde517f 100644
--- a/aarch64/el1_s/el1_sec.c
+++ b/aarch64/el1_s/el1_sec.c
@@ -3,6 +3,7 @@
 #include "libcflat.h"
 #include <stdint.h>
 #include "smc.h"
+#include "debug.h"
 
 extern void el1_map_pa(uintptr_t vaddr, uintptr_t paddr);
 void el1_sec_smc_loop()
@@ -10,19 +11,19 @@ void el1_sec_smc_loop()
     smc_op_desc_t *desc = smc_interop_buf;
     uint32_t op = SMC_OP_YIELD;
 
-    printf("EL1_S: In loop\n");
+    DEBUG_MSG("EL1_S: In loop\n");
 
     while (op != SMC_OP_EXIT) {
         switch (op) {
         case SMC_OP_MAP:
-            printf("EL1_S: Doing a MAP desc = %p\n", desc);
+            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:
-            printf("EL1_S: Doing a YIELD desc = %p\n", desc);
+            DEBUG_MSG("EL1_S: Doing a YIELD desc = %p\n", desc);
             break;
         default:
-            printf("Unrecognized SMC opcode %d.  Exiting ...\n", op);
+            DEBUG_MSG("Unrecognized SMC opcode %d.  Exiting ...\n", op);
             SMC_EXIT();
             break;
         }
diff --git a/aarch64/el3/el3.c b/aarch64/el3/el3.c
index 48441e0..46a2642 100644
--- a/aarch64/el3/el3.c
+++ b/aarch64/el3/el3.c
@@ -16,7 +16,7 @@
 state_buf sec_state;
 state_buf nsec_state;
 
-sys_control_t *sys_cntl;
+sys_control_t *syscntl;
 smc_op_desc_t *smc_interop_buf;
 
 uint64_t el3_next_pa = 0;
@@ -174,7 +174,7 @@ void 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);
-        printf("EL3: Mapped VA:0x%lx to PA:0x%lx\n", map->va, map->pa);
+        DEBUG_MSG("EL3: Mapped VA:0x%lx to PA:0x%lx\n", map->va, map->pa);
     }
 
 /*
@@ -224,12 +224,18 @@ int el3_handle_exception(uint64_t ec, uint64_t iss)
     __get_exception_address(far);
     __get_exception_return(elr);
 
-    sys_cntl->el3_excp.ec = ec;
-    sys_cntl->el3_excp.iss = iss;
-    sys_cntl->el3_excp.far = far;
+    if (syscntl->excp_log || syscntl->el3_excp.log) {
+        syscntl->el3_excp.taken = true;
+        syscntl->el3_excp.ec = ec;
+        syscntl->el3_excp.iss = iss;
+        syscntl->el3_excp.far = far;
+    }
 
     switch (ec) {
-    case EC_SMC64:      /* SMC from aarch64 */
+    case EC_SMC64:
+    case EC_SMC32:
+        printf("Took an SMC exception from EL3\n");
+        break;
     case EC_IABORT_LOWER:
         printf("Instruction abort at lower level: far = %0lx\n", far);
         break;
@@ -264,7 +270,7 @@ void el3_monitor_init()
      */
     sec_state.elr_el3 = EL1_S_FLASH_BASE;
     sec_state.spsr_el3 = 0x5;
-    sec_state.x[0] = (uint64_t)el3_lookup_pa(sys_cntl);
+    sec_state.x[0] = (uint64_t)el3_lookup_pa(syscntl);
 
     /* Set-up the nonsecure state buffer to return to the non-secure
      * initialization sequence. This will occur on the first monitor context
@@ -272,7 +278,7 @@ void el3_monitor_init()
      */
     nsec_state.elr_el3 = EL1_NS_FLASH_BASE;
     nsec_state.spsr_el3 = 0x5;
-    nsec_state.x[0] = (uint64_t)el3_lookup_pa(sys_cntl);
+    nsec_state.x[0] = (uint64_t)el3_lookup_pa(syscntl);
 }
 
 void el3_start(uint64_t base, uint64_t size)
@@ -280,16 +286,18 @@ void el3_start(uint64_t base, uint64_t size)
     uint64_t addr = base;
     size_t len;
 
+    printf("EL3 started...\n");
+
     /* Unmap the init segement so we don't accidentally use it */
     for (len = 0; len < ((size + 0xFFF) & ~0xFFF);
          len += 0x1000, addr += 0x1000) {
         el3_unmap_va(addr);
     }
 
-    sys_cntl = el3_heap_allocate(0x1000);
+    syscntl = el3_heap_allocate(0x1000);
     smc_interop_buf = el3_heap_allocate(0x1000);
-    sys_cntl->smc_interop.buf_va = smc_interop_buf;
-    sys_cntl->smc_interop.buf_pa = el3_lookup_pa(smc_interop_buf);
+    syscntl->smc_interop.buf_va = smc_interop_buf;
+    syscntl->smc_interop.buf_pa = el3_lookup_pa(smc_interop_buf);
 
     el3_monitor_init();