xref: /optee_os/core/arch/arm/kernel/secure_partition.c (revision d783b68157e86b9a494089ccf1a9905f38a1d4b7) 
1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3  * Copyright (c) 2020-2022, Arm Limited.
4  */
5 #include <bench.h>
6 #include <crypto/crypto.h>
7 #include <initcall.h>
8 #include <kernel/embedded_ts.h>
9 #include <kernel/ldelf_loader.h>
10 #include <kernel/secure_partition.h>
11 #include <kernel/spinlock.h>
12 #include <kernel/spmc_sp_handler.h>
13 #include <kernel/thread_private.h>
14 #include <kernel/thread_spmc.h>
15 #include <kernel/ts_store.h>
16 #include <ldelf.h>
17 #include <libfdt.h>
18 #include <mm/core_mmu.h>
19 #include <mm/fobj.h>
20 #include <mm/mobj.h>
21 #include <mm/vm.h>
22 #include <optee_ffa.h>
23 #include <stdio.h>
24 #include <string.h>
25 #include <tee_api_types.h>
26 #include <tee/uuid.h>
27 #include <trace.h>
28 #include <types_ext.h>
29 #include <utee_defines.h>
30 #include <util.h>
31 #include <zlib.h>
32 
33 #define SP_MANIFEST_ATTR_READ		BIT(0)
34 #define SP_MANIFEST_ATTR_WRITE		BIT(1)
35 #define SP_MANIFEST_ATTR_EXEC		BIT(2)
36 #define SP_MANIFEST_ATTR_NSEC		BIT(3)
37 
38 const struct ts_ops sp_ops;
39 
40 /* List that holds all of the loaded SP's */
41 static struct sp_sessions_head open_sp_sessions =
42 	TAILQ_HEAD_INITIALIZER(open_sp_sessions);
43 
44 static const struct embedded_ts *find_secure_partition(const TEE_UUID *uuid)
45 {
46 	const struct sp_image *sp = NULL;
47 
48 	for_each_secure_partition(sp) {
49 		if (!memcmp(&sp->image.uuid, uuid, sizeof(*uuid)))
50 			return &sp->image;
51 	}
52 	return NULL;
53 }
54 
55 bool is_sp_ctx(struct ts_ctx *ctx)
56 {
57 	return ctx && (ctx->ops == &sp_ops);
58 }
59 
60 static void set_sp_ctx_ops(struct ts_ctx *ctx)
61 {
62 	ctx->ops = &sp_ops;
63 }
64 
65 TEE_Result sp_find_session_id(const TEE_UUID *uuid, uint32_t *session_id)
66 {
67 	struct sp_session *s = NULL;
68 
69 	TAILQ_FOREACH(s, &open_sp_sessions, link) {
70 		if (!memcmp(&s->ts_sess.ctx->uuid, uuid, sizeof(*uuid))) {
71 			if (s->state == sp_dead)
72 				return TEE_ERROR_TARGET_DEAD;
73 
74 			*session_id  = s->endpoint_id;
75 			return TEE_SUCCESS;
76 		}
77 	}
78 
79 	return TEE_ERROR_ITEM_NOT_FOUND;
80 }
81 
82 struct sp_session *sp_get_session(uint32_t session_id)
83 {
84 	struct sp_session *s = NULL;
85 
86 	TAILQ_FOREACH(s, &open_sp_sessions, link) {
87 		if (s->endpoint_id == session_id)
88 			return s;
89 	}
90 
91 	return NULL;
92 }
93 
94 TEE_Result sp_partition_info_get_all(struct ffa_partition_info *fpi,
95 				     size_t *elem_count)
96 {
97 	size_t in_count = *elem_count;
98 	struct sp_session *s = NULL;
99 	size_t count = 0;
100 
101 	TAILQ_FOREACH(s, &open_sp_sessions, link) {
102 		if (s->state == sp_dead)
103 			continue;
104 		if (count < in_count) {
105 			spmc_fill_partition_entry(fpi, s->endpoint_id, 1);
106 			fpi++;
107 		}
108 		count++;
109 	}
110 
111 	*elem_count = count;
112 	if (count > in_count)
113 		return TEE_ERROR_SHORT_BUFFER;
114 
115 	return TEE_SUCCESS;
116 }
117 
118 bool sp_has_exclusive_access(struct sp_mem_map_region *mem,
119 			     struct user_mode_ctx *uctx)
120 {
121 	/*
122 	 * Check that we have access to the region if it is supposed to be
123 	 * mapped to the current context.
124 	 */
125 	if (uctx) {
126 		struct vm_region *region = NULL;
127 
128 		/* Make sure that each mobj belongs to the SP */
129 		TAILQ_FOREACH(region, &uctx->vm_info.regions, link) {
130 			if (region->mobj == mem->mobj)
131 				break;
132 		}
133 
134 		if (!region)
135 			return false;
136 	}
137 
138 	/* Check that it is not shared with another SP */
139 	return !sp_mem_is_shared(mem);
140 }
141 
142 /*
143  * sp_init_info allocates and maps the sp_ffa_init_info for the SP. It will copy
144  * the fdt into the allocated page(s) and return a pointer to the new location
145  * of the fdt. This pointer can be used to update data inside the fdt.
146  */
147 static TEE_Result sp_init_info(struct sp_ctx *ctx, struct thread_smc_args *args,
148 			       const void * const input_fdt, vaddr_t *va,
149 			       size_t *num_pgs, void **fdt_copy)
150 {
151 	struct sp_ffa_init_info *info = NULL;
152 	int nvp_count = 1;
153 	size_t nvp_size = sizeof(struct sp_name_value_pair) * nvp_count;
154 	size_t info_size = sizeof(*info) + nvp_size;
155 	size_t fdt_size = fdt_totalsize(input_fdt);
156 	TEE_Result res = TEE_SUCCESS;
157 	uint32_t perm = TEE_MATTR_URW | TEE_MATTR_PRW;
158 	struct fobj *fo = NULL;
159 	struct mobj *m = NULL;
160 	static const char fdt_name[16] = "TYPE_DT\0\0\0\0\0\0\0\0";
161 
162 	*num_pgs = ROUNDUP(fdt_size + info_size, SMALL_PAGE_SIZE) /
163 		   SMALL_PAGE_SIZE;
164 
165 	fo = fobj_sec_mem_alloc(*num_pgs);
166 	m = mobj_with_fobj_alloc(fo, NULL);
167 
168 	fobj_put(fo);
169 	if (!m)
170 		return TEE_ERROR_OUT_OF_MEMORY;
171 
172 	res = vm_map(&ctx->uctx, va, fdt_size + info_size,
173 		     perm, 0, m, 0);
174 	mobj_put(m);
175 	if (res)
176 		return res;
177 
178 	info = (struct sp_ffa_init_info *)*va;
179 
180 	/* magic field is 4 bytes, we don't copy /0 byte. */
181 	memcpy(&info->magic, "FF-A", 4);
182 	info->count = nvp_count;
183 	args->a0 = (vaddr_t)info;
184 
185 	/*
186 	 * Store the fdt after the boot_info and store the pointer in the
187 	 * first element.
188 	 */
189 	COMPILE_TIME_ASSERT(sizeof(info->nvp[0].name) == sizeof(fdt_name));
190 	memcpy(info->nvp[0].name, fdt_name, sizeof(fdt_name));
191 	info->nvp[0].value = *va + info_size;
192 	info->nvp[0].size = fdt_size;
193 	memcpy((void *)info->nvp[0].value, input_fdt, fdt_size);
194 	*fdt_copy = (void *)info->nvp[0].value;
195 
196 	return TEE_SUCCESS;
197 }
198 
199 static uint16_t new_session_id(struct sp_sessions_head *open_sessions)
200 {
201 	struct sp_session *last = NULL;
202 	uint16_t id = SPMC_ENDPOINT_ID + 1;
203 
204 	last = TAILQ_LAST(open_sessions, sp_sessions_head);
205 	if (last)
206 		id = last->endpoint_id + 1;
207 
208 	assert(id > SPMC_ENDPOINT_ID);
209 	return id;
210 }
211 
212 static TEE_Result sp_create_ctx(const TEE_UUID *uuid, struct sp_session *s)
213 {
214 	TEE_Result res = TEE_SUCCESS;
215 	struct sp_ctx *spc = NULL;
216 
217 	/* Register context */
218 	spc = calloc(1, sizeof(struct sp_ctx));
219 	if (!spc)
220 		return TEE_ERROR_OUT_OF_MEMORY;
221 
222 	spc->uctx.ts_ctx = &spc->ts_ctx;
223 	spc->open_session = s;
224 	s->ts_sess.ctx = &spc->ts_ctx;
225 	spc->ts_ctx.uuid = *uuid;
226 
227 	res = vm_info_init(&spc->uctx);
228 	if (res)
229 		goto err;
230 
231 	set_sp_ctx_ops(&spc->ts_ctx);
232 
233 	return TEE_SUCCESS;
234 
235 err:
236 	free(spc);
237 	return res;
238 }
239 
240 static TEE_Result sp_create_session(struct sp_sessions_head *open_sessions,
241 				    const TEE_UUID *uuid,
242 				    struct sp_session **sess)
243 {
244 	TEE_Result res = TEE_SUCCESS;
245 	struct sp_session *s = calloc(1, sizeof(struct sp_session));
246 
247 	if (!s)
248 		return TEE_ERROR_OUT_OF_MEMORY;
249 
250 	s->endpoint_id = new_session_id(open_sessions);
251 	if (!s->endpoint_id) {
252 		res = TEE_ERROR_OVERFLOW;
253 		goto err;
254 	}
255 
256 	DMSG("Loading Secure Partition %pUl", (void *)uuid);
257 	res = sp_create_ctx(uuid, s);
258 	if (res)
259 		goto err;
260 
261 	TAILQ_INSERT_TAIL(open_sessions, s, link);
262 	*sess = s;
263 	return TEE_SUCCESS;
264 
265 err:
266 	free(s);
267 	return res;
268 }
269 
270 static TEE_Result sp_init_set_registers(struct sp_ctx *ctx)
271 {
272 	struct thread_ctx_regs *sp_regs = &ctx->sp_regs;
273 
274 	memset(sp_regs, 0, sizeof(*sp_regs));
275 	sp_regs->sp = ctx->uctx.stack_ptr;
276 	sp_regs->pc = ctx->uctx.entry_func;
277 
278 	return TEE_SUCCESS;
279 }
280 
281 TEE_Result sp_map_shared(struct sp_session *s,
282 			 struct sp_mem_receiver *receiver,
283 			 struct sp_mem *smem,
284 			 uint64_t *va)
285 {
286 	TEE_Result res = TEE_SUCCESS;
287 	struct sp_ctx *ctx = NULL;
288 	uint32_t perm = TEE_MATTR_UR;
289 	struct sp_mem_map_region *reg = NULL;
290 
291 	ctx = to_sp_ctx(s->ts_sess.ctx);
292 
293 	/* Get the permission */
294 	if (receiver->perm.perm & FFA_MEM_ACC_EXE)
295 		perm |= TEE_MATTR_UX;
296 
297 	if (receiver->perm.perm & FFA_MEM_ACC_RW) {
298 		if (receiver->perm.perm & FFA_MEM_ACC_EXE)
299 			return TEE_ERROR_ACCESS_CONFLICT;
300 
301 		perm |= TEE_MATTR_UW;
302 	}
303 	/*
304 	 * Currently we don't support passing a va. We can't guarantee that the
305 	 * full region will be mapped in a contiguous region. A smem->region can
306 	 * have multiple mobj for one share. Currently there doesn't seem to be
307 	 * an option to guarantee that these will be mapped in a contiguous va
308 	 * space.
309 	 */
310 	if (*va)
311 		return TEE_ERROR_NOT_SUPPORTED;
312 
313 	SLIST_FOREACH(reg, &smem->regions, link) {
314 		res = vm_map(&ctx->uctx, va, reg->page_count * SMALL_PAGE_SIZE,
315 			     perm, 0, reg->mobj, reg->page_offset);
316 
317 		if (res != TEE_SUCCESS) {
318 			EMSG("Failed to map memory region %#"PRIx32, res);
319 			return res;
320 		}
321 	}
322 	return TEE_SUCCESS;
323 }
324 
325 TEE_Result sp_unmap_ffa_regions(struct sp_session *s, struct sp_mem *smem)
326 {
327 	TEE_Result res = TEE_SUCCESS;
328 	vaddr_t vaddr = 0;
329 	size_t len = 0;
330 	struct sp_ctx *ctx = to_sp_ctx(s->ts_sess.ctx);
331 	struct sp_mem_map_region *reg = NULL;
332 
333 	SLIST_FOREACH(reg, &smem->regions, link) {
334 		vaddr = (vaddr_t)sp_mem_get_va(&ctx->uctx, reg->page_offset,
335 					       reg->mobj);
336 		len = reg->page_count * SMALL_PAGE_SIZE;
337 
338 		res = vm_unmap(&ctx->uctx, vaddr, len);
339 		if (res != TEE_SUCCESS)
340 			return res;
341 	}
342 
343 	return TEE_SUCCESS;
344 }
345 
346 static TEE_Result sp_open_session(struct sp_session **sess,
347 				  struct sp_sessions_head *open_sessions,
348 				  const TEE_UUID *uuid)
349 {
350 	TEE_Result res = TEE_SUCCESS;
351 	struct sp_session *s = NULL;
352 	struct sp_ctx *ctx = NULL;
353 
354 	if (!find_secure_partition(uuid))
355 		return TEE_ERROR_ITEM_NOT_FOUND;
356 
357 	res = sp_create_session(open_sessions, uuid, &s);
358 	if (res != TEE_SUCCESS) {
359 		DMSG("sp_create_session failed %#"PRIx32, res);
360 		return res;
361 	}
362 
363 	ctx = to_sp_ctx(s->ts_sess.ctx);
364 	assert(ctx);
365 	if (!ctx)
366 		return TEE_ERROR_TARGET_DEAD;
367 	*sess = s;
368 
369 	ts_push_current_session(&s->ts_sess);
370 	/* Load the SP using ldelf. */
371 	ldelf_load_ldelf(&ctx->uctx);
372 	res = ldelf_init_with_ldelf(&s->ts_sess, &ctx->uctx);
373 
374 	if (res != TEE_SUCCESS) {
375 		EMSG("Failed. loading SP using ldelf %#"PRIx32, res);
376 		ts_pop_current_session();
377 		return TEE_ERROR_TARGET_DEAD;
378 	}
379 
380 	/* Make the SP ready for its first run */
381 	s->state = sp_idle;
382 	s->caller_id = 0;
383 	sp_init_set_registers(ctx);
384 	ts_pop_current_session();
385 
386 	return TEE_SUCCESS;
387 }
388 
389 static TEE_Result sp_dt_get_u64(const void *fdt, int node, const char *property,
390 				uint64_t *value)
391 {
392 	const fdt64_t *p = NULL;
393 	int len = 0;
394 
395 	p = fdt_getprop(fdt, node, property, &len);
396 	if (!p || len != sizeof(*p))
397 		return TEE_ERROR_ITEM_NOT_FOUND;
398 
399 	*value = fdt64_to_cpu(*p);
400 
401 	return TEE_SUCCESS;
402 }
403 
404 static TEE_Result sp_dt_get_u32(const void *fdt, int node, const char *property,
405 				uint32_t *value)
406 {
407 	const fdt32_t *p = NULL;
408 	int len = 0;
409 
410 	p = fdt_getprop(fdt, node, property, &len);
411 	if (!p || len != sizeof(*p))
412 		return TEE_ERROR_ITEM_NOT_FOUND;
413 
414 	*value = fdt32_to_cpu(*p);
415 
416 	return TEE_SUCCESS;
417 }
418 
419 static TEE_Result handle_fdt_dev_regions(struct sp_ctx *ctx, void *fdt)
420 {
421 	int node = 0;
422 	int subnode = 0;
423 	TEE_Result res = TEE_SUCCESS;
424 	const char *dt_device_match_table = {
425 		"arm,ffa-manifest-device-regions",
426 	};
427 
428 	/*
429 	 * Device regions are optional in the SP manifest, it's not an error if
430 	 * we don't find any
431 	 */
432 	node = fdt_node_offset_by_compatible(fdt, 0, dt_device_match_table);
433 	if (node < 0)
434 		return TEE_SUCCESS;
435 
436 	fdt_for_each_subnode(subnode, fdt, node) {
437 		uint64_t base_addr = 0;
438 		uint32_t pages_cnt = 0;
439 		uint32_t attributes = 0;
440 		struct mobj *m = NULL;
441 		bool is_secure = true;
442 		uint32_t perm = 0;
443 		vaddr_t va = 0;
444 		unsigned int idx = 0;
445 
446 		/*
447 		 * Physical base address of a device MMIO region.
448 		 * Currently only physically contiguous region is supported.
449 		 */
450 		if (sp_dt_get_u64(fdt, subnode, "base-address", &base_addr)) {
451 			EMSG("Mandatory field is missing: base-address");
452 			return TEE_ERROR_BAD_FORMAT;
453 		}
454 
455 		/* Total size of MMIO region as count of 4K pages */
456 		if (sp_dt_get_u32(fdt, subnode, "pages-count", &pages_cnt)) {
457 			EMSG("Mandatory field is missing: pages-count");
458 			return TEE_ERROR_BAD_FORMAT;
459 		}
460 
461 		/* Data access, instruction access and security attributes */
462 		if (sp_dt_get_u32(fdt, subnode, "attributes", &attributes)) {
463 			EMSG("Mandatory field is missing: attributes");
464 			return TEE_ERROR_BAD_FORMAT;
465 		}
466 
467 		/* Instruction access permission must be not executable */
468 		if (attributes & SP_MANIFEST_ATTR_EXEC) {
469 			EMSG("Invalid instruction access permission");
470 			return TEE_ERROR_BAD_FORMAT;
471 		}
472 
473 		/* Data access permission must be read-only or read/write */
474 		if (attributes & SP_MANIFEST_ATTR_READ) {
475 			perm = TEE_MATTR_UR;
476 
477 			if (attributes & SP_MANIFEST_ATTR_WRITE)
478 				perm |= TEE_MATTR_UW;
479 		} else {
480 			EMSG("Invalid data access permissions");
481 			return TEE_ERROR_BAD_FORMAT;
482 		}
483 
484 		/*
485 		 * The SP is a secure endpoint, security attribute can be
486 		 * secure or non-secure
487 		 */
488 		if (attributes & SP_MANIFEST_ATTR_NSEC)
489 			is_secure = false;
490 
491 		/* Memory attributes must be Device-nGnRnE */
492 		m = sp_mem_new_mobj(pages_cnt, TEE_MATTR_MEM_TYPE_STRONGLY_O,
493 				    is_secure);
494 		if (!m)
495 			return TEE_ERROR_OUT_OF_MEMORY;
496 
497 		res = sp_mem_add_pages(m, &idx, (paddr_t)base_addr, pages_cnt);
498 		if (res) {
499 			mobj_put(m);
500 			return res;
501 		}
502 
503 		res = vm_map(&ctx->uctx, &va, pages_cnt * SMALL_PAGE_SIZE,
504 			     perm, 0, m, 0);
505 		mobj_put(m);
506 		if (res)
507 			return res;
508 
509 		/*
510 		 * Overwrite the device region's PA in the fdt with the VA. This
511 		 * fdt will be passed to the SP.
512 		 */
513 		res = fdt_setprop_u64(fdt, subnode, "base-address", va);
514 
515 		/*
516 		 * Unmap the region if the overwrite failed since the SP won't
517 		 * be able to access it without knowing the VA.
518 		 */
519 		if (res) {
520 			vm_unmap(&ctx->uctx, va, pages_cnt * SMALL_PAGE_SIZE);
521 			return res;
522 		}
523 	}
524 
525 	return TEE_SUCCESS;
526 }
527 
528 static TEE_Result handle_fdt(const void * const fdt, const TEE_UUID *uuid)
529 {
530 	int len = 0;
531 	const fdt32_t *prop = NULL;
532 	int i = 0;
533 	const struct fdt_property *description = NULL;
534 	int description_name_len = 0;
535 	uint32_t uuid_array[4] = { 0 };
536 	TEE_UUID fdt_uuid = { };
537 
538 	if (fdt_node_check_compatible(fdt, 0, "arm,ffa-manifest-1.0")) {
539 		EMSG("Failed loading SP, manifest not found");
540 		return TEE_ERROR_BAD_PARAMETERS;
541 	}
542 
543 	description = fdt_get_property(fdt, 0, "description",
544 				       &description_name_len);
545 	if (description)
546 		DMSG("Loading SP: %s", description->data);
547 
548 	prop = fdt_getprop(fdt, 0, "uuid", &len);
549 	if (!prop || len != 16) {
550 		EMSG("Missing or invalid UUID in SP manifest");
551 		return TEE_ERROR_BAD_FORMAT;
552 	}
553 
554 	for (i = 0; i < 4; i++)
555 		uuid_array[i] = fdt32_to_cpu(prop[i]);
556 	tee_uuid_from_octets(&fdt_uuid, (uint8_t *)uuid_array);
557 
558 	if (memcmp(uuid, &fdt_uuid, sizeof(fdt_uuid))) {
559 		EMSG("Failed loading SP, UUID mismatch");
560 		return TEE_ERROR_BAD_FORMAT;
561 	}
562 
563 	return TEE_SUCCESS;
564 }
565 
566 static TEE_Result sp_init_uuid(const TEE_UUID *uuid, const void * const fdt)
567 {
568 	TEE_Result res = TEE_SUCCESS;
569 	struct sp_session *sess = NULL;
570 	struct thread_smc_args args = { };
571 	vaddr_t va = 0;
572 	size_t num_pgs = 0;
573 	struct sp_ctx *ctx = NULL;
574 	void *fdt_copy = NULL;
575 
576 	res = sp_open_session(&sess,
577 			      &open_sp_sessions,
578 			      uuid);
579 	if (res)
580 		return res;
581 
582 	res = handle_fdt(fdt, uuid);
583 	if (res)
584 		return res;
585 
586 	ctx = to_sp_ctx(sess->ts_sess.ctx);
587 	ts_push_current_session(&sess->ts_sess);
588 
589 	res = sp_init_info(ctx, &args, fdt, &va, &num_pgs, &fdt_copy);
590 	if (res)
591 		goto out;
592 
593 	res = handle_fdt_dev_regions(ctx, fdt_copy);
594 	if (res)
595 		goto out;
596 
597 	ts_pop_current_session();
598 
599 	if (sp_enter(&args, sess)) {
600 		vm_unmap(&ctx->uctx, va, num_pgs);
601 		return FFA_ABORTED;
602 	}
603 
604 	spmc_sp_msg_handler(&args, sess);
605 
606 	ts_push_current_session(&sess->ts_sess);
607 out:
608 	/* Free the boot info page from the SP memory */
609 	vm_unmap(&ctx->uctx, va, num_pgs);
610 	ts_pop_current_session();
611 
612 	return res;
613 }
614 
615 TEE_Result sp_enter(struct thread_smc_args *args, struct sp_session *sp)
616 {
617 	TEE_Result res = FFA_OK;
618 	struct sp_ctx *ctx = to_sp_ctx(sp->ts_sess.ctx);
619 
620 	ctx->sp_regs.x[0] = args->a0;
621 	ctx->sp_regs.x[1] = args->a1;
622 	ctx->sp_regs.x[2] = args->a2;
623 	ctx->sp_regs.x[3] = args->a3;
624 	ctx->sp_regs.x[4] = args->a4;
625 	ctx->sp_regs.x[5] = args->a5;
626 	ctx->sp_regs.x[6] = args->a6;
627 	ctx->sp_regs.x[7] = args->a7;
628 
629 	res = sp->ts_sess.ctx->ops->enter_invoke_cmd(&sp->ts_sess, 0);
630 
631 	args->a0 = ctx->sp_regs.x[0];
632 	args->a1 = ctx->sp_regs.x[1];
633 	args->a2 = ctx->sp_regs.x[2];
634 	args->a3 = ctx->sp_regs.x[3];
635 	args->a4 = ctx->sp_regs.x[4];
636 	args->a5 = ctx->sp_regs.x[5];
637 	args->a6 = ctx->sp_regs.x[6];
638 	args->a7 = ctx->sp_regs.x[7];
639 
640 	return res;
641 }
642 
643 static TEE_Result sp_enter_invoke_cmd(struct ts_session *s,
644 				      uint32_t cmd __unused)
645 {
646 	struct sp_ctx *ctx = to_sp_ctx(s->ctx);
647 	TEE_Result res = TEE_SUCCESS;
648 	uint32_t exceptions = 0;
649 	uint64_t cpsr = 0;
650 	struct sp_session *sp_s = to_sp_session(s);
651 	struct ts_session *sess = NULL;
652 	struct thread_ctx_regs *sp_regs = NULL;
653 	uint32_t panicked = false;
654 	uint32_t panic_code = 0;
655 
656 	bm_timestamp();
657 
658 	sp_regs = &ctx->sp_regs;
659 	ts_push_current_session(s);
660 
661 	cpsr = sp_regs->cpsr;
662 	sp_regs->cpsr = read_daif() & (SPSR_64_DAIF_MASK << SPSR_64_DAIF_SHIFT);
663 
664 	exceptions = thread_mask_exceptions(THREAD_EXCP_ALL);
665 	__thread_enter_user_mode(sp_regs, &panicked, &panic_code);
666 	sp_regs->cpsr = cpsr;
667 	thread_unmask_exceptions(exceptions);
668 
669 	thread_user_clear_vfp(&ctx->uctx);
670 
671 	if (panicked) {
672 		DMSG("SP panicked with code  %#"PRIx32, panic_code);
673 		abort_print_current_ts();
674 
675 		sess = ts_pop_current_session();
676 		cpu_spin_lock(&sp_s->spinlock);
677 		sp_s->state = sp_dead;
678 		cpu_spin_unlock(&sp_s->spinlock);
679 
680 		return TEE_ERROR_TARGET_DEAD;
681 	}
682 
683 	sess = ts_pop_current_session();
684 	assert(sess == s);
685 
686 	bm_timestamp();
687 
688 	return res;
689 }
690 
691 /* We currently don't support 32 bits */
692 #ifdef ARM64
693 static void sp_svc_store_registers(struct thread_svc_regs *regs,
694 				   struct thread_ctx_regs *sp_regs)
695 {
696 	COMPILE_TIME_ASSERT(sizeof(sp_regs->x[0]) == sizeof(regs->x0));
697 	memcpy(sp_regs->x, &regs->x0, 31 * sizeof(regs->x0));
698 	sp_regs->pc = regs->elr;
699 	sp_regs->sp = regs->sp_el0;
700 }
701 #endif
702 
703 static bool sp_handle_svc(struct thread_svc_regs *regs)
704 {
705 	struct ts_session *ts = ts_get_current_session();
706 	struct sp_ctx *uctx = to_sp_ctx(ts->ctx);
707 	struct sp_session *s = uctx->open_session;
708 
709 	assert(s);
710 
711 	sp_svc_store_registers(regs, &uctx->sp_regs);
712 
713 	regs->x0 = 0;
714 	regs->x1 = 0; /* panic */
715 	regs->x2 = 0; /* panic code */
716 
717 	/*
718 	 * All the registers of the SP are saved in the SP session by the SVC
719 	 * handler.
720 	 * We always return to S-El1 after handling the SVC. We will continue
721 	 * in sp_enter_invoke_cmd() (return from __thread_enter_user_mode).
722 	 * The sp_enter() function copies the FF-A parameters (a0-a7) from the
723 	 * saved registers to the thread_smc_args. The thread_smc_args object is
724 	 * afterward used by the spmc_sp_msg_handler() to handle the
725 	 * FF-A message send by the SP.
726 	 */
727 	return false;
728 }
729 
730 /*
731  * Note: this variable is weak just to ease breaking its dependency chain
732  * when added to the unpaged area.
733  */
734 const struct ts_ops sp_ops __weak __relrodata_unpaged("sp_ops") = {
735 	.enter_invoke_cmd = sp_enter_invoke_cmd,
736 	.handle_svc = sp_handle_svc,
737 };
738 
739 static TEE_Result sp_init_all(void)
740 {
741 	TEE_Result res = TEE_SUCCESS;
742 	const struct sp_image *sp = NULL;
743 	char __maybe_unused msg[60] = { '\0', };
744 
745 	for_each_secure_partition(sp) {
746 		if (sp->image.uncompressed_size)
747 			snprintf(msg, sizeof(msg),
748 				 " (compressed, uncompressed %u)",
749 				 sp->image.uncompressed_size);
750 		else
751 			msg[0] = '\0';
752 		DMSG("SP %pUl size %u%s", (void *)&sp->image.uuid,
753 		     sp->image.size, msg);
754 
755 		res = sp_init_uuid(&sp->image.uuid, sp->fdt);
756 
757 		if (res != TEE_SUCCESS) {
758 			EMSG("Failed initializing SP(%pUl) err:%#"PRIx32,
759 			     &sp->image.uuid, res);
760 			if (!IS_ENABLED(CFG_SP_SKIP_FAILED))
761 				panic();
762 		}
763 	}
764 
765 	return TEE_SUCCESS;
766 }
767 
768 boot_final(sp_init_all);
769 
770 static TEE_Result secure_partition_open(const TEE_UUID *uuid,
771 					struct ts_store_handle **h)
772 {
773 	return emb_ts_open(uuid, h, find_secure_partition);
774 }
775 
776 REGISTER_SP_STORE(2) = {
777 	.description = "SP store",
778 	.open = secure_partition_open,
779 	.get_size = emb_ts_get_size,
780 	.get_tag = emb_ts_get_tag,
781 	.read = emb_ts_read,
782 	.close = emb_ts_close,
783 };
784