General cleanup

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

20 files changed, 258 insertions, 214 deletions

diff --git a/aarch64/Makefile b/aarch64/Makefile
index 75f23e1..fc58b07 100644
--- a/aarch64/Makefile
+++ b/aarch64/Makefile
@@ -6,6 +6,7 @@ TZTEST_NS_IMAGE = el0_ns/el0_nsec.elf
 TZTEST_S_IMAGE 	= el0_s/el0_sec.elf
 
 CFLAGS += -I../../platform/$(PLAT) -I../../libcflat/include -I../common
+CFLAGS += -DAARCH64
 
 -include .*.d
 
diff --git a/aarch64/common/arm_builtins.h b/aarch64/common/arm_builtins.h
index 940b2fd..1ccbebe 100644
--- a/aarch64/common/arm_builtins.h
+++ b/aarch64/common/arm_builtins.h
@@ -53,4 +53,16 @@ extern void write_cpacr_el1(uint64_t);
 extern uint64_t read_sctlr_el1();
 extern void write_sctlr_el1(uint64_t);
 extern void __set_exception_return(uint64_t);
+
+#define READ_SCR() read_scr_el3()
+#define WRITE_SCR(_val) write_scr_el3(_val)
+#define READ_SDER() read_sder32_el3()
+#define WRITE_SDER(_val) write_sder32_el3(_val)
+#define READ_CPTR_EL3() read_cptr_el3()
+#define WRITE_CPTR_EL3(_val) write_cptr_el3(_val)
+#define READ_CPACR() read_cpacr_el1()
+#define WRITE_CPACR(_val) write_cpacr_el1(_val)
+#define READ_CURRENTEL() read_currentel()
+#define WRITE_CURRENTEL(_val) read_currentel(_val)
+
 #endif
diff --git a/aarch64/common/armv8_vmsa.h b/aarch64/common/armv8_vmsa.h
index 534747c..fda6c46 100644
--- a/aarch64/common/armv8_vmsa.h
+++ b/aarch64/common/armv8_vmsa.h
@@ -36,6 +36,7 @@ typedef union {
 } armv8_4k_tbl_pte_t;
 */
 
+#define PAGE_SIZE 0x1000
 #define ARMV8_PAGE_ATTRINDX_SHIFT 2
 #define ARMV8_PAGE_NS_SHIFT 5
 #define ARMV8_PAGE_AP_SHIFT 6
diff --git a/aarch64/common/interop.h b/aarch64/common/interop.h
index fcd7448..c4ac039 100644
--- a/aarch64/common/interop.h
+++ b/aarch64/common/interop.h
@@ -33,9 +33,9 @@ typedef struct {
 typedef enum {
     CURRENTEL = 1,
     CPTR_EL3,
-    CPACR_EL1,
-    SCR_EL3,
-    SCTLR_EL1,
+    CPACR,
+    SCR,
+    SCTLR,
 } op_reg_key_t;
 
 typedef struct {
diff --git a/aarch64/common/smc.h b/aarch64/common/smc.h
index 5b16b5e..b54991e 100644
--- a/aarch64/common/smc.h
+++ b/aarch64/common/smc.h
@@ -19,18 +19,6 @@
 
 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_GET_REG] = "SMC_OP_GET_REG",
-    [SMC_OP_SET_REG] = "SMC_OP_SET_REG",
-    [SMC_OP_TEST] = "SMC_OP_TEST",
-    [SMC_OP_DISPATCH] = "SMC_OP_DISPATCH",
-};
-
 typedef union {
     op_dispatch_t dispatch;
     op_map_mem_t map;
diff --git a/aarch64/el0_common/tztest.c b/aarch64/el0_common/tztest.c
index 477500c..f477433 100644
--- a/aarch64/el0_common/tztest.c
+++ b/aarch64/el0_common/tztest.c
@@ -31,37 +31,40 @@ uint32_t check_register_access(uint32_t el)
     TEST_HEAD("restricted register access");
 
     TEST_MSG("SCR read");
-    TEST_EL1_EXCEPTION(read_scr_el3(), EC_UNKNOWN);
+    TEST_EL1_EXCEPTION(READ_SCR(), EC_UNKNOWN);
 
     TEST_MSG("SCR write");
-    TEST_EL1_EXCEPTION(write_scr_el3(0), EC_UNKNOWN);
+    TEST_EL1_EXCEPTION(WRITE_SCR(0), EC_UNKNOWN);
 
     TEST_MSG("SDER read");
-    TEST_EL1_EXCEPTION(read_sder32_el3(), EC_UNKNOWN);
+    TEST_EL1_EXCEPTION(READ_SDER(), EC_UNKNOWN);
 
     TEST_MSG("SDER write");
-    TEST_EL1_EXCEPTION(write_sder32_el3(0), EC_UNKNOWN);
+    TEST_EL1_EXCEPTION(WRITE_SDER(0), EC_UNKNOWN);
 
-/*
+#ifdef AARCH32
     TEST_MSG("MVBAR read");
-    TEST_EL1_EXCEPTION(read_mvbar(), EC_UNKNOWN);
+    TEST_EL1_EXCEPTION(READ_MVBAR(), EC_UNKNOWN);
 
     TEST_MSG("MVBAR write");
-    TEST_EL1_EXCEPTION(write_mvbar(0), EC_UNKNOWN);
+    TEST_EL1_EXCEPTION(WRITE_MVBAR(0), EC_UNKNOWN);
 
     TEST_MSG("NSACR write");
-    TEST_EL1_EXCEPTION(write_nsacr(0), EC_UNKNOWN);
-*/
+    TEST_EL1_EXCEPTION(WRITE_NSACR(0), EC_UNKNOWN);
+#endif
 
+#ifdef AARCH64
     TEST_MSG("CPTR_EL3 read");
-    TEST_EL1_EXCEPTION(read_cptr_el3(), EC_UNKNOWN);
+    TEST_EL1_EXCEPTION(READ_CPTR_EL3(), EC_UNKNOWN);
 
     TEST_MSG("CPTR_EL3 write");
-    TEST_EL1_EXCEPTION(write_cptr_el3(0), EC_UNKNOWN);
+    TEST_EL1_EXCEPTION(WRITE_CPTR_EL3(0), EC_UNKNOWN);
+#endif
 
     return 0;
 }
 
+#ifdef AARCH64
 uint32_t check_cpacr_trap(uint32_t el)
 {
     uint64_t cptr_el3, cpacr;
@@ -76,11 +79,11 @@ uint32_t check_cpacr_trap(uint32_t el)
 
     /* Try to read CPACR */
     TEST_MSG("Read of disabled CPACR");
-    TEST_EL3_EXCEPTION(SVC_GET_REG(CPACR_EL1, 1, cpacr), EC_SYSINSN);
+    TEST_EL3_EXCEPTION(SVC_GET_REG(CPACR, 1, cpacr), EC_SYSINSN);
 
     /* Try to write CPACR */
     TEST_MSG("Write of disabled CPACR");
-    TEST_EL3_EXCEPTION(SVC_SET_REG(CPACR_EL1, 1, cpacr), EC_SYSINSN);
+    TEST_EL3_EXCEPTION(SVC_SET_REG(CPACR, 1, cpacr), EC_SYSINSN);
 
 #ifdef FP_TEST
     /* Disable FP access */
@@ -97,18 +100,18 @@ uint32_t check_cpacr_trap(uint32_t el)
 
 uint32_t check_wfx_trap(uint32_t el)
 {
-    uint64_t sctlr, scr_el3;
+    uint64_t sctlr, scr;
 
     TEST_HEAD("WFx traps");
 
     /* Get the current SCR so we can restore it later */
-    SVC_GET_REG(SCR_EL3, 3, scr_el3);
+    SVC_GET_REG(SCR, 3, scr);
 
     /* Get the current SCTLR so we can restore it later */
-    SVC_GET_REG(SCTLR_EL1, 1, sctlr);
+    SVC_GET_REG(SCTLR, 1, sctlr);
 
     /* Clear SCTLR.nTWE to cause WFE instructions to trap to EL1 */
-    SVC_SET_REG(SCTLR_EL1, 1, sctlr & ~SCTLR_nTWE);
+    SVC_SET_REG(SCTLR, 1, sctlr & ~SCTLR_nTWE);
     TEST_MSG("Execution of WFE trapped to EL1");
     TEST_EL1_EXCEPTION(asm volatile("wfe\n"), EC_WFI_WFE);
 
@@ -117,13 +120,13 @@ uint32_t check_wfx_trap(uint32_t el)
     /* Trap WFE instructions to EL3.  This should work even though SCTLR.nTWE
      * is clear
      */
-    SVC_SET_REG(SCR_EL3, 3, scr_el3 | SCR_WFE);
+    SVC_SET_REG(SCR, 3, scr | SCR_WFE);
     TEST_MSG("Execution of trapped WFE (SCTLR.nTWE clear)",
            SEC_STATE_STR);
     TEST_EL3_EXCEPTION(asm volatile("wfe\n"), EC_WFI_WFE);
 
     /* Restore SCTLR */
-    SVC_SET_REG(SCTLR_EL1, 1, sctlr);
+    SVC_SET_REG(SCTLR, 1, sctlr);
 
     /* This should trap to EL3 with SCTLR.nTWE set */
     TEST_MSG("Execution of trapped WFE (SCTLR.nTWE set)",
@@ -131,10 +134,10 @@ uint32_t check_wfx_trap(uint32_t el)
     TEST_EL3_EXCEPTION(asm volatile("wfe\n"), EC_WFI_WFE);
 
     /* Restore SCR */
-    SVC_SET_REG(SCR_EL3, 3, scr_el3);
+    SVC_SET_REG(SCR, 3, scr);
 
     /* Clear SCTLR.nTWI to cause WFI instructions to trap to EL1 */
-    SVC_SET_REG(SCTLR_EL1, 1, sctlr & ~SCTLR_nTWI);
+    SVC_SET_REG(SCTLR, 1, sctlr & ~SCTLR_nTWI);
     TEST_MSG("Execution of WFI trapped to EL1");
     TEST_EL1_EXCEPTION(asm volatile("wfi\n"), EC_WFI_WFE);
 
@@ -143,30 +146,33 @@ uint32_t check_wfx_trap(uint32_t el)
     /* Trap WFI instructions to EL3.  This should work even though SCTLR.nTWE
      * is clear
      */
-    SVC_SET_REG(SCR_EL3, 3, scr_el3 | SCR_WFI);
+    SVC_SET_REG(SCR, 3, scr | SCR_WFI);
 
     TEST_MSG("Execution of trapped WFI (SCTLR.nTWI clear)",
            SEC_STATE_STR);
     TEST_EL3_EXCEPTION(asm volatile("wfi\n"), EC_WFI_WFE);
 
     /* Restore SCTLR */
-    SVC_SET_REG(SCTLR_EL1, 1, sctlr);
+    SVC_SET_REG(SCTLR, 1, sctlr);
 
     TEST_MSG("Execution of trapped WFI (SCTLR.nTWI set)",
            SEC_STATE_STR);
     TEST_EL3_EXCEPTION(asm volatile("wfi\n"), EC_WFI_WFE);
 
     /* Restore SCR */
-    SVC_SET_REG(SCR_EL3, 3, scr_el3);
+    SVC_SET_REG(SCR, 3, scr);
 
     return 0;
 }
+#endif
 
 void tztest_init()
 {
     tztest[TZTEST_SMC] = check_smc;
     tztest[TZTEST_REG_ACCESS] = check_register_access;
+#ifdef AARCH64
     tztest[TZTEST_CPACR_TRAP] = check_cpacr_trap;
     tztest[TZTEST_WFX_TRAP] = check_wfx_trap;
+#endif
 }
 
diff --git a/aarch64/el1_common/el1.c b/aarch64/el1_common/el1.c
index 29551eb..539e9ab 100644
--- a/aarch64/el1_common/el1.c
+++ b/aarch64/el1_common/el1.c
@@ -1,25 +1,12 @@
-#include "libcflat.h"
-#include "platform.h"
-#include "smc.h"
-#include "svc.h"
-#include "string.h"
-#include "el1.h"
-#include "el1_loader.h"
-#include "armv8_exception.h"
-#include "armv8_vmsa.h"
-#include "arm_builtins.h"
-#include "elf.h"
-#include "debug.h"
-#include "syscntl.h"
-
-extern void el1_init_el0();
+#include "el1_common.h"
 
 smc_op_desc_t *smc_interop_buf;
 sys_control_t *syscntl;
 uint64_t el1_next_pa = 0;
 uint64_t el1_heap_pool = 0x40000000;
 
-uint64_t el1_allocate_pa() {
+uint64_t el1_allocate_pa()
+{
     uint64_t next = el1_next_pa;
     el1_next_pa += 0x1000;
     return next;
@@ -213,16 +200,16 @@ int el1_handle_svc(uint32_t op, svc_op_desc_t *desc)
         if (desc->get.el == 1) {
             switch (desc->get.key) {
             case CURRENTEL:
-                desc->get.data = read_currentel();
+                desc->get.data = READ_CURRENTEL();
                 break;
             case CPTR_EL3:
-                desc->get.data = read_cptr_el3();
+                desc->get.data = READ_CPTR_EL3();
                 break;
-            case CPACR_EL1:
-                desc->get.data = read_cpacr_el1();
+            case CPACR:
+                desc->get.data = READ_CPACR();
                 break;
-            case SCR_EL3:
-                desc->get.data = read_scr_el3();
+            case SCR:
+                desc->get.data = READ_SCR();
                 break;
             }
         } else if (desc->get.el == 3) {
@@ -235,16 +222,16 @@ int el1_handle_svc(uint32_t op, svc_op_desc_t *desc)
         if (desc->set.el == 1) {
             switch (desc->set.key) {
             case CURRENTEL:
-                write_currentel(desc->set.data);
+                WRITE_CURRENTEL(desc->set.data);
                 break;
             case CPTR_EL3:
-                write_cptr_el3(desc->set.data);
+                WRITE_CPTR_EL3(desc->set.data);
                 break;
-            case CPACR_EL1:
-                write_cpacr_el1(desc->set.data);
+            case CPACR:
+                WRITE_CPACR(desc->set.data);
                 break;
-            case SCR_EL3:
-                write_scr_el3(desc->set.data);
+            case SCR:
+                WRITE_SCR(desc->set.data);
                 break;
             }
         } else if (desc->set.el == 3) {
@@ -270,10 +257,6 @@ int el1_handle_svc(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);
@@ -305,13 +288,12 @@ void el1_handle_exception(uint64_t ec, uint64_t iss)
         SMC_EXIT();
         break;
     case EC_DABORT_LOWER:
-        DEBUG_MSG("Data abort (%s) at lower level: far = %0lx elr = %0lx\n",
-                  dai.wnr ? "write" : "read", far, elr);
+        DEBUG_MSG("Data abort at lower level: far = %0lx elr = %0lx\n",
+                  far, elr);
         SMC_EXIT();
         break;
     case EC_DABORT:
-        DEBUG_MSG("Data abort (%s) at EL1: far = %0lx elr = %0lx\n",
-                  dai.wnr ? "write" : "read", far, elr);
+        DEBUG_MSG("Data abort at EL1: far = %0lx elr = %0lx\n", far, elr);
         SMC_EXIT();
         break;
     case EC_WFI_WFE:
@@ -325,73 +307,12 @@ void el1_handle_exception(uint64_t ec, uint64_t iss)
         }
         break;
     default:
-        DEBUG_MSG("Unhandled EL1 exception: EC = %d  ISS = %d\n", ec, iss);
+        DEBUG_MSG("Unhandled EL1 exception: ec = %d iss = %d\n", ec, iss);
         SMC_EXIT();
         break;
     }
 }
 
-/* Simple ELF loader for loading EL0 image */
-void *el1_load_el0(char *elfbase, char *start_va)
-{
-    Elf64_Ehdr *ehdr = (Elf64_Ehdr *)elfbase;
-    size_t off;
-    int i;
-
-    /* Map the ELF header in so we can determine how much more to map */
-    el1_map_pa((uint64_t)elfbase, (uint64_t)elfbase);
-
-    /* Make sure this is an appropriate ELF image */
-    if (ehdr->e_ident[EI_MAG0] != ELFMAG0 ||
-        ehdr->e_ident[EI_MAG1] != ELFMAG1 ||
-        ehdr->e_ident[EI_MAG2] != ELFMAG2 ||
-        ehdr->e_ident[EI_MAG3] != ELFMAG3) {
-        printf("Invalid ELF header, exiting...\n");
-        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_EXIT();
-    } else {
-        printf("Loading %s EL0 test image...\n",
-               (ehdr->e_machine == EM_ARM) ?  "aarch32" : "aarch64");
-    }
-
-    /* Size of the ELF to map */
-    size_t elf_len = ehdr->e_shoff + (ehdr->e_shentsize * ehdr->e_shnum);
-
-    /* Finish mapping the remainder of the ELF pages in if any */
-    for (off = 0x1000; off < elf_len; off += 0x1000) {
-        el1_map_pa((uint64_t)elfbase + off, (uint64_t)elfbase + off);
-    }
-
-    Elf64_Shdr *shdr = (Elf64_Shdr *)((char *)elfbase + ehdr->e_shoff);
-
-    Elf64_Shdr *strshdr = &shdr[ehdr->e_shstrndx];
-    char *strsec = (char *)ehdr + strshdr->sh_offset;
-    for (i = 0; i < ehdr->e_shnum; i++) {
-        char *secname = strsec + shdr[i].sh_name;
-        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;
-            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);
-            }
-        }
-    }
-
-    /* Unmap the FLASH ELF image */
-    for (off = 0; off < elf_len; off += 0x1000) {
-        el1_map_va((uint64_t)elfbase + off);
-    }
-
-    return (void *)(start_va + ehdr->e_entry);
-}
-
 void el1_start(uint64_t base, uint64_t size)
 {
     uint64_t addr = base;
@@ -400,13 +321,13 @@ void el1_start(uint64_t base, uint64_t size)
     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);
-         len += 0x1000, addr += 0x1000) {
+    for (len = 0; len < ((size + (PAGE_SIZE - 1)) & ~(PAGE_SIZE - 1));
+         len += PAGE_SIZE, addr += PAGE_SIZE) {
         el1_unmap_va(addr);
     }
 
     void *pa = syscntl;
-    syscntl = (sys_control_t *)el1_heap_allocate(0x1000);
+    syscntl = (sys_control_t *)el1_heap_allocate(PAGE_SIZE);
     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);
diff --git a/aarch64/el1_common/el1_common.h b/aarch64/el1_common/el1_common.h
new file mode 100644
index 0000000..4b6493d
--- /dev/null
+++ b/aarch64/el1_common/el1_common.h
@@ -0,0 +1,27 @@
+#ifndef __EL1_COMMON_H
+#define __EL1_COMMON_H
+
+#include "libcflat.h"
+#include "platform.h"
+#include "svc.h"
+#include "smc.h"
+#include "string.h"
+#include "el1.h"
+#include "armv8_exception.h"
+#include "armv8_vmsa.h"
+#include "arm_builtins.h"
+#include "debug.h"
+#include "syscntl.h"
+
+extern void el1_init_el0();
+extern void *el1_load_el0(char *base, char *start_va);
+extern uint64_t el1_allocate_pa();
+extern void el1_map_pa(uintptr_t vaddr, uintptr_t paddr);
+extern void el1_map_va(uintptr_t addr);
+extern int el1_unmap_va(uint64_t addr);
+extern void *el1_heap_allocate(size_t len);
+extern void el1_alloc_mem(op_alloc_mem_t *alloc);
+extern void *el1_lookup_pa(void *va);
+extern void el1_map_secure(op_map_mem_t *map);
+
+#endif
diff --git a/aarch64/el1_common/el1_init.S b/aarch64/el1_common/el1_init.S
index 0faa564..ec3631a 100644
--- a/aarch64/el1_common/el1_init.S
+++ b/aarch64/el1_common/el1_init.S
@@ -16,11 +16,11 @@ el1_init:
     /* The stack still needs to be allocated and mapped so we set up a
      * temporary stack for the time being.
      */
-    ldr x10, =RAM_BASE+0x2000
+    ldr x10, =RAM_BASE + (2 * PAGE_SIZE)
 	mov sp, x10
 
     /* Use the top of the stack to track our PA pool pointer */
-    ldr x10, =EL1_PGTBL_BASE+0x1000
+    ldr x10, =EL1_PGTBL_BASE + PAGE_SIZE
     str x10, [sp]
 
     /* Save the input SMC interop buf pointer */
@@ -82,11 +82,11 @@ el1_map_data:
 
 el1_map_stacks:
     /* Map the first page of the stack so we can get off the ground */
-    ldr x0, =EL1_STACK_BASE-0x1000
+    ldr x0, =EL1_STACK_BASE - PAGE_SIZE
     mov x1, #(PTE_PAGE|PTE_ACCESS|PTE_PRIV_RW)
     bl map_va
 
-    ldr x0, =EL0_STACK_BASE-0x1000
+    ldr x0, =EL0_STACK_BASE - PAGE_SIZE
     mov x1, #(PTE_PAGE|PTE_ACCESS|PTE_USER_RW)
     bl map_va
 
diff --git a/aarch64/el1_common/el1_loader.c b/aarch64/el1_common/el1_loader.c
new file mode 100644
index 0000000..43a73ce
--- /dev/null
+++ b/aarch64/el1_common/el1_loader.c
@@ -0,0 +1,64 @@
+#include "el1_common.h"
+#include "elf.h"
+
+/* Simple ELF loader for loading EL0 image */
+void *el1_load_el0(char *elfbase, char *start_va)
+{
+    Elf64_Ehdr *ehdr = (Elf64_Ehdr *)elfbase;
+    size_t off;
+    int i;
+
+    /* Map the ELF header in so we can determine how much more to map */
+    el1_map_pa((uint64_t)elfbase, (uint64_t)elfbase);
+
+    /* Make sure this is an appropriate ELF image */
+    if (ehdr->e_ident[EI_MAG0] != ELFMAG0 ||
+        ehdr->e_ident[EI_MAG1] != ELFMAG1 ||
+        ehdr->e_ident[EI_MAG2] != ELFMAG2 ||
+        ehdr->e_ident[EI_MAG3] != ELFMAG3) {
+        printf("Invalid ELF header, exiting...\n");
+        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_EXIT();
+    } else {
+        printf("Loading %s EL0 test image...\n",
+               (ehdr->e_machine == EM_ARM) ?  "aarch32" : "aarch64");
+    }
+
+    /* Size of the ELF to map */
+    size_t elf_len = ehdr->e_shoff + (ehdr->e_shentsize * ehdr->e_shnum);
+
+    /* Finish mapping the remainder of the ELF pages in if any */
+    for (off = 0x1000; off < elf_len; off += 0x1000) {
+        el1_map_pa((uint64_t)elfbase + off, (uint64_t)elfbase + off);
+    }
+
+    Elf64_Shdr *shdr = (Elf64_Shdr *)((char *)elfbase + ehdr->e_shoff);
+
+    Elf64_Shdr *strshdr = &shdr[ehdr->e_shstrndx];
+    char *strsec = (char *)ehdr + strshdr->sh_offset;
+    for (i = 0; i < ehdr->e_shnum; i++) {
+        char *secname = strsec + shdr[i].sh_name;
+        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;
+            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);
+            }
+        }
+    }
+
+    /* Unmap the FLASH ELF image */
+    for (off = 0; off < elf_len; off += 0x1000) {
+        el1_map_va((uint64_t)elfbase + off);
+    }
+
+    return (void *)(start_va + ehdr->e_entry);
+}
+
diff --git a/aarch64/el1_ns/Makefile b/aarch64/el1_ns/Makefile
index 6a6b97c..351eeff 100644
--- a/aarch64/el1_ns/Makefile
+++ b/aarch64/el1_ns/Makefile
@@ -6,13 +6,14 @@ EL1_NS_LOAD		= el1_nsec.lds
 EL1_NS_OBJS		= el1_init.o \
                   el1_exception.o \
                   el1.o \
+                  el1_loader.o \
                   el1_nsec.o \
 				  svc.o \
                   builtins.o
 
 -include .*.d
 
-CFLAGS += -I../el0_ns/
+CFLAGS += -I../el0_ns/ -I../el1_common
 
 ##################################################################
 
diff --git a/aarch64/el1_ns/el1.h b/aarch64/el1_ns/el1.h
index 5e641f9..dd10a95 100644
--- a/aarch64/el1_ns/el1.h
+++ b/aarch64/el1_ns/el1.h
@@ -3,6 +3,23 @@
 
 #include "platform.h"
 
+#ifndef __ASSEMBLY__
+extern uintptr_t _EL1_NS_INIT_BASE;
+extern uintptr_t EL1_NS_INIT_BASE;
+extern uintptr_t _EL1_NS_INIT_SIZE;
+extern uintptr_t EL1_NS_INIT_SIZE;
+extern uintptr_t _EL1_NS_FLASH_TEXT;
+extern uintptr_t EL1_NS_FLASH_TEXT;
+extern uintptr_t _EL1_NS_TEXT_BASE;
+extern uintptr_t EL1_NS_TEXT_BASE;
+extern uintptr_t _EL1_NS_DATA_BASE;
+extern uintptr_t EL1_NS_DATA_BASE;
+extern uintptr_t _EL1_NS_TEXT_SIZE;
+extern uintptr_t EL1_NS_TEXT_SIZE;
+extern uintptr_t _EL1_NS_DATA_SIZE;
+extern uintptr_t EL1_NS_DATA_SIZE;
+#endif
+
 #define _EL1_INIT_BASE _EL1_NS_INIT_BASE
 #define _EL1_INIT_SIZE _EL1_NS_INIT_SIZE
 #define _EL1_FLASH_TEXT _EL1_NS_FLASH_TEXT
diff --git a/aarch64/el1_ns/el1_loader.h b/aarch64/el1_ns/el1_loader.h
deleted file mode 100644
index 739e5ea..0000000
--- a/aarch64/el1_ns/el1_loader.h
+++ /dev/null
@@ -1,13 +0,0 @@
-#ifndef _EL1_NS_LOADER_H
-#define _EL1_NS_LOADER_H
-
-extern uintptr_t _EL1_NS_TEXT_BASE;
-uintptr_t EL1_NS_TEXT_BASE = (uintptr_t)&_EL1_NS_TEXT_BASE;
-extern uintptr_t _EL1_NS_DATA_BASE;
-uintptr_t EL1_NS_DATA_BASE = (uintptr_t)&_EL1_NS_DATA_BASE;
-extern uintptr_t _EL1_NS_TEXT_SIZE;
-uint64_t EL1_NS_TEXT_SIZE = (uint64_t)&_EL1_NS_TEXT_SIZE;
-extern uintptr_t _EL1_NS_DATA_SIZE;
-uint64_t EL1_NS_DATA_SIZE = (uint64_t)&_EL1_NS_DATA_SIZE;
-
-#endif
diff --git a/aarch64/el1_ns/el1_nsec.c b/aarch64/el1_ns/el1_nsec.c
index 79dc4cb..4dc8988 100644
--- a/aarch64/el1_ns/el1_nsec.c
+++ b/aarch64/el1_ns/el1_nsec.c
@@ -1,9 +1,12 @@
-#include "platform.h"
-#include "arm_builtins.h"
-#include "el1.h"
-#include "debug.h"
+#include "el1_common.h"
 
-extern void *el1_load_el0(char *base, char *start_va);
+uintptr_t EL1_NS_INIT_BASE = (uintptr_t)&_EL1_NS_INIT_BASE;
+uintptr_t EL1_NS_INIT_SIZE = (uintptr_t)&_EL1_NS_INIT_SIZE;
+uintptr_t EL1_NS_FLASH_TEXT = (uintptr_t)&_EL1_NS_FLASH_TEXT;
+uintptr_t EL1_NS_TEXT_BASE = (uintptr_t)&_EL1_NS_TEXT_BASE;
+uintptr_t EL1_NS_DATA_BASE = (uintptr_t)&_EL1_NS_DATA_BASE;
+uintptr_t EL1_NS_TEXT_SIZE = (uint64_t)&_EL1_NS_TEXT_SIZE;
+uintptr_t EL1_NS_DATA_SIZE = (uintptr_t)&_EL1_NS_DATA_SIZE;
 
 void el1_init_el0()
 {
diff --git a/aarch64/el1_s/Makefile b/aarch64/el1_s/Makefile
index 0396603..5eebd65 100644
--- a/aarch64/el1_s/Makefile
+++ b/aarch64/el1_s/Makefile
@@ -6,13 +6,14 @@ EL1_S_LOAD		= el1_sec.lds
 EL1_S_OBJS		= el1_init.o \
                   el1_exception.o \
                   el1.o \
+                  el1_loader.o \
                   el1_sec.o \
 				  svc.o \
 				  builtins.o
 
 -include .*.d
 
-CFLAGS += -I../el0_s/
+CFLAGS += -I../el0_s/ -I../el1_common
 
 ##################################################################
 
diff --git a/aarch64/el1_s/el1.h b/aarch64/el1_s/el1.h
index a8fc6ad..d64db1f 100644
--- a/aarch64/el1_s/el1.h
+++ b/aarch64/el1_s/el1.h
@@ -3,6 +3,23 @@
 
 #include "platform.h"
 
+#ifndef __ASSEMBLY__
+extern uintptr_t _EL1_S_INIT_BASE;
+extern uintptr_t EL1_S_INIT_BASE;
+extern uintptr_t _EL1_S_INIT_SIZE;
+extern uintptr_t EL1_S_INIT_SIZE;
+extern uintptr_t _EL1_S_FLASH_TEXT;
+extern uintptr_t EL1_S_FLASH_TEXT;
+extern uintptr_t _EL1_S_TEXT_BASE;
+extern uintptr_t EL1_S_TEXT_BASE;
+extern uintptr_t _EL1_S_DATA_BASE;
+extern uintptr_t EL1_S_DATA_BASE;
+extern uintptr_t _EL1_S_TEXT_SIZE;
+extern uintptr_t EL1_S_TEXT_SIZE;
+extern uintptr_t _EL1_S_DATA_SIZE;
+extern uintptr_t EL1_S_DATA_SIZE;
+#endif
+
 #define _EL1_INIT_BASE _EL1_S_INIT_BASE
 #define _EL1_INIT_SIZE _EL1_S_INIT_SIZE
 #define _EL1_FLASH_TEXT _EL1_S_FLASH_TEXT
diff --git a/aarch64/el1_s/el1_loader.h b/aarch64/el1_s/el1_loader.h
deleted file mode 100644
index 10d283d..0000000
--- a/aarch64/el1_s/el1_loader.h
+++ /dev/null
@@ -1,19 +0,0 @@
-#ifndef _EL1_S_LOADER_H
-#define _EL1_S_LOADER_H
-
-extern uintptr_t _EL1_S_INIT_BASE;
-uintptr_t EL1_S_INIT_BASE = (uintptr_t)&_EL1_S_INIT_BASE;
-extern uintptr_t _EL1_S_INIT_SIZE;
-uintptr_t EL1_S_INIT_SIZE = (uintptr_t)&_EL1_S_INIT_SIZE;
-extern uintptr_t _EL1_S_FLASH_TEXT;
-uintptr_t EL1_S_FLASH_TEXT = (uintptr_t)&_EL1_S_FLASH_TEXT;
-extern uintptr_t _EL1_S_TEXT_BASE;
-uintptr_t EL1_S_TEXT_BASE = (uintptr_t)&_EL1_S_TEXT_BASE;
-extern uintptr_t _EL1_S_DATA_BASE;
-uintptr_t EL1_S_DATA_BASE = (uintptr_t)&_EL1_S_DATA_BASE;
-extern uintptr_t _EL1_S_TEXT_SIZE;
-uint64_t EL1_S_TEXT_SIZE = (uint64_t)&_EL1_S_TEXT_SIZE;
-extern uintptr_t _EL1_S_DATA_SIZE;
-uint64_t EL1_S_DATA_SIZE = (uint64_t)&_EL1_S_DATA_SIZE;
-
-#endif
diff --git a/aarch64/el1_s/el1_sec.c b/aarch64/el1_s/el1_sec.c
index 2aea580..3fc4d5b 100644
--- a/aarch64/el1_s/el1_sec.c
+++ b/aarch64/el1_s/el1_sec.c
@@ -1,11 +1,12 @@
-#include "platform.h"
-#include "arm_builtins.h"
-#include "libcflat.h"
-#include <stdint.h>
-#include "el1.h"
-#include "debug.h"
+#include "el1_common.h"
 
-extern void *el1_load_el0(char *base, char *start_va);
+uintptr_t EL1_S_INIT_BASE = (uintptr_t)&_EL1_S_INIT_BASE;
+uintptr_t EL1_S_INIT_SIZE = (uintptr_t)&_EL1_S_INIT_SIZE;
+uintptr_t EL1_S_FLASH_TEXT = (uintptr_t)&_EL1_S_FLASH_TEXT;
+uintptr_t EL1_S_TEXT_BASE = (uintptr_t)&_EL1_S_TEXT_BASE;
+uintptr_t EL1_S_DATA_BASE = (uintptr_t)&_EL1_S_DATA_BASE;
+uintptr_t EL1_S_TEXT_SIZE = (uint64_t)&_EL1_S_TEXT_SIZE;
+uintptr_t EL1_S_DATA_SIZE = (uint64_t)&_EL1_S_DATA_SIZE;
 
 void el1_init_el0()
 {
diff --git a/aarch64/el3/el3.c b/aarch64/el3/el3.c
index ac4919e..2961a2f 100644
--- a/aarch64/el3/el3.c
+++ b/aarch64/el3/el3.c
@@ -15,6 +15,20 @@
 #define SEC_STATE_STR "EL3"
 #include "debug.h"
 
+#if DEBUG
+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_GET_REG] = "SMC_OP_GET_REG",
+    [SMC_OP_SET_REG] = "SMC_OP_SET_REG",
+    [SMC_OP_TEST] = "SMC_OP_TEST",
+    [SMC_OP_DISPATCH] = "SMC_OP_DISPATCH",
+};
+#endif
+
 state_buf sec_state;
 state_buf nsec_state;
 
@@ -211,16 +225,16 @@ int el3_handle_smc(uint64_t op, smc_op_desc_t *desc)
         if (desc->get.el == 3) {
             switch (desc->get.key) {
             case CURRENTEL:
-                desc->get.data = read_currentel();
+                desc->get.data = READ_CURRENTEL();
                 break;
             case CPTR_EL3:
-                desc->get.data = read_cptr_el3();
+                desc->get.data = READ_CPTR_EL3();
                 break;
-            case CPACR_EL1:
-                desc->get.data = read_cpacr_el1();
+            case CPACR:
+                desc->get.data = READ_CPACR();
                 break;
-            case SCR_EL3:
-                desc->get.data = read_scr_el3();
+            case SCR:
+                desc->get.data = READ_SCR();
                 break;
             }
         }
@@ -229,16 +243,16 @@ int el3_handle_smc(uint64_t op, smc_op_desc_t *desc)
         if (desc->set.el == 3) {
             switch (desc->set.key) {
             case CURRENTEL:
-                write_currentel(desc->set.data);
+                WRITE_CURRENTEL(desc->set.data);
                 break;
             case CPTR_EL3:
-                write_cptr_el3(desc->set.data);
+                WRITE_CPTR_EL3(desc->set.data);
                 break;
-            case CPACR_EL1:
-                write_cpacr_el1(desc->set.data);
+            case CPACR:
+                WRITE_CPACR(desc->set.data);
                 break;
-            case SCR_EL3:
-                write_scr_el3(desc->set.data);
+            case SCR:
+                WRITE_SCR(desc->set.data);
                 break;
             }
         }
@@ -254,7 +268,6 @@ int el3_handle_smc(uint64_t op, smc_op_desc_t *desc)
 
 int el3_handle_exception(uint64_t ec, uint64_t iss)
 {
-    armv8_data_abort_iss_t dai = {.raw = iss};
     uint64_t elr, far;
 
     __get_exception_address(far);
@@ -281,13 +294,12 @@ int el3_handle_exception(uint64_t ec, uint64_t iss)
         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);
+        printf("Data abort at lower level: far = %0lx elr = %0lx\n",
+               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);
+        printf("Data abort at EL3: far = %0lx elr = %0lx\n", far, elr);
         el3_shutdown();
         break;
     case EC_SYSINSN:
@@ -347,18 +359,22 @@ void el3_monitor_init()
      * sequence. This will occur when we return from exception after monitor
      * initialization.
      */
+#ifdef AARCH64
     sec_state.elr_el3 = EL1_S_FLASH_BASE;
     sec_state.spsr_el3 = 0x5;
     sec_state.spsel = 0x1;
+#endif
     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
      * switch (smc) from secure to non-secure.
      */
+#ifdef AARCH64
     nsec_state.elr_el3 = EL1_NS_FLASH_BASE;
     nsec_state.spsr_el3 = 0x5;
     nsec_state.spsel = 0x1;
+#endif
     nsec_state.x[0] = (uint64_t)el3_lookup_pa(syscntl);
 }
 
@@ -370,14 +386,14 @@ void el3_start(uint64_t base, uint64_t size)
     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) {
+    for (len = 0; len < ((size + (PAGE_SIZE - 1)) & ~(PAGE_SIZE - 1));
+         len += PAGE_SIZE, addr += PAGE_SIZE) {
         el3_unmap_va(addr);
     }
 
-    syscntl = el3_heap_allocate(0x1000);
+    syscntl = el3_heap_allocate(PAGE_SIZE);
 
-    smc_interop_buf = el3_heap_allocate(0x1000);
+    smc_interop_buf = el3_heap_allocate(PAGE_SIZE);
     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_init.S b/aarch64/el3/el3_init.S
index 3de4566..c075972 100644
--- a/aarch64/el3/el3_init.S
+++ b/aarch64/el3/el3_init.S
@@ -22,11 +22,11 @@ init_uart:
     /* The stack still needs to be allocated and mapped so we set up a
      * temporary stack for the time being.
      */
-    ldr x10, =RAM_BASE+0x2000
+    ldr x10, =RAM_BASE + (2 * PAGE_SIZE)
 	mov sp, x10
 
     /* Use the top of the stack to track our PA pool pointer */
-    ldr x10, =PT_BASE+0x1000
+    ldr x10, =PT_BASE + (PAGE_SIZE)
     str x10, [sp]
 
     /* Enable floating point register usage as printf uses it */
@@ -87,7 +87,7 @@ el3_map_data:
 
 el3_map_stack:
     /* Map the first page of the stack so we can get off the ground */
-    ldr x0, =EL3_STACK_BASE-0x1000
+    ldr x0, =EL3_STACK_BASE - PAGE_SIZE
     mov x1, #(PTE_PAGE|PTE_ACCESS|PTE_PRIV_RW)
     bl map_va