Get initial monitor working

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

18 files changed, 1099 insertions, 209 deletions

diff --git a/.gdbinit64 b/.gdbinit64
index e8fb602..112ea62 100644
--- a/.gdbinit64
+++ b/.gdbinit64
@@ -1,3 +1,6 @@
 set arch aarch64
 target remote :1234
-file aarch64/tztest_secure.elf
+file aarch64/tzboot.elf
+add-symbol-file aarch64/tzboot.elf 0x40000000
+add-symbol-file aarch64/tztest_secure.elf 0x41000000
+set print pretty
diff --git a/aarch64/Makefile b/aarch64/Makefile
index ed2d996..b66cb4d 100644
--- a/aarch64/Makefile
+++ b/aarch64/Makefile
@@ -1,34 +1,45 @@
 SECTEST			= tztest_secure.elf
 SECTESTIMAGE 	= tztest_secure.flat
 TZTEST_IMAGE 	= tztest.img
-TZBOOT			= tzboot.o
+TZBOOT			= tzboot.o \
+                  el3_init.o \
+                  monitor_asm.o \
+                  monitor.o
+TZBOOTELF		= tzboot.elf
+TZBOOTIMAGE		= tzboot.flat
 TZSECLOAD		= tztest_secure.lds
-#TZSECOBJS		= secure_init.o \
+TZBOOTLOAD		= tzboot.lds
+TZSECOBJS		= secure_init.o #\
 			  	  secure_svc.o \
 			  	  secure_asm.o \
-                  monitor.o \
-                  monitor_asm.o \
                   sm_asm.o \
 			  	  sm.o
-#TZOBJS			= common_mmu.o \
+TZOBJS			= common_mmu.o #\
                   common_svc.o
 
 -include .*.d
 
 ##################################################################
 
-$(SECTEST): $(TZSECOBJS) $(TZOBJS) $(TZSECLOAD) $(TZNSECLOAD) $(TZBOOT)
-	$(LD) -o $@ $(TZSECOBJS) $(FLATLIBS) $(TZOBJS) --script=$(TZSECLOAD)
+$(TZBOOTELF): $(TZBOOT) $(TZOBJS) $(TZBOOTLOAD)
+	$(LD) -o $@ $(TZBOOT) $(TZOBJS) $(FLATLIBS) --script=$(TZBOOTLOAD)
+
+$(TZBOOTIMAGE): $(TZBOOTELF)
+	$(OBJCOPY) -O binary $< $@
+
+$(SECTEST): $(TZSECOBJS) $(TZOBJS) $(TZSECLOAD)
+	$(LD) -o $@ $(TZSECOBJS) $(TZOBJS) $(FLATLIBS) --script=$(TZSECLOAD)
 
 $(SECTESTIMAGE): $(SECTEST)
-	$(OBJCOPY) -O binary $(SECTEST) $(SECTESTIMAGE)
+	$(OBJCOPY) -O binary $< $@
 
-$(TZTEST_IMAGE): $(SECTESTIMAGE) $(NSECTESTIMAGE)
-	dd if=$(SECTESTIMAGE) of=$@ 2> /dev/null
+$(TZTEST_IMAGE): $(TZBOOTIMAGE) $(SECTESTIMAGE) $(NSECTESTIMAGE)
+	dd if=$(TZBOOTIMAGE) of=$@ 2> /dev/null
+	dd oflag=seek_bytes seek=65536 if=$(SECTESTIMAGE) of=$@ 2> /dev/null
 	chmod +x $(TZTEST_IMAGE)
 
-$(TZBOOT): tzboot.S
-	$(CC) $(CFLAGS) -c -o $@ $<
+$(TZBOOTLOAD): tzboot.lds.S Makefile ../platform/$(PLAT)/
+	$(CC) $(CFLAGS) -E -P -C -o $@ $<
 
 $(TZSECLOAD): tztest_secure.lds.S Makefile ../platform/$(PLAT)/
 	$(CC) $(CFLAGS) -E -P -C -o $@ $<
@@ -40,6 +51,7 @@ all: $(TZTEST_IMAGE)
 
 clean:
 	$(RM) $(TZOBJS) $(TZSECOBJS) $(TZNSECOBJS) \
-		  $(TZSECLOAD) $(TZNSECLOAD) $(TZBOOT) \
+		  $(TZSECLOAD) $(TZNSECLOAD) \
+          $(TZBOOT) $(TZBOOTELF) $(TZBOOTIMAGE) $(TZBOOTLOAD) \
 		  $(SECTEST) $(SECTESTIMAGE) $(NSECTEST) $(NSECTESTIMAGE) \
 		  $(TZTEST_IMAGE) .*.d
diff --git a/aarch64/common_defs.h b/aarch64/common_defs.h
new file mode 100644
index 0000000..2d1d238
--- /dev/null
+++ b/aarch64/common_defs.h
@@ -0,0 +1,26 @@
+#ifndef _TZTEST_CONSTANTS_H
+#define _TZTEST_CONSTANTS_H
+
+#include "platform.h"
+
+#define STDOUT 1
+#define TZTEST_STATE_SECURE !SCR_NS
+#define TZTEST_STATE_NONSECURE SCR_NS
+
+#define SMC_NOOP 0
+#define SMC_DISPATCH_MONITOR 1
+#define SMC_YIELD 2
+#define SMC_DISPATCH_SECURE_USR 3
+#define SMC_DISPATCH_SECURE_SVC 4
+#define SMC_ALLOCATE_SECURE_MEMORY 4
+#define SMC_EXIT 5
+
+#define SVC_RETURN_FROM_SECURE_USR 0
+#define SVC_DISPATCH_MONITOR 1
+#define SVC_DISPATCH_SECURE_USR 2
+#define SVC_DISPATCH_SECURE_SVC 3
+#define SVC_DISPATCH_NONSECURE_SVC 4
+#define SVC_GET_SECURE_STATE 5
+#define SVC_EXIT 6
+
+#endif
diff --git a/aarch64/common_mmu.c b/aarch64/common_mmu.c
new file mode 100644
index 0000000..42cf56d
--- /dev/null
+++ b/aarch64/common_mmu.c
@@ -0,0 +1,129 @@
+#include "libcflat.h"
+#include "common_svc.h"
+#include "common_mmu.h"
+#include "common_defs.h"
+
+void pagetable_add_section_entries(uintptr_t *ttbr, pagetable_map_entry_t *pe)
+{
+    uintptr_t va = pe->va;
+    uintptr_t pa = pe->pa;
+    uintptr_t len = pe->len;
+    uint32_t l1idx = va >> L1_INDEX_SHIFT;
+    uint32_t l1attr = SECTION;
+    uint32_t size = SECTION_SIZE;
+
+    l1attr |= IS_SET(pe, NONSECURE) ?  SECTION_NS : 0;
+    l1attr |= IS_SET(pe, P0_R)
+              ? IS_SET(pe, P0_W) ? SECTION_P0_RW : SECTION_P0_RO
+              : SECTION_P0_NONE;
+    l1attr |= IS_SET(pe, P1_R)
+              ? IS_SET(pe, P1_W) ? SECTION_P1_RW : SECTION_P1_RO
+              : SECTION_P1_NONE;
+    l1attr |= IS_SET(pe, P0_X) ? 0 : SECTION_XN;
+    l1attr |= IS_SET(pe, P1_X) ? 0 : SECTION_PXN;
+    l1attr |= IS_SET(pe, NOTGLOBAL) ? (SECTION_NG) : 0;
+    l1attr |= IS_SET(pe, DEVICE)
+              ? IS_SET(pe, SHARED)
+                  ? SECTION_DEVICE_SHAREABLE : SECTION_DEVICE_UNSHAREABLE
+              : IS_SET(pe, SHARED) ? (SECTION_S) : 0;
+    l1attr |= IS_SET(pe, WT_CACHED)
+                  ? SECTION_WT_CACHED : IS_SET(pe, WB_CACHED)
+                      ? SECTION_WB_CACHED
+                      : IS_SET(pe, WBA_CACHED)
+                          ? SECTION_WBA_CACHED : SECTION_UNCACHED;
+
+    /* Round up to the next segment increment */
+    len = (len + (SECTION_SIZE-1)) & ~(SECTION_SIZE-1);
+
+    for (; len > 0; va += size, pa += size, len -= size) {
+        l1idx = L1_INDEX(va);
+        ttbr[l1idx] = (pa & SECTION_PA_MASK) | l1attr;
+        DEBUG_MSG("Mapped L1 0x%x to 0x%x (%p (0x%x) = 0x%x)\n",
+                   va, pa, &ttbr[l1idx], l1idx, ttbr[l1idx]);
+    }
+}
+
+void pagetable_add_page_entries(uintptr_t *ttbr, pagetable_map_entry_t *pe)
+{
+    uintptr_t va = pe->va;
+    uintptr_t pa = pe->pa;
+    uintptr_t len = pe->len;
+    uintptr_t l2len = 0;;
+    uintptr_t *l2base = ttbr + BYTE_TO_INT(L1_TABLE_SIZE);
+    uintptr_t *l2addr;
+    uint32_t size = PAGE_SIZE(PAGE);
+    uint32_t l1attr = L1_TYPE_PAGE, l2attr = L2_TYPE(pe->type);
+    uint32_t l1idx, l2idx;
+
+    l1attr |= IS_SET(pe, NONSECURE) ?  PAGE_NS : 0;
+    l1attr |= IS_SET(pe, P1_X) ? 0 : PAGE_PXN;
+
+    l2attr |= IS_SET(pe, P0_R)
+            ? IS_SET(pe, P0_W) ? PAGE_P0_RW : PAGE_P0_RO
+            : PAGE_P0_NONE;
+    l2attr |= IS_SET(pe, P1_R)
+            ? IS_SET(pe, P1_W) ? PAGE_P1_RW : PAGE_P1_RO
+            : PAGE_P1_NONE;
+    l2attr |= IS_SET(pe, P0_X) ? 0 : PAGE_XN(PAGE_SMALL);
+    l2attr |= IS_SET(pe, NOTGLOBAL) ? (PAGE_NG) : 0;
+    l2attr |= IS_SET(pe, DEVICE)
+            ? IS_SET(pe, SHARED)
+                ? PAGE_DEVICE_SHAREABLE
+                : PAGE_DEVICE_UNSHAREABLE(PAGE_SMALL)
+            : IS_SET(pe, SHARED) ? (PAGE_S) : 0;
+    l2attr |= IS_SET(pe, WT_CACHED)
+                ? PAGE_WT_CACHED
+                : IS_SET(pe, WB_CACHED)
+                    ? PAGE_WB_CACHED
+                    : IS_SET(pe, WBA_CACHED)
+                        ? PAGE_WBA_CACHED(PAGE_SMALL)
+                        : PAGE_UNCACHED(PAGE_SMALL);
+
+    /* Round up to the next page increment */
+    len = (len + (PAGE_SIZE(PAGE)-1)) & ~(PAGE_SIZE(PAGE)-1);
+
+    while (len > 0) {
+        l1idx = L1_INDEX(va);
+        l2addr = l2base + BYTE_TO_INT(L2_TABLE_SIZE * l1idx);
+        ttbr[l1idx] = (uintptr_t)l2addr | l1attr;
+
+        DEBUG_MSG("Added L1 0x%x to 0x%x (ttbrn[%x] = %x)\n",
+                  va, l2addr, l1idx, ttbr[l1idx]);
+
+        /* Set the level 2 page table size to the smaller of the total
+         * remaining length or the remaining length in the 1MB section.
+         */
+        l2len = L1_MAP_SIZE - (va & (L1_MAP_SIZE - 1));
+        l2len = (l2len > len) ? len : l2len;
+
+        for (; l2len > 0; va += size, pa += size, l2len -= size, len -= size) {
+            l2idx = L2_INDEX(va);
+            l2addr[l2idx] = (pa & PAGE_PA_MASK) | l2attr;
+            DEBUG_MSG("Mapped L2 0x%x to 0x%x (%p (0x%x) = 0x%x)\n",
+                       va, pa, &l2addr[l2idx], l2idx, l2addr[l2idx]);
+        }
+    }
+}
+
+void pagetable_add_entries(uintptr_t *ttbr, pagetable_map_entry_t *entries,
+                           uint32_t count)
+{
+    uint32_t i;
+
+    for (i = 0; i < count; i++) {
+        if (entries[i].type == SECTION) {
+            pagetable_add_section_entries(ttbr, &entries[i]);
+        } else if (entries[i].type == PAGE || entries[i].type == LARGE_PAGE) {
+            pagetable_add_page_entries(ttbr, &entries[i]);
+        }
+    }
+}
+
+void pagetable_init(uintptr_t *ttbr)
+{
+    int i;
+
+    for (i = 0; i < BYTE_TO_INT(L1_TABLE_SIZE); i++) {
+        ttbr[i] = 0;
+    }
+}
diff --git a/aarch64/common_mmu.h b/aarch64/common_mmu.h
new file mode 100644
index 0000000..026a9df
--- /dev/null
+++ b/aarch64/common_mmu.h
@@ -0,0 +1,145 @@
+
+#ifndef _TZTEST_MMU_H
+#define _TZTEST_MMU_H
+
+#include "sm.h"
+
+#define LARGE_PAGE  (0 << 0)
+#define PAGE        (1 << 0)
+#define SECTION     (1 << 1)
+#define SECURE      (0 << 0)
+#define NONSECURE   (1 << 0)
+#define P0_R        (1 << 1)
+#define P0_W        (2 << 1)
+#define P0_X        (4 << 1)
+#define P0_NONE     (0 << 1)
+#define P1_R        (1 << 4)
+#define P1_W        (2 << 4)
+#define P1_X        (4 << 4)
+#define SHARED      (1 << 7)
+#define NOTGLOBAL   (1 << 8)
+#define DEVICE      (1 << 9)
+#define UNCACHED    (0 << 10)
+#define WT_CACHED   (1 << 10)
+#define WB_CACHED   (2 << 10)
+#define WBA_CACHED  (3 << 10)
+#define DOMAIN(_d)  ((_d) << 16)
+
+#define L1_TYPE_PAGE                (1 << 0)
+#define L1_TYPE_SECTION             ((0 << 18) | (1 << 1))
+
+#define L1_INDEX_BITS               12
+#define L1_TABLE_SIZE               ((1 << L1_INDEX_BITS) << 2)
+#define L1_INDEX_SHIFT              20
+#define L1_MAP_SIZE                 (1 << L1_INDEX_SHIFT)
+#define L1_INDEX_MASK               ((1 << L1_INDEX_BITS) - 1)
+
+#define L2_TYPE(_sz)                (1 << (_sz))
+
+#define L2_INDEX_BITS               8
+#define L2_TABLE_SIZE               (1 << 10)
+#define L2_INDEX_SHIFT              12
+#define L2_MAP_SIZE                 (1 << L2_INDEX_SHIFT)
+#define L2_INDEX_MASK               ((1 << L2_INDEX_BITS) - 1)
+
+#define BYTE_TO_INT(_val)           ((_val) >> 2)
+#define INT_TO_BYTE(_val)           ((_val) << 2)
+#define L1_INDEX(_va)               (((_va)>>L1_INDEX_SHIFT) & L1_INDEX_MASK)
+#define L2_INDEX(_va)               (((_va)>>L2_INDEX_SHIFT) & L2_INDEX_MASK)
+
+#define SECTION_PXN                 (1 << 0)
+#define SECTION_B                   (1 << 2)
+#define SECTION_C                   (1 << 3)
+#define SECTION_XN                  (1 << 4)
+#define SECTION_DOMAIN(_d)          ((_d) << 5)
+#define SECTION_NS                  (1 << 19)
+#define SECTION_TEX(v)              (1 << 12)
+#define SECTION_S                   (1 << 16)
+#define SECTION_NG                  (1 << 17)
+#define SECTION_SIZE                L1_MAP_SIZE
+#define SECTION_PA_SHIFT            L1_INDEX_SHIFT
+#define SECTION_PA_MASK             ~((1 << SECTION_PA_SHIFT) - 1)
+
+/* Fix SCTLR.AFE bit */
+
+#define SECTION_P1_RW               ((0 << 15) | (1 << 10))
+#define SECTION_P1_RO               ((1 << 15) | (1 << 10))
+#define SECTION_P1_NONE             0
+#define SECTION_P0_RW               ((0 << 15) | (3 << 10))
+#define SECTION_P0_RO               ((0 << 15) | (2 << 10))
+#define SECTION_P0_NONE             ((0 << 15) | (1 << 10))
+
+/* These remap description describe the memory region if SCTLR.TRE = 0
+ * (no region atrribute remap).
+ */
+#define SECTION_DEVICE_SHAREABLE    (SECTION_B)
+#define SECTION_DEVICE_UNSHAREABLE  (SECTION_TEX(2))
+#define SECTION_WT_CACHED           (SECTION_C)
+#define SECTION_WB_CACHED           (SECTION_C | SECTION_B)
+#define SECTION_UNCACHED            (SECTION_TEX(1))
+#define SECTION_WBA_CACHED          (SECTION_TEX(1) | SECTION_C | SECTION_B)
+
+#define SMALL_PAGE_SIZE             0x1000
+#define LARGE_PAGE_SIZE             0x10000
+#define PAGE_SIZE(_type)            ((_type == PAGE) \
+                                        ? SMALL_PAGE_SIZE : LARGE_PAGE_SIZE)
+
+#define PAGE_PASHIFT                L2_INDEX_SHIFT
+#define PAGE_PA_MASK                ~((1 << PAGE_PASHIFT) - 1)
+#define PAGE_LARGE                  (0 << 1)
+#define PAGE_SMALL                  (1 << 1)
+#define PAGE_PXN                    (1 << 2)
+#define PAGE_NS                     (1 << 3)
+#define PAGE_DOMAIN(_d)             ((_d) << 5)
+#define PAGE_B                      (1 << 2)
+#define PAGE_C                      (1 << 3)
+#define PAGE_LARGE_XN               (1 << 15)
+#define PAGE_SMALL_XN               (1 << 0)
+#define PAGE_XN(_sz)                ((PAGE_LARGE == (_sz)) \
+                                        ? PAGE_LARGE_XN : PAGE_SMALL_XN)
+#define PAGE_S                      (1 << 10)
+#define PAGE_NG                     (1 << 11)
+#define PAGE_LARGE_TEX(_v)          ((_v) << 12)
+#define PAGE_SMALL_TEX(_v)          ((_v) << 6)
+#define PAGE_TEX(_sz, _v)           (PAGE_LARGE == (_sz)) \
+                                        ? PAGE_LARGE_TEX(_v) \
+                                        : PAGE_SMALL_TEX(_v)
+
+#define PAGE_P1_RW                  ((0 << 9) | (1 << 4))
+#define PAGE_P1_RO                  ((1 << 9) | (1 << 4))
+#define PAGE_P1_NONE                0
+#define PAGE_P0_RW                  ((0 << 9) | (3 << 4))
+#define PAGE_P0_RO                  ((0 << 9) | (2 << 4))
+#define PAGE_P0_NONE                ((0 << 9) | (1 << 4))
+
+/* These remap description describe the memory region if SCTLR.TRE = 0
+ * (no region atrribute remap).
+ */
+#define PAGE_DEVICE_SHAREABLE           (PAGE_B)
+#define PAGE_DEVICE_UNSHAREABLE(_sz)    (PAGE_TEX((_sz), 2))
+#define PAGE_WT_CACHED                  (PAGE_C)
+#define PAGE_WB_CACHED                  (PAGE_C | PAGE_B)
+#define PAGE_UNCACHED(_sz)              (PAGE_TEX((_sz), 1))
+#define PAGE_WBA_CACHED(_sz)            (PAGE_TEX((_sz), 1) | PAGE_C | PAGE_B)
+
+typedef struct {
+    uint32_t    type;
+    uint32_t    attr;
+    uintptr_t   va;
+    uintptr_t   pa;
+    uintptr_t   len;
+} pagetable_map_entry_t;
+
+#define ENTRIES_COUNT(_ent) (sizeof(_ent)/sizeof((_ent)[0]))
+
+#define PT_ADD_ENTRIES(_tbl, _ent) \
+            pagetable_add_entries((_tbl), (_ent), ENTRIES_COUNT(_ent))
+#define IS_SET(_pe, _attr) ((_attr) == ((_pe)->attr & (_attr)))
+
+
+extern void pagetable_init(uintptr_t *);
+extern void pagetable_init_common(uintptr_t *table);
+extern void pagetable_add_entries(uintptr_t *, pagetable_map_entry_t *,
+                                   uint32_t);
+
+#endif
diff --git a/aarch64/common_svc.h b/aarch64/common_svc.h
new file mode 100644
index 0000000..a8b737e
--- /dev/null
+++ b/aarch64/common_svc.h
@@ -0,0 +1,71 @@
+#ifndef _COMMON_SVC_H
+#define _COMMON_SVC_H
+
+//#include "tztest_builtins.h"
+#include "sm.h"
+#include "libcflat.h"
+
+typedef struct {
+    uint32_t (*func)(uint32_t);
+    uint32_t arg;
+    uint32_t ret;
+} tztest_dispatch_t;
+
+typedef struct {
+    union {
+        tztest_dispatch_t dispatch;
+        struct {
+            uint32_t reg;
+            uint32_t val;
+        } reg_read;
+        struct {
+            uint32_t state;
+        } secure_state;
+    };
+} tztest_svc_desc_t;
+
+#define CLEAR_SVC_DESC(_desc) memset(&(_desc), sizeof(tztest_svc_desc_t), 0)
+
+typedef struct {
+    union {
+        tztest_dispatch_t dispatch;
+    };
+} tztest_smc_desc_t;
+
+#ifdef DEBUG
+#define DEBUG_MSG(_str, ...) \
+    printf("\n[DEBUG] %s: " _str, __FUNCTION__, ##__VA_ARGS__)
+#define DEBUG_ARG
+#else
+#define DEBUG_MSG(_str, ...)
+#define DEBUG_ARG __attribute__ ((unused))
+#endif
+
+#define MODE_STR(_mode)             \
+    ((_mode == MON) ? "MON" :        \
+     (_mode == SVC) ? "SVC" :        \
+     (_mode == SYS) ? "SYS" :        \
+     (_mode == USR) ? "USR" : "Unknown")
+
+
+#define FAULT_STR(_s) \
+    ((0x01 == (_s)) ? "Alignment fault" :                   \
+     (0x02 == (_s)) ? "Debug event" :                       \
+     (0x03 == (_s)) ? "Access flag fault" :                 \
+     (0x04 == (_s)) ? "Instruction cache maintenance" :     \
+     (0x05 == (_s)) ? "Translation fault" :                 \
+     (0x06 == (_s)) ? "Access flag fault" :                 \
+     (0x07 == (_s)) ? "Translation fault" :                 \
+     (0x08 == (_s)) ? "Synchronous external abort" :        \
+     (0x09 == (_s)) ? "Domain fault" :                      \
+     (0x0b == (_s)) ? "Domain fault" :                      \
+     (0x0c == (_s)) ? "External abort on table walk" :      \
+     (0x0d == (_s)) ? "Permisison fault" :                  \
+     (0x0e == (_s)) ? "Synchronous external abort" :        \
+     (0x0f == (_s)) ? "Permisison fault" :                  \
+     (0x10 == (_s)) ? "TLB conflict abort" :                \
+     (0x1c == (_s)) ? "External parity err on table walk" : \
+     (0x1e == (_s)) ? "External parity err on table walk" : \
+     "Unknown")
+
+#endif
diff --git a/aarch64/el3_init.S b/aarch64/el3_init.S
new file mode 100644
index 0000000..c37c82b
--- /dev/null
+++ b/aarch64/el3_init.S
@@ -0,0 +1,99 @@
+#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
+
+.align 12
+.section .text
+.global el3_init
+/* el3_init(sec_elr, nsec_elr) */
+el3_init:
+    /* Set-up the EL3 vbar */
+    ldr x10, =el3_vectors
+    msr vbar_el3, x10
+
+    /* Set-up the monitor stack */
+    ldr x10, =EL3_STACK_BASE
+	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 */
+    msr cptr_el3, x10
+
+el3_init_mmu:
+    /* Disable data and instruction caches */
+    mrs x10, sctlr_el3
+    bic x10, x10, #0x0004
+    bic x10, x10, #0x1000
+    msr sctlr_el3, x10
+
+    bl el3_pagetable_init
+
+    /* Set TTBR0 to the initialized address plus enable shareable write-back
+     * write-allocate.
+     */
+    ldr x10, =EL3_PGTBL_BASE
+    msr ttbr0_el3, x10
+
+    mrs x10, sctlr_el3
+    orr x10, x10, #0x1      // Enable MMU
+    msr sctlr_el3, x10
+    isb
+    dsb sy
+
+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
+
+.end
diff --git a/aarch64/el3_loader.h b/aarch64/el3_loader.h
new file mode 100644
index 0000000..8e7dbf2
--- /dev/null
+++ b/aarch64/el3_loader.h
@@ -0,0 +1,13 @@
+#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_SIZE;
+uint64_t EL3_TEXT_SIZE = (uint64_t)&_EL3_TEXT_SIZE;
+extern uintptr_t _EL3_DATA_SIZE;
+uint64_t EL3_DATA_SIZE = (uint64_t)&_EL3_DATA_SIZE;
+
+#endif
diff --git a/aarch64/monitor.c b/aarch64/monitor.c
new file mode 100644
index 0000000..cde5e0a
--- /dev/null
+++ b/aarch64/monitor.c
@@ -0,0 +1,95 @@
+#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
new file mode 100644
index 0000000..f15599d
--- /dev/null
+++ b/aarch64/monitor_asm.S
@@ -0,0 +1,89 @@
+#include "common_defs.h"
+
+.section .text
+.globl monitor_save_state
+monitor_save_state:
+    stp x0, x1, [sp, #-16]!     /* Save the input regs x0:x3 */
+    stp x2, x3, [sp, #-16]!
+    mrs x1, elr_el3
+    mrs x2, spsr_el3
+    stp x1, x2, [x0], #16
+    mrs x1, sp_el0
+    mrs x2, sp_el1
+    stp x1, x2, [x0], #16
+    mrs x1, elr_el1
+    mrs x2, esr_el1
+    stp x1, x2, [x0], #16
+    stp x4, x5, [x0], #16
+    stp x6, x7, [x0], #16
+    stp x8, x9, [x0], #16
+    stp x10, x11, [x0], #16
+    stp x12, x13, [x0], #16
+    stp x14, x15, [x0], #16
+    stp x16, x17, [x0], #16
+    stp x18, x19, [x0], #16
+    stp x20, x21, [x0], #16
+    stp x22, x23, [x0], #16
+    stp x24, x25, [x0], #16
+    stp x26, x27, [x0], #16
+    stp x28, x29, [x0], #16
+    str x30, [x0], #8
+    ldp x2, x3, [sp], #16
+    ldp x0, x1, [sp], #16
+    ret
+
+.globl monitor_restore_state
+monitor_restore_state:
+    str x30, [sp, #-8]!         /* Save the LR onto the stack for later */
+    stp x0, x1, [sp, #-16]!
+    ldp x1, x2, [x0], #16
+    msr elr_el3, x1
+    msr spsr_el3, x2
+    ldp x1, x2, [x0], #16
+    msr sp_el0, x1
+    msr sp_el1, x2
+    ldp x1, x2, [x0], #16
+    msr elr_el1, x1
+    msr esr_el1, x2
+    ldp x4, x5, [x0], #16
+    ldp x6, x7, [x0], #16
+    ldp x8, x9, [x0], #16
+    ldp x10, x11, [x0], #16
+    ldp x12, x13, [x0], #16
+    ldp x14, x15, [x0], #16
+    ldp x16, x17, [x0], #16
+    ldp x18, x19, [x0], #16
+    ldp x20, x21, [x0], #16
+    ldp x22, x23, [x0], #16
+    ldp x24, x25, [x0], #16
+    ldp x26, x27, [x0], #16
+    ldp x28, x29, [x0], #16
+    ldr x30, [x0], #8
+    ldp x0, x1, [sp], #16
+    ldr x3, [sp], #8            /* Restore the LR so we can jump back */
+    br x3
+
+.globl monitor_switch
+monitor_switch:
+    stp x0, x1, [sp, #-16]!     /* Save the input regs x0:x3 */
+    mrs x1, scr_el3
+    and x1, x1, #0x1
+    cbnz x1, switch_to_nsec
+switch_to_sec:
+    adr x0, sec_state
+    bl monitor_save_state
+    adr x0, nsec_state
+    bl monitor_restore_state
+    b switch_state
+switch_to_nsec:
+    adr x0, nsec_state
+    bl monitor_save_state
+    adr x0, sec_state
+    bl monitor_restore_state
+switch_state:
+    eor x1, x1, #0x1
+    msr scr_el3, x1
+    ldp x0, x1, [sp], #16
+    eret
+
+.end
diff --git a/aarch64/secure_init.S b/aarch64/secure_init.S
new file mode 100644
index 0000000..29172fe
--- /dev/null
+++ b/aarch64/secure_init.S
@@ -0,0 +1,127 @@
+#include "common_defs.h"
+#include "platform.h"
+
+.section .vectors
+secure_vectors:
+	.word 0        /* reset */
+	b   secure_undef_vec    /* undef */
+	b   secure_svc_vec      /* svc */
+	b   secure_pabort_vec   /* pabt */
+	b   secure_dabort_vec   /* dabt */
+	.word 0 /* hmc */
+	.word 0 /* irq */
+	.word 0 /* fiq */
+
+secure_undef_vec:
+//    ldr x10, =secure_undef_handler
+//    blr x10
+    eret
+
+secure_pabort_vec:
+//    bl secure_pabort_handler
+//    b end
+
+secure_dabort_vec:
+//    bl secure_dabort_handler
+//    b end
+
+secure_svc_vec:
+    /* Check if this is a return from USR mode and pop the return address off
+     * the stack.  If so, we got here through the dispatch mechanism that
+     * pushed the return on the stack which should be secure svc loop.  For
+     * this reason we want to return to SVC mode and not a return from
+     * exception.
+     * Otherwise, route handling to the secure svc_handler.  This is the case
+     * where we came from secure usr mode.
+     */
+    cmp x0, #SVC_RETURN_FROM_SECURE_USR
+    bne 1f
+    mov x0, x1
+    ret
+1:
+//    ldr x10, =secure_svc_handler
+//    blr x10
+    eret
+
+.section .init
+secure_init:
+    /* We enter with R0 pointing to the nonsecure entry point.  Put it in R11
+     * for now to avoid overwriting it on calls. */
+    mov x11, x0
+
+    /* Disable interrupts for now */
+    mrs x10, daif
+    orr x10, x10, #0xc0     /* Mask IRQ and FIQ */
+    msr daif, x10
+
+    /* Setup the secure EL1 vectors
+     */
+    ldr x10, =secure_vectors
+    msr vbar_el1, x10
+
+    isb
+
+secure_stack_init:
+    /* Set-up the secure SVC stack */
+	ldr x10, =SEC_STACK_BASE
+	mov sp, x10
+
+    smc #SMC_YIELD
+
+secure_mmu_init:
+    /* Disable data and instruction caches */
+    mrs x10, sctlr_el3
+    bic x10, x10, #0x0004
+    bic x10, x10, #0x1000
+    msr sctlr_el3, x10
+
+    /* Set-up the initial secure page tables */
+//    bl secure_pagetable_init
+
+    /* Set TTBR0 to the initialized address plus enable shareable write-back
+     * write-allocate.
+     */
+    ldr x10, =SEC_PGTBL_BASE
+    orr x10, x10, #0x8
+    orr x10, x10, #0x2
+    orr x10, x10, #0x1
+    msr ttbr0_el3, x10
+
+    /* Enable the mmu */
+//    tlbi alle3
+    mrs x10, sctlr_el3
+    orr x10, x10, #0x1
+    msr sctlr_el3, x10
+    isb
+    dsb sy
+
+check_secure:
+    /* Check that are entry state makes sense before initializing the monitor
+     * mode.
+     */
+//    bl secure_check_init
+
+secure_init_monitor:
+    /* Call monitor mode initialization and pass to it the non-secure execution
+     * entry point.
+     */
+    mov x0, x11
+//    bl monitor_init
+
+    /* Once monitor mode is set-up, we yield to non-secure execution.  The
+     * non-secure entrypoint was established in monitor init.
+     */
+    mov x0, #SMC_YIELD
+    smc #1
+
+    /* The first return to the secure world will set us off into our
+     * secure-side monitor loop.  The only way out is to issue an EXIT SMC call
+     * to the secure world.
+     */
+//    bl secure_svc_loop
+
+	/* If we get here we are on the way out, poweroff the device */
+end:
+//    b secure_shutdown
+b end
+
diff --git a/aarch64/sm.h b/aarch64/sm.h
new file mode 100644
index 0000000..5472107
--- /dev/null
+++ b/aarch64/sm.h
@@ -0,0 +1,99 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef SM_SM_H
+#define SM_SM_H
+
+//#include <sys/types.h>
+#include <stdint.h>
+typedef unsigned int uint32_t;
+//typedef unsigned long uintptr_t;
+typedef uintptr_t vaddr_t;
+
+struct sm_nsec_ctx {
+	uint32_t usr_sp;
+	uint32_t usr_lr;
+	uint32_t irq_spsr;
+	uint32_t irq_sp;
+	uint32_t irq_lr;
+	uint32_t svc_spsr;
+	uint32_t svc_sp;
+	uint32_t svc_lr;
+	uint32_t abt_spsr;
+	uint32_t abt_sp;
+	uint32_t abt_lr;
+	uint32_t und_spsr;
+	uint32_t und_sp;
+	uint32_t und_lr;
+	uint32_t mon_lr;
+	uint32_t mon_spsr;
+	uint32_t r4;
+	uint32_t r5;
+	uint32_t r6;
+	uint32_t r7;
+	uint32_t r8;
+	uint32_t r9;
+	uint32_t r10;
+	uint32_t r11;
+	uint32_t r12;
+};
+
+struct sm_sec_ctx {
+	uint32_t usr_sp;
+	uint32_t usr_lr;
+	uint32_t irq_spsr;
+	uint32_t irq_sp;
+	uint32_t irq_lr;
+	uint32_t svc_spsr;
+	uint32_t svc_sp;
+	uint32_t svc_lr;
+	uint32_t abt_spsr;
+	uint32_t abt_sp;
+	uint32_t abt_lr;
+	uint32_t und_spsr;
+	uint32_t und_sp;
+	uint32_t und_lr;
+	uint32_t mon_lr;
+	uint32_t mon_spsr;
+};
+
+/* Returns storage location of non-secure context for current CPU */
+struct sm_nsec_ctx *sm_get_nsec_ctx(void);
+
+/* Returns storage location of secure context for current CPU */
+struct sm_sec_ctx *sm_get_sec_ctx(void);
+
+/* Returns stack pointer to use in monitor mode for current CPU */
+void *sm_get_sp(void);
+
+
+/*
+ * Initializes secure monitor, must be called by each CPU
+ */
+void sm_init(vaddr_t stack_pointer);
+
+#endif /*SM_SM_H*/
diff --git a/aarch64/tzboot.S b/aarch64/tzboot.S
index d6d19fb..01a34e8 100644
--- a/aarch64/tzboot.S
+++ b/aarch64/tzboot.S
@@ -1,6 +1,7 @@
 #include "platform.h"
 
 .align 5
+.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.
  */
@@ -15,66 +16,51 @@ boot_vectors:
 	.word 0 /* fiq */
 
 reset:
-init_uart:
-	/*
-	 * UART initialisation (38400 8N1)
-	 */
-	ldr	x0,	=UART0_BASE     // UART base (Versatile Express)
-	mov	x1, #0x10			// ibrd
-	str	x1, [x0, #0x24]
-	mov	x1, #0xc300
-	orr	x1, x1, #0x0001			// cr
-	str	x1, [x0, #0x30]
-
-init_cpu:
     /* Disable interrupts for now */
     mrs x10, daif
     orr x10, x10, #0xc0     // Mask IRQ and FIQ
     msr daif, x10
 
-    /* Set up boot VBAR */
+    /* Set up VBAR */
     adr x11, boot_vectors
     msr vbar_el3, x11
     isb
 
-    /* Start by setting up the boot stack*/
-    ldr x11, =boot_stacktop
-	mov sp, x11
+init_uart:
+	/* UART initialisation (38400 8N1) */
+	ldr	x0,	=UART0_BASE     // UART base
+	mov	x1, #0x10			// ibrd
+	str	x1, [x0, #0x24]
+	mov	x1, #0xc300
+	orr	x1, x1, #0x0001	    // cr
+	str	x1, [x0, #0x30]
 
-load_secure:
-copy_secvecs:
-    /* First copy the vector section text into RAM */
-    ldr x0, =_flash_secvecs_start
-    ldr x1, =_ram_secvecs_start
-    ldr x2, =_secvecs_size
-    bl memcpy
+    /* Start by setting up a temporary stack */
+    ldr x11, =(RAM_BASE+RAM_SIZE)
+	mov sp, x11
 
-copy_sectext:
-    /* Next copy the text section into RAM */
-    ldr x0, =_flash_sectext_start
-    ldr x1, =_ram_sectext_start
-    ldr x2, =_sectext_size
+    /* Copy the text section to RAM */
+    ldr x0, =_EL3_RAM_TEXT
+    ldr x1, =_EL3_FLASH_TEXT
+    ldr x2, =_EL3_TEXT_SIZE
     bl memcpy
 
-copy_secdata:
-    /* Last copy the text section into RAM */
-    ldr x0, =_flash_secdata_start
-    ldr x1, =_ram_secdata_start
-    ldr x2, =_secdata_size
+    /* Copy the data section to RAM */
+    ldr x0, =_EL3_RAM_DATA
+    ldr x1, =_EL3_FLASH_DATA
+    ldr x2, =_EL3_DATA_SIZE
     bl memcpy
 
-load_nonsecure:
-    /* Branch to the nonsecure image base.  It should be set-up so that this is
-     * the image loader which returns the starting execution point.
-     */
-    mov x0, #FLASH_NSEC_BASE
-    blr x0
-
-    /* Branch to the starting point of the secure image in RAM which should
-     * kick off initialization.
+    /* 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
+     * to secure EL1 and sets up non-secure EL1 for the first SMC context
+     * switch.
      */
-    mov x1, #RAM_SEC_BASE
-    br x1
+    mov x0, SEC_FLASH_BASE
+    mov x1, NSEC_FLASH_BASE
+    ldr x11, =_EL3_RAM_TEXT
+    blr x11
 
     /* We shouldn't get here as the secure image will shutdown */
 end:
@@ -82,8 +68,8 @@ end:
 
 memcpy:
     cbz x2, memcpy_done
-    ldrb w10, [x0], #1
-    strb w10, [x1], #1
+    ldrb w10, [x1], #1
+    strb w10, [x0], #1
     subs x2, x2, #1
     b memcpy
 memcpy_done:
diff --git a/aarch64/tzboot.lds b/aarch64/tzboot.lds
new file mode 100644
index 0000000..ed8e289
--- /dev/null
+++ b/aarch64/tzboot.lds
@@ -0,0 +1,40 @@
+/*
+ * 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
new file mode 100644
index 0000000..56c0f7b
--- /dev/null
+++ b/aarch64/tzboot.lds.S
@@ -0,0 +1,41 @@
+/*
+ * 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)
+
+#include "platform.h"
+
+SECTIONS
+{
+    . = EL3_FLASH_BASE;
+    .init . : {
+        tzboot.o(.init);
+    }
+
+    . = ALIGN(16);
+    _EL3_FLASH_TEXT = .;
+    . = EL3_RAM_BASE;
+    .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/tztest_secure.lds b/aarch64/tztest_secure.lds
index 7773624..fd4722c 100644
--- a/aarch64/tztest_secure.lds
+++ b/aarch64/tztest_secure.lds
@@ -1,11 +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)
@@ -14,65 +6,26 @@ TARGET(binary)
  * It is acceptable to have the text here as it is RO.
  */
 /* Taken from Linux vexpress support */
-_ram_nsec_base = 0x48000000;
 SECTIONS
 {
- . = 0x00000000;
- .boot : { tzboot.o }
- _eboot = .;
- _flash_secvecs_start = .;
- . = 0x40000000;
- _ram_sec_base = .;
- /* We separate out the secure vector table and level 1 handlers, so we
-     * can map it to hivecs (0xffff0000).
-	 */
- _ram_secvecs_start = .;
- .vectors : AT ( _flash_secvecs_start ) { *(.vectors); }
- _ram_secvecs_end = .;
- _secvecs_size = _ram_secvecs_end - _ram_secvecs_start;
- /* Page align the text section as we map it differently than the above
-     * vector text.
-	 */
- . = ALIGN(0x1000);
- _flash_sectext_start = _flash_secvecs_start + _secvecs_size;
- _ram_sectext_start = .;
- .text : AT ( _flash_sectext_start ) { *(.text); }
- _ram_sectext_end = .;
- _sectext_size = _ram_sectext_end - _ram_sectext_start;
- _flash_secdata_start = _flash_sectext_start + _sectext_size;
- . = ALIGN(1M);
- _ram_secdata_start = .;
- .rodata : AT ( _flash_secdata_start ) { *(.rodata); }
- .data : { *(.data); }
- .bss : { *(.bss); }
- _ram_secdata_end = .;
- _secdata_size = _ram_secdata_end - _ram_secdata_start;
- . = ALIGN(0x4000);
- _secstack_start = .;
- sec_usr_stacktop = . + 0x4000;
- sec_und_stacktop = sec_usr_stacktop + 0x4000;
- sec_abt_stacktop = sec_und_stacktop + 0x4000;
- sec_svc_stacktop = sec_und_stacktop + 0x4000;
- boot_stacktop = sec_svc_stacktop;
- mon_stacktop = sec_svc_stacktop + 0x4000;
- _secstack_size = mon_stacktop - _secstack_start;
- . = ALIGN(1M);
- secure_memory_heap = .;
- . = ALIGN(16M);
- _sec_l1_page_table = .;
- . = 0x49000000;
- _common_memory_heap_base = .;
- . = 0x4A000000;
- _shared_memory_heap_base = .;
- . += 4;
- _tztest_test_count = .;
- . += 4;
- _tztest_fail_count = .;
- . += 4;
- _tztest_exception = .;
- . += 4;
- _tztest_exception_status = .;
- . += 4;
- _tztest_exception_addr = .;
- . += 4;
+    . = (0x00000000 +0x10000);
+    .init . : {
+        secure_init.o(.init);
+    }
+    . = ALIGN(16);
+    _SEC_FLASH_TEXT = .;
+    . = (0x40000000 +0x1000000);
+    .text . : AT(_SEC_FLASH_TEXT) {
+        _SEC_RAM_TEXT = .;
+        *(.text);
+        *(.vectors);
+    }
+    _SEC_TEXT_SIZE = SIZEOF(.text);
+    .data ALIGN(0x1000) : {
+        _SEC_RAM_DATA = .;
+        _SEC_FLASH_DATA = LOADADDR(.data);
+        *(.*data);
+        *(.*bss);
+    }
+    _SEC_DATA_SIZE = SIZEOF(.data);
 }
diff --git a/aarch64/tztest_secure.lds.S b/aarch64/tztest_secure.lds.S
index a64d7c1..28677c3 100644
--- a/aarch64/tztest_secure.lds.S
+++ b/aarch64/tztest_secure.lds.S
@@ -1,87 +1,31 @@
-/*
- * 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)
 
 #include "platform.h"
 
-_ram_nsec_base = RAM_NSEC_BASE;
-
 SECTIONS
 {
-	. = FLASH_SEC_BASE;
-	.boot : { tzboot.o }
-	_eboot = .;
-
-	_flash_secvecs_start = .;
-	. = RAM_SEC_BASE;
-	_ram_sec_base = .;
-
-	/* We separate out the secure vector table and level 1 handlers, so we
-     * can map it to hivecs (0xffff0000).
-	 */
-	_ram_secvecs_start = .;
-	.vectors : AT ( _flash_secvecs_start ) { *(.vectors); }
-	_ram_secvecs_end = .;
-	_secvecs_size = _ram_secvecs_end - _ram_secvecs_start;
-
-	/* Page align the text section as we map it differently than the above
-     * vector text.
-	 */
-	. = ALIGN(0x1000);
-	_flash_sectext_start = _flash_secvecs_start + _secvecs_size;
-	_ram_sectext_start = .;
-	.text : AT ( _flash_sectext_start ) { *(.text); }
-	_ram_sectext_end = .;
-	_sectext_size = _ram_sectext_end - _ram_sectext_start;
-
-	_flash_secdata_start = _flash_sectext_start + _sectext_size;
-	. = ALIGN(1M);
-	_ram_secdata_start = .;
-	.rodata : AT ( _flash_secdata_start ) { *(.rodata); }
-	.data : { *(.data); }
-	.bss :  { *(.bss); }
-	_ram_secdata_end = .;
-	_secdata_size = _ram_secdata_end - _ram_secdata_start;
-
-	. = ALIGN(STACK_SIZE);
-	_secstack_start = .;
-	sec_usr_stacktop = . + STACK_SIZE;
-	sec_und_stacktop = sec_usr_stacktop + STACK_SIZE;
-	sec_abt_stacktop = sec_und_stacktop + STACK_SIZE;
-	sec_svc_stacktop = sec_und_stacktop + STACK_SIZE;
-	boot_stacktop = sec_svc_stacktop;
-	mon_stacktop = sec_svc_stacktop + STACK_SIZE;
-	_secstack_size = mon_stacktop - _secstack_start;
-
-	. = ALIGN(1M);
-	secure_memory_heap = .;
-
-	. = ALIGN(16M);
-	_sec_l1_page_table = .;
-
-	. = TZTEST_COMMON_HEAP_BASE;
-	_common_memory_heap_base = .;
-
-	. = TZTEST_SHARED_HEAP_BASE;
-	_shared_memory_heap_base = .;
-	. += 4;
-	_tztest_test_count = .;
-	. += 4;
-	_tztest_fail_count = .;
-	. += 4;
-	_tztest_exception = .;
-	. += 4;
-	_tztest_exception_status = .;
-	. += 4;
-	_tztest_exception_addr = .;
-	. += 4;
+    . = SEC_FLASH_BASE;
+    .init . : {
+        secure_init.o(.init);
+    }
+
+    . = ALIGN(16);
+    _SEC_FLASH_TEXT = .;
+    . = SEC_RAM_BASE;
+    .text . : AT(_SEC_FLASH_TEXT) {
+        _SEC_RAM_TEXT = .;
+        *(.text);
+        *(.vectors);
+    }
+    _SEC_TEXT_SIZE = SIZEOF(.text);
+
+    .data ALIGN(0x1000) : {
+        _SEC_RAM_DATA = .;
+        _SEC_FLASH_DATA = LOADADDR(.data);
+        *(.*data);
+        *(.*bss);
+    }
+    _SEC_DATA_SIZE = SIZEOF(.data);
 }
diff --git a/platform/virt/platform.h b/platform/virt/platform.h
index 956d819..3d5c1a2 100644
--- a/platform/virt/platform.h
+++ b/platform/virt/platform.h
@@ -5,11 +5,29 @@
  * which is 32MB.  It is also aliased to 0x0 (to 0x2000000).
  * It is acceptable to have the text here as it is RO.
  */
-#define FLASH_SEC_BASE 0x00000000
-#define FLASH_NSEC_BASE 0x00010000
+#define FLASH_BASE 0x00000000               // From QEMU virt.c
+#define FLASH_SIZE 0x08000000               // From QEMU virt.c
+#define EL3_FLASH_BASE FLASH_BASE
+#define SEC_FLASH_BASE (FLASH_BASE+0x10000)
+#define NSEC_FLASH_BASE (FLASH_BASE+0x20000)
 
-#define RAM_SEC_BASE 0x40000000
-#define RAM_NSEC_BASE 0x48000000
+#define RAM_BASE 0x40000000                 // From QEMU virt.c
+#define RAM_SIZE (2*1024*1024*1024)         // Only 2GB needed
+#define EL3_RAM_BASE RAM_BASE
+#define EL3_RAM_SIZE (256*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 EL3_STACK_SIZE 0x40000
+#define EL3_PGTBL_BASE 0x50000000
+#define EL3_STACK_BASE (EL3_PGTBL_BASE-0x1000)
+#define SEC_STACK_SIZE 0x40000
+#define SEC_PGTBL_BASE 0x80000000
+#define SEC_STACK_BASE (EL3_PGTBL_BASE-0x1000)
+#define NSEC_STACK_SIZE 0x40000
+#define NSEC_STACK_BASE (NSEC_RAM_BASE+NSEC_RAM_SIZE-NSEC_STACK_SIZE)
 
 #define UART0_BASE 0x09000000