Commonize mem routines and cleanup

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

20 files changed, 248 insertions, 318 deletions

diff --git a/.gdbinit64 b/.gdbinit64
index 164e1ab..a8b1876 100644
--- a/.gdbinit64
+++ b/.gdbinit64
@@ -18,12 +18,10 @@ add-symbol-file aarch64/el1_ns/el1_nsec.elf &_EL1_NS_TEXT_BASE
 end
 
 define load_el0ns
-file aarch64/el0_ns/el0_nsec.elf
-add-symbol-file aarch64/el0_ns/el0_nsec.elf &_EL0_NS_TEXT_BASE
+add-symbol-file aarch64/el0_ns/el0_nsec.elf 0x80000
 end
 
 define load_el0s
-file aarch64/el0_s/el0_sec.elf
-add-symbol-file aarch64/el0_s/el0_sec.elf &_EL0_S_TEXT_BASE
+add-symbol-file aarch64/el0_s/el0_sec.elf 0xc0000
 end
 
diff --git a/aarch64/common/debug.h b/aarch64/common/debug.h
index 673f6dd..c9af5fe 100644
--- a/aarch64/common/debug.h
+++ b/aarch64/common/debug.h
@@ -1,9 +1,11 @@
 #ifndef _DEBUG_H
 #define _DEBUG_H
 
+extern const char *sec_state_str;
+
 #ifdef DEBUG
 #define DEBUG_MSG(_str, ...) \
-    printf("\n[DEBUG] %s (%s): " _str, __FUNCTION__, SEC_STATE_STR, ##__VA_ARGS__)
+    printf("\n[DEBUG] %s (%s): " _str, __FUNCTION__, (sec_state_str)?sec_state_str:"MISSING", ##__VA_ARGS__)
 #define DEBUG_ARG
 #else
 #define DEBUG_MSG(_str, ...)
diff --git a/aarch64/common/mem_util.c b/aarch64/common/mem_util.c
new file mode 100644
index 0000000..d313454
--- /dev/null
+++ b/aarch64/common/mem_util.c
@@ -0,0 +1,144 @@
+#include "libcflat.h"
+#include "armv8_vmsa.h"
+#include <stdint.h>
+#include <stddef.h>
+#include "mem_util.h"
+
+#define SEC_STATE_STR "EL3"
+#include "debug.h"
+
+extern uint64_t mem_pgtbl_base;
+extern uint64_t mem_next_pa;
+extern uint64_t mem_heap_pool;
+
+uint64_t mem_allocate_pa()
+{
+    uint64_t next = mem_next_pa;
+    mem_next_pa += PAGE_SIZE;
+    return next;
+}
+
+void mem_map_pa(uintptr_t vaddr, uintptr_t paddr,
+                uintptr_t tblattr, uintptr_t pgattr)
+{
+    uint64_t pa = mem_pgtbl_base, off;
+    uint32_t i;
+    uint64_t *pte;
+
+    for (i = 0; i < 3; i++) {
+        /* Each successive level uses the next lower 9 VA bits in a 48-bit
+         * address, hence the i*9.
+         */
+        off = ((vaddr >> (39-(i*9))) & 0x1FF) << 3;
+        pte = (uint64_t *)(pa | off);
+        if (!(*pte & 0x1)) {
+            pa = mem_allocate_pa();
+            *pte = pa;
+            *pte |= (PTE_TABLE | tblattr);
+        } else {
+            pa = *pte & 0x000FFFFFF000;
+        }
+    }
+
+    /* The last level is the physical page to map */
+    off = ((vaddr >> (39-(i*9))) & 0x1FF) << 3;
+    pte = (uint64_t *)(pa | off);
+    *pte = paddr & ~0xFFF;
+    *pte |= (PTE_PAGE | PTE_ACCESS | pgattr);
+    DEBUG_MSG("mapped VA:0x%lx to PA:0x%x - PTE:0x%lx (0x%lx)",
+              vaddr, paddr, pte, *pte);
+}
+
+void mem_map_va(uintptr_t addr)
+{
+    uint64_t pa = mem_pgtbl_base, off;
+    uint32_t i;
+    uint64_t *pte;
+
+    for (i = 0; i < 4; i++) {
+        /* Each successive level uses the next lower 9 VA bits in a 48-bit
+         * address, hence the i*9.
+         */
+        off = ((addr >> (39-(i*9))) & 0x1FF) << 3;
+        pte = (uint64_t *)(pa | off);
+        if (!(*pte & 0x1)) {
+            pa = mem_allocate_pa();
+            *pte = pa;
+            *pte |= PTE_PAGE;
+        } else {
+            pa = *pte & 0x000FFFFFF000;
+        }
+    }
+
+    *pte |= (PTE_ACCESS | PTE_USER_RW);
+    DEBUG_MSG("mapped VA:0x%lx to PA:0x%x - PTE:0x%lx (0x%lx)",
+                addr, pa, pte, *pte);
+}
+
+int mem_unmap_va(uint64_t addr)
+{
+    uint64_t pa = mem_pgtbl_base;
+    uint32_t i;
+    uint64_t *pte;
+
+    for (i = 0; i < 4; i++) {
+        /* Each successive level uses the next lower 9 VA bits in a 48-bit
+         * address, hence the i*9.
+         */
+        uint64_t off = ((addr >> (39-(i*9))) & 0x1FF) << 3;
+        pte = (uint64_t *)(pa | off);
+        if (!(*pte & 0x1)) {
+            DEBUG_MSG("Failed unmap: invalid table page");
+            /* This is not a valid page, return an error */
+            return -1;
+        } else {
+            pa = *pte & 0x000FFFFFF000;
+        }
+    }
+
+    /* Clear the page descriptor */
+    *pte = 0;
+    DEBUG_MSG("unmapped PTE 0x%lx (VA:0x%lx, PA:0x%x)",
+              pte, addr, pa);
+
+    return 0;
+}
+
+void *mem_heap_allocate(size_t len)
+{
+    void *addr = (void *)mem_heap_pool;
+    size_t off;
+
+    for (off = 0; off < len; off += 0x1000) {
+        mem_map_va(mem_heap_pool + off);
+    }
+
+    mem_heap_pool += off;
+
+    return addr;
+}
+
+void *mem_lookup_pa(void *va)
+{
+    uint64_t pa = mem_pgtbl_base;
+    uint32_t i;
+    uint64_t *pte;
+
+    for (i = 0; i < 4; i++) {
+        /* Each successive level uses the next lower 9 VA bits in a 48-bit
+         * address, hence the i*9.
+         */
+        uint64_t off = ((((uint64_t)va) >> (39-(i*9))) & 0x1FF) << 3;
+        pte = (uint64_t *)(pa | off);
+        if (!(*pte & 0x1)) {
+            DEBUG_MSG("Failed Lookup: invalid table page");
+            /* This is not a valid page, return an error */
+            return (void *)-1;
+        } else {
+            pa = *pte & 0x000FFFFFF000;
+        }
+    }
+
+    return (void *)pa;
+}
+
diff --git a/aarch64/common/mem_util.h b/aarch64/common/mem_util.h
new file mode 100644
index 0000000..89f3625
--- /dev/null
+++ b/aarch64/common/mem_util.h
@@ -0,0 +1,11 @@
+#ifndef _MEM_UTIL_H
+#define _MEM_UTIL_H
+
+extern uint64_t mem_allocate_pa();
+extern void mem_map_pa(uintptr_t, uintptr_t, uintptr_t, uintptr_t);
+extern void mem_map_va(uintptr_t addr);
+extern int mem_unmap_va(uint64_t addr);
+extern void *mem_heap_allocate(size_t len);
+extern void *mem_lookup_pa(void *va);
+
+#endif
diff --git a/aarch64/el0_common/tztest.c b/aarch64/el0_common/tztest.c
index f477433..38c43ca 100644
--- a/aarch64/el0_common/tztest.c
+++ b/aarch64/el0_common/tztest.c
@@ -6,15 +6,10 @@
 #include "el0.h"
 #include "debug.h"
 #include "el0_common.h"
+#include "tztest.h"
 
 sys_control_t *syscntl = NULL;
 
-#define TEST_HEAD(_str, ...) \
-    printf("\nValidating %s EL%d " _str ":\n", SEC_STATE_STR, el, ##__VA_ARGS__)
-
-#define TEST_MSG(_str, ...) \
-    printf("\tEL%d (%s): " _str "... ", el, SEC_STATE_STR, ##__VA_ARGS__)
-
 uint32_t check_smc(uint32_t el)
 {
     TEST_HEAD("smc behavior");
diff --git a/aarch64/el0_common/tztest.h b/aarch64/el0_common/tztest.h
index 5f8bf73..d66c582 100644
--- a/aarch64/el0_common/tztest.h
+++ b/aarch64/el0_common/tztest.h
@@ -10,6 +10,12 @@ extern uint32_t check_register_access(uint32_t el);
 extern uint32_t check_cpacr_trap(uint32_t el);
 extern uint32_t check_wfx_trap(uint32_t el);
 
+#define TEST_HEAD(_str, ...) \
+    printf("\nValidating %s EL%d " _str ":\n", sec_state_str, el, ##__VA_ARGS__)
+
+#define TEST_MSG(_str, ...) \
+    printf("\tEL%d (%s): " _str "... ", el, sec_state_str, ##__VA_ARGS__)
+
 typedef enum {
     TZTEST_SMC = 0,
     TZTEST_REG_ACCESS,
diff --git a/aarch64/el0_ns/tztest_nsec.c b/aarch64/el0_ns/tztest_nsec.c
index ffe0f66..5fa0afb 100644
--- a/aarch64/el0_ns/tztest_nsec.c
+++ b/aarch64/el0_ns/tztest_nsec.c
@@ -1,6 +1,8 @@
 #include "el0_common.h"
+#include "tztest.h"
 
 tztest_t tztest[TZTEST_COUNT];
+const char *sec_state_str;
 
 void interop_test()
 {
@@ -8,9 +10,8 @@ void interop_test()
 
     test.orig = test.val = 1024;
     test.fail = test.count = 0;
-    __svc(SVC_OP_TEST, (svc_op_desc_t *)&test);
-
     printf("\nValidating interop communication between ELs... ");
+    __svc(SVC_OP_TEST, (svc_op_desc_t *)&test);
     TEST_CONDITION(!test.fail && test.val == (test.orig >> test.count));
 }
 
@@ -28,7 +29,15 @@ int main()
 {
     svc_op_desc_t desc;
 
-    printf("Starting TZ test ...\n");
+    /* ISSUE: For some reason, static initialization of the global security
+     * state string fails.  The pointer ends up being NULL in some cases, but
+     * not in others.  This likely has something to do with the position
+     * independence of the EL0 code.  The below workaround works fine.
+     */
+    const char *str = "non-secure";
+    sec_state_str = str;
+
+    printf("EL0 (%s) started...\n", sec_state_str);
 
     tztest_init();
 
@@ -40,6 +49,8 @@ int main()
     syscntl->test_cntl = (test_control_t*)alloc_mem(0, 0x1000);
     map_va(syscntl->test_cntl, 0x1000, OP_MAP_ALL);
 
+    printf("Starting TZ test...\n");
+
     /* Test EL to EL communication */
     interop_test();
 
@@ -56,9 +67,9 @@ int main()
     run_test(TZTEST_CPACR_TRAP, 0);
     run_test(TZTEST_WFX_TRAP, 0);
 
-    printf("\nValidation complete.  Passed %d of %d tests\n",
-           syscntl->test_cntl->test_count - syscntl->test_cntl->fail_count,
-           syscntl->test_cntl->test_count);
+    printf("\nValidation complete.  Passed %d of %d tests.\n",
+              syscntl->test_cntl->test_count - syscntl->test_cntl->fail_count,
+              syscntl->test_cntl->test_count);
 
     __svc(SVC_OP_EXIT, NULL);
 
diff --git a/aarch64/el0_s/Makefile b/aarch64/el0_s/Makefile
index fcf36be..67a5319 100644
--- a/aarch64/el0_s/Makefile
+++ b/aarch64/el0_s/Makefile
@@ -10,18 +10,19 @@ EL0_S_OBJS		= tztest_sec.o \
 
 -include .*.d
 
+CFLAGS += -pie
 CFLAGS += -I../el0_common
 
 ##################################################################
 
 $(EL0_S_ELF): $(EL0_S_OBJS) $(EL0_S_LOAD)
-	$(LD) -fpic -pie -o $@ $(EL0_S_OBJS) $(FLATLIBS) --script=$(EL0_S_LOAD)
+	$(LD) -pie -o $@ $(EL0_S_OBJS) $(FLATLIBS) --script=$(EL0_S_LOAD)
 
 $(EL0_S_IMAGE): $(EL0_S_ELF)
 	$(OBJCOPY) -O binary $< $@
 
 $(EL0_S_LOAD): el0_sec.lds.S Makefile ../../platform/$(PLAT)/
-	$(CC) $(CFLAGS) -fpic -pie -E -P -C -o $@ $<
+	$(CC) $(CFLAGS) -E -P -C -o $@ $<
 
 %.o: %.S
 	$(CC) $(CFLAGS) -mcmodel=large -c -nostdlib -o $(notdir $@) $<
diff --git a/aarch64/el0_s/tztest_sec.c b/aarch64/el0_s/tztest_sec.c
index 35b4972..74e0518 100644
--- a/aarch64/el0_s/tztest_sec.c
+++ b/aarch64/el0_s/tztest_sec.c
@@ -1,5 +1,6 @@
 #include "el0_common.h"
 
+const char *sec_state_str;
 tztest_t tztest[TZTEST_COUNT];
 
 void el0_sec_loop()
@@ -52,7 +53,16 @@ int main()
 {
     svc_op_desc_t desc;
 
-    printf("Starting secure-side EL0  ...\n");
+    /* ISSUE: For some reason, static initialization of the global security
+     * state string fails.  The pointer ends up being NULL in some cases, but
+     * not in others.  This likely has something to do with the position
+     * independence of the EL0 code.  The below workaround works fine.
+     */
+    const char *str = "secure";
+    sec_state_str = str;
+
+    printf("EL0 (%s) started...\n", sec_state_str);
+
     tztest_init();
 
     /* Fetch the system-wide control structure */
@@ -63,7 +73,7 @@ int main()
      * gone drastically wrong.
      */
     if (!syscntl) {
-        printf("Failed to acquire system control structure\n");
+        DEBUG_MSG("Failed to acquire system control structure\n");
         __svc(SVC_OP_EXIT, NULL);
     }
 
diff --git a/aarch64/el1_common/el1.c b/aarch64/el1_common/el1.c
index 539e9ab..89e9109 100644
--- a/aarch64/el1_common/el1.c
+++ b/aarch64/el1_common/el1.c
@@ -1,143 +1,15 @@
 #include "el1_common.h"
+#include "mem_util.h"
 
+uint64_t mem_pgtbl_base = EL1_PGTBL_BASE;
 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 next = el1_next_pa;
-    el1_next_pa += 0x1000;
-    return next;
-}
-
-void el1_map_pa(uintptr_t vaddr, uintptr_t paddr)
-{
-    uint64_t pa = EL1_PGTBL_BASE, off;
-    uint32_t i;
-    uint64_t *pte;
-
-    for (i = 0; i < 3; i++) {
-        /* Each successive level uses the next lower 9 VA bits in a 48-bit
-         * address, hence the i*9.
-         */
-        off = ((vaddr >> (39-(i*9))) & 0x1FF) << 3;
-        pte = (uint64_t *)(pa | off);
-        if (!(*pte & 0x1)) {
-            pa = el1_allocate_pa();
-            *pte = pa;
-            *pte |= PTE_PAGE;
-        } else {
-            pa = *pte & 0x000FFFFFF000;
-        }
-    }
-
-    /* The last level is the physical page to map */
-    off = ((vaddr >> (39-(i*9))) & 0x1FF) << 3;
-    pte = (uint64_t *)(pa | off);
-    *pte = paddr & ~0xFFF;
-    *pte |= (PTE_PAGE | PTE_ACCESS | PTE_USER_RW);
-    DEBUG_MSG("mapped VA:0x%lx to PA:0x%x (PTE:0x%lx)",
-              vaddr, paddr, pte);
-}
-
-void el1_map_va(uintptr_t addr)
-{
-    uint64_t pa = EL1_PGTBL_BASE, off;
-    uint32_t i;
-    uint64_t *pte;
-
-    for (i = 0; i < 4; i++) {
-        /* Each successive level uses the next lower 9 VA bits in a 48-bit
-         * address, hence the i*9.
-         */
-        off = ((addr >> (39-(i*9))) & 0x1FF) << 3;
-        pte = (uint64_t *)(pa | off);
-        if (!(*pte & 0x1)) {
-            pa = el1_allocate_pa();
-            *pte = pa;
-            *pte |= PTE_PAGE;
-        } else {
-            pa = *pte & 0x000FFFFFF000;
-        }
-    }
-
-    *pte |= (PTE_ACCESS | PTE_USER_RW);
-    DEBUG_MSG("mapped VA:0x%lx to PA:0x%x (PTE:0x%lx)",
-                addr, pa, pte);
-}
-
-int el1_unmap_va(uint64_t addr)
-{
-    uint64_t pa = EL1_PGTBL_BASE;
-    uint32_t i;
-    uint64_t *pte;
-
-    for (i = 0; i < 4; i++) {
-        /* Each successive level uses the next lower 9 VA bits in a 48-bit
-         * address, hence the i*9.
-         */
-        uint64_t off = ((addr >> (39-(i*9))) & 0x1FF) << 3;
-        pte = (uint64_t *)(pa | off);
-        if (!(*pte & 0x1)) {
-            DEBUG_MSG("Failed unmap: invalid table page");
-            /* This is not a valid page, return an error */
-            return -1;
-        } else {
-            pa = *pte & 0x000FFFFFF000;
-        }
-    }
-
-    /* Clear the page descriptor */
-    *pte = 0;
-    DEBUG_MSG("unmapped PTE 0x%lx (VA:0x%lx, PA:0x%x)",
-              pte, addr, pa);
-
-    return 0;
-}
-
-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;
-}
+uint64_t mem_next_pa = 0;
+uint64_t mem_heap_pool = 0x40000000;
 
 void el1_alloc_mem(op_alloc_mem_t *alloc)
 {
-    alloc->addr = el1_heap_allocate(alloc->len);
-}
-
-void *el1_lookup_pa(void *va)
-{
-    uint64_t pa = EL1_PGTBL_BASE;
-    uint32_t i;
-    uint64_t *pte;
-
-    for (i = 0; i < 4; i++) {
-        /* Each successive level uses the next lower 9 VA bits in a 48-bit
-         * address, hence the i*9.
-         */
-        uint64_t off = ((((uint64_t)va) >> (39-(i*9))) & 0x1FF) << 3;
-        pte = (uint64_t *)(pa | off);
-        if (!(*pte & 0x1)) {
-            DEBUG_MSG("Failed Lookup: invalid table page");
-            /* This is not a valid page, return an error */
-            return (void *)-1;
-        } else {
-            pa = *pte & 0x000FFFFFF000;
-        }
-    }
-
-    return (void *)pa;
+    alloc->addr = mem_heap_allocate(alloc->len);
 }
 
 void el1_map_secure(op_map_mem_t *map)
@@ -146,11 +18,11 @@ void el1_map_secure(op_map_mem_t *map)
         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);
+            desc->map.pa = mem_lookup_pa(desc->map.va);
         }
         __smc(SMC_OP_MAP, desc);
     } else {
-        el1_map_pa((uint64_t)map->va, (uint64_t)map->pa);
+        mem_map_pa((uint64_t)map->va, (uint64_t)map->pa, 0, PTE_USER_RW);
     }
 }
 
@@ -318,19 +190,19 @@ void el1_start(uint64_t base, uint64_t size)
     uint64_t addr = base;
     size_t len;
 
-    printf("EL1 (%s) started...\n", SEC_STATE_STR);
+    printf("EL1 (%s) started...\n", sec_state_str);
 
     /* Unmap the init segement so we don't accidentally use it */
     for (len = 0; len < ((size + (PAGE_SIZE - 1)) & ~(PAGE_SIZE - 1));
          len += PAGE_SIZE, addr += PAGE_SIZE) {
-        el1_unmap_va(addr);
+        mem_unmap_va(addr);
     }
 
     void *pa = syscntl;
-    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);
+    syscntl = (sys_control_t *)mem_heap_allocate(PAGE_SIZE);
+    mem_map_pa((uintptr_t)syscntl, (uintptr_t)pa, 0, PTE_USER_RW);
+    mem_map_pa((uintptr_t)syscntl->smc_interop.buf_va,
+               (uintptr_t)syscntl->smc_interop.buf_pa, 0, PTE_USER_RW);
     smc_interop_buf = syscntl->smc_interop.buf_va;
 
     el1_init_el0();
diff --git a/aarch64/el1_common/el1_common.h b/aarch64/el1_common/el1_common.h
index 4b6493d..d5221eb 100644
--- a/aarch64/el1_common/el1_common.h
+++ b/aarch64/el1_common/el1_common.h
@@ -15,13 +15,5 @@
 
 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 ec3631a..d471224 100644
--- a/aarch64/el1_common/el1_init.S
+++ b/aarch64/el1_common/el1_init.S
@@ -165,7 +165,7 @@ el1_init_start:
     ldp x16, x17, [sp], #16
 
 /* Migrate the next PA to the non-init code */
-    ldr x10, =el1_next_pa
+    ldr x10, =mem_next_pa
     str x17, [x10]
     ldr x10, =syscntl
     str x16, [x10]
diff --git a/aarch64/el1_common/el1_loader.c b/aarch64/el1_common/el1_loader.c
index 43a73ce..312fd6e 100644
--- a/aarch64/el1_common/el1_loader.c
+++ b/aarch64/el1_common/el1_loader.c
@@ -1,5 +1,6 @@
 #include "el1_common.h"
 #include "elf.h"
+#include "mem_util.h"
 
 /* Simple ELF loader for loading EL0 image */
 void *el1_load_el0(char *elfbase, char *start_va)
@@ -9,23 +10,23 @@ void *el1_load_el0(char *elfbase, char *start_va)
     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);
+    mem_map_pa((uint64_t)elfbase, (uint64_t)elfbase, 0, PTE_USER_RW);
 
     /* 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");
+        DEBUG_MSG("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);
+        DEBUG_MSG("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");
+        DEBUG_MSG("Loading %s EL0 test image...\n",
+                  (ehdr->e_machine == EM_ARM) ?  "aarch32" : "aarch64");
     }
 
     /* Size of the ELF to map */
@@ -33,7 +34,8 @@ void *el1_load_el0(char *elfbase, char *start_va)
 
     /* 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);
+        mem_map_pa((uint64_t)elfbase + off, (uint64_t)elfbase + off,
+                   0, PTE_USER_RW);
     }
 
     Elf64_Shdr *shdr = (Elf64_Shdr *)((char *)elfbase + ehdr->e_shoff);
@@ -48,7 +50,7 @@ void *el1_load_el0(char *elfbase, char *start_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));
+                mem_map_va((uintptr_t)(base_va + off));
                 memcpy((void *)(base_va + off), (void *)(sect + off), 0x1000);
             }
         }
@@ -56,7 +58,7 @@ void *el1_load_el0(char *elfbase, char *start_va)
 
     /* Unmap the FLASH ELF image */
     for (off = 0; off < elf_len; off += 0x1000) {
-        el1_map_va((uint64_t)elfbase + off);
+        mem_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 351eeff..c7e7419 100644
--- a/aarch64/el1_ns/Makefile
+++ b/aarch64/el1_ns/Makefile
@@ -9,6 +9,7 @@ EL1_NS_OBJS		= el1_init.o \
                   el1_loader.o \
                   el1_nsec.o \
 				  svc.o \
+                  mem_util.o \
                   builtins.o
 
 -include .*.d
diff --git a/aarch64/el1_ns/el1_nsec.c b/aarch64/el1_ns/el1_nsec.c
index 4dc8988..adde2a9 100644
--- a/aarch64/el1_ns/el1_nsec.c
+++ b/aarch64/el1_ns/el1_nsec.c
@@ -8,6 +8,8 @@ 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;
 
+const char *sec_state_str = "non-secure";
+
 void el1_init_el0()
 {
     int (*main)(void);
diff --git a/aarch64/el1_s/Makefile b/aarch64/el1_s/Makefile
index 5eebd65..317561f 100644
--- a/aarch64/el1_s/Makefile
+++ b/aarch64/el1_s/Makefile
@@ -9,6 +9,7 @@ EL1_S_OBJS		= el1_init.o \
                   el1_loader.o \
                   el1_sec.o \
 				  svc.o \
+                  mem_util.o \
 				  builtins.o
 
 -include .*.d
diff --git a/aarch64/el1_s/el1_sec.c b/aarch64/el1_s/el1_sec.c
index 3fc4d5b..c9b6cef 100644
--- a/aarch64/el1_s/el1_sec.c
+++ b/aarch64/el1_s/el1_sec.c
@@ -8,11 +8,13 @@ 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;
 
+const char *sec_state_str = "secure";
+
 void el1_init_el0()
 {
     int (*main)(void);
 
-    main = el1_load_el0((char *)EL0_S_FLASH_BASE, (char *)EL0_NS_BASE_VA);
+    main = el1_load_el0((char *)EL0_S_FLASH_BASE, (char *)EL0_S_BASE_VA);
 
     __set_exception_return((uint64_t)main);
     __exception_return();
diff --git a/aarch64/el3/Makefile b/aarch64/el3/Makefile
index f049cb6..de22441 100644
--- a/aarch64/el3/Makefile
+++ b/aarch64/el3/Makefile
@@ -4,6 +4,7 @@ EL3_OBJS		= el3_init.o \
                   el3_exception.o \
                   el3.o \
                   el3_monitor_asm.o \
+                  mem_util.o \
                   builtins.o
 EL3_ELF			= el3.elf
 EL3_IMAGE		= el3.bin
diff --git a/aarch64/el3/el3.c b/aarch64/el3/el3.c
index 2961a2f..5203f76 100644
--- a/aarch64/el3/el3.c
+++ b/aarch64/el3/el3.c
@@ -11,6 +11,7 @@
 #include "el3_monitor.h"
 #include "arm_builtins.h"
 #include "syscntl.h"
+#include "mem_util.h"
 
 #define SEC_STATE_STR "EL3"
 #include "debug.h"
@@ -27,6 +28,8 @@ const char *smc_op_name[] = {
     [SMC_OP_TEST] = "SMC_OP_TEST",
     [SMC_OP_DISPATCH] = "SMC_OP_DISPATCH",
 };
+
+const char *sec_state_str = "EL3";
 #endif
 
 state_buf sec_state;
@@ -35,153 +38,29 @@ state_buf nsec_state;
 sys_control_t *syscntl;
 smc_op_desc_t *smc_interop_buf;
 
-uint64_t el3_next_pa = 0;
-uint64_t el3_heap_pool = 0xF800000000;
+uint64_t mem_pgtbl_base = EL3_PGTBL_BASE;
+uint64_t mem_next_pa = 0;
+uint64_t mem_heap_pool = 0xF800000000;
 
 void el3_shutdown() {
     uintptr_t *sysreg_cfgctrl = (uintptr_t *)(SYSREG_BASE + SYSREG_CFGCTRL);
 
-    printf("\nTest complete\n");
+    DEBUG_MSG("Test complete");
 
     *sysreg_cfgctrl = SYS_SHUTDOWN;
 
     while(1);   /* Shutdown does not work on all machines */
 }
 
-uint64_t el3_allocate_pa()
-{
-    uint64_t next = el3_next_pa;
-    el3_next_pa += 0x1000;
-    return next;
-}
-
-void el3_map_pa(uintptr_t vaddr, uint64_t paddr)
-{
-    uint64_t pa = EL3_PGTBL_BASE, off;
-    uint32_t i;
-    uint64_t *pte;
-
-    for (i = 0; i < 3; i++) {
-        /* Each successive level uses the next lower 9 VA bits in a 48-bit
-         * address, hence the i*9.
-         */
-        off = ((vaddr >> (39-(i*9))) & 0x1FF) << 3;
-        pte = (uint64_t *)(pa | off);
-        if (!(*pte & 0x1)) {
-            pa = el3_allocate_pa();
-            *pte = pa;
-            *pte |= PTE_PAGE;
-        } else {
-            pa = *pte & 0x000FFFFFF000;
-        }
-    }
-
-    /* The last level is the physical page to map */
-    off = ((vaddr >> (39-(i*9))) & 0x1FF) << 3;
-    pte = (uint64_t *)(pa | off);
-    *pte = paddr & ~0xFFF;
-    *pte |= (PTE_PAGE | PTE_ACCESS);
-    DEBUG_MSG("mapped VA:0x%lx to PA:0x%x (PTE:0x%lx)",
-              vaddr, paddr, pte);
-}
-
-void el3_map_va(uintptr_t addr)
-{
-    uint64_t pa = EL3_PGTBL_BASE, off;
-    uint32_t i;
-    uint64_t *pte;
-
-    for (i = 0; i < 4; i++) {
-        /* Each successive level uses the next lower 9 VA bits in a 48-bit
-         * address, hence the i*9.
-         */
-        off = ((addr >> (39-(i*9))) & 0x1FF) << 3;
-        pte = (uint64_t *)(pa | off);
-        if (!(*pte & 0x1)) {
-            pa = el3_allocate_pa();
-            *pte = pa;
-            *pte |= PTE_PAGE;
-        } else {
-            pa = *pte & 0x000FFFFFF000;
-        }
-    }
-
-    *pte |= PTE_ACCESS;
-}
-
-int el3_unmap_va(uint64_t addr)
-{
-    uint64_t pa = EL3_PGTBL_BASE;
-    uint32_t i;
-    uint64_t *pte;
-
-    for (i = 0; i < 4; i++) {
-        /* Each successive level uses the next lower 9 VA bits in a 48-bit
-         * address, hence the i*9.
-         */
-        uint64_t off = ((addr >> (39-(i*9))) & 0x1FF) << 3;
-        pte = (uint64_t *)(pa | off);
-        if (!(*pte & 0x1)) {
-            /* This is not a valid page, return an error */
-            return -1;
-        } else {
-            pa = *pte & 0x000FFFFFF000;
-        }
-    }
-
-    /* Clear the page descriptor */
-    *pte = 0;
-
-    return 0;
-}
-
-void *el3_heap_allocate(size_t len)
-{
-    void *addr = (void *)el3_heap_pool;
-    size_t off;
-
-    for (off = 0; off < len; off += 0x1000) {
-        el3_map_va(el3_heap_pool + off);
-    }
-
-    el3_heap_pool += off;
-
-    return addr;
-}
-
 void el3_alloc_mem(op_alloc_mem_t *alloc)
 {
-    alloc->addr = el3_heap_allocate(alloc->len);
-}
-
-void *el3_lookup_pa(void *va)
-{
-    uint64_t pa = EL3_PGTBL_BASE;
-    uint32_t i;
-    uint64_t *pte;
-
-    for (i = 0; i < 4; i++) {
-        /* Each successive level uses the next lower 9 VA bits in a 48-bit
-         * address, hence the i*9.
-         */
-        uint64_t off = ((((uint64_t)va) >> (39-(i*9))) & 0x1FF) << 3;
-        pte = (uint64_t *)(pa | off);
-        if (!(*pte & 0x1)) {
-            DEBUG_MSG("Failed Lookup: invalid table page");
-            /* This is not a valid page, return an error */
-            return (void *)-1;
-        } else {
-            pa = *pte & 0x000FFFFFF000;
-        }
-    }
-
-    return (void *)pa;
+    alloc->addr = mem_heap_allocate(alloc->len);
 }
 
 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);
+        mem_map_pa((uintptr_t)map->va, (uintptr_t)map->pa, 0, PTE_PRIV_RW);
         DEBUG_MSG("Mapped VA:0x%lx to PA:0x%lx\n", map->va, map->pa);
     }
 
@@ -364,7 +243,7 @@ void el3_monitor_init()
     sec_state.spsr_el3 = 0x5;
     sec_state.spsel = 0x1;
 #endif
-    sec_state.x[0] = (uint64_t)el3_lookup_pa(syscntl);
+    sec_state.x[0] = (uint64_t)mem_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
@@ -375,7 +254,7 @@ void el3_monitor_init()
     nsec_state.spsr_el3 = 0x5;
     nsec_state.spsel = 0x1;
 #endif
-    nsec_state.x[0] = (uint64_t)el3_lookup_pa(syscntl);
+    nsec_state.x[0] = (uint64_t)mem_lookup_pa(syscntl);
 }
 
 void el3_start(uint64_t base, uint64_t size)
@@ -388,14 +267,14 @@ void el3_start(uint64_t base, uint64_t size)
     /* Unmap the init segement so we don't accidentally use it */
     for (len = 0; len < ((size + (PAGE_SIZE - 1)) & ~(PAGE_SIZE - 1));
          len += PAGE_SIZE, addr += PAGE_SIZE) {
-        el3_unmap_va(addr);
+        mem_unmap_va(addr);
     }
 
-    syscntl = el3_heap_allocate(PAGE_SIZE);
+    syscntl = mem_heap_allocate(PAGE_SIZE);
 
-    smc_interop_buf = el3_heap_allocate(PAGE_SIZE);
+    smc_interop_buf = mem_heap_allocate(PAGE_SIZE);
     syscntl->smc_interop.buf_va = smc_interop_buf;
-    syscntl->smc_interop.buf_pa = el3_lookup_pa(smc_interop_buf);
+    syscntl->smc_interop.buf_pa = mem_lookup_pa(smc_interop_buf);
 
     el3_monitor_init();
 
diff --git a/aarch64/el3/el3_init.S b/aarch64/el3/el3_init.S
index c075972..fbb0274 100644
--- a/aarch64/el3/el3_init.S
+++ b/aarch64/el3/el3_init.S
@@ -158,7 +158,7 @@ el3_copy_data:
 
 el3_init_start:
     ldr x17, [sp], #8       /* next PA */
-    ldr x10, =el3_next_pa
+    ldr x10, =mem_next_pa
     str x17, [x10]
 
     ldr x0, =_EL3_INIT_BASE