From 99c5c75599e1dde6b1027256fb7cc0c65d9874ef Mon Sep 17 00:00:00 2001 From: Greg Bellows Date: Fri, 27 Mar 2015 16:31:42 -0500 Subject: General cleanup Signed-off-by: Greg Bellows --- aarch64/Makefile | 1 + aarch64/common/arm_builtins.h | 12 ++++ aarch64/common/armv8_vmsa.h | 1 + aarch64/common/interop.h | 6 +- aarch64/common/smc.h | 12 ---- aarch64/el0_common/tztest.c | 54 ++++++++++-------- aarch64/el1_common/el1.c | 123 +++++++--------------------------------- aarch64/el1_common/el1_common.h | 27 +++++++++ aarch64/el1_common/el1_init.S | 8 +-- aarch64/el1_common/el1_loader.c | 64 +++++++++++++++++++++ aarch64/el1_ns/Makefile | 3 +- aarch64/el1_ns/el1.h | 17 ++++++ aarch64/el1_ns/el1_loader.h | 13 ----- aarch64/el1_ns/el1_nsec.c | 13 +++-- aarch64/el1_s/Makefile | 3 +- aarch64/el1_s/el1.h | 17 ++++++ aarch64/el1_s/el1_loader.h | 19 ------- aarch64/el1_s/el1_sec.c | 15 ++--- aarch64/el3/el3.c | 58 ++++++++++++------- aarch64/el3/el3_init.S | 6 +- 20 files changed, 258 insertions(+), 214 deletions(-) create mode 100644 aarch64/el1_common/el1_common.h create mode 100644 aarch64/el1_common/el1_loader.c delete mode 100644 aarch64/el1_ns/el1_loader.h delete mode 100644 aarch64/el1_s/el1_loader.h diff --git a/aarch64/Makefile b/aarch64/Makefile index 75f23e1..fc58b07 100644 --- a/aarch64/Makefile +++ b/aarch64/Makefile @@ -6,6 +6,7 @@ TZTEST_NS_IMAGE = el0_ns/el0_nsec.elf TZTEST_S_IMAGE = el0_s/el0_sec.elf CFLAGS += -I../../platform/$(PLAT) -I../../libcflat/include -I../common +CFLAGS += -DAARCH64 -include .*.d diff --git a/aarch64/common/arm_builtins.h b/aarch64/common/arm_builtins.h index 940b2fd..1ccbebe 100644 --- a/aarch64/common/arm_builtins.h +++ b/aarch64/common/arm_builtins.h @@ -53,4 +53,16 @@ extern void write_cpacr_el1(uint64_t); extern uint64_t read_sctlr_el1(); extern void write_sctlr_el1(uint64_t); extern void __set_exception_return(uint64_t); + +#define READ_SCR() read_scr_el3() +#define WRITE_SCR(_val) write_scr_el3(_val) +#define READ_SDER() read_sder32_el3() +#define WRITE_SDER(_val) write_sder32_el3(_val) +#define READ_CPTR_EL3() read_cptr_el3() +#define WRITE_CPTR_EL3(_val) write_cptr_el3(_val) +#define READ_CPACR() read_cpacr_el1() +#define WRITE_CPACR(_val) write_cpacr_el1(_val) +#define READ_CURRENTEL() read_currentel() +#define WRITE_CURRENTEL(_val) read_currentel(_val) + #endif diff --git a/aarch64/common/armv8_vmsa.h b/aarch64/common/armv8_vmsa.h index 534747c..fda6c46 100644 --- a/aarch64/common/armv8_vmsa.h +++ b/aarch64/common/armv8_vmsa.h @@ -36,6 +36,7 @@ typedef union { } armv8_4k_tbl_pte_t; */ +#define PAGE_SIZE 0x1000 #define ARMV8_PAGE_ATTRINDX_SHIFT 2 #define ARMV8_PAGE_NS_SHIFT 5 #define ARMV8_PAGE_AP_SHIFT 6 diff --git a/aarch64/common/interop.h b/aarch64/common/interop.h index fcd7448..c4ac039 100644 --- a/aarch64/common/interop.h +++ b/aarch64/common/interop.h @@ -33,9 +33,9 @@ typedef struct { typedef enum { CURRENTEL = 1, CPTR_EL3, - CPACR_EL1, - SCR_EL3, - SCTLR_EL1, + CPACR, + SCR, + SCTLR, } op_reg_key_t; typedef struct { diff --git a/aarch64/common/smc.h b/aarch64/common/smc.h index 5b16b5e..b54991e 100644 --- a/aarch64/common/smc.h +++ b/aarch64/common/smc.h @@ -19,18 +19,6 @@ extern uint32_t __smc(uint32_t, void *); -const char *smc_op_name[] = { - [SMC_OP_NOOP] = "SMC_OP_NOOP", - [SMC_OP_DISPATCH_MONITOR] = "SMC_DISPATCH_MONITOR", - [SMC_OP_YIELD] = "SMC_OP_YIELD", - [SMC_OP_EXIT] = "SMC_OP_EXIT", - [SMC_OP_MAP] = "SMC_OP_MAP", - [SMC_OP_GET_REG] = "SMC_OP_GET_REG", - [SMC_OP_SET_REG] = "SMC_OP_SET_REG", - [SMC_OP_TEST] = "SMC_OP_TEST", - [SMC_OP_DISPATCH] = "SMC_OP_DISPATCH", -}; - typedef union { op_dispatch_t dispatch; op_map_mem_t map; diff --git a/aarch64/el0_common/tztest.c b/aarch64/el0_common/tztest.c index 477500c..f477433 100644 --- a/aarch64/el0_common/tztest.c +++ b/aarch64/el0_common/tztest.c @@ -31,37 +31,40 @@ uint32_t check_register_access(uint32_t el) TEST_HEAD("restricted register access"); TEST_MSG("SCR read"); - TEST_EL1_EXCEPTION(read_scr_el3(), EC_UNKNOWN); + TEST_EL1_EXCEPTION(READ_SCR(), EC_UNKNOWN); TEST_MSG("SCR write"); - TEST_EL1_EXCEPTION(write_scr_el3(0), EC_UNKNOWN); + TEST_EL1_EXCEPTION(WRITE_SCR(0), EC_UNKNOWN); TEST_MSG("SDER read"); - TEST_EL1_EXCEPTION(read_sder32_el3(), EC_UNKNOWN); + TEST_EL1_EXCEPTION(READ_SDER(), EC_UNKNOWN); TEST_MSG("SDER write"); - TEST_EL1_EXCEPTION(write_sder32_el3(0), EC_UNKNOWN); + TEST_EL1_EXCEPTION(WRITE_SDER(0), EC_UNKNOWN); -/* +#ifdef AARCH32 TEST_MSG("MVBAR read"); - TEST_EL1_EXCEPTION(read_mvbar(), EC_UNKNOWN); + TEST_EL1_EXCEPTION(READ_MVBAR(), EC_UNKNOWN); TEST_MSG("MVBAR write"); - TEST_EL1_EXCEPTION(write_mvbar(0), EC_UNKNOWN); + TEST_EL1_EXCEPTION(WRITE_MVBAR(0), EC_UNKNOWN); TEST_MSG("NSACR write"); - TEST_EL1_EXCEPTION(write_nsacr(0), EC_UNKNOWN); -*/ + TEST_EL1_EXCEPTION(WRITE_NSACR(0), EC_UNKNOWN); +#endif +#ifdef AARCH64 TEST_MSG("CPTR_EL3 read"); - TEST_EL1_EXCEPTION(read_cptr_el3(), EC_UNKNOWN); + TEST_EL1_EXCEPTION(READ_CPTR_EL3(), EC_UNKNOWN); TEST_MSG("CPTR_EL3 write"); - TEST_EL1_EXCEPTION(write_cptr_el3(0), EC_UNKNOWN); + TEST_EL1_EXCEPTION(WRITE_CPTR_EL3(0), EC_UNKNOWN); +#endif return 0; } +#ifdef AARCH64 uint32_t check_cpacr_trap(uint32_t el) { uint64_t cptr_el3, cpacr; @@ -76,11 +79,11 @@ uint32_t check_cpacr_trap(uint32_t el) /* Try to read CPACR */ TEST_MSG("Read of disabled CPACR"); - TEST_EL3_EXCEPTION(SVC_GET_REG(CPACR_EL1, 1, cpacr), EC_SYSINSN); + TEST_EL3_EXCEPTION(SVC_GET_REG(CPACR, 1, cpacr), EC_SYSINSN); /* Try to write CPACR */ TEST_MSG("Write of disabled CPACR"); - TEST_EL3_EXCEPTION(SVC_SET_REG(CPACR_EL1, 1, cpacr), EC_SYSINSN); + TEST_EL3_EXCEPTION(SVC_SET_REG(CPACR, 1, cpacr), EC_SYSINSN); #ifdef FP_TEST /* Disable FP access */ @@ -97,18 +100,18 @@ uint32_t check_cpacr_trap(uint32_t el) uint32_t check_wfx_trap(uint32_t el) { - uint64_t sctlr, scr_el3; + uint64_t sctlr, scr; TEST_HEAD("WFx traps"); /* Get the current SCR so we can restore it later */ - SVC_GET_REG(SCR_EL3, 3, scr_el3); + SVC_GET_REG(SCR, 3, scr); /* Get the current SCTLR so we can restore it later */ - SVC_GET_REG(SCTLR_EL1, 1, sctlr); + SVC_GET_REG(SCTLR, 1, sctlr); /* Clear SCTLR.nTWE to cause WFE instructions to trap to EL1 */ - SVC_SET_REG(SCTLR_EL1, 1, sctlr & ~SCTLR_nTWE); + SVC_SET_REG(SCTLR, 1, sctlr & ~SCTLR_nTWE); TEST_MSG("Execution of WFE trapped to EL1"); TEST_EL1_EXCEPTION(asm volatile("wfe\n"), EC_WFI_WFE); @@ -117,13 +120,13 @@ uint32_t check_wfx_trap(uint32_t el) /* Trap WFE instructions to EL3. This should work even though SCTLR.nTWE * is clear */ - SVC_SET_REG(SCR_EL3, 3, scr_el3 | SCR_WFE); + SVC_SET_REG(SCR, 3, scr | SCR_WFE); TEST_MSG("Execution of trapped WFE (SCTLR.nTWE clear)", SEC_STATE_STR); TEST_EL3_EXCEPTION(asm volatile("wfe\n"), EC_WFI_WFE); /* Restore SCTLR */ - SVC_SET_REG(SCTLR_EL1, 1, sctlr); + SVC_SET_REG(SCTLR, 1, sctlr); /* This should trap to EL3 with SCTLR.nTWE set */ TEST_MSG("Execution of trapped WFE (SCTLR.nTWE set)", @@ -131,10 +134,10 @@ uint32_t check_wfx_trap(uint32_t el) TEST_EL3_EXCEPTION(asm volatile("wfe\n"), EC_WFI_WFE); /* Restore SCR */ - SVC_SET_REG(SCR_EL3, 3, scr_el3); + SVC_SET_REG(SCR, 3, scr); /* Clear SCTLR.nTWI to cause WFI instructions to trap to EL1 */ - SVC_SET_REG(SCTLR_EL1, 1, sctlr & ~SCTLR_nTWI); + SVC_SET_REG(SCTLR, 1, sctlr & ~SCTLR_nTWI); TEST_MSG("Execution of WFI trapped to EL1"); TEST_EL1_EXCEPTION(asm volatile("wfi\n"), EC_WFI_WFE); @@ -143,30 +146,33 @@ uint32_t check_wfx_trap(uint32_t el) /* Trap WFI instructions to EL3. This should work even though SCTLR.nTWE * is clear */ - SVC_SET_REG(SCR_EL3, 3, scr_el3 | SCR_WFI); + SVC_SET_REG(SCR, 3, scr | SCR_WFI); TEST_MSG("Execution of trapped WFI (SCTLR.nTWI clear)", SEC_STATE_STR); TEST_EL3_EXCEPTION(asm volatile("wfi\n"), EC_WFI_WFE); /* Restore SCTLR */ - SVC_SET_REG(SCTLR_EL1, 1, sctlr); + SVC_SET_REG(SCTLR, 1, sctlr); TEST_MSG("Execution of trapped WFI (SCTLR.nTWI set)", SEC_STATE_STR); TEST_EL3_EXCEPTION(asm volatile("wfi\n"), EC_WFI_WFE); /* Restore SCR */ - SVC_SET_REG(SCR_EL3, 3, scr_el3); + SVC_SET_REG(SCR, 3, scr); return 0; } +#endif void tztest_init() { tztest[TZTEST_SMC] = check_smc; tztest[TZTEST_REG_ACCESS] = check_register_access; +#ifdef AARCH64 tztest[TZTEST_CPACR_TRAP] = check_cpacr_trap; tztest[TZTEST_WFX_TRAP] = check_wfx_trap; +#endif } diff --git a/aarch64/el1_common/el1.c b/aarch64/el1_common/el1.c index 29551eb..539e9ab 100644 --- a/aarch64/el1_common/el1.c +++ b/aarch64/el1_common/el1.c @@ -1,25 +1,12 @@ -#include "libcflat.h" -#include "platform.h" -#include "smc.h" -#include "svc.h" -#include "string.h" -#include "el1.h" -#include "el1_loader.h" -#include "armv8_exception.h" -#include "armv8_vmsa.h" -#include "arm_builtins.h" -#include "elf.h" -#include "debug.h" -#include "syscntl.h" - -extern void el1_init_el0(); +#include "el1_common.h" smc_op_desc_t *smc_interop_buf; sys_control_t *syscntl; uint64_t el1_next_pa = 0; uint64_t el1_heap_pool = 0x40000000; -uint64_t el1_allocate_pa() { +uint64_t el1_allocate_pa() +{ uint64_t next = el1_next_pa; el1_next_pa += 0x1000; return next; @@ -213,16 +200,16 @@ int el1_handle_svc(uint32_t op, svc_op_desc_t *desc) if (desc->get.el == 1) { switch (desc->get.key) { case CURRENTEL: - desc->get.data = read_currentel(); + desc->get.data = READ_CURRENTEL(); break; case CPTR_EL3: - desc->get.data = read_cptr_el3(); + desc->get.data = READ_CPTR_EL3(); break; - case CPACR_EL1: - desc->get.data = read_cpacr_el1(); + case CPACR: + desc->get.data = READ_CPACR(); break; - case SCR_EL3: - desc->get.data = read_scr_el3(); + case SCR: + desc->get.data = READ_SCR(); break; } } else if (desc->get.el == 3) { @@ -235,16 +222,16 @@ int el1_handle_svc(uint32_t op, svc_op_desc_t *desc) if (desc->set.el == 1) { switch (desc->set.key) { case CURRENTEL: - write_currentel(desc->set.data); + WRITE_CURRENTEL(desc->set.data); break; case CPTR_EL3: - write_cptr_el3(desc->set.data); + WRITE_CPTR_EL3(desc->set.data); break; - case CPACR_EL1: - write_cpacr_el1(desc->set.data); + case CPACR: + WRITE_CPACR(desc->set.data); break; - case SCR_EL3: - write_scr_el3(desc->set.data); + case SCR: + WRITE_SCR(desc->set.data); break; } } else if (desc->set.el == 3) { @@ -270,10 +257,6 @@ int el1_handle_svc(uint32_t op, svc_op_desc_t *desc) void el1_handle_exception(uint64_t ec, uint64_t iss) { -#ifdef DEBUG - armv8_data_abort_iss_t dai = {.raw = iss}; -// armv8_inst_abort_iss_t iai = {.raw = iss}; -#endif uint64_t elr, far; __get_exception_address(far); @@ -305,13 +288,12 @@ void el1_handle_exception(uint64_t ec, uint64_t iss) SMC_EXIT(); break; case EC_DABORT_LOWER: - DEBUG_MSG("Data abort (%s) at lower level: far = %0lx elr = %0lx\n", - dai.wnr ? "write" : "read", far, elr); + DEBUG_MSG("Data abort at lower level: far = %0lx elr = %0lx\n", + far, elr); SMC_EXIT(); break; case EC_DABORT: - DEBUG_MSG("Data abort (%s) at EL1: far = %0lx elr = %0lx\n", - dai.wnr ? "write" : "read", far, elr); + DEBUG_MSG("Data abort at EL1: far = %0lx elr = %0lx\n", far, elr); SMC_EXIT(); break; case EC_WFI_WFE: @@ -325,73 +307,12 @@ void el1_handle_exception(uint64_t ec, uint64_t iss) } break; default: - DEBUG_MSG("Unhandled EL1 exception: EC = %d ISS = %d\n", ec, iss); + DEBUG_MSG("Unhandled EL1 exception: ec = %d iss = %d\n", ec, iss); SMC_EXIT(); break; } } -/* 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); -} - void el1_start(uint64_t base, uint64_t size) { uint64_t addr = base; @@ -400,13 +321,13 @@ void el1_start(uint64_t base, uint64_t size) printf("EL1 (%s) started...\n", SEC_STATE_STR); /* Unmap the init segement so we don't accidentally use it */ - for (len = 0; len < ((size + 0xFFF) & ~0xFFF); - len += 0x1000, addr += 0x1000) { + for (len = 0; len < ((size + (PAGE_SIZE - 1)) & ~(PAGE_SIZE - 1)); + len += PAGE_SIZE, addr += PAGE_SIZE) { el1_unmap_va(addr); } void *pa = syscntl; - syscntl = (sys_control_t *)el1_heap_allocate(0x1000); + syscntl = (sys_control_t *)el1_heap_allocate(PAGE_SIZE); el1_map_pa((uintptr_t)syscntl, (uintptr_t)pa); el1_map_pa((uintptr_t)syscntl->smc_interop.buf_va, (uintptr_t)syscntl->smc_interop.buf_pa); diff --git a/aarch64/el1_common/el1_common.h b/aarch64/el1_common/el1_common.h new file mode 100644 index 0000000..4b6493d --- /dev/null +++ b/aarch64/el1_common/el1_common.h @@ -0,0 +1,27 @@ +#ifndef __EL1_COMMON_H +#define __EL1_COMMON_H + +#include "libcflat.h" +#include "platform.h" +#include "svc.h" +#include "smc.h" +#include "string.h" +#include "el1.h" +#include "armv8_exception.h" +#include "armv8_vmsa.h" +#include "arm_builtins.h" +#include "debug.h" +#include "syscntl.h" + +extern void el1_init_el0(); +extern void *el1_load_el0(char *base, char *start_va); +extern uint64_t el1_allocate_pa(); +extern void el1_map_pa(uintptr_t vaddr, uintptr_t paddr); +extern void el1_map_va(uintptr_t addr); +extern int el1_unmap_va(uint64_t addr); +extern void *el1_heap_allocate(size_t len); +extern void el1_alloc_mem(op_alloc_mem_t *alloc); +extern void *el1_lookup_pa(void *va); +extern void el1_map_secure(op_map_mem_t *map); + +#endif diff --git a/aarch64/el1_common/el1_init.S b/aarch64/el1_common/el1_init.S index 0faa564..ec3631a 100644 --- a/aarch64/el1_common/el1_init.S +++ b/aarch64/el1_common/el1_init.S @@ -16,11 +16,11 @@ el1_init: /* 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 + ldr x10, =RAM_BASE + (2 * PAGE_SIZE) mov sp, x10 /* Use the top of the stack to track our PA pool pointer */ - ldr x10, =EL1_PGTBL_BASE+0x1000 + ldr x10, =EL1_PGTBL_BASE + PAGE_SIZE str x10, [sp] /* Save the input SMC interop buf pointer */ @@ -82,11 +82,11 @@ el1_map_data: el1_map_stacks: /* Map the first page of the stack so we can get off the ground */ - ldr x0, =EL1_STACK_BASE-0x1000 + ldr x0, =EL1_STACK_BASE - PAGE_SIZE mov x1, #(PTE_PAGE|PTE_ACCESS|PTE_PRIV_RW) bl map_va - ldr x0, =EL0_STACK_BASE-0x1000 + ldr x0, =EL0_STACK_BASE - PAGE_SIZE mov x1, #(PTE_PAGE|PTE_ACCESS|PTE_USER_RW) bl map_va 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); +} + diff --git a/aarch64/el1_ns/Makefile b/aarch64/el1_ns/Makefile index 6a6b97c..351eeff 100644 --- a/aarch64/el1_ns/Makefile +++ b/aarch64/el1_ns/Makefile @@ -6,13 +6,14 @@ EL1_NS_LOAD = el1_nsec.lds EL1_NS_OBJS = el1_init.o \ el1_exception.o \ el1.o \ + el1_loader.o \ el1_nsec.o \ svc.o \ builtins.o -include .*.d -CFLAGS += -I../el0_ns/ +CFLAGS += -I../el0_ns/ -I../el1_common ################################################################## diff --git a/aarch64/el1_ns/el1.h b/aarch64/el1_ns/el1.h index 5e641f9..dd10a95 100644 --- a/aarch64/el1_ns/el1.h +++ b/aarch64/el1_ns/el1.h @@ -3,6 +3,23 @@ #include "platform.h" +#ifndef __ASSEMBLY__ +extern uintptr_t _EL1_NS_INIT_BASE; +extern uintptr_t EL1_NS_INIT_BASE; +extern uintptr_t _EL1_NS_INIT_SIZE; +extern uintptr_t EL1_NS_INIT_SIZE; +extern uintptr_t _EL1_NS_FLASH_TEXT; +extern uintptr_t EL1_NS_FLASH_TEXT; +extern uintptr_t _EL1_NS_TEXT_BASE; +extern uintptr_t EL1_NS_TEXT_BASE; +extern uintptr_t _EL1_NS_DATA_BASE; +extern uintptr_t EL1_NS_DATA_BASE; +extern uintptr_t _EL1_NS_TEXT_SIZE; +extern uintptr_t EL1_NS_TEXT_SIZE; +extern uintptr_t _EL1_NS_DATA_SIZE; +extern uintptr_t EL1_NS_DATA_SIZE; +#endif + #define _EL1_INIT_BASE _EL1_NS_INIT_BASE #define _EL1_INIT_SIZE _EL1_NS_INIT_SIZE #define _EL1_FLASH_TEXT _EL1_NS_FLASH_TEXT diff --git a/aarch64/el1_ns/el1_loader.h b/aarch64/el1_ns/el1_loader.h deleted file mode 100644 index 739e5ea..0000000 --- a/aarch64/el1_ns/el1_loader.h +++ /dev/null @@ -1,13 +0,0 @@ -#ifndef _EL1_NS_LOADER_H -#define _EL1_NS_LOADER_H - -extern uintptr_t _EL1_NS_TEXT_BASE; -uintptr_t EL1_NS_TEXT_BASE = (uintptr_t)&_EL1_NS_TEXT_BASE; -extern uintptr_t _EL1_NS_DATA_BASE; -uintptr_t EL1_NS_DATA_BASE = (uintptr_t)&_EL1_NS_DATA_BASE; -extern uintptr_t _EL1_NS_TEXT_SIZE; -uint64_t EL1_NS_TEXT_SIZE = (uint64_t)&_EL1_NS_TEXT_SIZE; -extern uintptr_t _EL1_NS_DATA_SIZE; -uint64_t EL1_NS_DATA_SIZE = (uint64_t)&_EL1_NS_DATA_SIZE; - -#endif diff --git a/aarch64/el1_ns/el1_nsec.c b/aarch64/el1_ns/el1_nsec.c index 79dc4cb..4dc8988 100644 --- a/aarch64/el1_ns/el1_nsec.c +++ b/aarch64/el1_ns/el1_nsec.c @@ -1,9 +1,12 @@ -#include "platform.h" -#include "arm_builtins.h" -#include "el1.h" -#include "debug.h" +#include "el1_common.h" -extern void *el1_load_el0(char *base, char *start_va); +uintptr_t EL1_NS_INIT_BASE = (uintptr_t)&_EL1_NS_INIT_BASE; +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_DATA_SIZE = (uintptr_t)&_EL1_NS_DATA_SIZE; void el1_init_el0() { diff --git a/aarch64/el1_s/Makefile b/aarch64/el1_s/Makefile index 0396603..5eebd65 100644 --- a/aarch64/el1_s/Makefile +++ b/aarch64/el1_s/Makefile @@ -6,13 +6,14 @@ EL1_S_LOAD = el1_sec.lds EL1_S_OBJS = el1_init.o \ el1_exception.o \ el1.o \ + el1_loader.o \ el1_sec.o \ svc.o \ builtins.o -include .*.d -CFLAGS += -I../el0_s/ +CFLAGS += -I../el0_s/ -I../el1_common ################################################################## diff --git a/aarch64/el1_s/el1.h b/aarch64/el1_s/el1.h index a8fc6ad..d64db1f 100644 --- a/aarch64/el1_s/el1.h +++ b/aarch64/el1_s/el1.h @@ -3,6 +3,23 @@ #include "platform.h" +#ifndef __ASSEMBLY__ +extern uintptr_t _EL1_S_INIT_BASE; +extern uintptr_t EL1_S_INIT_BASE; +extern uintptr_t _EL1_S_INIT_SIZE; +extern uintptr_t EL1_S_INIT_SIZE; +extern uintptr_t _EL1_S_FLASH_TEXT; +extern uintptr_t EL1_S_FLASH_TEXT; +extern uintptr_t _EL1_S_TEXT_BASE; +extern uintptr_t EL1_S_TEXT_BASE; +extern uintptr_t _EL1_S_DATA_BASE; +extern uintptr_t EL1_S_DATA_BASE; +extern uintptr_t _EL1_S_TEXT_SIZE; +extern uintptr_t EL1_S_TEXT_SIZE; +extern uintptr_t _EL1_S_DATA_SIZE; +extern uintptr_t EL1_S_DATA_SIZE; +#endif + #define _EL1_INIT_BASE _EL1_S_INIT_BASE #define _EL1_INIT_SIZE _EL1_S_INIT_SIZE #define _EL1_FLASH_TEXT _EL1_S_FLASH_TEXT diff --git a/aarch64/el1_s/el1_loader.h b/aarch64/el1_s/el1_loader.h deleted file mode 100644 index 10d283d..0000000 --- a/aarch64/el1_s/el1_loader.h +++ /dev/null @@ -1,19 +0,0 @@ -#ifndef _EL1_S_LOADER_H -#define _EL1_S_LOADER_H - -extern uintptr_t _EL1_S_INIT_BASE; -uintptr_t EL1_S_INIT_BASE = (uintptr_t)&_EL1_S_INIT_BASE; -extern uintptr_t _EL1_S_INIT_SIZE; -uintptr_t EL1_S_INIT_SIZE = (uintptr_t)&_EL1_S_INIT_SIZE; -extern uintptr_t _EL1_S_FLASH_TEXT; -uintptr_t EL1_S_FLASH_TEXT = (uintptr_t)&_EL1_S_FLASH_TEXT; -extern uintptr_t _EL1_S_TEXT_BASE; -uintptr_t EL1_S_TEXT_BASE = (uintptr_t)&_EL1_S_TEXT_BASE; -extern uintptr_t _EL1_S_DATA_BASE; -uintptr_t EL1_S_DATA_BASE = (uintptr_t)&_EL1_S_DATA_BASE; -extern uintptr_t _EL1_S_TEXT_SIZE; -uint64_t EL1_S_TEXT_SIZE = (uint64_t)&_EL1_S_TEXT_SIZE; -extern uintptr_t _EL1_S_DATA_SIZE; -uint64_t EL1_S_DATA_SIZE = (uint64_t)&_EL1_S_DATA_SIZE; - -#endif diff --git a/aarch64/el1_s/el1_sec.c b/aarch64/el1_s/el1_sec.c index 2aea580..3fc4d5b 100644 --- a/aarch64/el1_s/el1_sec.c +++ b/aarch64/el1_s/el1_sec.c @@ -1,11 +1,12 @@ -#include "platform.h" -#include "arm_builtins.h" -#include "libcflat.h" -#include -#include "el1.h" -#include "debug.h" +#include "el1_common.h" -extern void *el1_load_el0(char *base, char *start_va); +uintptr_t EL1_S_INIT_BASE = (uintptr_t)&_EL1_S_INIT_BASE; +uintptr_t EL1_S_INIT_SIZE = (uintptr_t)&_EL1_S_INIT_SIZE; +uintptr_t EL1_S_FLASH_TEXT = (uintptr_t)&_EL1_S_FLASH_TEXT; +uintptr_t EL1_S_TEXT_BASE = (uintptr_t)&_EL1_S_TEXT_BASE; +uintptr_t EL1_S_DATA_BASE = (uintptr_t)&_EL1_S_DATA_BASE; +uintptr_t EL1_S_TEXT_SIZE = (uint64_t)&_EL1_S_TEXT_SIZE; +uintptr_t EL1_S_DATA_SIZE = (uint64_t)&_EL1_S_DATA_SIZE; void el1_init_el0() { diff --git a/aarch64/el3/el3.c b/aarch64/el3/el3.c index ac4919e..2961a2f 100644 --- a/aarch64/el3/el3.c +++ b/aarch64/el3/el3.c @@ -15,6 +15,20 @@ #define SEC_STATE_STR "EL3" #include "debug.h" +#if DEBUG +const char *smc_op_name[] = { + [SMC_OP_NOOP] = "SMC_OP_NOOP", + [SMC_OP_DISPATCH_MONITOR] = "SMC_DISPATCH_MONITOR", + [SMC_OP_YIELD] = "SMC_OP_YIELD", + [SMC_OP_EXIT] = "SMC_OP_EXIT", + [SMC_OP_MAP] = "SMC_OP_MAP", + [SMC_OP_GET_REG] = "SMC_OP_GET_REG", + [SMC_OP_SET_REG] = "SMC_OP_SET_REG", + [SMC_OP_TEST] = "SMC_OP_TEST", + [SMC_OP_DISPATCH] = "SMC_OP_DISPATCH", +}; +#endif + state_buf sec_state; state_buf nsec_state; @@ -211,16 +225,16 @@ int el3_handle_smc(uint64_t op, smc_op_desc_t *desc) if (desc->get.el == 3) { switch (desc->get.key) { case CURRENTEL: - desc->get.data = read_currentel(); + desc->get.data = READ_CURRENTEL(); break; case CPTR_EL3: - desc->get.data = read_cptr_el3(); + desc->get.data = READ_CPTR_EL3(); break; - case CPACR_EL1: - desc->get.data = read_cpacr_el1(); + case CPACR: + desc->get.data = READ_CPACR(); break; - case SCR_EL3: - desc->get.data = read_scr_el3(); + case SCR: + desc->get.data = READ_SCR(); break; } } @@ -229,16 +243,16 @@ int el3_handle_smc(uint64_t op, smc_op_desc_t *desc) if (desc->set.el == 3) { switch (desc->set.key) { case CURRENTEL: - write_currentel(desc->set.data); + WRITE_CURRENTEL(desc->set.data); break; case CPTR_EL3: - write_cptr_el3(desc->set.data); + WRITE_CPTR_EL3(desc->set.data); break; - case CPACR_EL1: - write_cpacr_el1(desc->set.data); + case CPACR: + WRITE_CPACR(desc->set.data); break; - case SCR_EL3: - write_scr_el3(desc->set.data); + case SCR: + WRITE_SCR(desc->set.data); break; } } @@ -254,7 +268,6 @@ int el3_handle_smc(uint64_t op, smc_op_desc_t *desc) int el3_handle_exception(uint64_t ec, uint64_t iss) { - armv8_data_abort_iss_t dai = {.raw = iss}; uint64_t elr, far; __get_exception_address(far); @@ -281,13 +294,12 @@ int el3_handle_exception(uint64_t ec, uint64_t iss) el3_shutdown(); break; case EC_DABORT_LOWER: - printf("Data abort (%s) at lower level: far = %0lx elr = %0lx\n", - dai.wnr ? "write" : "read", far, elr); + printf("Data abort at lower level: far = %0lx elr = %0lx\n", + far, elr); el3_shutdown(); break; case EC_DABORT: - printf("Data abort (%s) at EL3: far = %0lx elr = %0lx\n", - dai.wnr ? "write" : "read", far, elr); + printf("Data abort at EL3: far = %0lx elr = %0lx\n", far, elr); el3_shutdown(); break; case EC_SYSINSN: @@ -347,18 +359,22 @@ void el3_monitor_init() * sequence. This will occur when we return from exception after monitor * initialization. */ +#ifdef AARCH64 sec_state.elr_el3 = EL1_S_FLASH_BASE; sec_state.spsr_el3 = 0x5; sec_state.spsel = 0x1; +#endif sec_state.x[0] = (uint64_t)el3_lookup_pa(syscntl); /* Set-up the nonsecure state buffer to return to the non-secure * initialization sequence. This will occur on the first monitor context * switch (smc) from secure to non-secure. */ +#ifdef AARCH64 nsec_state.elr_el3 = EL1_NS_FLASH_BASE; nsec_state.spsr_el3 = 0x5; nsec_state.spsel = 0x1; +#endif nsec_state.x[0] = (uint64_t)el3_lookup_pa(syscntl); } @@ -370,14 +386,14 @@ void el3_start(uint64_t base, uint64_t size) printf("EL3 started...\n"); /* Unmap the init segement so we don't accidentally use it */ - for (len = 0; len < ((size + 0xFFF) & ~0xFFF); - len += 0x1000, addr += 0x1000) { + for (len = 0; len < ((size + (PAGE_SIZE - 1)) & ~(PAGE_SIZE - 1)); + len += PAGE_SIZE, addr += PAGE_SIZE) { el3_unmap_va(addr); } - syscntl = el3_heap_allocate(0x1000); + syscntl = el3_heap_allocate(PAGE_SIZE); - smc_interop_buf = el3_heap_allocate(0x1000); + smc_interop_buf = el3_heap_allocate(PAGE_SIZE); syscntl->smc_interop.buf_va = smc_interop_buf; syscntl->smc_interop.buf_pa = el3_lookup_pa(smc_interop_buf); diff --git a/aarch64/el3/el3_init.S b/aarch64/el3/el3_init.S index 3de4566..c075972 100644 --- a/aarch64/el3/el3_init.S +++ b/aarch64/el3/el3_init.S @@ -22,11 +22,11 @@ init_uart: /* 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 + ldr x10, =RAM_BASE + (2 * PAGE_SIZE) mov sp, x10 /* Use the top of the stack to track our PA pool pointer */ - ldr x10, =PT_BASE+0x1000 + ldr x10, =PT_BASE + (PAGE_SIZE) str x10, [sp] /* Enable floating point register usage as printf uses it */ @@ -87,7 +87,7 @@ el3_map_data: el3_map_stack: /* Map the first page of the stack so we can get off the ground */ - ldr x0, =EL3_STACK_BASE-0x1000 + ldr x0, =EL3_STACK_BASE - PAGE_SIZE mov x1, #(PTE_PAGE|PTE_ACCESS|PTE_PRIV_RW) bl map_va -- cgit v1.2.3