1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3 * Copyright (c) 2019, Linaro Limited
4 * Copyright (c) 2020, Arm Limited.
5 */
6
7 #include <crypto/crypto.h>
8 #include <ffa.h>
9 #include <keep.h>
10 #include <kernel/abort.h>
11 #include <kernel/stmm_sp.h>
12 #include <kernel/thread_private.h>
13 #include <kernel/user_mode_ctx.h>
14 #include <mempool.h>
15 #include <mm/fobj.h>
16 #include <mm/mobj.h>
17 #include <mm/vm.h>
18 #include <pta_stmm.h>
19 #include <tee_api_defines_extensions.h>
20 #include <tee/tee_pobj.h>
21 #include <tee/tee_svc.h>
22 #include <tee/tee_svc_storage.h>
23 #include <zlib.h>
24
25 #ifdef ARM64
26 #define SVC_REGS_A0(_regs) ((_regs)->x0)
27 #define SVC_REGS_A1(_regs) ((_regs)->x1)
28 #define SVC_REGS_A2(_regs) ((_regs)->x2)
29 #define SVC_REGS_A3(_regs) ((_regs)->x3)
30 #define SVC_REGS_A4(_regs) ((_regs)->x4)
31 #define SVC_REGS_A5(_regs) ((_regs)->x5)
32 #define SVC_REGS_A6(_regs) ((_regs)->x6)
33 #define SVC_REGS_A7(_regs) ((_regs)->x7)
34 #define __FFA_SVC_RPMB_READ FFA_SVC_RPMB_READ
35 #define __FFA_SVC_RPMB_WRITE FFA_SVC_RPMB_WRITE
36 #define __FFA_SVC_MEMORY_ATTRIBUTES_GET FFA_SVC_MEMORY_ATTRIBUTES_GET_64
37 #define __FFA_SVC_MEMORY_ATTRIBUTES_SET FFA_SVC_MEMORY_ATTRIBUTES_SET_64
38 #define __FFA_MSG_SEND_DIRECT_RESP FFA_MSG_SEND_DIRECT_RESP_64
39 #define __FFA_MSG_SEND_DIRECT_REQ FFA_MSG_SEND_DIRECT_REQ_64
40 #endif
41 #ifdef ARM32
42 #define SVC_REGS_A0(_regs) ((_regs)->r0)
43 #define SVC_REGS_A1(_regs) ((_regs)->r1)
44 #define SVC_REGS_A2(_regs) ((_regs)->r2)
45 #define SVC_REGS_A3(_regs) ((_regs)->r3)
46 #define SVC_REGS_A4(_regs) ((_regs)->r4)
47 #define SVC_REGS_A5(_regs) ((_regs)->r5)
48 #define SVC_REGS_A6(_regs) ((_regs)->r6)
49 #define SVC_REGS_A7(_regs) ((_regs)->r7)
50 #define __FFA_SVC_RPMB_READ FFA_SVC_RPMB_READ_32
51 #define __FFA_SVC_RPMB_WRITE FFA_SVC_RPMB_WRITE_32
52 #define __FFA_SVC_MEMORY_ATTRIBUTES_GET FFA_SVC_MEMORY_ATTRIBUTES_GET_32
53 #define __FFA_SVC_MEMORY_ATTRIBUTES_SET FFA_SVC_MEMORY_ATTRIBUTES_SET_32
54 #define __FFA_MSG_SEND_DIRECT_RESP FFA_MSG_SEND_DIRECT_RESP_32
55 #define __FFA_MSG_SEND_DIRECT_REQ FFA_MSG_SEND_DIRECT_REQ_32
56 #endif
57
58 static const TEE_UUID stmm_uuid = PTA_STMM_UUID;
59
60 /*
61 * Once a complete FFA spec is added, these will become discoverable.
62 * Until then these are considered part of the internal ABI between
63 * OP-TEE and StMM.
64 */
65 static const uint16_t stmm_id = 1U;
66 static const uint16_t stmm_pta_id = 2U;
67 static const uint16_t mem_mgr_id = 3U;
68 static const uint16_t ffa_storage_id = 4U;
69
70 static const unsigned int stmm_stack_size = 4 * SMALL_PAGE_SIZE;
71 static const unsigned int stmm_heap_size = 398 * SMALL_PAGE_SIZE;
72 static const unsigned int stmm_sec_buf_size = 4 * SMALL_PAGE_SIZE;
73 static const unsigned int stmm_ns_comm_buf_size = 4 * SMALL_PAGE_SIZE;
74
75 extern unsigned char stmm_image[];
76 extern const unsigned int stmm_image_size;
77 extern const unsigned int stmm_image_uncompressed_size;
78
stmm_alloc_ctx(const TEE_UUID * uuid)79 static struct stmm_ctx *stmm_alloc_ctx(const TEE_UUID *uuid)
80 {
81 TEE_Result res = TEE_SUCCESS;
82 struct stmm_ctx *spc = NULL;
83
84 spc = calloc(1, sizeof(*spc));
85 if (!spc)
86 return NULL;
87
88 spc->ta_ctx.ts_ctx.ops = &stmm_sp_ops;
89 spc->ta_ctx.ts_ctx.uuid = *uuid;
90 spc->ta_ctx.flags = TA_FLAG_SINGLE_INSTANCE |
91 TA_FLAG_INSTANCE_KEEP_ALIVE;
92
93 res = vm_info_init(&spc->uctx, &spc->ta_ctx.ts_ctx);
94 if (res) {
95 free(spc);
96 return NULL;
97 }
98
99 spc->ta_ctx.ref_count = 1;
100 condvar_init(&spc->ta_ctx.busy_cv);
101
102 return spc;
103 }
104
stmm_enter_user_mode(struct stmm_ctx * spc)105 static TEE_Result stmm_enter_user_mode(struct stmm_ctx *spc)
106 {
107 uint32_t exceptions = 0;
108 uint32_t panic_code = 0;
109 uint32_t panicked = 0;
110 uint64_t cntkctl = 0;
111
112 exceptions = thread_mask_exceptions(THREAD_EXCP_ALL);
113 cntkctl = read_cntkctl();
114 write_cntkctl(cntkctl | CNTKCTL_PL0PCTEN);
115
116 #ifdef ARM32
117 /* Handle usr_lr in place of __thread_enter_user_mode() */
118 thread_set_usr_lr(spc->regs.usr_lr);
119 #endif
120
121 __thread_enter_user_mode(&spc->regs, &panicked, &panic_code);
122
123 #ifdef ARM32
124 spc->regs.usr_lr = thread_get_usr_lr();
125 #endif
126
127 write_cntkctl(cntkctl);
128 thread_unmask_exceptions(exceptions);
129
130 thread_user_clear_vfp(&spc->uctx);
131
132 if (panicked) {
133 abort_print_current_ts();
134 DMSG("stmm panicked with code %#"PRIx32, panic_code);
135 return TEE_ERROR_TARGET_DEAD;
136 }
137
138 return TEE_SUCCESS;
139 }
140
141 #ifdef ARM64
init_stmm_regs(struct stmm_ctx * spc,unsigned long a0,unsigned long a1,unsigned long sp,unsigned long pc)142 static void init_stmm_regs(struct stmm_ctx *spc, unsigned long a0,
143 unsigned long a1, unsigned long sp, unsigned long pc)
144 {
145 spc->regs.x[0] = a0;
146 spc->regs.x[1] = a1;
147 spc->regs.sp = sp;
148 spc->regs.pc = pc;
149 }
150 #endif
151
152 #ifdef ARM32
get_spsr(void)153 static uint32_t __maybe_unused get_spsr(void)
154 {
155 uint32_t s = 0;
156
157 s = read_cpsr();
158 s &= ~(CPSR_MODE_MASK | CPSR_T | ARM32_CPSR_IT_MASK);
159 s |= CPSR_MODE_USR;
160
161 return s;
162 }
163
init_stmm_regs(struct stmm_ctx * spc,unsigned long a0,unsigned long a1,unsigned long sp,unsigned long pc)164 static void init_stmm_regs(struct stmm_ctx *spc, unsigned long a0,
165 unsigned long a1, unsigned long sp, unsigned long pc)
166 {
167 spc->regs.r0 = a0;
168 spc->regs.r1 = a1;
169 spc->regs.usr_sp = sp;
170 spc->regs.cpsr = get_spsr();
171 spc->regs.pc = pc;
172 }
173 #endif
174
alloc_and_map_sp_fobj(struct stmm_ctx * spc,size_t sz,uint32_t prot,vaddr_t * va)175 static TEE_Result alloc_and_map_sp_fobj(struct stmm_ctx *spc, size_t sz,
176 uint32_t prot, vaddr_t *va)
177 {
178 size_t num_pgs = ROUNDUP(sz, SMALL_PAGE_SIZE) / SMALL_PAGE_SIZE;
179 struct fobj *fobj = fobj_ta_mem_alloc(num_pgs);
180 TEE_Result res = TEE_SUCCESS;
181 struct mobj *mobj = NULL;
182
183 mobj = mobj_with_fobj_alloc(fobj, NULL, TEE_MATTR_MEM_TYPE_TAGGED);
184 fobj_put(fobj);
185 if (!mobj)
186 return TEE_ERROR_OUT_OF_MEMORY;
187
188 res = vm_map(&spc->uctx, va, num_pgs * SMALL_PAGE_SIZE,
189 prot, 0, mobj, 0);
190 if (res)
191 mobj_put(mobj);
192
193 return TEE_SUCCESS;
194 }
195
zalloc(void * opaque __unused,unsigned int items,unsigned int size)196 static void *zalloc(void *opaque __unused, unsigned int items,
197 unsigned int size)
198 {
199 return mempool_alloc(mempool_default, items * size);
200 }
201
zfree(void * opaque __unused,void * address)202 static void zfree(void *opaque __unused, void *address)
203 {
204 mempool_free(mempool_default, address);
205 }
206
uncompress_image(void * dst,size_t dst_size,void * src,size_t src_size)207 static void uncompress_image(void *dst, size_t dst_size, void *src,
208 size_t src_size)
209 {
210 z_stream strm = {
211 .next_in = src,
212 .avail_in = src_size,
213 .next_out = dst,
214 .avail_out = dst_size,
215 .zalloc = zalloc,
216 .zfree = zfree,
217 };
218
219 if (inflateInit(&strm) != Z_OK)
220 panic("inflateInit");
221
222 if (inflate(&strm, Z_SYNC_FLUSH) != Z_STREAM_END)
223 panic("inflate");
224
225 if (inflateEnd(&strm) != Z_OK)
226 panic("inflateEnd");
227 }
228
load_stmm(struct stmm_ctx * spc)229 static TEE_Result load_stmm(struct stmm_ctx *spc)
230 {
231 struct stmm_boot_info *boot_info = NULL;
232 struct stmm_mp_info *mp_info = NULL;
233 TEE_Result res = TEE_SUCCESS;
234 vaddr_t sp_addr = 0;
235 vaddr_t image_addr = 0;
236 vaddr_t heap_addr = 0;
237 vaddr_t stack_addr = 0;
238 vaddr_t sec_buf_addr = 0;
239 vaddr_t comm_buf_addr = 0;
240 unsigned int sp_size = 0;
241 unsigned int uncompressed_size_roundup = 0;
242
243 uncompressed_size_roundup = ROUNDUP(stmm_image_uncompressed_size,
244 SMALL_PAGE_SIZE);
245 sp_size = uncompressed_size_roundup + stmm_stack_size +
246 stmm_heap_size + stmm_sec_buf_size;
247 res = alloc_and_map_sp_fobj(spc, sp_size,
248 TEE_MATTR_PRW, &sp_addr);
249 if (res)
250 return res;
251
252 res = alloc_and_map_sp_fobj(spc, stmm_ns_comm_buf_size,
253 TEE_MATTR_URW | TEE_MATTR_PRW,
254 &comm_buf_addr);
255 /*
256 * We don't need to free the previous instance here, they'll all be
257 * handled during the destruction call (stmm_ctx_destroy())
258 */
259 if (res)
260 return res;
261
262 image_addr = sp_addr;
263 heap_addr = image_addr + uncompressed_size_roundup;
264 stack_addr = heap_addr + stmm_heap_size;
265 sec_buf_addr = stack_addr + stmm_stack_size;
266
267 vm_set_ctx(&spc->ta_ctx.ts_ctx);
268 uncompress_image((void *)image_addr, stmm_image_uncompressed_size,
269 stmm_image, stmm_image_size);
270
271 res = vm_set_prot(&spc->uctx, image_addr, uncompressed_size_roundup,
272 TEE_MATTR_URX | TEE_MATTR_PR);
273 if (res)
274 return res;
275
276 res = vm_set_prot(&spc->uctx, heap_addr, stmm_heap_size,
277 TEE_MATTR_URW | TEE_MATTR_PRW);
278 if (res)
279 return res;
280
281 res = vm_set_prot(&spc->uctx, stack_addr, stmm_stack_size,
282 TEE_MATTR_URW | TEE_MATTR_PRW);
283 if (res)
284 return res;
285
286 res = vm_set_prot(&spc->uctx, sec_buf_addr, stmm_sec_buf_size,
287 TEE_MATTR_URW | TEE_MATTR_PRW);
288 if (res)
289 return res;
290
291 DMSG("stmm load address %#"PRIxVA, image_addr);
292
293 boot_info = (struct stmm_boot_info *)sec_buf_addr;
294 mp_info = (struct stmm_mp_info *)(boot_info + 1);
295 *boot_info = (struct stmm_boot_info){
296 .h.type = STMM_PARAM_SP_IMAGE_BOOT_INFO,
297 .h.version = STMM_PARAM_VERSION_1,
298 .h.size = sizeof(struct stmm_boot_info),
299 .h.attr = 0,
300 .sp_mem_base = sp_addr,
301 .sp_mem_limit = sp_addr + sp_size,
302 .sp_image_base = image_addr,
303 .sp_stack_base = stack_addr,
304 .sp_heap_base = heap_addr,
305 .sp_ns_comm_buf_base = comm_buf_addr,
306 .sp_shared_buf_base = sec_buf_addr,
307 .sp_image_size = stmm_image_size,
308 .sp_pcpu_stack_size = stmm_stack_size,
309 .sp_heap_size = stmm_heap_size,
310 .sp_ns_comm_buf_size = stmm_ns_comm_buf_size,
311 .sp_shared_buf_size = stmm_sec_buf_size,
312 .num_sp_mem_regions = 6,
313 .num_cpus = 1,
314 .mp_info = mp_info,
315 };
316 mp_info->mpidr = read_mpidr();
317 mp_info->linear_id = 0;
318 mp_info->flags = MP_INFO_FLAG_PRIMARY_CPU;
319 spc->ns_comm_buf_addr = comm_buf_addr;
320 spc->ns_comm_buf_size = stmm_ns_comm_buf_size;
321
322 init_stmm_regs(spc, sec_buf_addr,
323 (vaddr_t)(mp_info + 1) - sec_buf_addr,
324 stack_addr + stmm_stack_size, image_addr);
325
326 return stmm_enter_user_mode(spc);
327 }
328
stmm_init_session(const TEE_UUID * uuid,struct tee_ta_session * sess)329 TEE_Result stmm_init_session(const TEE_UUID *uuid, struct tee_ta_session *sess)
330 {
331 struct stmm_ctx *spc = NULL;
332 TEE_Result res = TEE_SUCCESS;
333
334 if (memcmp(uuid, &stmm_uuid, sizeof(*uuid)))
335 return TEE_ERROR_ITEM_NOT_FOUND;
336
337 spc = stmm_alloc_ctx(uuid);
338 if (!spc)
339 return TEE_ERROR_OUT_OF_MEMORY;
340
341 spc->is_initializing = true;
342
343 mutex_lock(&tee_ta_mutex);
344 sess->ts_sess.ctx = &spc->ta_ctx.ts_ctx;
345 sess->ts_sess.handle_svc = sess->ts_sess.ctx->ops->handle_svc;
346 mutex_unlock(&tee_ta_mutex);
347
348 ts_push_current_session(&sess->ts_sess);
349 res = load_stmm(spc);
350 ts_pop_current_session();
351 vm_set_ctx(NULL);
352 if (res) {
353 sess->ts_sess.ctx = NULL;
354 spc->ta_ctx.ts_ctx.ops->destroy(&spc->ta_ctx.ts_ctx);
355
356 return res;
357 }
358
359 mutex_lock(&tee_ta_mutex);
360 spc->is_initializing = false;
361 TAILQ_INSERT_TAIL(&tee_ctxes, &spc->ta_ctx, link);
362 mutex_unlock(&tee_ta_mutex);
363
364 return TEE_SUCCESS;
365 }
366
stmm_enter_open_session(struct ts_session * s)367 static TEE_Result stmm_enter_open_session(struct ts_session *s)
368 {
369 struct stmm_ctx *spc = to_stmm_ctx(s->ctx);
370 struct tee_ta_session *ta_sess = to_ta_session(s);
371 const uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_NONE,
372 TEE_PARAM_TYPE_NONE,
373 TEE_PARAM_TYPE_NONE,
374 TEE_PARAM_TYPE_NONE);
375
376 if (ta_sess->param->types != exp_pt)
377 return TEE_ERROR_BAD_PARAMETERS;
378
379 if (spc->is_initializing) {
380 /* StMM is initialized in stmm_init_session() */
381 ta_sess->err_origin = TEE_ORIGIN_TEE;
382 return TEE_ERROR_BAD_STATE;
383 }
384
385 return TEE_SUCCESS;
386 }
387
stmm_enter_invoke_cmd(struct ts_session * s,uint32_t cmd)388 static TEE_Result stmm_enter_invoke_cmd(struct ts_session *s, uint32_t cmd)
389 {
390 struct stmm_ctx *spc = to_stmm_ctx(s->ctx);
391 struct tee_ta_session *ta_sess = to_ta_session(s);
392 TEE_Result res = TEE_SUCCESS;
393 TEE_Result __maybe_unused tmp_res = TEE_SUCCESS;
394 unsigned int ns_buf_size = 0;
395 struct param_mem *mem = NULL;
396 void *va = NULL;
397 const uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_MEMREF_INOUT,
398 TEE_PARAM_TYPE_VALUE_OUTPUT,
399 TEE_PARAM_TYPE_NONE,
400 TEE_PARAM_TYPE_NONE);
401
402 if (cmd != PTA_STMM_CMD_COMMUNICATE)
403 return TEE_ERROR_BAD_PARAMETERS;
404
405 if (ta_sess->param->types != exp_pt)
406 return TEE_ERROR_BAD_PARAMETERS;
407
408 mem = &ta_sess->param->u[0].mem;
409 ns_buf_size = mem->size;
410 if (ns_buf_size > spc->ns_comm_buf_size) {
411 mem->size = spc->ns_comm_buf_size;
412 return TEE_ERROR_EXCESS_DATA;
413 }
414
415 res = mobj_inc_map(mem->mobj);
416 if (res)
417 return res;
418
419 va = mobj_get_va(mem->mobj, mem->offs, mem->size);
420 if (!va) {
421 EMSG("Can't get a valid VA for NS buffer");
422 res = TEE_ERROR_BAD_PARAMETERS;
423 goto out_va;
424 }
425
426 #ifdef ARM64
427 spc->regs.x[0] = __FFA_MSG_SEND_DIRECT_REQ;
428 spc->regs.x[1] = (stmm_pta_id << 16) | stmm_id;
429 spc->regs.x[2] = FFA_PARAM_MBZ;
430 spc->regs.x[3] = spc->ns_comm_buf_addr;
431 spc->regs.x[4] = ns_buf_size;
432 spc->regs.x[5] = 0;
433 spc->regs.x[6] = 0;
434 spc->regs.x[7] = 0;
435 #endif
436 #ifdef ARM32
437 spc->regs.r0 = __FFA_MSG_SEND_DIRECT_REQ;
438 spc->regs.r1 = (stmm_pta_id << 16) | stmm_id;
439 spc->regs.r2 = FFA_PARAM_MBZ;
440 spc->regs.r3 = spc->ns_comm_buf_addr;
441 spc->regs.r4 = ns_buf_size;
442 spc->regs.r5 = 0;
443 spc->regs.r6 = 0;
444 spc->regs.r7 = 0;
445 #endif
446
447 ts_push_current_session(s);
448
449 memcpy((void *)spc->ns_comm_buf_addr, va, ns_buf_size);
450
451 res = stmm_enter_user_mode(spc);
452 if (res)
453 goto out_session;
454 /*
455 * Copy the SPM response from secure partition back to the non-secure
456 * buffer of the client that called us.
457 */
458 #ifdef ARM64
459 ta_sess->param->u[1].val.a = spc->regs.x[4];
460 #endif
461 #ifdef ARM32
462 ta_sess->param->u[1].val.a = spc->regs.r4;
463 #endif
464
465 memcpy(va, (void *)spc->ns_comm_buf_addr, ns_buf_size);
466
467 out_session:
468 ts_pop_current_session();
469 out_va:
470 tmp_res = mobj_dec_map(mem->mobj);
471 assert(!tmp_res);
472
473 return res;
474 }
475
stmm_enter_close_session(struct ts_session * s __unused)476 static void stmm_enter_close_session(struct ts_session *s __unused)
477 {
478 }
479
stmm_dump_state(struct ts_ctx * ctx)480 static void stmm_dump_state(struct ts_ctx *ctx)
481 {
482 user_mode_ctx_print_mappings(to_user_mode_ctx(ctx));
483 }
484 DECLARE_KEEP_PAGER(stmm_dump_state);
485
stmm_get_instance_id(struct ts_ctx * ctx)486 static uint32_t stmm_get_instance_id(struct ts_ctx *ctx)
487 {
488 return to_stmm_ctx(ctx)->uctx.vm_info.asid;
489 }
490
stmm_ctx_destroy(struct ts_ctx * ctx)491 static void stmm_ctx_destroy(struct ts_ctx *ctx)
492 {
493 struct stmm_ctx *spc = to_stmm_ctx(ctx);
494
495 vm_info_final(&spc->uctx);
496 free(spc);
497 }
498
sp_svc_get_mem_attr(vaddr_t va)499 static uint32_t sp_svc_get_mem_attr(vaddr_t va)
500 {
501 TEE_Result res = TEE_ERROR_BAD_PARAMETERS;
502 struct ts_session *sess = NULL;
503 struct stmm_ctx *spc = NULL;
504 uint16_t attrs = 0;
505 uint16_t perm = 0;
506
507 if (!va)
508 goto err;
509
510 sess = ts_get_current_session();
511 spc = to_stmm_ctx(sess->ctx);
512
513 res = vm_get_prot(&spc->uctx, va, SMALL_PAGE_SIZE, &attrs);
514 if (res)
515 goto err;
516
517 if (attrs & TEE_MATTR_UR)
518 perm |= STMM_MEM_ATTR_ACCESS_RO;
519 else if (attrs & TEE_MATTR_UW)
520 perm |= STMM_MEM_ATTR_ACCESS_RW;
521
522 if (attrs & TEE_MATTR_UX)
523 perm |= STMM_MEM_ATTR_EXEC;
524
525 return perm;
526 err:
527 return STMM_RET_DENIED;
528 }
529
sp_svc_set_mem_attr(vaddr_t va,unsigned int nr_pages,uint32_t perm)530 static int sp_svc_set_mem_attr(vaddr_t va, unsigned int nr_pages, uint32_t perm)
531 {
532 TEE_Result res = TEE_ERROR_BAD_PARAMETERS;
533 struct ts_session *sess = NULL;
534 struct stmm_ctx *spc = NULL;
535 size_t sz = 0;
536 uint32_t prot = 0;
537
538 if (!va || !nr_pages || MUL_OVERFLOW(nr_pages, SMALL_PAGE_SIZE, &sz))
539 return STMM_RET_INVALID_PARAM;
540
541 if (perm & ~STMM_MEM_ATTR_ALL)
542 return STMM_RET_INVALID_PARAM;
543
544 sess = ts_get_current_session();
545 spc = to_stmm_ctx(sess->ctx);
546
547 if ((perm & STMM_MEM_ATTR_ACCESS_MASK) == STMM_MEM_ATTR_ACCESS_RO)
548 prot |= TEE_MATTR_UR;
549 else if ((perm & STMM_MEM_ATTR_ACCESS_MASK) == STMM_MEM_ATTR_ACCESS_RW)
550 prot |= TEE_MATTR_URW;
551
552 if ((perm & STMM_MEM_ATTR_EXEC_NEVER) == STMM_MEM_ATTR_EXEC)
553 prot |= TEE_MATTR_UX;
554
555 res = vm_set_prot(&spc->uctx, va, sz, prot);
556 if (res)
557 return STMM_RET_DENIED;
558
559 return STMM_RET_SUCCESS;
560 }
561
562 #ifdef ARM64
save_sp_ctx(struct stmm_ctx * spc,struct thread_svc_regs * svc_regs)563 static void save_sp_ctx(struct stmm_ctx *spc, struct thread_svc_regs *svc_regs)
564 {
565 size_t n = 0;
566
567 /* Save the return values from StMM */
568 for (n = 0; n <= 7; n++)
569 spc->regs.x[n] = *(&svc_regs->x0 + n);
570
571 spc->regs.sp = svc_regs->sp_el0;
572 spc->regs.pc = svc_regs->elr;
573 spc->regs.cpsr = svc_regs->spsr;
574 }
575 #endif
576
577 #ifdef ARM32
save_sp_ctx(struct stmm_ctx * spc,struct thread_svc_regs * svc_regs)578 static void save_sp_ctx(struct stmm_ctx *spc, struct thread_svc_regs *svc_regs)
579 {
580 spc->regs.r0 = svc_regs->r0;
581 spc->regs.r1 = svc_regs->r1;
582 spc->regs.r2 = svc_regs->r2;
583 spc->regs.r3 = svc_regs->r3;
584 spc->regs.r4 = svc_regs->r4;
585 spc->regs.r5 = svc_regs->r5;
586 spc->regs.r6 = svc_regs->r6;
587 spc->regs.r7 = svc_regs->r7;
588 spc->regs.pc = svc_regs->lr;
589 spc->regs.cpsr = svc_regs->spsr;
590 spc->regs.usr_sp = thread_get_usr_sp();
591 }
592 #endif
593
return_from_sp_helper(bool panic,uint32_t panic_code,struct thread_svc_regs * svc_regs)594 static void return_from_sp_helper(bool panic, uint32_t panic_code,
595 struct thread_svc_regs *svc_regs)
596 {
597 struct ts_session *sess = ts_get_current_session();
598 struct stmm_ctx *spc = to_stmm_ctx(sess->ctx);
599
600 if (panic)
601 spc->ta_ctx.panicked = true;
602 else
603 save_sp_ctx(spc, svc_regs);
604
605 SVC_REGS_A0(svc_regs) = 0;
606 SVC_REGS_A1(svc_regs) = panic;
607 SVC_REGS_A2(svc_regs) = panic_code;
608 }
609
service_compose_direct_resp(struct thread_svc_regs * regs,uint32_t ret_val)610 static void service_compose_direct_resp(struct thread_svc_regs *regs,
611 uint32_t ret_val)
612 {
613 uint16_t src_id = 0;
614 uint16_t dst_id = 0;
615
616 /* extract from request */
617 src_id = (SVC_REGS_A1(regs) >> 16) & UINT16_MAX;
618 dst_id = SVC_REGS_A1(regs) & UINT16_MAX;
619
620 /* compose message */
621 SVC_REGS_A0(regs) = __FFA_MSG_SEND_DIRECT_RESP;
622 /* swap endpoint ids */
623 SVC_REGS_A1(regs) = SHIFT_U32(dst_id, 16) | src_id;
624 SVC_REGS_A2(regs) = FFA_PARAM_MBZ;
625 SVC_REGS_A3(regs) = ret_val;
626 SVC_REGS_A4(regs) = 0;
627 SVC_REGS_A5(regs) = 0;
628 SVC_REGS_A6(regs) = 0;
629 SVC_REGS_A7(regs) = 0;
630 }
631
632 /*
633 * Combined read from secure partition, this will open, read and
634 * close the file object.
635 */
sec_storage_obj_read(unsigned long storage_id,char * obj_id,unsigned long obj_id_len,void * data,unsigned long len,unsigned long offset,unsigned long flags)636 static TEE_Result sec_storage_obj_read(unsigned long storage_id, char *obj_id,
637 unsigned long obj_id_len, void *data,
638 unsigned long len, unsigned long offset,
639 unsigned long flags)
640 {
641 const struct tee_file_operations *fops = NULL;
642 TEE_Result res = TEE_ERROR_BAD_STATE;
643 struct ts_session *sess = NULL;
644 struct tee_file_handle *fh = NULL;
645 struct stmm_ctx *spc = NULL;
646 struct tee_pobj *po = NULL;
647 size_t file_size = 0;
648 size_t read_len = 0;
649
650 fops = tee_svc_storage_file_ops(storage_id);
651 if (!fops)
652 return TEE_ERROR_ITEM_NOT_FOUND;
653
654 if (obj_id_len > TEE_OBJECT_ID_MAX_LEN)
655 return TEE_ERROR_BAD_PARAMETERS;
656
657 sess = ts_get_current_session();
658 spc = to_stmm_ctx(sess->ctx);
659 res = vm_check_access_rights(&spc->uctx,
660 TEE_MEMORY_ACCESS_WRITE |
661 TEE_MEMORY_ACCESS_ANY_OWNER,
662 (uaddr_t)data, len);
663 if (res != TEE_SUCCESS)
664 return res;
665
666 res = tee_pobj_get(&sess->ctx->uuid, obj_id, obj_id_len, flags,
667 false, fops, &po);
668 if (res != TEE_SUCCESS)
669 return res;
670
671 res = po->fops->open(po, &file_size, &fh);
672 if (res != TEE_SUCCESS)
673 goto out;
674
675 read_len = len;
676 res = po->fops->read(fh, offset, data, &read_len);
677 if (res == TEE_ERROR_CORRUPT_OBJECT) {
678 EMSG("Object corrupt");
679 po->fops->remove(po);
680 } else if (res == TEE_SUCCESS && len != read_len) {
681 res = TEE_ERROR_CORRUPT_OBJECT;
682 }
683
684 po->fops->close(&fh);
685
686 out:
687 tee_pobj_release(po);
688
689 return res;
690 }
691
692 /*
693 * Combined write from secure partition, this will create/open, write and
694 * close the file object.
695 */
sec_storage_obj_write(unsigned long storage_id,char * obj_id,unsigned long obj_id_len,void * data,unsigned long len,unsigned long offset,unsigned long flags)696 static TEE_Result sec_storage_obj_write(unsigned long storage_id, char *obj_id,
697 unsigned long obj_id_len, void *data,
698 unsigned long len, unsigned long offset,
699 unsigned long flags)
700
701 {
702 const struct tee_file_operations *fops = NULL;
703 struct ts_session *sess = NULL;
704 struct tee_file_handle *fh = NULL;
705 struct stmm_ctx *spc = NULL;
706 TEE_Result res = TEE_SUCCESS;
707 struct tee_pobj *po = NULL;
708
709 fops = tee_svc_storage_file_ops(storage_id);
710 if (!fops)
711 return TEE_ERROR_ITEM_NOT_FOUND;
712
713 if (obj_id_len > TEE_OBJECT_ID_MAX_LEN)
714 return TEE_ERROR_BAD_PARAMETERS;
715
716 sess = ts_get_current_session();
717 spc = to_stmm_ctx(sess->ctx);
718 res = vm_check_access_rights(&spc->uctx,
719 TEE_MEMORY_ACCESS_READ |
720 TEE_MEMORY_ACCESS_ANY_OWNER,
721 (uaddr_t)data, len);
722 if (res != TEE_SUCCESS)
723 return res;
724
725 res = tee_pobj_get(&sess->ctx->uuid, obj_id, obj_id_len, flags,
726 false, fops, &po);
727 if (res != TEE_SUCCESS)
728 return res;
729
730 res = po->fops->open(po, NULL, &fh);
731 if (res == TEE_ERROR_ITEM_NOT_FOUND)
732 res = po->fops->create(po, false, NULL, 0, NULL, 0, NULL, 0,
733 &fh);
734 if (res == TEE_SUCCESS) {
735 res = po->fops->write(fh, offset, data, len);
736 po->fops->close(&fh);
737 }
738
739 tee_pobj_release(po);
740
741 return res;
742 }
743
stmm_handle_mem_mgr_service(struct thread_svc_regs * regs)744 static void stmm_handle_mem_mgr_service(struct thread_svc_regs *regs)
745 {
746 uint32_t action = SVC_REGS_A3(regs);
747 uintptr_t va = SVC_REGS_A4(regs);
748 uint32_t nr_pages = SVC_REGS_A5(regs);
749 uint32_t perm = SVC_REGS_A6(regs);
750
751 switch (action) {
752 case __FFA_SVC_MEMORY_ATTRIBUTES_GET:
753 service_compose_direct_resp(regs, sp_svc_get_mem_attr(va));
754 break;
755 case __FFA_SVC_MEMORY_ATTRIBUTES_SET:
756 service_compose_direct_resp(regs,
757 sp_svc_set_mem_attr(va, nr_pages,
758 perm));
759 break;
760 default:
761 EMSG("Undefined service id %#"PRIx32, action);
762 service_compose_direct_resp(regs, STMM_RET_INVALID_PARAM);
763 break;
764 }
765 }
766
tee2stmm_ret_val(TEE_Result res)767 static uint32_t tee2stmm_ret_val(TEE_Result res)
768 {
769 switch (res) {
770 case TEE_SUCCESS:
771 return STMM_RET_SUCCESS;
772 case TEE_ERROR_NOT_SUPPORTED:
773 return STMM_RET_NOT_SUPPORTED;
774 case TEE_ERROR_ACCESS_DENIED:
775 return STMM_RET_DENIED;
776 case TEE_ERROR_OUT_OF_MEMORY:
777 return STMM_RET_NO_MEM;
778 case TEE_ERROR_BAD_PARAMETERS:
779 default:
780 return STMM_RET_INVALID_PARAM;
781 }
782 }
783
784 #define FILENAME "EFI_VARS"
stmm_handle_storage_service(struct thread_svc_regs * regs)785 static void stmm_handle_storage_service(struct thread_svc_regs *regs)
786 {
787 uint32_t flags = TEE_DATA_FLAG_ACCESS_READ |
788 TEE_DATA_FLAG_ACCESS_WRITE |
789 TEE_DATA_FLAG_SHARE_READ |
790 TEE_DATA_FLAG_SHARE_WRITE;
791 uint32_t action = SVC_REGS_A3(regs);
792 void *va = (void *)SVC_REGS_A4(regs);
793 unsigned long len = SVC_REGS_A5(regs);
794 unsigned long offset = SVC_REGS_A6(regs);
795 char obj_id[] = FILENAME;
796 size_t obj_id_len = strlen(obj_id);
797 TEE_Result res = TEE_SUCCESS;
798 uint32_t stmm_rc = STMM_RET_INVALID_PARAM;
799
800 switch (action) {
801 case __FFA_SVC_RPMB_READ:
802 DMSG("RPMB read");
803 res = sec_storage_obj_read(TEE_STORAGE_PRIVATE_RPMB, obj_id,
804 obj_id_len, va, len, offset, flags);
805 stmm_rc = tee2stmm_ret_val(res);
806 break;
807 case __FFA_SVC_RPMB_WRITE:
808 DMSG("RPMB write");
809 res = sec_storage_obj_write(TEE_STORAGE_PRIVATE_RPMB, obj_id,
810 obj_id_len, va, len, offset, flags);
811 stmm_rc = tee2stmm_ret_val(res);
812 break;
813 default:
814 EMSG("Undefined service id %#"PRIx32, action);
815 break;
816 }
817
818 service_compose_direct_resp(regs, stmm_rc);
819 }
820
spm_eret_error(int32_t error_code,struct thread_svc_regs * regs)821 static void spm_eret_error(int32_t error_code, struct thread_svc_regs *regs)
822 {
823 SVC_REGS_A0(regs) = FFA_ERROR;
824 SVC_REGS_A1(regs) = FFA_PARAM_MBZ;
825 SVC_REGS_A2(regs) = error_code;
826 SVC_REGS_A3(regs) = FFA_PARAM_MBZ;
827 SVC_REGS_A4(regs) = FFA_PARAM_MBZ;
828 SVC_REGS_A5(regs) = FFA_PARAM_MBZ;
829 SVC_REGS_A6(regs) = FFA_PARAM_MBZ;
830 SVC_REGS_A7(regs) = FFA_PARAM_MBZ;
831 }
832
spm_handle_direct_req(struct thread_svc_regs * regs)833 static void spm_handle_direct_req(struct thread_svc_regs *regs)
834 {
835 uint16_t dst_id = SVC_REGS_A1(regs) & UINT16_MAX;
836
837 if (dst_id == mem_mgr_id) {
838 stmm_handle_mem_mgr_service(regs);
839 } else if (dst_id == ffa_storage_id) {
840 stmm_handle_storage_service(regs);
841 } else {
842 EMSG("Undefined endpoint id %#"PRIx16, dst_id);
843 spm_eret_error(STMM_RET_INVALID_PARAM, regs);
844 }
845 }
846
847 /* Return true if returning to SP, false if returning to caller */
spm_handle_svc(struct thread_svc_regs * regs)848 static bool spm_handle_svc(struct thread_svc_regs *regs)
849 {
850 #ifdef ARM64
851 uint64_t *a0 = ®s->x0;
852 #endif
853 #ifdef ARM32
854 uint32_t *a0 = ®s->r0;
855 #endif
856
857 switch (*a0) {
858 case FFA_VERSION:
859 DMSG("Received FFA version");
860 *a0 = MAKE_FFA_VERSION(FFA_VERSION_MAJOR, FFA_VERSION_MINOR);
861 return true;
862 case __FFA_MSG_SEND_DIRECT_RESP:
863 DMSG("Received FFA direct response");
864 return_from_sp_helper(false, 0, regs);
865 return false;
866 case __FFA_MSG_SEND_DIRECT_REQ:
867 DMSG("Received FFA direct request");
868 spm_handle_direct_req(regs);
869 return true;
870 default:
871 EMSG("Undefined syscall %#"PRIx32, (uint32_t)*a0);
872 return_from_sp_helper(true /*panic*/, 0xabcd, regs);
873 return false;
874 }
875 }
876
877 /*
878 * Note: this variable is weak just to ease breaking its dependency chain
879 * when added to the unpaged area.
880 */
881 const struct ts_ops stmm_sp_ops __weak __relrodata_unpaged("stmm_sp_ops") = {
882 .enter_open_session = stmm_enter_open_session,
883 .enter_invoke_cmd = stmm_enter_invoke_cmd,
884 .enter_close_session = stmm_enter_close_session,
885 .dump_state = stmm_dump_state,
886 .destroy = stmm_ctx_destroy,
887 .get_instance_id = stmm_get_instance_id,
888 .handle_svc = spm_handle_svc,
889 };
890