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