Enable EL3 MMU

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

15 files changed, 491 insertions, 248 deletions

diff --git a/.gdbinit64 b/.gdbinit64
index 112ea62..4240602 100644
--- a/.gdbinit64
+++ b/.gdbinit64
@@ -1,6 +1,6 @@
 set arch aarch64
 target remote :1234
 file aarch64/tzboot.elf
-add-symbol-file aarch64/tzboot.elf 0x40000000
+add-symbol-file aarch64/tzboot.elf &_EL3_TEXT_BASE
 add-symbol-file aarch64/tztest_secure.elf 0x41000000
 set print pretty
diff --git a/.gitignore b/.gitignore
index e7f2e46..21bb414 100644
--- a/.gitignore
+++ b/.gitignore
@@ -6,5 +6,8 @@
 *.img
 cscope.*
 /config.mak
+arm/tzboot.lds
 arm/tztest_secure.lds
 arm/tztest_nonsecure.lds
+aarch64/tzboot.lds
+aarch64/tztest_secure.lds
diff --git a/TZTEST_MEMLAYOUT.txt b/TZTEST_MEMLAYOUT.txt
new file mode 100644
index 0000000..efabc1e
--- /dev/null
+++ b/TZTEST_MEMLAYOUT.txt
@@ -0,0 +1,28 @@
+                    FLASH
+   0x00000000 ┏━━━━━━━━━━━━━━━━┓
+              ┃    el3_init    ┃
+              ┠┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┨
+tzboot.img    ┃  monitor text  ┃
+              ┠┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┨
+              ┃  monitor data  ┃
+   0x00010000 ┠────────────────┨
+              ┃  secure_init   ┃
+              ┠┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┨
+              ┃  sec el1 text  ┃
+              ┠┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┨
+tzsec.img     ┃  sec el1 data  ┃
+              ┠┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┨
+              ┃  sec el0 text  ┃
+              ┠┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┨
+              ┃  sec el0 data  ┃
+   0x00020000 ┠────────────────┨
+              ┃  nsecure_init  ┃
+              ┠┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┨
+              ┃  nsec el1 text ┃
+              ┠┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┨
+tznsec.img    ┃  nsec el1 data ┃
+              ┠┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┨
+              ┃  nsec el0 text ┃
+              ┠┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┄┨
+              ┃  nsec el0 data ┃
+              ┗━━━━━━━━━━━━━━━━┛
diff --git a/aarch64/Makefile b/aarch64/Makefile
index b66cb4d..1b2a979 100644
--- a/aarch64/Makefile
+++ b/aarch64/Makefile
@@ -3,8 +3,9 @@ SECTESTIMAGE 	= tztest_secure.flat
 TZTEST_IMAGE 	= tztest.img
 TZBOOT			= tzboot.o \
                   el3_init.o \
+                  el3_exception.o \
                   monitor_asm.o \
-                  monitor.o
+                  el3.o
 TZBOOTELF		= tzboot.elf
 TZBOOTIMAGE		= tzboot.flat
 TZSECLOAD		= tztest_secure.lds
@@ -45,7 +46,7 @@ $(TZSECLOAD): tztest_secure.lds.S Makefile ../platform/$(PLAT)/
 	$(CC) $(CFLAGS) -E -P -C -o $@ $<
 
 %.o: %.S
-	$(CC) $(CFLAGS) -c -nostdlib -o $@ $<
+	$(CC) $(CFLAGS) -mcmodel=large -c -nostdlib -o $@ $<
 
 all: $(TZTEST_IMAGE)
 
diff --git a/aarch64/el3.c b/aarch64/el3.c
new file mode 100644
index 0000000..73fb214
--- /dev/null
+++ b/aarch64/el3.c
@@ -0,0 +1,155 @@
+#include "platform.h"
+#include "common_svc.h"
+#include "common_defs.h"
+#include "common_mmu.h"
+#include "el3_loader.h"
+#include "string.h"
+
+typedef struct state_buf {
+    uintptr_t elr_el3;
+    uintptr_t spsr_el3;
+    uintptr_t sp_el0;
+    uintptr_t sp_el1;
+    uintptr_t elr_el1;
+    uintptr_t esr_el1;
+    uintptr_t x[27];        /* X4:X30 */
+} state_buf;
+
+state_buf sec_state;
+state_buf nsec_state;
+
+extern void monitor_switch(void);
+extern void monitor_save_state(state_buf *);
+extern void monitor_restore_state(state_buf *);
+
+void el3_dispatch(tztest_smc_desc_t *desc)
+{
+    uint32_t (*func)(uint32_t) = desc->dispatch.func;
+    DEBUG_MSG("Entered\n");
+    desc->dispatch.ret = func(desc->dispatch.arg);
+    DEBUG_MSG("Exiting\n");
+}
+
+typedef union {
+    struct {
+        uint32_t dfsc : 6;
+        uint32_t wnr : 1;
+        uint32_t s1ptw : 1;
+        uint32_t cm : 1;
+        uint32_t ea : 1;
+        uint32_t fnv  : 1;
+        uint32_t res0 : 3;
+        uint32_t ar : 1;
+        uint32_t sf : 1;
+        uint32_t srt : 5;
+        uint32_t sse: 1;
+        uint32_t sas: 2;
+        uint32_t isv: 1;
+    };
+    uint32_t raw;
+} armv8_data_abort_iss_t;
+
+typedef union {
+    struct {
+        uint64_t type : 2;
+        uint64_t attridx : 3;
+        uint64_t ns : 1;
+        uint64_t ap2_1 : 2;
+        uint64_t sh1_0 : 2;
+        uint64_t af : 1;
+        uint64_t ng : 1;
+        uint64_t pa : 36;
+        uint64_t res0 : 4;
+        uint64_t contig : 1;
+        uint64_t pxn : 1;
+        uint64_t xn : 1;
+    };
+    uint64_t raw;
+} armv8_4k_pg_pte_t;
+
+typedef union {
+    struct {
+        uint64_t type : 2;
+        uint64_t ignored0 : 10;
+        uint64_t pa : 36;
+        uint64_t res0 : 4;
+        uint64_t ignored1 : 7;
+        uint64_t pxn : 1;
+        uint64_t xn : 1;
+        uint64_t ap : 2;
+        uint64_t ns : 1;
+    };
+    uint64_t raw;
+} armv8_4k_tbl_pte_t;
+
+uint64_t el3_next_pa = 0;
+uint64_t el3_allocate_pa() {
+    uint64_t next = el3_next_pa;
+    el3_next_pa += 0x1000;
+    return next;
+}
+
+void el3_map_va(uintptr_t addr)
+{
+    uint64_t pa = EL3_PGTBL_BASE;
+    uint32_t i;
+    armv8_4k_tbl_pte_t *pte;
+    armv8_4k_pg_pte_t *l3pte;
+
+    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 = (armv8_4k_tbl_pte_t *)(pa | off);
+        if (!pte->type) {
+            pa = el3_allocate_pa();
+            pte->pa = pa >> 12;
+            pte->type = 3;
+        } else {
+            pa = pte->pa << 12;
+        }
+    }
+
+    l3pte = (armv8_4k_pg_pte_t *)pte;
+    l3pte->af = 1;
+}
+
+void el3_handle_exception(uint64_t ec, uint64_t iss, uint64_t addr)
+{
+    armv8_data_abort_iss_t dai = {.raw = iss};
+    switch (ec) {
+    case 0x17:      /* SMC from aarch64 */
+        switch (iss) {
+        case SMC_YIELD:
+            DEBUG_MSG("took an SMC(SMC_YIELD) exception\n");
+            monitor_switch();
+            break;
+        case SMC_DISPATCH_MONITOR:
+            DEBUG_MSG("took an SMC(SMC_DSPATCH_MONITOR) exception\n");
+            el3_dispatch(NULL);
+            break;
+        case SMC_NOOP:
+            DEBUG_MSG("took an SMC(SMC_NOOP) exception\n");
+            break;
+        default:
+            printf("Unrecognized AArch64 SMC opcode: iss = %d\n", iss);
+        }
+        break;
+    case 0x24:
+        printf("Data abort (%s) at lower level: address = %0lx\n",
+               dai.wnr ? "write" : "read", addr);
+        break;
+    case 0x25:
+        printf("Data abort (%s) at current level (EL3): address = %0lx\n",
+               dai.wnr ? "write" : "read", addr);
+        el3_map_va(addr);
+        break;
+
+    default:
+        printf("Unhandled EL3 exception: EC = %d  ISS = %d\n", ec, iss);
+        break;
+    }
+}
+
+
diff --git a/aarch64/el3_exception.S b/aarch64/el3_exception.S
new file mode 100644
index 0000000..2ee09cf
--- /dev/null
+++ b/aarch64/el3_exception.S
@@ -0,0 +1,33 @@
+.align 11                   // Align to vector table size (0x800)
+.section .vectors
+.globl el3_vectors
+el3_vectors:
+.word 0                     // Add padding to force the below alignment
+.align 9                    // Force these vectors to 0x400 alignment
+el3_sync_exception_current:
+    mrs x0, esr_el3
+    mov x1, #0xffffff
+    and x1, x1, x0
+    lsr x0, x0, #26
+    mrs x2, far_el3
+    bl el3_handle_exception
+    eret
+.align 10                   // Force these vectors to 0x400 alignment
+el3_sync_exception_lower64:
+    mrs x0, esr_el3
+    mov x1, #0xffffff
+    and x1, x1, x0
+    lsr x0, x0, #26
+    bl el3_handle_exception
+    eret
+.align 7
+el3_serr_exception:
+	b	el3_serr_exception
+.align 7
+el3_irq_exception:
+	b	el3_irq_exception
+.align 7
+el3_fiq_exception:
+	b	el3_fiq_exception
+
+.end
diff --git a/aarch64/el3_init.S b/aarch64/el3_init.S
index c37c82b..4dd1c54 100644
--- a/aarch64/el3_init.S
+++ b/aarch64/el3_init.S
@@ -1,57 +1,28 @@
 #include "common_defs.h"
 
-.align 11                   // Align to vector table size (0x800)
-.section .vectors
-el3_vectors:
-.word 0                     // Add padding to force the below alignment
-.align 10                   // Force these vectors to 0x400 alignment
-el3_sync_exception:
-    mrs x0, esr_el3
-    mov x1, #0xffffff
-    and x1, x1, x0
-    lsr x0, x0, #26
-    bl el3_handle_exception
-    eret
-.align 7
-el3_serr_exception:
-	b	el3_serr_exception
-.align 7
-el3_irq_exception:
-	b	el3_irq_exception
-.align 7
-el3_fiq_exception:
-	b	el3_fiq_exception
-
-monitor_smc_vect:
-//    cmp x0, #SMC_YIELD
-//    bge sm_smc_entry
-    cmp x0, #SMC_NOOP
-    beq 1f
-	stp x2, x3, [sp, #-16]!
-	stp x1, x0, [sp, #-16]!
-    mov x0, x1
-    bl monitor_dispatch
-	ldp x0, x1, [sp], #16
-	ldp x2, x3, [sp], #16
-1:  eret
+#define PT_BASE EL3_PGTBL_BASE
+
+#include "init_util.S"
 
 .align 12
-.section .text
+.section .init
 .global el3_init
 /* el3_init(sec_elr, nsec_elr) */
 el3_init:
+    /* Save the input args for later */
+    mov x15, x0
+    mov x16, x1
+
     /* Set-up the EL3 vbar */
     ldr x10, =el3_vectors
     msr vbar_el3, x10
 
-    /* Set-up the monitor stack */
-    ldr x10, =EL3_STACK_BASE
+    /* 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
 	mov sp, x10
 
-    /* Save the incoming LR so we can return to the right place */
-    str x30, [sp, #-8]!
-    stp x0, x1, [sp, #-16]!
-
     /* Enable floating point register usage as printf uses it */
     mrs x10, cptr_el3
     and x10, x10, #~0x400       /* CPTR.TFP */
@@ -64,36 +35,122 @@ el3_init_mmu:
     bic x10, x10, #0x1000
     msr sctlr_el3, x10
 
-    bl el3_pagetable_init
+    ldr x10, =PT_BASE           /* Base of L0 page table */
+    lsr x11, x10, #TnSZ         /* Shift the invalid bits out */
+    lsl x11, x11, #TnSZ
+    msr ttbr0_el3, x11
+    mov x11, #TnSZ
+    msr tcr_el3, x11            /* PS = 32bit, TG0 = 4k, TnSZ */
+
+    /* Use the top of the stack to track our PA pool pointer */
+    ldr x10, =PT_BASE+0x1000
+    str x10, [sp]
 
-    /* Set TTBR0 to the initialized address plus enable shareable write-back
-     * write-allocate.
+el3_map_init:
+    /* Direct map the init code */
+    ldr x0, =_EL3_INIT_BASE
+    ldr x1, =_EL3_INIT_BASE
+    ldr x2, =_EL3_INIT_SIZE
+    bl map_va_to_pa_range
+
+el3_map_flash:
+    /* Direct map the EL3 flash sections so we can copy from them once
+     * the MMU has been enabled.
      */
-    ldr x10, =EL3_PGTBL_BASE
-    msr ttbr0_el3, x10
+    ldr x0, =_EL3_FLASH_TEXT
+    ldr x1, =_EL3_FLASH_TEXT
+    ldr x2, =_EL3_TEXT_SIZE
+    bl map_va_to_pa_range
+
+    ldr x0, =_EL3_FLASH_DATA
+    ldr x1, =_EL3_FLASH_DATA
+    ldr x2, =_EL3_DATA_SIZE
+    bl map_va_to_pa_range
+
+el3_map_text:
+    /* Map the EL3 text address range */
+    ldr x0, =_EL3_TEXT_BASE
+    ldr x1, =_EL3_TEXT_SIZE
+    bl map_va_range
+
+el3_map_data:
+    /* Map the EL3 data address range */
+    ldr x0, =_EL3_DATA_BASE
+    ldr x1, =_EL3_DATA_SIZE
+    bl map_va_range
 
+el3_map_stack:
+    /* Map the first page of the stack so we can get off the ground */
+    ldr x0, =EL3_STACK_BASE-0x1000
+    bl map_va
+
+el3_map_pt:
+    /* Direct map the page table pool */
+    ldr x0, =EL3_PGTBL_BASE
+    ldr x1, =EL3_PGTBL_BASE
+    ldr x2, =EL3_PGTBL_SIZE
+    bl map_va_to_pa_range
+
+el3_map_uart:
+    ldr x0, =UART0_BASE
+    ldr x1, =UART0_BASE
+    bl map_va_to_pa
+
+save_last_pa:
+    ldr x17, =RAM_BASE+0x2000
+    ldr x17, [x17]
+
+/* We should have all the critical address regions mapped at this point.
+ * Anything that has not already been mapped will be handled on demand.  If it
+ * needs to be copied out of flash, it needs to be mapped.
+ * The mapped regions are:
+ *  - EL3 init code - So we can keep executing after the MMU is enabled
+ *  - EL3 flash region - So we can copy the code and data
+ *  - EL3 code and text segments - Need to be copied from flash
+ *  - EL3 initial stack page - Needed by the exception handler
+ *  - EL3 PT PA pages - Needed so we don't fault on demand paging
+ *  - UART - So we can use it to print
+ */
+el3_enable_mmu:
     mrs x10, sctlr_el3
     orr x10, x10, #0x1      // Enable MMU
     msr sctlr_el3, x10
     isb
     dsb sy
 
+/* The EL3 address space is set-up and the MMU is started so it is safe to copy
+ * the text and data sections in.
+ */
+el3_copy_text:
+    ldr x0, =_EL3_TEXT_BASE
+    ldr x1, =_EL3_FLASH_TEXT
+    ldr x2, =_EL3_TEXT_SIZE
+    bl memcpy
+
+el3_copy_data:
+    ldr x0, =_EL3_DATA_BASE
+    ldr x1, =_EL3_FLASH_DATA
+    ldr x2, =_EL3_DATA_SIZE
+    bl memcpy
+
+/* Now that the MMU is enabled and the inital stack page is mapped we can
+ * safely set the stack pointer.
+ */
+el3_init_stack:
+    mov sp, EL3_STACK_BASE
+
+/* Migrate the next PA to the non-init code */
+    ldr x10, =el3_next_pa
+    str x16, [x10]
+
 el3_init_monitor:
-    ldp x0, x1, [sp], #16
-    mov x4, #5
-    adr x2, sec_state
-    stp x0, x4, [x2], #16        /* S.ELR_EL3 = sec_base */
-    adr x3, nsec_state
-    stp x1, x4, [x3], #16        /* NS.ELR_EL3 = nsec_base */
-    mov x0, #0
-    mov x1, #31
-zero_loop:
-    sub x1, x1, #1
-    str x0, [x2], #8
-    str x0, [x3], #8
-    cbnz x1, zero_loop
-    adr x0, sec_state
-    bl monitor_restore_state
-    eret
+    mov x0, x15
+    mov x1, x16
+    ldr x2, =monitor_init
+    br x2
+
+/* We should never get here */
+el3_init_end:
+    b el3_init_end
 
 .end
diff --git a/aarch64/el3_loader.h b/aarch64/el3_loader.h
index 8e7dbf2..6f5828e 100644
--- a/aarch64/el3_loader.h
+++ b/aarch64/el3_loader.h
@@ -1,10 +1,10 @@
 #ifndef _EL3_LOADER_H
 #define _EL3_LOADER_H
 
-extern uintptr_t _EL3_RAM_TEXT;
-uintptr_t EL3_RAM_TEXT = (uintptr_t)&_EL3_RAM_TEXT;
-extern uintptr_t _EL3_RAM_DATA;
-uintptr_t EL3_RAM_DATA = (uintptr_t)&_EL3_RAM_DATA;
+extern uintptr_t _EL3_TEXT_BASE;
+uintptr_t EL3_TEXT_BASE = (uintptr_t)&_EL3_TEXT_BASE;
+extern uintptr_t _EL3_DATA_BASE;
+uintptr_t EL3_DATA_BASE = (uintptr_t)&_EL3_DATA_BASE;
 extern uintptr_t _EL3_TEXT_SIZE;
 uint64_t EL3_TEXT_SIZE = (uint64_t)&_EL3_TEXT_SIZE;
 extern uintptr_t _EL3_DATA_SIZE;
diff --git a/aarch64/init_util.S b/aarch64/init_util.S
new file mode 100644
index 0000000..8bc4e50
--- /dev/null
+++ b/aarch64/init_util.S
@@ -0,0 +1,108 @@
+
+.section .init
+/* allocate_pa() - Allocates and returns next pool PA */
+allocate_pa:
+    stp x10, x11, [sp, #-16]!
+    ldr x10, =RAM_BASE+0x2000
+    ldr x0, [x10]
+    add x11, x0, #0x1000
+    str x11, [x10]
+    ldp x10, x11, [sp], #16
+    ret
+
+.globl map_va_to_pa
+/* map_va_to_pa(VA, PA) */
+map_va_to_pa:
+    stp x30, x10, [sp, #-16]!
+    stp x11, x12, [sp, #-16]!
+    stp x13, x14, [sp, #-16]!
+    ldr x12, =PT_BASE
+    mov x13, #0x4
+    mov x14, #39
+map_loop:
+    and x11, x12, #~0xFFF           /* Strip off descriptor non-address bits */
+    lsr x10, x0, x14                /* Shift out VA bits for the level */
+    sub x14, x14, #9                /* Update shift amount for next level */
+    and x10, x10, #0x1FF            /* Filter top VA bits for PT offset */
+    lsl x10, x10, #3                /* Shift PT offset to bytes */
+    orr x10, x10, x11               /* Compute descriptor address */
+    sub x13, x13, #1                /* Decrease level */
+    cbz x13, map_done               /* If we reached level 0 then finalize */
+    ldr x12, [x10]                  /* Otherwise, fetch the descriptor */
+    and x11, x12, #0x1              /* Filter valid bit */
+    cbnz x11, map_loop              /* If the descriptor is valid then next */
+    mov x11, x0                     /* Save VA across call */
+    bl allocate_pa                  /* Allocate a PT phys page */
+    mov x12, x0                     /* Got a PA */
+    mov x0, x11                     /* Restore VA */
+    orr x12, x12, #0x3              /* This is a table entry */
+    str x12, [x10]                  /* Fill in PT entry */
+    b map_loop                      /* Next level */
+map_done:
+    mov x12, #0x403                 /* Last level entry is a page */
+    orr x12, x12, x1                /* Create PTE for target PA */
+    str x12, [x10]                  /* Fill in PT entry */
+    ldp x13, x14, [sp], #16
+    ldp x11, x12, [sp], #16
+    ldp x30, x10, [sp], #16
+    ret
+
+.globl map_va
+/* map_va(VA) */
+map_va:
+    str x30, [sp, #-8]!
+    stp x1, x10, [sp, #-16]!
+    mov x10, x0
+    bl allocate_pa
+    mov x1, x0
+    mov x0, x10
+    bl map_va_to_pa
+    ldp x1, x10, [sp], #16
+    ldr x30, [sp], #8
+    ret
+
+.globl map_va_to_pa_range
+/* map_va_to_pa_range(VA, PA, len) */
+map_va_to_pa_range:
+    stp x30, x2, [sp, #-16]!
+    stp x0, x1, [sp, #-16]!
+    add x2, x2, #0xFFF
+    and x2, x2, #~0xFFF
+map_va_to_pa_loop:
+    cbz x2, map_va_to_pa_done
+    bl map_va_to_pa
+    add x0, x0, #0x1000
+    add x1, x1, #0x1000
+    sub x2, x2, #0x1000
+    b map_va_to_pa_loop
+map_va_to_pa_done:
+    ldp x0, x1, [sp], #16
+    ldp x30, x2, [sp], #16
+    ret
+
+/* map_va_range(VA, len) */
+map_va_range:
+    str x30, [sp, #-8]!
+    stp x0, x1, [sp, #-16]!
+    add x1, x1, #0xFFF
+    and x1, x1, #~0xFFF
+map_va_loop:
+    cbz x1, map_va_done
+    bl map_va
+    add x0, x0, #0x1000
+    sub x1, x1, #0x1000
+    b map_va_loop
+map_va_done:
+    ldp x0, x1, [sp], #16
+    ldr x30, [sp], #8
+    ret
+
+/* memcpy(dest, src) */
+memcpy:
+    cbz x2, memcpy_done
+    ldr x10, [x1], #8
+    str x10, [x0], #8
+    subs x2, x2, #8
+    b memcpy
+memcpy_done:
+    ret
diff --git a/aarch64/monitor.c b/aarch64/monitor.c
deleted file mode 100644
index cde5e0a..0000000
--- a/aarch64/monitor.c
+++ /dev/null
@@ -1,95 +0,0 @@
-#include "platform.h"
-#include "common_svc.h"
-#include "common_defs.h"
-#include "common_mmu.h"
-#include "el3_loader.h"
-#include "string.h"
-
-typedef struct state_buf {
-    uintptr_t elr_el3;
-    uintptr_t spsr_el3;
-    uintptr_t sp_el0;
-    uintptr_t sp_el1;
-    uintptr_t elr_el1;
-    uintptr_t esr_el1;
-    uintptr_t x[27];        /* X4:X30 */
-} state_buf;
-
-state_buf sec_state;
-state_buf nsec_state;
-
-extern void monitor_switch(void);
-extern void monitor_save_state(state_buf *);
-extern void monitor_restore_state(state_buf *);
-
-void monitor_dispatch(tztest_smc_desc_t *desc)
-{
-    uint32_t (*func)(uint32_t) = desc->dispatch.func;
-    DEBUG_MSG("Entered\n");
-    desc->dispatch.ret = func(desc->dispatch.arg);
-    DEBUG_MSG("Exiting\n");
-}
-
-void el3_handle_exception(uint64_t ec, uint64_t iss)
-{
-    switch (ec) {
-    case 0x17:      /* SMC from aarch64 */
-        switch (iss) {
-        case SMC_YIELD:
-            DEBUG_MSG("took an SMC(SMC_YIELD) exception\n");
-            monitor_switch();
-            break;
-        case SMC_DISPATCH_MONITOR:
-            DEBUG_MSG("took an SMC(SMC_DSPATCH_MONITOR) exception\n");
-            monitor_dispatch(NULL);
-            break;
-        case SMC_NOOP:
-            DEBUG_MSG("took an SMC(SMC_NOOP) exception\n");
-            break;
-        default:
-            printf("Unrecognized AArch64 SMC opcode: iss = %d\n", iss);
-        }
-        break;
-    default:
-        printf("Unhandled EL3 exception: EC = %d  ISS = %d\n", ec, iss);
-        break;
-    }
-}
-
-void el3_pagetable_init()
-{
-    uint64_t el3_l1_page_table = EL3_PGTBL_BASE;
-
-    pagetable_map_entry_t el3_pagetable_entries[] = {
-        {.va = EL3_RAM_TEXT, .pa = EL3_RAM_TEXT,
-         .type = PAGE, .len = EL3_TEXT_SIZE,
-         .attr = SHARED | NOTGLOBAL | WBA_CACHED | P1_R | P1_X | P0_R | P0_X },
-        {.va = EL3_RAM_DATA, .pa = EL3_RAM_DATA,
-         .type = PAGE, .len = EL3_DATA_SIZE,
-         .attr = SHARED | NOTGLOBAL | WBA_CACHED | P1_R | P1_W | P0_R | P0_W },
-        {.va = (uintptr_t)EL3_STACK_BASE, .pa = (uintptr_t)EL3_STACK_BASE,
-         .type = PAGE, .len = EL3_STACK_SIZE,
-         .attr = SHARED | NOTGLOBAL | WBA_CACHED | P1_R | P1_W | P0_R | P0_W },
-/*
-        {.va = (uintptr_t)0xFFFF0000, .pa = (uintptr_t)ram_secvecs_start,
-         .type = PAGE, .len = secvecs_size,
-         .attr = SHARED | NOTGLOBAL | WBA_CACHED | P1_R | P1_X | P0_R | P0_X }
-        {.va = (uintptr_t)ram_sectext_start, .pa = (uintptr_t)ram_sectext_start,
-         .type = PAGE, .len = sectext_size,
-         .attr = SHARED | NOTGLOBAL | WBA_CACHED | P1_R | P1_X | P0_R | P0_X },
-        {.va = (uintptr_t)ram_secdata_start, .pa = (uintptr_t)ram_secdata_start,
-         .type = PAGE, .len = secdata_size,
-         .attr = SHARED | NOTGLOBAL | WBA_CACHED | P1_R | P1_W | P0_R | P0_W },
-        {.va = (uintptr_t)secstack_start, .pa = (uintptr_t)secstack_start,
-         .type = PAGE, .len = secstack_size,
-         .attr = SHARED | NOTGLOBAL | WBA_CACHED | P1_R | P1_W | P0_R | P0_W },
-        {.va = (uintptr_t)sec_l1_page_table, .pa = (uintptr_t)sec_l1_page_table,
-         .type = SECTION, .len = 16*1024*1024,
-         .attr = SHARED | NOTGLOBAL | WBA_CACHED | P1_R | P1_W },
-*/
-    };
-
-    pagetable_init((void *)el3_l1_page_table);
-
-    PT_ADD_ENTRIES((void *)el3_l1_page_table, el3_pagetable_entries);
-}
diff --git a/aarch64/monitor_asm.S b/aarch64/monitor_asm.S
index f15599d..a2a70c2 100644
--- a/aarch64/monitor_asm.S
+++ b/aarch64/monitor_asm.S
@@ -1,6 +1,24 @@
 #include "common_defs.h"
 
 .section .text
+.globl monitor_init
+monitor_init:
+    mov x4, #5
+    adr x2, sec_state
+    stp x0, x4, [x2], #16        /* S.ELR_EL3 = sec_base */
+    adr x3, nsec_state
+    stp x1, x4, [x3], #16        /* NS.ELR_EL3 = nsec_base */
+    mov x0, #0
+    mov x1, #31
+zero_loop:
+    sub x1, x1, #1
+    str x0, [x2], #8
+    str x0, [x3], #8
+    cbnz x1, zero_loop
+    adr x0, sec_state
+    bl monitor_restore_state
+    eret
+
 .globl monitor_save_state
 monitor_save_state:
     stp x0, x1, [sp, #-16]!     /* Save the input regs x0:x3 */
diff --git a/aarch64/tzboot.S b/aarch64/tzboot.S
index 01a34e8..9878979 100644
--- a/aarch64/tzboot.S
+++ b/aarch64/tzboot.S
@@ -1,6 +1,6 @@
 #include "platform.h"
 
-.align 5
+.align 12
 .section .init
 /* We use the same vector table for Hyp and Monitor mode, since
  * we will only use each once and they don't overlap.
@@ -35,22 +35,6 @@ init_uart:
 	orr	x1, x1, #0x0001	    // cr
 	str	x1, [x0, #0x30]
 
-    /* Start by setting up a temporary stack */
-    ldr x11, =(RAM_BASE+RAM_SIZE)
-	mov sp, x11
-
-    /* Copy the text section to RAM */
-    ldr x0, =_EL3_RAM_TEXT
-    ldr x1, =_EL3_FLASH_TEXT
-    ldr x2, =_EL3_TEXT_SIZE
-    bl memcpy
-
-    /* Copy the data section to RAM */
-    ldr x0, =_EL3_RAM_DATA
-    ldr x1, =_EL3_FLASH_DATA
-    ldr x2, =_EL3_DATA_SIZE
-    bl memcpy
-
     /* Branch to initialize EL3 mode. We pass the secure and non-secure EL1
      * entry points as it sets up the initial exception return to these modes
      * We don't come back from the branch to EL3 as it initiates the transition
@@ -59,18 +43,11 @@ init_uart:
      */
     mov x0, SEC_FLASH_BASE
     mov x1, NSEC_FLASH_BASE
-    ldr x11, =_EL3_RAM_TEXT
-    blr x11
+    ldr x11, =el3_init
+    br x11
 
     /* We shouldn't get here as the secure image will shutdown */
 end:
     b      end
 
-memcpy:
-    cbz x2, memcpy_done
-    ldrb w10, [x1], #1
-    strb w10, [x0], #1
-    subs x2, x2, #1
-    b memcpy
-memcpy_done:
-    ret
+.end
diff --git a/aarch64/tzboot.lds b/aarch64/tzboot.lds
deleted file mode 100644
index ed8e289..0000000
--- a/aarch64/tzboot.lds
+++ /dev/null
@@ -1,40 +0,0 @@
-/*
- * model.lds.S - simple linker script for stand-alone Linux booting
- *
- * Copyright (C) 2011 ARM Limited. All rights reserved.
- *
- * Use of this source code is governed by a BSD-style license that can be
- * found in the LICENSE.txt file.
- */
-OUTPUT_FORMAT("elf64-littleaarch64")
-OUTPUT_ARCH(aarch64)
-TARGET(binary)
-/* NORFLASH0 on Vexpress aseries (a15) is mapped from 0x08000000 to 0x0A000000
- * which is 32MB.  It is also aliased to 0x0 (to 0x2000000).
- * It is acceptable to have the text here as it is RO.
- */
-/* Taken from Linux vexpress support */
-SECTIONS
-{
-    . = 0x00000000;
-    .init . : {
-        tzboot.o(.init);
-    }
-    . = ALIGN(16);
-    _EL3_FLASH_TEXT = .;
-    . = 0x40000000;
-    .text . : AT(_EL3_FLASH_TEXT) {
-        _EL3_RAM_TEXT = .;
-        *(.text);
-        *(.vectors);
-    }
-    _EL3_TEXT_SIZE = SIZEOF(.text);
-    . = ALIGN(0x1000);
-    .data . : {
-        _EL3_RAM_DATA = .;
-        _EL3_FLASH_DATA = LOADADDR(.data);
-        *(.*data);
-        *(.*bss);
-    }
-    _EL3_DATA_SIZE = SIZEOF(.data);
-}
diff --git a/aarch64/tzboot.lds.S b/aarch64/tzboot.lds.S
index 56c0f7b..4c35327 100644
--- a/aarch64/tzboot.lds.S
+++ b/aarch64/tzboot.lds.S
@@ -1,12 +1,3 @@
-/*
- * model.lds.S - simple linker script for stand-alone Linux booting
- *
- * Copyright (C) 2011 ARM Limited. All rights reserved.
- *
- * Use of this source code is governed by a BSD-style license that can be
- * found in the LICENSE.txt file.
- */
-
 OUTPUT_FORMAT("elf64-littleaarch64")
 OUTPUT_ARCH(aarch64)
 TARGET(binary)
@@ -17,22 +8,25 @@ SECTIONS
 {
     . = EL3_FLASH_BASE;
     .init . : {
+        _EL3_INIT_BASE = .;
         tzboot.o(.init);
+        *(.init);
     }
+    _EL3_INIT_SIZE = SIZEOF(.init);
 
-    . = ALIGN(16);
+    . = ALIGN(4K);
     _EL3_FLASH_TEXT = .;
-    . = EL3_RAM_BASE;
+    . = EL3_BASE_VA;
     .text . : AT(_EL3_FLASH_TEXT) {
-        _EL3_RAM_TEXT = .;
+        _EL3_TEXT_BASE = .;
         *(.text);
         *(.vectors);
     }
     _EL3_TEXT_SIZE = SIZEOF(.text);
 
-    . = ALIGN(0x1000);
+    . = ALIGN(4k);
     .data . : {
-        _EL3_RAM_DATA = .;
+        _EL3_DATA_BASE = .;
         _EL3_FLASH_DATA = LOADADDR(.data);
         *(.*data);
         *(.*bss);
diff --git a/platform/virt/platform.h b/platform/virt/platform.h
index 3d5c1a2..dc4280f 100644
--- a/platform/virt/platform.h
+++ b/platform/virt/platform.h
@@ -13,16 +13,20 @@
 
 #define RAM_BASE 0x40000000                 // From QEMU virt.c
 #define RAM_SIZE (2*1024*1024*1024)         // Only 2GB needed
+#define EL3_BASE_VA 0xF000000000
 #define EL3_RAM_BASE RAM_BASE
-#define EL3_RAM_SIZE (256*1024)
+#define EL3_RAM_SIZE (512*1024)
 #define SEC_RAM_BASE (RAM_BASE+0x1000000)
 #define SEC_RAM_SIZE ((RAM_SIZE>>1)-EL3_RAM_SIZE)
 #define NSEC_RAM_BASE (RAM_BASE+0x8000000)
 #define NSEC_RAM_SIZE (RAM_SIZE/2)
+#define VA_SIZE 48
+#define TnSZ (64-VA_SIZE)
 
+#define EL3_PGTBL_SIZE 0x40000
+#define EL3_PGTBL_BASE EL3_RAM_BASE+EL3_RAM_SIZE-EL3_PGTBL_SIZE
 #define EL3_STACK_SIZE 0x40000
-#define EL3_PGTBL_BASE 0x50000000
-#define EL3_STACK_BASE (EL3_PGTBL_BASE-0x1000)
+#define EL3_STACK_BASE 0xFF00000000
 #define SEC_STACK_SIZE 0x40000
 #define SEC_PGTBL_BASE 0x80000000
 #define SEC_STACK_BASE (EL3_PGTBL_BASE-0x1000)