Enable EL0

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

19 files changed, 294 insertions, 51 deletions

diff --git a/.gdbinit64 b/.gdbinit64
index 36651a5..7776c70 100644
--- a/.gdbinit64
+++ b/.gdbinit64
@@ -19,3 +19,7 @@ file aarch64/el1_ns/el1_nsec.elf
 add-symbol-file aarch64/el1_ns/el1_nsec.elf &_EL1_NS_TEXT_BASE
 end
 
+define load_el0ns
+add-symbol-file aarch64/el0_ns/tztest.elf 0x10000
+end
+
diff --git a/Makefile b/Makefile
index e17351b..a107bc0 100644
--- a/Makefile
+++ b/Makefile
@@ -29,6 +29,7 @@ CFLAGS += -I../platform/$(PLAT)
 CFLAGS += $(call cc-option, -fomit-frame-pointer, "")
 CFLAGS += $(call cc-option, -fno-stack-protector, "")
 CFLAGS += $(call cc-option, -fno-stack-protector-all, "")
+CFLAGS += -I/usr/$(CROSS)/include
 
 export CFLAGS
 
diff --git a/aarch64/Makefile b/aarch64/Makefile
index 87cd2c7..2223c6b 100644
--- a/aarch64/Makefile
+++ b/aarch64/Makefile
@@ -2,6 +2,7 @@ BIOS_IMAGE 		= tztest.img
 EL3_IMAGE		= el3/el3.bin
 EL1_S_IMAGE		= el1_s/el1_sec.bin
 EL1_NS_IMAGE	= el1_ns/el1_nsec.bin
+TZTEST_IMAGE 	= el0_ns/tztest.elf
 
 CFLAGS += -I../../platform/$(PLAT) -I../../libcflat/include -I../common
 
@@ -9,10 +10,11 @@ CFLAGS += -I../../platform/$(PLAT) -I../../libcflat/include -I../common
 
 ##################################################################
 
-$(BIOS_IMAGE): $(EL3_IMAGE) $(EL1_S_IMAGE) $(EL1_NS_IMAGE)
+$(BIOS_IMAGE): $(EL3_IMAGE) $(EL1_S_IMAGE) $(EL1_NS_IMAGE) $(TZTEST_IMAGE)
 	dd if=$(EL3_IMAGE) of=$@ 2> /dev/null
 	dd oflag=seek_bytes seek=65536 if=$(EL1_S_IMAGE) of=$@ 2> /dev/null
 	dd oflag=seek_bytes seek=131072 if=$(EL1_NS_IMAGE) of=$@ 2> /dev/null
+	dd oflag=seek_bytes seek=196608 if=$(TZTEST_IMAGE) of=$@ 2> /dev/null
 	chmod +x $(BIOS_IMAGE)
 
 $(EL3_IMAGE):
@@ -24,10 +26,14 @@ $(EL1_S_IMAGE):
 $(EL1_NS_IMAGE):
 	$(MAKE) -C el1_ns all
 
+$(TZTEST_IMAGE):
+	$(MAKE) -C el0_ns all
+
 all: $(BIOS_IMAGE)
 
 clean:
 	$(MAKE) -C el3 clean
 	$(MAKE) -C el1_s clean
 	$(MAKE) -C el1_ns clean
+	$(MAKE) -C el0_ns clean
 	$(RM) $(BIOS_IMAGE) .*.d
diff --git a/aarch64/common/arm_builtins.h b/aarch64/common/arm_builtins.h
index a333af9..99d8749 100644
--- a/aarch64/common/arm_builtins.h
+++ b/aarch64/common/arm_builtins.h
@@ -2,6 +2,9 @@
 #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 __exception_return() asm volatile ("eret \n")
+#define __set_exception_return(_elr) \
+    asm volatile("msr elr_el1, %[elr]\n"::[elr] "r" (_elr))
 
 #endif
diff --git a/aarch64/common/svc.h b/aarch64/common/svc.h
index c3be066..838785c 100644
--- a/aarch64/common/svc.h
+++ b/aarch64/common/svc.h
@@ -9,6 +9,7 @@
 #define SVC_GET_SECURE_STATE 5
 #define SVC_EXIT 6
 
+/*
 typedef struct {
     union {
         tztest_dispatch_t dispatch;
@@ -23,5 +24,6 @@ typedef struct {
 } tztest_svc_desc_t;
 
 #define CLEAR_SVC_DESC(_desc) memset(&(_desc), sizeof(tztest_svc_desc_t), 0)
+*/
 
 #endif
diff --git a/aarch64/el0_ns/Makefile b/aarch64/el0_ns/Makefile
new file mode 100644
index 0000000..2409a1a
--- /dev/null
+++ b/aarch64/el0_ns/Makefile
@@ -0,0 +1,25 @@
+EL0_NS_ELF		= tztest.elf
+EL0_NS_IMAGE 	= tztest.bin
+EL0_NS_LOAD		= tztest.lds
+EL0_NS_OBJS		= tztest.o
+
+-include .*.d
+
+##################################################################
+
+$(EL0_NS_ELF): $(EL0_NS_OBJS) $(EL0_NS_LOAD)
+	$(LD) -fpic -pie -o $@ $(EL0_NS_OBJS) $(FLATLIBS) --script=$(EL0_NS_LOAD)
+
+$(EL0_NS_IMAGE): $(EL0_NS_ELF)
+	$(OBJCOPY) -O binary $< $@
+
+$(EL0_NS_LOAD): tztest.lds.S Makefile ../../platform/$(PLAT)/
+	$(CC) $(CFLAGS) -fpic -pie -E -P -C -o $@ $<
+
+%.o: %.S
+	$(CC) $(CFLAGS) -mcmodel=large -c -nostdlib -o $(notdir $@) $<
+
+all: $(EL0_NS_IMAGE)
+
+clean:
+	$(RM) $(EL0_NS_OBJS) $(EL0_NS_LOAD) $(EL0_NS_ELF) $(EL0_NS_IMAGE) .*.d
diff --git a/aarch64/el0_ns/tztest.c b/aarch64/el0_ns/tztest.c
new file mode 100644
index 0000000..9004475
--- /dev/null
+++ b/aarch64/el0_ns/tztest.c
@@ -0,0 +1,23 @@
+
+#include "libcflat.h"
+#include "arm_builtins.h"
+#include "svc.h"
+
+int y = 0;
+
+int foo()
+{
+    printf("Hello from foo\n");
+
+    return 4;
+}
+
+int main()
+{
+    int x = foo();
+
+    y = x;
+
+    __svc(SVC_EXIT);
+    return y;
+}
diff --git a/aarch64/el0_ns/tztest.lds.S b/aarch64/el0_ns/tztest.lds.S
new file mode 100644
index 0000000..35eeb67
--- /dev/null
+++ b/aarch64/el0_ns/tztest.lds.S
@@ -0,0 +1,22 @@
+OUTPUT_FORMAT("elf64-littleaarch64")
+OUTPUT_ARCH(aarch64)
+TARGET(binary)
+ENTRY(main)
+
+#include "platform.h"
+
+SECTIONS
+{
+    . = 0x0;
+    .text . : {
+        *(.text);
+        _EL0_NS_TEXT_SIZE = SIZEOF(.text);
+    }
+
+    . = ALIGN(4k);
+    .data . : {
+        *(.*data);
+        *(.*bss);
+        _EL0_NS_DATA_SIZE = SIZEOF(.data);
+    }
+}
diff --git a/aarch64/el1_common/el1.c b/aarch64/el1_common/el1.c
index aee6857..ab42040 100644
--- a/aarch64/el1_common/el1.c
+++ b/aarch64/el1_common/el1.c
@@ -8,6 +8,10 @@
 #include "armv8_exception.h"
 #include "armv8_vmsa.h"
 #include "arm_builtins.h"
+#include "elf.h"
+#include "debug.h"
+
+extern void el1_init_el0();
 
 uint64_t el1_next_pa = 0;
 uint64_t el1_allocate_pa() {
@@ -16,6 +20,36 @@ uint64_t el1_allocate_pa() {
     return next;
 }
 
+void el1_map_pa(uintptr_t vaddr, uint64_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);
+    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;
@@ -31,13 +65,15 @@ void el1_map_va(uintptr_t addr)
         if (!(*pte & 0x1)) {
             pa = el1_allocate_pa();
             *pte = pa;
-            *pte = PTE_PAGE;
+            *pte |= PTE_PAGE;
         } else {
             pa = *pte & 0x000FFFFFF000;
         }
     }
 
-    *pte |= PTE_ACCESS;
+    *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)
@@ -53,6 +89,7 @@ int el1_unmap_va(uint64_t addr)
         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 {
@@ -62,36 +99,44 @@ int el1_unmap_va(uint64_t addr)
 
     /* 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_handle_exception(uint64_t ec, uint64_t iss, uint64_t addr)
+void el1_handle_exception(uint64_t ec, uint64_t iss, uint64_t far,
+                          uint64_t elr)
 {
     armv8_data_abort_iss_t dai = {.raw = iss};
 //    armv8_inst_abort_iss_t iai = {.raw = iss};
     switch (ec) {
     case EC_SVC32:
     case EC_SVC64:
-        printf("Took and SVC exception\n");
+        printf("SVC exception: iss = %d\n", iss);
+        switch (iss) {
+        case SVC_EXIT:
+            __smc(SMC_EXIT);
+            break;
+        default:
+            break;
+        }
         break;
     case EC_IABORT_LOWER:
-        printf("Instruction abort at lower level: address = %0lx\n",
-               addr);
+        printf("Instruction abort at lower level: address = %0lx\n", far);
         break;
     case EC_IABORT:
-        printf("Instruction abort at current level (EL3): address = %0lx\n",
-               addr);
-        el1_map_va(addr);
+        printf("Instruction abort at current level: address = %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);
+        el1_map_va(far);
         break;
     case EC_DABORT:
-        printf("Data abort (%s) at current level (EL3): address = %0lx\n",
-               dai.wnr ? "write" : "read", addr);
-        el1_map_va(addr);
+        printf("Data abort (%s) at current level: 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);
@@ -99,28 +144,81 @@ void el1_handle_exception(uint64_t ec, uint64_t iss, uint64_t addr)
     }
 }
 
+/* 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");
+        __svc(SVC_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);
+    } 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;
+            printf("\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)
 {
-    int i = 0;
     uint64_t addr = base;
     size_t len;
 
+    printf("EL1 (%s) started...\n", SECURE_STATE);
+
     /* Unmap the init segement so we don't accidentally use it */
     for (len = 0; len < ((size + 0xFFF) & ~0xFFF);
          len += 0x1000, addr += 0x1000) {
-        if (el1_unmap_va(addr)) {
-            printf("Failed to unmap va 0x%x\n", addr);
-        } else {
-            printf("Unmapped va 0x%x\n", addr);
-        }
+        el1_unmap_va(addr);
     }
 
-    while (1) {
-        printf("%d: Entered %s el1_start\n", i, SECURE_STATE);
-
-        __smc(SMC_YIELD);
-        i++;
-    }
+    el1_init_el0();
 
     return;
 }
diff --git a/aarch64/el1_common/el1_exception.S b/aarch64/el1_common/el1_exception.S
index 87ccbc5..81acf66 100644
--- a/aarch64/el1_common/el1_exception.S
+++ b/aarch64/el1_common/el1_exception.S
@@ -10,6 +10,7 @@ el1_sync_exception_current:
     and x1, x1, x0
     lsr x0, x0, #26
     mrs x2, far_el1
+    mrs x3, elr_el1
     bl el1_handle_exception
     eret
 .align 10                   // Force these vectors to 0x400 alignment
@@ -18,6 +19,8 @@ el1_sync_exception_lower64:
     mov x1, #0xffffff
     and x1, x1, x0
     lsr x0, x0, #26
+    mrs x2, far_el1
+    mrs x3, elr_el1
     bl el1_handle_exception
     eret
 .align 7
diff --git a/aarch64/el1_common/el1_init.S b/aarch64/el1_common/el1_init.S
index ee44d4e..9d07674 100644
--- a/aarch64/el1_common/el1_init.S
+++ b/aarch64/el1_common/el1_init.S
@@ -42,7 +42,7 @@ el1_init_mmu:
 el1_map_init:
     /* Direct map the init code */
     ldr x0, =_EL1_INIT_BASE
-    mov x1, #(PTE_PAGE|PTE_ACCESS|PTE_PRIV_RO)
+    mov x1, #(PTE_PAGE|PTE_ACCESS|PTE_USER_RW)
     ldr x2, =_EL1_INIT_SIZE
     bl map_pa_range
 
@@ -51,12 +51,12 @@ el1_map_flash:
      * the MMU has been enabled.
      */
     ldr x0, =_EL1_FLASH_TEXT
-    mov x1, #(PTE_PAGE|PTE_ACCESS|PTE_PRIV_RO)
+    mov x1, #(PTE_PAGE|PTE_ACCESS|PTE_USER_RW)
     ldr x2, =_EL1_TEXT_SIZE
     bl map_pa_range
 
     ldr x0, =_EL1_FLASH_DATA
-    mov x1, #(PTE_PAGE|PTE_ACCESS|PTE_PRIV_RO)
+    mov x1, #(PTE_PAGE|PTE_ACCESS|PTE_USER_RW)
     ldr x2, =_EL1_DATA_SIZE
     bl map_pa_range
 
@@ -74,12 +74,16 @@ el1_map_data:
     ldr x2, =_EL1_DATA_SIZE
     bl map_va_range
 
-el1_map_stack:
+el1_map_stacks:
     /* Map the first page of the stack so we can get off the ground */
     ldr x0, =EL1_STACK_BASE-0x1000
     mov x1, #(PTE_PAGE|PTE_ACCESS|PTE_PRIV_RW)
     bl map_va
 
+    ldr x0, =EL0_STACK_BASE-0x1000
+    mov x1, #(PTE_PAGE|PTE_ACCESS|PTE_USER_RW)
+    bl map_va
+
 el1_map_pt:
     /* Direct map the page table pool */
     ldr x0, =EL1_PGTBL_BASE
@@ -89,7 +93,7 @@ el1_map_pt:
 
 el1_map_uart:
     ldr x0, =UART0_BASE
-    mov x1, #(PTE_PAGE|PTE_ACCESS|PTE_PRIV_RW)
+    mov x1, #(PTE_PAGE|PTE_ACCESS|PTE_USER_RW)
     bl map_pa
 
 /* Save the next PA pointer until after we have enabled the MMU */
@@ -116,15 +120,16 @@ el1_enable_mmu:
     dsb sy
 
 /* Now that the MMU is enabled and the inital stack page is mapped we can
- * safely set the stack pointer.
+ * safely set the stack pointer.  The stacks are started at on quadword below
+ * the stack base so two pages don't need to be mapped.
  */
 el1_init_stack:
-    ldr x10, =EL1_STACK_BASE
+    ldr x10, =EL1_STACK_BASE-0x10
     mov sp, x10
 
-/* Migrate the next PA to the non-init code */
-    ldr x10, =el1_next_pa
-    str x17, [x10]
+el0_init_stack:
+    ldr x10, =EL0_STACK_BASE-0x10
+    msr sp_el0, x10
 
 /* The EL1 address space is set-up and the MMU is started so it is safe to copy
  * the text and data sections in and unmap the respective flash.
@@ -149,6 +154,10 @@ el1_copy_data:
     ldr x1, =_EL1_DATA_SIZE
     bl unmap_va_range
 
+/* Migrate the next PA to the non-init code */
+    ldr x10, =el1_next_pa
+    str x17, [x10]
+
 el1_init_start:
     /* Pass the address and size of the init section to start so it
      * can unmap it.  We must wait until we are in start because we are still
diff --git a/aarch64/el1_ns/Makefile b/aarch64/el1_ns/Makefile
index 38289e7..9279c03 100644
--- a/aarch64/el1_ns/Makefile
+++ b/aarch64/el1_ns/Makefile
@@ -5,7 +5,8 @@ EL1_NS_IMAGE 	= el1_nsec.bin
 EL1_NS_LOAD		= el1_nsec.lds
 EL1_NS_OBJS		= el1_init.o \
                   el1_exception.o \
-                  el1.o
+                  el1.o \
+                  el1_nsec.o
 
 -include .*.d
 
diff --git a/aarch64/el1_ns/el1_nsec.c b/aarch64/el1_ns/el1_nsec.c
new file mode 100644
index 0000000..de4560a
--- /dev/null
+++ b/aarch64/el1_ns/el1_nsec.c
@@ -0,0 +1,14 @@
+#include "platform.h"
+#include "arm_builtins.h"
+
+extern void *el1_load_el0(char *base, char *start_va);
+
+void el1_init_el0()
+{
+    int (*main)(void);
+
+    main = el1_load_el0((char *)EL0_NS_FLASH_BASE, (char *)EL0_NS_BASE_VA);
+
+    __set_exception_return(main);
+    __exception_return();
+}
diff --git a/aarch64/el1_s/Makefile b/aarch64/el1_s/Makefile
index 0166447..f519be1 100644
--- a/aarch64/el1_s/Makefile
+++ b/aarch64/el1_s/Makefile
@@ -5,7 +5,8 @@ EL1_S_IMAGE 	= el1_sec.bin
 EL1_S_LOAD		= el1_sec.lds
 EL1_S_OBJS		= el1_init.o \
                   el1_exception.o \
-                  el1.o
+                  el1.o \
+                  el1_sec.o
 
 -include .*.d
 
diff --git a/aarch64/el1_s/el1_sec.c b/aarch64/el1_s/el1_sec.c
new file mode 100644
index 0000000..eedb944
--- /dev/null
+++ b/aarch64/el1_s/el1_sec.c
@@ -0,0 +1,9 @@
+#include "platform.h"
+#include "arm_builtins.h"
+#include <stdint.h>
+#include "smc.h"
+
+void el1_init_el0()
+{
+    __smc(SMC_YIELD);
+}
diff --git a/aarch64/el3/el3.c b/aarch64/el3/el3.c
index e96b85d..9f0d061 100644
--- a/aarch64/el3/el3.c
+++ b/aarch64/el3/el3.c
@@ -23,6 +23,17 @@ void el3_dispatch(tztest_smc_desc_t *desc)
     DEBUG_MSG("Exiting\n");
 }
 
+
+void el3_shutdown() {
+    uintptr_t *sysreg_cfgctrl = (uintptr_t *)(SYSREG_BASE + SYSREG_CFGCTRL);
+
+    printf("Shutting down\n");
+
+    *sysreg_cfgctrl = SYS_SHUTDOWN;
+
+    while(1);   /* Shutdown does not work on all machines */
+}
+
 uint64_t el3_next_pa = 0;
 uint64_t el3_allocate_pa()
 {
@@ -100,6 +111,9 @@ void el3_handle_exception(uint64_t ec, uint64_t iss, uint64_t addr)
         case SMC_NOOP:
             DEBUG_MSG("took an SMC(SMC_NOOP) exception\n");
             break;
+        case SMC_EXIT:
+            el3_shutdown();
+            break;
         default:
             printf("Unrecognized AArch64 SMC opcode: iss = %d\n", iss);
         }
@@ -128,11 +142,7 @@ 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 + 0xFFF) & ~0xFFF);
          len += 0x1000, addr += 0x1000) {
-        if (el3_unmap_va(addr)) {
-            printf("Failed to unmap va 0x%x\n", addr);
-        } else {
-            printf("Unmapped va 0x%x\n", addr);
-        }
+        el3_unmap_va(addr);
     }
 
     /* Clear out our secure and non-secure state buffers */
diff --git a/aarch64/el3/el3_init.S b/aarch64/el3/el3_init.S
index 06ae259..6b5a619 100644
--- a/aarch64/el3/el3_init.S
+++ b/aarch64/el3/el3_init.S
@@ -103,6 +103,11 @@ el3_map_uart:
     mov x1, #(PTE_PAGE|PTE_ACCESS|PTE_PRIV_RW)
     bl map_pa
 
+el3_map_sysreg:
+    ldr x0, =SYSREG_BASE;
+    mov x1, #(PTE_PAGE|PTE_ACCESS|PTE_PRIV_RW)
+    bl map_pa
+
 save_last_pa:
     ldr x17, =RAM_BASE+0x2000
     ldr x17, [x17]
diff --git a/configure b/configure
index c325967..c4ff9e1 100755
--- a/configure
+++ b/configure
@@ -77,9 +77,9 @@ fi
 
 if [ -z "$cross" ]; then
     if [ "$arch" = "arm" ]; then
-        cross="arm-linux-gnueabi-"
+        cross="arm-linux-gnueabi"
     elif [ "$arch" = "aarch64" ]; then
-        cross="aarch64-linux-gnu-"
+        cross="aarch64-linux-gnu"
     fi
 fi
 
@@ -92,9 +92,10 @@ cat <<EOF > config.mak
 export ARCH=$arch
 export PLAT=$plat
 export PROCESSOR=$cpu
-export CC=$cross$cc
-export LD=$cross$ld
-export OBJCOPY=$cross$objcopy
-export AR=$cross$ar
+export CROSS=$cross
+export CC=$cross-$cc
+export LD=$cross-$ld
+export OBJCOPY=$cross-$objcopy
+export AR=$cross-$ar
 export CFLAGS+=$cflags
 EOF
diff --git a/platform/virt/platform.h b/platform/virt/platform.h
index 827605b..97b90a4 100644
--- a/platform/virt/platform.h
+++ b/platform/virt/platform.h
@@ -10,18 +10,22 @@
 #define EL3_FLASH_BASE FLASH_BASE
 #define EL1_S_FLASH_BASE (FLASH_BASE+0x10000)
 #define EL1_NS_FLASH_BASE (FLASH_BASE+0x20000)
+#define EL0_NS_FLASH_BASE (FLASH_BASE+0x30000)
 
 #define RAM_BASE 0x40000000                 // From QEMU virt.c
 #define RAM_SIZE (2*1024*1024*1024)         // Only 2GB needed
 #define EL3_BASE_VA 0xF000000000
 #define EL1_S_BASE_VA 0xC000000000
 #define EL1_NS_BASE_VA 0x80000000
+#define EL0_NS_BASE_VA 0x00010000
 #define EL3_RAM_BASE RAM_BASE
 #define EL3_RAM_SIZE (512*1024)
 #define EL1_S_RAM_BASE (RAM_BASE+0x1000000)
 #define EL1_S_RAM_SIZE (512*1024)
-#define EL1_NS_RAM_BASE (RAM_BASE+0x4000000)
+#define EL1_NS_RAM_BASE (RAM_BASE+0x2000000)
 #define EL1_NS_RAM_SIZE (512*1024)
+#define EL0_NS_RAM_BASE (RAM_BASE+0x4000000)
+#define EL0_NS_RAM_SIZE (512*1024)
 #define VA_SIZE 48
 #define TnSZ (64-VA_SIZE)
 
@@ -37,6 +41,8 @@
 #define EL1_NS_PGTBL_BASE EL1_NS_RAM_BASE+EL1_NS_RAM_SIZE-EL1_NS_PGTBL_SIZE
 #define EL1_NS_STACK_SIZE 0x40000
 #define EL1_NS_STACK_BASE 0xFFFFF000
+#define EL0_STACK_SIZE 0x40000
+#define EL0_STACK_BASE 0x80000000-0x1000
 
 #define UART0_BASE 0x09000000