Commonize 32/64 EL1 code

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

17 files changed, 158 insertions, 109 deletions

diff --git a/aarch64/common/mem_util.c b/aarch64/common/mem_util.c
index d313454..76ea6b8 100644
--- a/aarch64/common/mem_util.c
+++ b/aarch64/common/mem_util.c
@@ -1,5 +1,5 @@
 #include "libcflat.h"
-#include "armv8_vmsa.h"
+#include "arch.h"
 #include <stdint.h>
 #include <stddef.h>
 #include "mem_util.h"
@@ -7,13 +7,14 @@
 #define SEC_STATE_STR "EL3"
 #include "debug.h"
 
-extern uint64_t mem_pgtbl_base;
-extern uint64_t mem_next_pa;
-extern uint64_t mem_heap_pool;
+extern uintptr_t mem_pgtbl_base;
+extern uintptr_t mem_next_pa;
+extern uintptr_t mem_next_l1_page;
+extern uintptr_t mem_heap_pool;
 
-uint64_t mem_allocate_pa()
+uintptr_t mem_allocate_pa()
 {
-    uint64_t next = mem_next_pa;
+    uintptr_t next = mem_next_pa;
     mem_next_pa += PAGE_SIZE;
     return next;
 }
@@ -21,18 +22,19 @@ uint64_t mem_allocate_pa()
 void mem_map_pa(uintptr_t vaddr, uintptr_t paddr,
                 uintptr_t tblattr, uintptr_t pgattr)
 {
-    uint64_t pa = mem_pgtbl_base, off;
+    uintptr_t pa = mem_pgtbl_base, off;
     uint32_t i;
-    uint64_t *pte;
+    uintptr_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);
+        pte = (uintptr_t *)(pa | off);
         if (!(*pte & 0x1)) {
             pa = mem_allocate_pa();
+        	mem_map_pa(pa, pa, 0, PTE_USER_RW);
             *pte = pa;
             *pte |= (PTE_TABLE | tblattr);
         } else {
@@ -42,51 +44,32 @@ void mem_map_pa(uintptr_t vaddr, uintptr_t paddr,
 
     /* The last level is the physical page to map */
     off = ((vaddr >> (39-(i*9))) & 0x1FF) << 3;
-    pte = (uint64_t *)(pa | off);
+    pte = (uintptr_t *)(pa | off);
     *pte = paddr & ~0xFFF;
     *pte |= (PTE_PAGE | PTE_ACCESS | pgattr);
     DEBUG_MSG("mapped VA:0x%lx to PA:0x%x - PTE:0x%lx (0x%lx)",
               vaddr, paddr, pte, *pte);
 }
 
-void mem_map_va(uintptr_t addr)
+void mem_map_va(uintptr_t addr, uintptr_t tblattr, uintptr_t pgattr)
 {
-    uint64_t pa = mem_pgtbl_base, off;
-    uint32_t i;
-    uint64_t *pte;
-
-    for (i = 0; i < 4; i++) {
-        /* Each successive level uses the next lower 9 VA bits in a 48-bit
-         * address, hence the i*9.
-         */
-        off = ((addr >> (39-(i*9))) & 0x1FF) << 3;
-        pte = (uint64_t *)(pa | off);
-        if (!(*pte & 0x1)) {
-            pa = mem_allocate_pa();
-            *pte = pa;
-            *pte |= PTE_PAGE;
-        } else {
-            pa = *pte & 0x000FFFFFF000;
-        }
-    }
+    uintptr_t pa = mem_allocate_pa();
 
-    *pte |= (PTE_ACCESS | PTE_USER_RW);
-    DEBUG_MSG("mapped VA:0x%lx to PA:0x%x - PTE:0x%lx (0x%lx)",
-                addr, pa, pte, *pte);
+    mem_map_pa(addr, pa, tblattr, pgattr);
 }
 
-int mem_unmap_va(uint64_t addr)
+int mem_unmap_va(uintptr_t addr)
 {
-    uint64_t pa = mem_pgtbl_base;
+    uintptr_t pa = mem_pgtbl_base;
     uint32_t i;
-    uint64_t *pte;
+    uintptr_t *pte;
 
     for (i = 0; i < 4; i++) {
         /* Each successive level uses the next lower 9 VA bits in a 48-bit
          * address, hence the i*9.
          */
-        uint64_t off = ((addr >> (39-(i*9))) & 0x1FF) << 3;
-        pte = (uint64_t *)(pa | off);
+        uintptr_t off = ((addr >> (39-(i*9))) & 0x1FF) << 3;
+        pte = (uintptr_t *)(pa | off);
         if (!(*pte & 0x1)) {
             DEBUG_MSG("Failed unmap: invalid table page");
             /* This is not a valid page, return an error */
@@ -110,7 +93,7 @@ void *mem_heap_allocate(size_t len)
     size_t off;
 
     for (off = 0; off < len; off += 0x1000) {
-        mem_map_va(mem_heap_pool + off);
+        mem_map_va(mem_heap_pool + off, 0, PTE_USER_RW);
     }
 
     mem_heap_pool += off;
@@ -120,16 +103,16 @@ void *mem_heap_allocate(size_t len)
 
 void *mem_lookup_pa(void *va)
 {
-    uint64_t pa = mem_pgtbl_base;
+    uintptr_t pa = mem_pgtbl_base;
     uint32_t i;
-    uint64_t *pte;
+    uintptr_t *pte;
 
     for (i = 0; i < 4; i++) {
         /* Each successive level uses the next lower 9 VA bits in a 48-bit
          * address, hence the i*9.
          */
-        uint64_t off = ((((uint64_t)va) >> (39-(i*9))) & 0x1FF) << 3;
-        pte = (uint64_t *)(pa | off);
+        uintptr_t off = ((((uint64_t)va) >> (39-(i*9))) & 0x1FF) << 3;
+        pte = (uintptr_t *)(pa | off);
         if (!(*pte & 0x1)) {
             DEBUG_MSG("Failed Lookup: invalid table page");
             /* This is not a valid page, return an error */
diff --git a/aarch64/common/mem_util.h b/aarch64/common/mem_util.h
index 89f3625..c7beec8 100644
--- a/aarch64/common/mem_util.h
+++ b/aarch64/common/mem_util.h
@@ -1,10 +1,10 @@
 #ifndef _MEM_UTIL_H
 #define _MEM_UTIL_H
 
-extern uint64_t mem_allocate_pa();
+extern uintptr_t mem_allocate_pa();
 extern void mem_map_pa(uintptr_t, uintptr_t, uintptr_t, uintptr_t);
-extern void mem_map_va(uintptr_t addr);
-extern int mem_unmap_va(uint64_t addr);
+extern void mem_map_va(uintptr_t, uintptr_t, uintptr_t);
+extern int mem_unmap_va(uintptr_t addr);
 extern void *mem_heap_allocate(size_t len);
 extern void *mem_lookup_pa(void *va);
 
diff --git a/aarch64/el1_common/el1_exception.S b/aarch64/el1_common/aarch64/el1_exception.S
index 15c90d7..1c5ca15 100644
--- a/aarch64/el1_common/el1_exception.S
+++ b/aarch64/el1_common/aarch64/el1_exception.S
@@ -9,20 +9,25 @@ el1_vectors:
 .word 0                     // Add padding to force the below alignment
 .align 9                    // Force these vectors to 0x400 alignment
 el1_sync_exception_current:
-    stp x30, x2, [sp, #-16]!
+    str x30, [sp, #-8]!
+    stp x2, x3, [sp, #-16]!
     stp x0, x1, [sp, #-16]!
     mov x2, x0;
     mrs x0, esr_el1
     mov x1, #0xffffff
     and x1, x1, x0
     lsr x0, x0, #26
+    mrs x2, far_el3
+    mrs x3, elr_el3
     bl el1_handle_exception
     ldp x0, x1, [sp], #16
-    ldp x30, x2, [sp], #16
+    ldp x2, x3, [sp], #16
+    ldr x30, [sp], #8
     eret
 .align 10                   // Force these vectors to 0x400 alignment
 el1_sync_exception_lower64:
-    stp x30, x2, [sp, #-16]!
+    str x30, [sp, #-8]!
+    stp x2, x3, [sp, #-16]!
     stp x0, x1, [sp, #-16]!
     mrs x0, esr_el1
     mov x1, #0xffffff
@@ -32,15 +37,19 @@ el1_sync_exception_lower64:
     b.eq el1_sync_exception_lower64_svc
     cmp x0, #EC_SVC32
     b.eq el1_sync_exception_lower64_svc
+    mrs x2, far_el1
+    mrs x3, elr_el1
     bl el1_handle_exception
     ldp x0, x1, [sp], #16
+    ldp x2, x3, [sp], #16
     b el1_sync_exception_lower64_done
 el1_sync_exception_lower64_svc:
-    ldp x0, x1, [sp]       /* Fetch our inputs as SVC args */
+    ldp x0, x1, [sp]        /* Fetch copies of our inputs as SVC args */
     bl el1_handle_svc
-    ldp x2, x1, [sp], #16
+    ldp x2, x1, [sp], #16   /* We don't want to overwrite x0, so use x2 */
+    ldp x2, x3, [sp], #16   /* We can throw away the old x0, and restore x2 */
 el1_sync_exception_lower64_done:
-    ldp x30, x2, [sp], #16
+    ldr x30, [sp], #8
     eret
 .align 7
 el1_serr_exception:
diff --git a/aarch64/el1_common/el1_init.S b/aarch64/el1_common/aarch64/el1_init.S
index b567404..b567404 100644
--- a/aarch64/el1_common/el1_init.S
+++ b/aarch64/el1_common/aarch64/el1_init.S
diff --git a/aarch64/el1_common/el1_loader.c b/aarch64/el1_common/aarch64/el1_loader.c
index 202526d..989195c 100644
--- a/aarch64/el1_common/el1_loader.c
+++ b/aarch64/el1_common/aarch64/el1_loader.c
@@ -33,8 +33,7 @@ bool el1_load_el0(uintptr_t elfbase, uintptr_t *entry)
 
     /* Finish mapping the remainder of the ELF pages in if any */
     for (off = 0x1000; off < elf_len; off += 0x1000) {
-        mem_map_pa((uint64_t)elfbase + off, (uint64_t)elfbase + off,
-                   0, PTE_USER_RW);
+        mem_map_pa(elfbase + off, elfbase + off, 0, PTE_USER_RW);
     }
 
     Elf64_Shdr *shdr = (Elf64_Shdr *)(elfbase + ehdr->e_shoff);
@@ -44,12 +43,12 @@ bool el1_load_el0(uintptr_t elfbase, uintptr_t *entry)
     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)(elfbase + shdr[i].sh_offset);
+            uintptr_t sect = elfbase + shdr[i].sh_offset;
             uintptr_t base_va = shdr[i].sh_addr;
             DEBUG_MSG("\tloading %s section: 0x%x bytes @ 0x%lx\n",
                       secname, shdr[i].sh_size, base_va);
             for (off = 0; off < shdr[i].sh_size; off += 0x1000) {
-                mem_map_va((uintptr_t)(base_va + off));
+                mem_map_va(base_va + off, 0, PTE_USER_RW);
                 memcpy((void *)(base_va + off), (void *)(sect + off), 0x1000);
             }
         }
@@ -57,7 +56,7 @@ bool el1_load_el0(uintptr_t elfbase, uintptr_t *entry)
 
     /* Unmap the FLASH ELF image */
     for (off = 0; off < elf_len; off += 0x1000) {
-        mem_map_va((uint64_t)elfbase + off);
+        mem_unmap_va(elfbase + off);
     }
 
     *entry = ehdr->e_entry;
diff --git a/aarch64/el1_common/el1.c b/aarch64/el1_common/el1.c
index 89e9109..a908625 100644
--- a/aarch64/el1_common/el1.c
+++ b/aarch64/el1_common/el1.c
@@ -1,11 +1,24 @@
 #include "el1_common.h"
 #include "mem_util.h"
 
-uint64_t mem_pgtbl_base = EL1_PGTBL_BASE;
+uintptr_t mem_pgtbl_base = EL1_PGTBL_BASE;
 smc_op_desc_t *smc_interop_buf;
 sys_control_t *syscntl;
-uint64_t mem_next_pa = 0;
-uint64_t mem_heap_pool = 0x40000000;
+uintptr_t mem_next_pa = 0;
+uintptr_t mem_next_l1_page = 0;
+uintptr_t mem_heap_pool = EL1_VA_HEAP_BASE;
+
+const char *svc_op_name[] = {
+    [SVC_OP_EXIT] = "SVC_OP_EXIT",
+    [SVC_OP_ALLOC] = "SVC_OP_ALLOC",
+    [SVC_OP_MAP] = "SVC_OP_MAP",
+    [SVC_OP_YIELD] = "SVC_OP_YIELD",
+    [SVC_OP_GET_SYSCNTL] = "SVC_OP_GET_SYSCNTL",
+    [SVC_OP_GET_REG] = "SVC_OP_GET_REG",
+    [SVC_OP_SET_REG] = "SVC_OP_SET_REG",
+    [SVC_OP_TEST] = "SVC_OP_TEST",
+    [SVC_OP_DISPATCH] = "SVC_OP_DISPATCH"
+};
 
 void el1_alloc_mem(op_alloc_mem_t *alloc)
 {
@@ -22,7 +35,7 @@ void el1_map_secure(op_map_mem_t *map)
         }
         __smc(SMC_OP_MAP, desc);
     } else {
-        mem_map_pa((uint64_t)map->va, (uint64_t)map->pa, 0, PTE_USER_RW);
+        mem_map_pa((uintptr_t)map->va, (uintptr_t)map->pa, 0, PTE_USER_RW);
     }
 }
 
@@ -66,11 +79,12 @@ int el1_handle_svc(uint32_t op, svc_op_desc_t *desc)
         el1_map_secure((op_map_mem_t *)desc);
         break;
     case SVC_OP_GET_SYSCNTL:
-        desc->get.data = (uint64_t)syscntl;
+        desc->get.data = (uintptr_t)syscntl;
         break;
     case SVC_OP_GET_REG:
         if (desc->get.el == 1) {
             switch (desc->get.key) {
+#if AARCH64
             case CURRENTEL:
                 desc->get.data = READ_CURRENTEL();
                 break;
@@ -83,6 +97,7 @@ int el1_handle_svc(uint32_t op, svc_op_desc_t *desc)
             case SCR:
                 desc->get.data = READ_SCR();
                 break;
+#endif
             }
         } else if (desc->get.el == 3) {
             memcpy(smc_interop_buf, desc, sizeof(smc_op_desc_t));
@@ -93,6 +108,7 @@ int el1_handle_svc(uint32_t op, svc_op_desc_t *desc)
     case SVC_OP_SET_REG:
         if (desc->set.el == 1) {
             switch (desc->set.key) {
+#if AARCH64
             case CURRENTEL:
                 WRITE_CURRENTEL(desc->set.data);
                 break;
@@ -105,6 +121,7 @@ int el1_handle_svc(uint32_t op, svc_op_desc_t *desc)
             case SCR:
                 WRITE_SCR(desc->set.data);
                 break;
+#endif
             }
         } else if (desc->set.el == 3) {
             memcpy(smc_interop_buf, desc, sizeof(smc_op_desc_t));
@@ -127,13 +144,9 @@ int el1_handle_svc(uint32_t op, svc_op_desc_t *desc)
     return ret;
 }
 
-void el1_handle_exception(uint64_t ec, uint64_t iss)
+void el1_handle_exception(uintptr_t ec, uintptr_t iss, uintptr_t far,
+     					uintptr_t elr)
 {
-    uint64_t elr, far;
-
-    __get_exception_address(far);
-    __get_exception_return(elr);
-
     if (syscntl->excp_log || syscntl->el1_excp[SEC_STATE].log) {
         syscntl->el1_excp[SEC_STATE].taken = true;
         syscntl->el1_excp[SEC_STATE].ec = ec;
@@ -145,11 +158,26 @@ void el1_handle_exception(uint64_t ec, uint64_t iss)
     case EC_UNKNOWN:
         DEBUG_MSG("Unknown exception far = 0x%lx  elr = 0x%lx\n", far, elr);
 
+        /* The preferred return address in the case of an undefined instruction
+         * is the actual offending instruction.  In the case of ARMv7, we need
+         * to decrement the address by 4 to get this behavior as the PC has
+         * already been moved past this instruction.
+         * If SKIP is enabled then we can just do nothing and get the correct
+         * behavior.
+         */
+#if AARCH32
+        if (syscntl->el1_excp[SEC_STATE].action != EXCP_ACTION_SKIP &&
+            syscntl->excp_action != EXCP_ACTION_SKIP) {
+            elr += 4;
+            __set_exception_return(elr);
+        }
+#else
         if (syscntl->el1_excp[SEC_STATE].action == EXCP_ACTION_SKIP ||
             syscntl->excp_action == EXCP_ACTION_SKIP) {
             elr +=4;
             __set_exception_return(elr);
         }
+#endif
         break;
     case EC_IABORT_LOWER:
         DEBUG_MSG("Instruction abort at lower level: far = %0lx\n", far);
@@ -185,9 +213,9 @@ void el1_handle_exception(uint64_t ec, uint64_t iss)
     }
 }
 
-void el1_start(uint64_t base, uint64_t size)
+void el1_start(uintptr_t base, uintptr_t size)
 {
-    uint64_t addr = base;
+    uintptr_t addr = base;
     size_t len;
 
     printf("EL1 (%s) started...\n", sec_state_str);
diff --git a/aarch64/el1_common/el1_common.h b/aarch64/el1_common/el1_common.h
index 72db47f..27750bf 100644
--- a/aarch64/el1_common/el1_common.h
+++ b/aarch64/el1_common/el1_common.h
@@ -7,12 +7,10 @@
 #include "smc.h"
 #include "string.h"
 #include "el1.h"
-#include "armv8_exception.h"
-#include "armv8_vmsa.h"
+#include "arch.h"
 #include "arm_builtins.h"
 #include "debug.h"
 #include "syscntl.h"
-#include "arm64.h"
 
 extern void el1_init_el0();
 extern bool el1_load_el0(uintptr_t base, uintptr_t *entry);
diff --git a/aarch64/el1_common/svc.c b/aarch64/el1_common/svc.c
deleted file mode 100644
index 3118702..0000000
--- a/aarch64/el1_common/svc.c
+++ /dev/null
@@ -1,14 +0,0 @@
-#include "svc.h"
-
-const char *svc_op_name[] = {
-    [SVC_OP_EXIT] = "SVC_OP_EXIT",
-    [SVC_OP_ALLOC] = "SVC_OP_ALLOC",
-    [SVC_OP_MAP] = "SVC_OP_MAP",
-    [SVC_OP_YIELD] = "SVC_OP_YIELD",
-    [SVC_OP_GET_SYSCNTL] = "SVC_OP_GET_SYSCNTL",
-    [SVC_OP_GET_REG] = "SVC_OP_GET_REG",
-    [SVC_OP_SET_REG] = "SVC_OP_SET_REG",
-    [SVC_OP_TEST] = "SVC_OP_TEST",
-    [SVC_OP_DISPATCH] = "SVC_OP_DISPATCH"
-};
-
diff --git a/aarch64/el1_ns/Makefile b/aarch64/el1_ns/Makefile
index c7e7419..f7ac0fd 100644
--- a/aarch64/el1_ns/Makefile
+++ b/aarch64/el1_ns/Makefile
@@ -1,4 +1,4 @@
-VPATH	= ../el1_common:../common
+VPATH	= $(ARCH):../el1_common/$(ARCH):../el1_common:../common
 
 EL1_NS_ELF		= el1_nsec.elf
 EL1_NS_IMAGE 	= el1_nsec.bin
@@ -8,7 +8,6 @@ EL1_NS_OBJS		= el1_init.o \
                   el1.o \
                   el1_loader.o \
                   el1_nsec.o \
-				  svc.o \
                   mem_util.o \
                   builtins.o
 
@@ -28,7 +27,7 @@ $(EL1_NS_LOAD): el1_nsec.lds.S Makefile ../../platform/$(PLAT)/
 	$(CC) $(CFLAGS) -E -P -C -o $@ $<
 
 %.o: %.S
-	$(CC) $(CFLAGS) -mcmodel=large -c -nostdlib -o $(notdir $@) $<
+	$(CC) $(CFLAGS) -c -nostdlib -o $(notdir $@) $<
 
 all: $(EL1_NS_IMAGE)
 
diff --git a/aarch64/el1_ns/el1.h b/aarch64/el1_ns/aarch64/el1.h
index 91a92ec..3f84ec6 100644
--- a/aarch64/el1_ns/el1.h
+++ b/aarch64/el1_ns/aarch64/el1.h
@@ -31,6 +31,11 @@ extern uintptr_t EL1_NS_DATA_SIZE;
 #define EL1_STACK_BASE EL1_NS_STACK_BASE
 #define EL1_PGTBL_BASE EL1_NS_PGTBL_BASE
 #define EL1_PGTBL_SIZE EL1_NS_PGTBL_SIZE
+#define EL1_INIT_STACK EL1_NS_INIT_STACK
+#define EL1_PA_POOL_BASE EL1_NS_PA_POOL_BASE
+#define EL1_PTE_POOL_BASE EL1_NS_PTE_POOL_BASE
+#define EL1_BASE_VA EL1_NS_BASE_VA
+#define EL1_VA_HEAP_BASE EL1_NS_VA_HEAP_BASE
 
 #define SEC_STATE_STR "non-secure"
 #define SEC_STATE 1
diff --git a/aarch64/el1_ns/el1_nsec.c b/aarch64/el1_ns/el1_nsec.c
index 7dcf94c..5bd5d22 100644
--- a/aarch64/el1_ns/el1_nsec.c
+++ b/aarch64/el1_ns/el1_nsec.c
@@ -5,7 +5,7 @@ uintptr_t EL1_NS_INIT_SIZE = (uintptr_t)&_EL1_NS_INIT_SIZE;
 uintptr_t EL1_NS_FLASH_TEXT = (uintptr_t)&_EL1_NS_FLASH_TEXT;
 uintptr_t EL1_NS_TEXT_BASE = (uintptr_t)&_EL1_NS_TEXT_BASE;
 uintptr_t EL1_NS_DATA_BASE = (uintptr_t)&_EL1_NS_DATA_BASE;
-uintptr_t EL1_NS_TEXT_SIZE = (uint64_t)&_EL1_NS_TEXT_SIZE;
+uintptr_t EL1_NS_TEXT_SIZE = (uintptr_t)&_EL1_NS_TEXT_SIZE;
 uintptr_t EL1_NS_DATA_SIZE = (uintptr_t)&_EL1_NS_DATA_SIZE;
 
 const char *sec_state_str = "non-secure";
@@ -13,11 +13,9 @@ const char *sec_state_str = "non-secure";
 void el1_init_el0()
 {
     uintptr_t main;
-    bool is_32 = false;
 
-    is_32 = el1_load_el0(EL0_NS_FLASH_BASE, &main);
+    el1_load_el0(EL0_NS_FLASH_BASE, &main);
+
+    __exception_return(main, SPSR_EL0);
 
-    if (!is_32) {
-        __exception_return(main, EL0T);
-    }
 }
diff --git a/aarch64/el1_ns/el1_nsec.lds.S b/aarch64/el1_ns/el1_nsec.lds.S
index 0aba7d9..21c1a33 100644
--- a/aarch64/el1_ns/el1_nsec.lds.S
+++ b/aarch64/el1_ns/el1_nsec.lds.S
@@ -1,5 +1,5 @@
-OUTPUT_FORMAT("elf64-littleaarch64")
-OUTPUT_ARCH(aarch64)
+OUTPUT_FORMAT(FORMAT)
+OUTPUT_ARCH(ARCH)
 TARGET(binary)
 
 #include "memory.h"
diff --git a/aarch64/el1_s/Makefile b/aarch64/el1_s/Makefile
index 317561f..2d46262 100644
--- a/aarch64/el1_s/Makefile
+++ b/aarch64/el1_s/Makefile
@@ -1,4 +1,4 @@
-VPATH	= ../el1_common:../common
+VPATH	= $(ARCH):../el1_common/$(ARCH):../el1_common:../common
 
 EL1_S_ELF		= el1_sec.elf
 EL1_S_IMAGE 	= el1_sec.bin
@@ -8,7 +8,6 @@ EL1_S_OBJS		= el1_init.o \
                   el1.o \
                   el1_loader.o \
                   el1_sec.o \
-				  svc.o \
                   mem_util.o \
 				  builtins.o
 
@@ -28,7 +27,7 @@ $(EL1_S_LOAD): el1_sec.lds.S Makefile ../../platform/$(PLAT)/
 	$(CC) $(CFLAGS) -E -P -C -o $@ $<
 
 %.o: %.S
-	$(CC) $(CFLAGS) -mcmodel=large -c -nostdlib -o $(notdir $@) $<
+	$(CC) $(CFLAGS) -c -nostdlib -o $(notdir $@) $<
 
 all: $(EL1_S_IMAGE)
 
diff --git a/aarch64/el1_s/el1.h b/aarch64/el1_s/aarch64/el1.h
index 8b36964..99cc302 100644
--- a/aarch64/el1_s/el1.h
+++ b/aarch64/el1_s/aarch64/el1.h
@@ -31,6 +31,11 @@ extern uintptr_t EL1_S_DATA_SIZE;
 #define EL1_STACK_BASE EL1_S_STACK_BASE
 #define EL1_PGTBL_BASE EL1_S_PGTBL_BASE
 #define EL1_PGTBL_SIZE EL1_S_PGTBL_SIZE
+#define EL1_INIT_STACK EL1_S_INIT_STACK
+#define EL1_PA_POOL_BASE EL1_S_PA_POOL_BASE
+#define EL1_PTE_POOL_BASE EL1_S_PTE_POOL_BASE
+#define EL1_BASE_VA EL1_S_BASE_VA
+#define EL1_VA_HEAP_BASE EL1_S_VA_HEAP_BASE
 
 #define SEC_STATE_STR "secure"
 #define SEC_STATE 0
diff --git a/aarch64/el1_s/el1_sec.c b/aarch64/el1_s/el1_sec.c
index 21f2a9c..36066c9 100644
--- a/aarch64/el1_s/el1_sec.c
+++ b/aarch64/el1_s/el1_sec.c
@@ -10,14 +10,55 @@ uintptr_t EL1_S_DATA_SIZE = (uintptr_t)&_EL1_S_DATA_SIZE;
 
 const char *sec_state_str = "secure";
 
+#if REMOVE_OR_INTEGRATE
+void el1_sec_check_init()
+{
+    printf("\nValidating startup state:\n");
+
+    printf("\tChecking for security extension ...");
+    int idpfr1 = 0;
+    /* Read the ID_PFR1 CP register and check that it is marked for support of
+     * the security extension.
+     */
+    __mrc(15, 0, idpfr1, 0, 1, 1);
+    if (0x10 != (idpfr1 & 0xf0)) {
+        printf("FAILED\n");
+        DEBUG_MSG("current IDPFR1 (%d) != expected IDPFR1 (%d)\n",
+                  (idpfr1 & 0xf0), 0x10);
+        exit(1);
+    } else {
+        printf("PASSED\n");
+    }
+
+    printf("\tChecking initial processor mode... ");
+    if (CPSR_M_SVC != (_read_cpsr() & 0x1f)) {
+        printf("FAILED\n");
+        DEBUG_MSG("current CPSR (%d) != expected CPSR (%d)\n",
+                  (_read_cpsr() & 0x1f), CPSR_M_SVC);
+        assert(CPSR_M_SVC == (_read_cpsr() & 0x1f));
+    } else {
+        printf("PASSED\n");
+    }
+
+    // Test: Check that on reset if sec et present, starts in sec state
+    //      pg. B1-1204
+    printf("\tChecking initial security state... ");
+    if (0 != (_read_scr() & SCR_NS)) {
+        printf("Failed\n");
+        DEBUG_MSG("current SCR.NS (%d) != expected SCR.NS (%d)\n",
+                  (_read_cpsr() & SCR_NS), 0);
+        assert(0 == (_read_scr() & SCR_NS));
+    } else {
+        printf("PASSED\n");
+    }
+}
+#endif
+
 void el1_init_el0()
 {
     uintptr_t main;
-    bool is_32 = false;
 
-    is_32 = el1_load_el0(EL0_S_FLASH_BASE, &main);
+    el1_load_el0(EL0_S_FLASH_BASE, &main);
 
-    if (!is_32) {
-        __exception_return(main, EL0T);
-    }
+        __exception_return(main, SPSR_EL0);
 }
diff --git a/aarch64/el1_s/el1_sec.lds.S b/aarch64/el1_s/el1_sec.lds.S
index ab440e2..14b4b47 100644
--- a/aarch64/el1_s/el1_sec.lds.S
+++ b/aarch64/el1_s/el1_sec.lds.S
@@ -1,5 +1,5 @@
-OUTPUT_FORMAT("elf64-littleaarch64")
-OUTPUT_ARCH(aarch64)
+OUTPUT_FORMAT(FORMAT)
+OUTPUT_ARCH(ARCH)
 TARGET(binary)
 
 #include "memory.h"
diff --git a/aarch64/el3/Makefile b/aarch64/el3/Makefile
index e3bd55c..a0b2373 100644
--- a/aarch64/el3/Makefile
+++ b/aarch64/el3/Makefile
@@ -21,8 +21,7 @@ $(EL3_IMAGE): $(EL3_ELF)
 	$(OBJCOPY) -O binary $< $@
 
 $(EL3_LOAD): el3.lds.S Makefile ../../platform/$(PLAT)/
-	$(CC) $(CFLAGS) -DARCH=$(ARCH) \
-		-E -P -C -o $@ $<
+	$(CC) $(CFLAGS) -E -P -C -o $@ $<
 
 %.o: %.S
 	$(CC) $(CFLAGS) -c -nostdlib -o $@ $<