1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3 * Copyright (c) 2014, STMicroelectronics International N.V.
4 * Copyright (c) 2022, Linaro Limited.
5 */
6 #include <compiler.h>
7 #include <link.h>
8 #include <malloc.h>
9 #include <memtag.h>
10 #include <stdbool.h>
11 #include <stdlib.h>
12 #include <string.h>
13 #include <sys/queue.h>
14 #include <tee_api.h>
15 #include <tee_arith_internal.h>
16 #include <tee_internal_api_extensions.h>
17 #include <tee_ta_api.h>
18 #include <user_ta_header.h>
19 #include <utee_syscalls.h>
20 #include "tee_api_private.h"
21
22 struct ta_session {
23 uint32_t session_id;
24 void *session_ctx;
25 TAILQ_ENTRY(ta_session) link;
26 };
27
28 static TAILQ_HEAD(ta_sessions, ta_session) ta_sessions =
29 TAILQ_HEAD_INITIALIZER(ta_sessions);
30
31 static bool init_done;
32
33 /* From user_ta_header.c, built within TA */
34 extern uint8_t ta_heap[];
35 extern const size_t ta_heap_size;
36 extern struct ta_head ta_head;
37
38 uint32_t ta_param_types;
39 TEE_Param ta_params[TEE_NUM_PARAMS];
40 struct __elf_phdr_info __elf_phdr_info;
41
42 struct phdr_info {
43 struct dl_phdr_info info;
44 TAILQ_ENTRY(phdr_info) link;
45 };
46
47 static TAILQ_HEAD(phdr_info_head, phdr_info) __phdr_info_head =
48 TAILQ_HEAD_INITIALIZER(__phdr_info_head);
49 /*
50 * Keep track of how many modules have been initialized so that subsequent
51 * dlopen() calls will not run the same initializers again
52 */
53 static size_t _num_mod_init;
54
_init_iterate_phdr_cb(struct dl_phdr_info * info,size_t size __unused,void * data)55 static int _init_iterate_phdr_cb(struct dl_phdr_info *info,
56 size_t size __unused, void *data)
57 {
58 struct phdr_info *qe = NULL;
59 size_t *count = data;
60
61 qe = malloc(sizeof(*qe));
62 if (!qe) {
63 EMSG("init/fini: out of memory");
64 abort();
65 }
66 qe->info = *info;
67 TAILQ_INSERT_TAIL(&__phdr_info_head, qe, link);
68 (*count)++;
69 return 0;
70 }
71
_get_fn_array(struct dl_phdr_info * info,Elf_Sword tag_a,Elf_Sword tag_s,void (*** fn)(void),size_t * num_fn)72 static void _get_fn_array(struct dl_phdr_info *info, Elf_Sword tag_a,
73 Elf_Sword tag_s, void (***fn)(void), size_t *num_fn)
74 {
75 const Elf_Phdr *phdr = NULL;
76 Elf_Dyn *dyn = NULL;
77 size_t num_dyn = 0;
78 size_t i = 0;
79 size_t j = 0;
80
81 for (i = 0; i < info->dlpi_phnum; i++) {
82 phdr = info->dlpi_phdr + i;
83 if (phdr->p_type != PT_DYNAMIC)
84 continue;
85 num_dyn = phdr->p_memsz / sizeof(Elf_Dyn);
86 dyn = (Elf_Dyn *)(phdr->p_vaddr + info->dlpi_addr);
87 for (j = 0; j < num_dyn; j++) {
88 if (*fn && *num_fn)
89 break;
90 if (dyn->d_tag == DT_NULL) {
91 break;
92 } else if (dyn->d_tag == tag_a) {
93 *fn = (void (**)(void))(dyn->d_un.d_ptr +
94 info->dlpi_addr);
95 } else if (dyn->d_tag == tag_s) {
96 *num_fn = dyn->d_un.d_val / sizeof(Elf_Addr);
97 }
98 dyn++;
99 }
100 }
101 }
102
__utee_call_elf_init_fn(void)103 void __utee_call_elf_init_fn(void)
104 {
105 void (**fn)(void) = NULL;
106 size_t num_mod = 0;
107 size_t num_fn = 0;
108 size_t mod = 0;
109 size_t i = 0;
110 struct phdr_info *qe = NULL;
111 struct phdr_info *qe2 = NULL;
112
113 dl_iterate_phdr(_init_iterate_phdr_cb, &num_mod);
114
115 /* Reverse order: dependencies first */
116 TAILQ_FOREACH_REVERSE(qe, &__phdr_info_head, phdr_info_head, link) {
117 if (mod == num_mod - _num_mod_init)
118 break;
119 _get_fn_array(&qe->info, DT_INIT_ARRAY, DT_INIT_ARRAYSZ, &fn,
120 &num_fn);
121 for (i = 0; i < num_fn; i++)
122 fn[i]();
123 fn = NULL;
124 num_fn = 0;
125 mod++;
126 }
127 _num_mod_init += mod;
128
129 TAILQ_FOREACH_SAFE(qe, &__phdr_info_head, link, qe2) {
130 TAILQ_REMOVE(&__phdr_info_head, qe, link);
131 free(qe);
132 }
133 }
134
_fini_iterate_phdr_cb(struct dl_phdr_info * info,size_t size __unused,void * data __unused)135 static int _fini_iterate_phdr_cb(struct dl_phdr_info *info,
136 size_t size __unused, void *data __unused)
137 {
138 void (**fn)(void) = NULL;
139 size_t num_fn = 0;
140 size_t i = 0;
141
142 _get_fn_array(info, DT_FINI_ARRAY, DT_FINI_ARRAYSZ, &fn, &num_fn);
143
144 for (i = 1; i <= num_fn; i++)
145 fn[num_fn - i]();
146
147 return 0;
148 }
149
__utee_call_elf_fini_fn(void)150 void __utee_call_elf_fini_fn(void)
151 {
152 dl_iterate_phdr(_fini_iterate_phdr_cb, NULL);
153 }
154
get_memtag_implementation(void)155 static unsigned int get_memtag_implementation(void)
156 {
157 const char *s = "org.trustedfirmware.optee.cpu.feat_memtag_implemented";
158 uint32_t v = 0;
159
160 if (TEE_GetPropertyAsU32(TEE_PROPSET_TEE_IMPLEMENTATION, s, &v))
161 return 0;
162 return v;
163 }
164
init_instance(void)165 static TEE_Result init_instance(void)
166 {
167 trace_set_level(tahead_get_trace_level());
168 __utee_gprof_init();
169 malloc_add_pool(ta_heap, ta_heap_size);
170 memtag_init_ops(get_memtag_implementation());
171 _TEE_MathAPI_Init();
172 __utee_tcb_init();
173 __utee_call_elf_init_fn();
174 return TA_CreateEntryPoint();
175 }
176
uninit_instance(void)177 static void uninit_instance(void)
178 {
179 __utee_gprof_fini();
180 TA_DestroyEntryPoint();
181 __utee_call_elf_fini_fn();
182 }
183
ta_header_save_params(uint32_t param_types,TEE_Param params[TEE_NUM_PARAMS])184 static void ta_header_save_params(uint32_t param_types,
185 TEE_Param params[TEE_NUM_PARAMS])
186 {
187 ta_param_types = param_types;
188
189 if (params)
190 memcpy(ta_params, params, sizeof(ta_params));
191 else
192 memset(ta_params, 0, sizeof(ta_params));
193 }
194
ta_header_get_session(uint32_t session_id)195 static struct ta_session *ta_header_get_session(uint32_t session_id)
196 {
197 struct ta_session *itr;
198
199 TAILQ_FOREACH(itr, &ta_sessions, link) {
200 if (itr->session_id == session_id)
201 return itr;
202 }
203 return NULL;
204 }
205
ta_header_add_session(uint32_t session_id)206 static TEE_Result ta_header_add_session(uint32_t session_id)
207 {
208 struct ta_session *itr = ta_header_get_session(session_id);
209 TEE_Result res;
210
211 if (itr)
212 return TEE_SUCCESS;
213
214 if (!init_done) {
215 init_done = true;
216 res = init_instance();
217 if (res)
218 return res;
219 }
220
221 itr = TEE_Malloc(sizeof(struct ta_session),
222 TEE_USER_MEM_HINT_NO_FILL_ZERO);
223 if (!itr)
224 return TEE_ERROR_OUT_OF_MEMORY;
225 itr->session_id = session_id;
226 itr->session_ctx = 0;
227 TAILQ_INSERT_TAIL(&ta_sessions, itr, link);
228
229 return TEE_SUCCESS;
230 }
231
ta_header_remove_session(uint32_t session_id)232 static void ta_header_remove_session(uint32_t session_id)
233 {
234 struct ta_session *itr;
235 bool keep_alive;
236
237 TAILQ_FOREACH(itr, &ta_sessions, link) {
238 if (itr->session_id == session_id) {
239 TAILQ_REMOVE(&ta_sessions, itr, link);
240 TEE_Free(itr);
241
242 keep_alive =
243 (ta_head.flags & TA_FLAG_SINGLE_INSTANCE) &&
244 (ta_head.flags & TA_FLAG_INSTANCE_KEEP_ALIVE);
245 if (TAILQ_EMPTY(&ta_sessions) && !keep_alive)
246 uninit_instance();
247
248 return;
249 }
250 }
251 }
252
to_utee_params(struct utee_params * up,uint32_t param_types,const TEE_Param params[TEE_NUM_PARAMS])253 static void to_utee_params(struct utee_params *up, uint32_t param_types,
254 const TEE_Param params[TEE_NUM_PARAMS])
255 {
256 size_t n = 0;
257
258 up->types = param_types;
259 for (n = 0; n < TEE_NUM_PARAMS; n++) {
260 switch (TEE_PARAM_TYPE_GET(param_types, n)) {
261 case TEE_PARAM_TYPE_VALUE_INPUT:
262 case TEE_PARAM_TYPE_VALUE_OUTPUT:
263 case TEE_PARAM_TYPE_VALUE_INOUT:
264 up->vals[n * 2] = params[n].value.a;
265 up->vals[n * 2 + 1] = params[n].value.b;
266 break;
267 case TEE_PARAM_TYPE_MEMREF_INPUT:
268 case TEE_PARAM_TYPE_MEMREF_OUTPUT:
269 case TEE_PARAM_TYPE_MEMREF_INOUT:
270 up->vals[n * 2] = (uintptr_t)params[n].memref.buffer;
271 up->vals[n * 2 + 1] = params[n].memref.size;
272 break;
273 default:
274 up->vals[n * 2] = 0;
275 up->vals[n * 2 + 1] = 0;
276 break;
277 }
278 }
279 }
280
from_utee_params(TEE_Param params[TEE_NUM_PARAMS],uint32_t * param_types,const struct utee_params * up)281 static void from_utee_params(TEE_Param params[TEE_NUM_PARAMS],
282 uint32_t *param_types,
283 const struct utee_params *up)
284 {
285 size_t n;
286 uint32_t types = up->types;
287
288 for (n = 0; n < TEE_NUM_PARAMS; n++) {
289 uintptr_t a = up->vals[n * 2];
290 uintptr_t b = up->vals[n * 2 + 1];
291
292 switch (TEE_PARAM_TYPE_GET(types, n)) {
293 case TEE_PARAM_TYPE_VALUE_INPUT:
294 case TEE_PARAM_TYPE_VALUE_OUTPUT:
295 case TEE_PARAM_TYPE_VALUE_INOUT:
296 params[n].value.a = a;
297 params[n].value.b = b;
298 break;
299 case TEE_PARAM_TYPE_MEMREF_INPUT:
300 case TEE_PARAM_TYPE_MEMREF_OUTPUT:
301 case TEE_PARAM_TYPE_MEMREF_INOUT:
302 params[n].memref.buffer = (void *)a;
303 params[n].memref.size = b;
304 break;
305 default:
306 break;
307 }
308 }
309
310 if (param_types)
311 *param_types = types;
312 }
313
entry_open_session(unsigned long session_id,struct utee_params * up)314 static TEE_Result entry_open_session(unsigned long session_id,
315 struct utee_params *up)
316 {
317 TEE_Result res;
318 struct ta_session *session;
319 uint32_t param_types;
320 TEE_Param params[TEE_NUM_PARAMS];
321
322 res = ta_header_add_session(session_id);
323 if (res != TEE_SUCCESS)
324 return res;
325
326 session = ta_header_get_session(session_id);
327 if (!session)
328 return TEE_ERROR_BAD_STATE;
329
330 from_utee_params(params, ¶m_types, up);
331 ta_header_save_params(param_types, params);
332
333 res = TA_OpenSessionEntryPoint(param_types, params,
334 &session->session_ctx);
335
336 to_utee_params(up, param_types, params);
337
338 if (res != TEE_SUCCESS)
339 ta_header_remove_session(session_id);
340 return res;
341 }
342
entry_close_session(unsigned long session_id)343 static TEE_Result entry_close_session(unsigned long session_id)
344 {
345 struct ta_session *session = ta_header_get_session(session_id);
346
347 if (!session)
348 return TEE_ERROR_BAD_STATE;
349
350 TA_CloseSessionEntryPoint(session->session_ctx);
351
352 ta_header_remove_session(session_id);
353 return TEE_SUCCESS;
354 }
355
entry_invoke_command(unsigned long session_id,struct utee_params * up,unsigned long cmd_id)356 static TEE_Result entry_invoke_command(unsigned long session_id,
357 struct utee_params *up, unsigned long cmd_id)
358 {
359 TEE_Result res;
360 uint32_t param_types;
361 TEE_Param params[TEE_NUM_PARAMS];
362 struct ta_session *session = ta_header_get_session(session_id);
363
364 if (!session)
365 return TEE_ERROR_BAD_STATE;
366
367 from_utee_params(params, ¶m_types, up);
368 ta_header_save_params(param_types, params);
369
370 res = TA_InvokeCommandEntryPoint(session->session_ctx, cmd_id,
371 param_types, params);
372
373 to_utee_params(up, param_types, params);
374 return res;
375 }
376
377 #if defined(CFG_TA_STATS)
entry_dump_memstats(unsigned long session_id __unused,struct utee_params * up)378 static TEE_Result entry_dump_memstats(unsigned long session_id __unused,
379 struct utee_params *up)
380 {
381 uint32_t param_types = 0;
382 TEE_Param params[TEE_NUM_PARAMS] = { };
383 struct malloc_stats stats = { };
384
385 from_utee_params(params, ¶m_types, up);
386 ta_header_save_params(param_types, params);
387
388 if (TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_OUTPUT,
389 TEE_PARAM_TYPE_VALUE_OUTPUT,
390 TEE_PARAM_TYPE_VALUE_OUTPUT,
391 TEE_PARAM_TYPE_NONE) != param_types)
392 return TEE_ERROR_BAD_PARAMETERS;
393
394 malloc_get_stats(&stats);
395 params[0].value.a = stats.allocated;
396 params[0].value.b = stats.max_allocated;
397 params[1].value.a = stats.size;
398 params[1].value.b = stats.num_alloc_fail;
399 params[2].value.a = stats.biggest_alloc_fail;
400 params[2].value.b = stats.biggest_alloc_fail_used;
401 to_utee_params(up, param_types, params);
402
403 return TEE_SUCCESS;
404 }
405 #endif
406
__utee_entry(unsigned long func,unsigned long session_id,struct utee_params * up,unsigned long cmd_id)407 TEE_Result __utee_entry(unsigned long func, unsigned long session_id,
408 struct utee_params *up, unsigned long cmd_id)
409 {
410 TEE_Result res;
411
412 switch (func) {
413 case UTEE_ENTRY_FUNC_OPEN_SESSION:
414 res = entry_open_session(session_id, up);
415 break;
416 case UTEE_ENTRY_FUNC_CLOSE_SESSION:
417 res = entry_close_session(session_id);
418 break;
419 case UTEE_ENTRY_FUNC_INVOKE_COMMAND:
420 res = entry_invoke_command(session_id, up, cmd_id);
421 break;
422 #if defined(CFG_TA_STATS)
423 case UTEE_ENTRY_FUNC_DUMP_MEMSTATS:
424 res = entry_dump_memstats(session_id, up);
425 break;
426 #endif
427 default:
428 res = TEE_ERROR_NOT_SUPPORTED;
429 break;
430 }
431 ta_header_save_params(0, NULL);
432
433 return res;
434 }
435