1 files changed, 64 insertions, 0 deletions
diff --git a/aarch64/el1_common/el1_loader.c b/aarch64/el1_common/el1_loader.c
new file mode 100644
index 0000000..43a73ce
--- /dev/null
+++ b/aarch64/el1_common/el1_loader.c
@@ -0,0 +1,64 @@
+#include "el1_common.h"
+#include "elf.h"
+
+/* Simple ELF loader for loading EL0 image */
+void *el1_load_el0(char *elfbase, char *start_va)
+{
+    Elf64_Ehdr *ehdr = (Elf64_Ehdr *)elfbase;
+    size_t off;
+    int i;
+
+    /* Map the ELF header in so we can determine how much more to map */
+    el1_map_pa((uint64_t)elfbase, (uint64_t)elfbase);
+
+    /* Make sure this is an appropriate ELF image */
+    if (ehdr->e_ident[EI_MAG0] != ELFMAG0 ||
+        ehdr->e_ident[EI_MAG1] != ELFMAG1 ||
+        ehdr->e_ident[EI_MAG2] != ELFMAG2 ||
+        ehdr->e_ident[EI_MAG3] != ELFMAG3) {
+        printf("Invalid ELF header, exiting...\n");
+        SMC_EXIT();
+    } else if (ehdr->e_type != ET_DYN &&
+               (ehdr->e_machine != EM_ARM || ehdr->e_machine != EM_AARCH64)) {
+        printf("Incorrect ELF type (type = %d, machine = %d), exiting...\n",
+               ehdr->e_type, ehdr->e_machine);
+        SMC_EXIT();
+    } else {
+        printf("Loading %s EL0 test image...\n",
+               (ehdr->e_machine == EM_ARM) ?  "aarch32" : "aarch64");
+    }
+
+    /* Size of the ELF to map */
+    size_t elf_len = ehdr->e_shoff + (ehdr->e_shentsize * ehdr->e_shnum);
+
+    /* Finish mapping the remainder of the ELF pages in if any */
+    for (off = 0x1000; off < elf_len; off += 0x1000) {
+        el1_map_pa((uint64_t)elfbase + off, (uint64_t)elfbase + off);
+    }
+
+    Elf64_Shdr *shdr = (Elf64_Shdr *)((char *)elfbase + ehdr->e_shoff);
+
+    Elf64_Shdr *strshdr = &shdr[ehdr->e_shstrndx];
+    char *strsec = (char *)ehdr + strshdr->sh_offset;
+    for (i = 0; i < ehdr->e_shnum; i++) {
+        char *secname = strsec + shdr[i].sh_name;
+        if (!strcmp(secname, ".text") || !strcmp(secname, ".data")) {
+            uint64_t sect = (uint64_t)((char *)elfbase + shdr[i].sh_offset);
+            char *base_va = start_va + shdr[i].sh_addr;
+            DEBUG_MSG("\tloading %s section: 0x%x bytes @ 0x%lx\n",
+                      secname, shdr[i].sh_size, base_va);
+            for (off = 0; off < shdr[i].sh_size; off += 0x1000) {
+                el1_map_va((uintptr_t)(base_va + off));
+                memcpy((void *)(base_va + off), (void *)(sect + off), 0x1000);
+            }
+        }
+    }
+
+    /* Unmap the FLASH ELF image */
+    for (off = 0; off < elf_len; off += 0x1000) {
+        el1_map_va((uint64_t)elfbase + off);
+    }
+
+    return (void *)(start_va + ehdr->e_entry);
+}
+