1 // SPDX-License-Identifier: BSD-2-Clause 2 /* 3 * Copyright (c) 2019, Linaro Limited 4 */ 5 6 #include <assert.h> 7 #include <ldelf.h> 8 #include <malloc.h> 9 #include <printk.h> 10 #include <string.h> 11 #include <sys/queue.h> 12 #include <tee_api_types.h> 13 #include <trace.h> 14 #include <types_ext.h> 15 #include <util.h> 16 17 #include "ftrace.h" 18 #include "sys.h" 19 #include "ta_elf.h" 20 21 static size_t mpool_size = 2 * SMALL_PAGE_SIZE; 22 static vaddr_t mpool_base; 23 24 static void __printf(2, 0) print_to_console(void *pctx __unused, 25 const char *fmt, va_list ap) 26 { 27 trace_vprintf(NULL, 0, TRACE_ERROR, true, fmt, ap); 28 } 29 30 static void __noreturn __maybe_unused dump_ta_state(struct dump_entry_arg *arg) 31 { 32 struct ta_elf *elf = TAILQ_FIRST(&main_elf_queue); 33 34 assert(elf && elf->is_main); 35 EMSG_RAW("Status of TA %pUl", (void *)&elf->uuid); 36 EMSG_RAW(" arch: %s", elf->is_32bit ? "arm" : "aarch64"); 37 38 39 ta_elf_print_mappings(NULL, print_to_console, &main_elf_queue, 40 arg->num_maps, arg->maps, mpool_base); 41 42 if (arg->is_arm32) 43 ta_elf_stack_trace_a32(arg->arm32.regs); 44 else 45 ta_elf_stack_trace_a64(arg->arm64.fp, arg->arm64.sp, 46 arg->arm64.pc); 47 48 sys_return_cleanup(); 49 } 50 51 #ifdef CFG_TA_FTRACE_SUPPORT 52 struct print_buf_ctx { 53 char *buf; 54 size_t blen; 55 size_t ret; 56 }; 57 58 static void __printf(2, 0) print_to_pbuf(void *pctx, const char *fmt, 59 va_list ap) 60 { 61 struct print_buf_ctx *pbuf = pctx; 62 char *buf = NULL; 63 size_t blen = 0; 64 int ret = 0; 65 66 if (pbuf->buf && pbuf->blen > pbuf->ret) { 67 buf = pbuf->buf + pbuf->ret; 68 blen = pbuf->blen - pbuf->ret; 69 } 70 71 ret = vsnprintk(buf, blen, fmt, ap); 72 assert(ret >= 0); 73 74 pbuf->ret += ret; 75 } 76 77 static void copy_to_pbuf(void *pctx, void *b, size_t bl) 78 { 79 struct print_buf_ctx *pbuf = pctx; 80 char *buf = NULL; 81 size_t blen = 0; 82 83 if (pbuf->buf && pbuf->blen > pbuf->ret) { 84 buf = pbuf->buf + pbuf->ret; 85 blen = pbuf->blen - pbuf->ret; 86 memcpy(buf, b, MIN(blen, bl)); 87 } 88 89 pbuf->ret += bl; 90 91 } 92 93 static void __noreturn ftrace_dump(void *buf, size_t *blen) 94 { 95 struct print_buf_ctx pbuf = { .buf = buf, .blen = *blen }; 96 97 ta_elf_print_mappings(&pbuf, print_to_pbuf, &main_elf_queue, 98 0, NULL, mpool_base); 99 ftrace_copy_buf(&pbuf, copy_to_pbuf); 100 *blen = pbuf.ret; 101 sys_return_cleanup(); 102 } 103 #endif 104 105 /* 106 * ldelf()- Loads ELF into memory 107 * @arg: Argument passing to/from TEE Core 108 * 109 * Only called from assembly 110 */ 111 void __noreturn ldelf(struct ldelf_arg *arg); 112 void ldelf(struct ldelf_arg *arg) 113 { 114 TEE_Result res = TEE_SUCCESS; 115 struct ta_elf *elf = NULL; 116 117 DMSG("Loading TA %pUl", (void *)&arg->uuid); 118 res = sys_map_zi(mpool_size, 0, &mpool_base, 0, 0); 119 if (res) { 120 EMSG("sys_map_zi(%zu): result %"PRIx32, mpool_size, res); 121 panic(); 122 } 123 malloc_add_pool((void *)mpool_base, mpool_size); 124 125 /* Load the main binary and get a list of dependencies, if any. */ 126 ta_elf_load_main(&arg->uuid, &arg->is_32bit, &arg->entry_func, 127 &arg->stack_ptr, &arg->flags); 128 129 /* 130 * Load binaries, ta_elf_load() may add external libraries to the 131 * list, so the loop will end when all the dependencies are 132 * satisfied. 133 */ 134 TAILQ_FOREACH(elf, &main_elf_queue, link) 135 ta_elf_load_dependency(elf, arg->is_32bit); 136 137 TAILQ_FOREACH(elf, &main_elf_queue, link) { 138 ta_elf_relocate(elf); 139 ta_elf_finalize_mappings(elf); 140 } 141 142 arg->ftrace_entry = 0; 143 #ifdef CFG_TA_FTRACE_SUPPORT 144 if (ftrace_init()) 145 arg->ftrace_entry = (vaddr_t)(void *)ftrace_dump; 146 #endif 147 148 TAILQ_FOREACH(elf, &main_elf_queue, link) 149 DMSG("ELF (%pUl) at %#"PRIxVA, 150 (void *)&elf->uuid, elf->load_addr); 151 152 #if TRACE_LEVEL >= TRACE_ERROR 153 arg->dump_entry = (vaddr_t)(void *)dump_ta_state; 154 #else 155 arg->dump_entry = 0; 156 #endif 157 158 sys_return_cleanup(); 159 } 160