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