1 /* 2 * Copyright (c) 2014, STMicroelectronics International N.V. 3 * All rights reserved. 4 * 5 * Redistribution and use in source and binary forms, with or without 6 * modification, are permitted provided that the following conditions are met: 7 * 8 * 1. Redistributions of source code must retain the above copyright notice, 9 * this list of conditions and the following disclaimer. 10 * 11 * 2. Redistributions in binary form must reproduce the above copyright notice, 12 * this list of conditions and the following disclaimer in the documentation 13 * and/or other materials provided with the distribution. 14 * 15 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 19 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 * POSSIBILITY OF SUCH DAMAGE. 26 */ 27 #include <platform_config.h> 28 #include <kernel/panic.h> 29 #include <kernel/thread.h> 30 #include <kernel/thread_defs.h> 31 #include "thread_private.h" 32 #include <sm/sm_defs.h> 33 #include <sm/sm.h> 34 #include <sm/teesmc.h> 35 #include <sm/teesmc_optee.h> 36 #include <arm.h> 37 #include <kernel/tz_proc_def.h> 38 #include <kernel/tz_proc.h> 39 #include <kernel/misc.h> 40 #include <mm/tee_mmu.h> 41 #include <mm/tee_mmu_defs.h> 42 #include <mm/tee_mm.h> 43 #include <mm/tee_pager.h> 44 #include <kernel/tee_ta_manager.h> 45 #include <util.h> 46 #include <trace.h> 47 #include <assert.h> 48 49 #ifdef ARM32 50 #define STACK_TMP_SIZE 1024 51 #define STACK_THREAD_SIZE 8192 52 53 #if TRACE_LEVEL > 0 54 #define STACK_ABT_SIZE 2048 55 #else 56 #define STACK_ABT_SIZE 1024 57 #endif 58 59 #endif /*ARM32*/ 60 61 #ifdef ARM64 62 #define STACK_TMP_SIZE 2048 63 #define STACK_THREAD_SIZE 8192 64 65 #if TRACE_LEVEL > 0 66 #define STACK_ABT_SIZE 3072 67 #else 68 #define STACK_ABT_SIZE 1024 69 #endif 70 #endif /*ARM64*/ 71 72 #define RPC_MAX_PARAMS 2 73 74 /* 75 * The big lock for threads. Since OP-TEE currently is single threaded 76 * all standard calls (non-fast calls) must take this mutex before starting 77 * to do any real work. 78 */ 79 static struct mutex thread_big_lock = MUTEX_INITIALIZER; 80 81 struct thread_ctx threads[CFG_NUM_THREADS]; 82 83 static struct thread_core_local thread_core_local[CFG_TEE_CORE_NB_CORE]; 84 85 #ifdef CFG_WITH_STACK_CANARIES 86 #ifdef ARM32 87 #define STACK_CANARY_SIZE (4 * sizeof(uint32_t)) 88 #endif 89 #ifdef ARM64 90 #define STACK_CANARY_SIZE (8 * sizeof(uint32_t)) 91 #endif 92 #define START_CANARY_VALUE 0xdededede 93 #define END_CANARY_VALUE 0xabababab 94 #define GET_START_CANARY(name, stack_num) name[stack_num][0] 95 #define GET_END_CANARY(name, stack_num) \ 96 name[stack_num][sizeof(name[stack_num]) / sizeof(uint32_t) - 1] 97 #else 98 #define STACK_CANARY_SIZE 0 99 #endif 100 101 #define DECLARE_STACK(name, num_stacks, stack_size) \ 102 static uint32_t name[num_stacks][ \ 103 ROUNDUP(stack_size + STACK_CANARY_SIZE, STACK_ALIGNMENT) / \ 104 sizeof(uint32_t)] \ 105 __attribute__((section(".nozi.stack"), \ 106 aligned(STACK_ALIGNMENT))) 107 108 #define GET_STACK(stack) \ 109 ((vaddr_t)(stack) + sizeof(stack) - STACK_CANARY_SIZE / 2) 110 111 DECLARE_STACK(stack_tmp, CFG_TEE_CORE_NB_CORE, STACK_TMP_SIZE); 112 DECLARE_STACK(stack_abt, CFG_TEE_CORE_NB_CORE, STACK_ABT_SIZE); 113 #if !defined(CFG_WITH_ARM_TRUSTED_FW) 114 DECLARE_STACK(stack_sm, CFG_TEE_CORE_NB_CORE, SM_STACK_SIZE); 115 #endif 116 #ifndef CFG_WITH_PAGER 117 DECLARE_STACK(stack_thread, CFG_NUM_THREADS, STACK_THREAD_SIZE); 118 #endif 119 120 const vaddr_t stack_tmp_top[CFG_TEE_CORE_NB_CORE] = { 121 GET_STACK(stack_tmp[0]), 122 #if CFG_TEE_CORE_NB_CORE > 1 123 GET_STACK(stack_tmp[1]), 124 #endif 125 #if CFG_TEE_CORE_NB_CORE > 2 126 GET_STACK(stack_tmp[2]), 127 #endif 128 #if CFG_TEE_CORE_NB_CORE > 3 129 GET_STACK(stack_tmp[3]), 130 #endif 131 #if CFG_TEE_CORE_NB_CORE > 4 132 GET_STACK(stack_tmp[4]), 133 #endif 134 #if CFG_TEE_CORE_NB_CORE > 5 135 GET_STACK(stack_tmp[5]), 136 #endif 137 #if CFG_TEE_CORE_NB_CORE > 6 138 GET_STACK(stack_tmp[6]), 139 #endif 140 #if CFG_TEE_CORE_NB_CORE > 7 141 GET_STACK(stack_tmp[7]), 142 #endif 143 #if CFG_TEE_CORE_NB_CORE > 8 144 #error "Top of tmp stacks aren't defined for more than 8 CPUS" 145 #endif 146 }; 147 148 thread_smc_handler_t thread_std_smc_handler_ptr; 149 static thread_smc_handler_t thread_fast_smc_handler_ptr; 150 thread_fiq_handler_t thread_fiq_handler_ptr; 151 thread_svc_handler_t thread_svc_handler_ptr; 152 static thread_abort_handler_t thread_abort_handler_ptr; 153 thread_pm_handler_t thread_cpu_on_handler_ptr; 154 thread_pm_handler_t thread_cpu_off_handler_ptr; 155 thread_pm_handler_t thread_cpu_suspend_handler_ptr; 156 thread_pm_handler_t thread_cpu_resume_handler_ptr; 157 thread_pm_handler_t thread_system_off_handler_ptr; 158 thread_pm_handler_t thread_system_reset_handler_ptr; 159 160 161 static unsigned int thread_global_lock = SPINLOCK_UNLOCK; 162 163 static void init_canaries(void) 164 { 165 #ifdef CFG_WITH_STACK_CANARIES 166 size_t n; 167 #define INIT_CANARY(name) \ 168 for (n = 0; n < ARRAY_SIZE(name); n++) { \ 169 uint32_t *start_canary = &GET_START_CANARY(name, n); \ 170 uint32_t *end_canary = &GET_END_CANARY(name, n); \ 171 \ 172 *start_canary = START_CANARY_VALUE; \ 173 *end_canary = END_CANARY_VALUE; \ 174 DMSG("#Stack canaries for %s[%zu] with top at %p\n", \ 175 #name, n, (void *)(end_canary - 1)); \ 176 DMSG("watch *%p\n", (void *)end_canary); \ 177 } 178 179 INIT_CANARY(stack_tmp); 180 INIT_CANARY(stack_abt); 181 #if !defined(CFG_WITH_ARM_TRUSTED_FW) 182 INIT_CANARY(stack_sm); 183 #endif 184 #ifndef CFG_WITH_PAGER 185 INIT_CANARY(stack_thread); 186 #endif 187 #endif/*CFG_WITH_STACK_CANARIES*/ 188 } 189 190 void thread_check_canaries(void) 191 { 192 #ifdef CFG_WITH_STACK_CANARIES 193 size_t n; 194 195 for (n = 0; n < ARRAY_SIZE(stack_tmp); n++) { 196 assert(GET_START_CANARY(stack_tmp, n) == START_CANARY_VALUE); 197 assert(GET_END_CANARY(stack_tmp, n) == END_CANARY_VALUE); 198 } 199 200 for (n = 0; n < ARRAY_SIZE(stack_abt); n++) { 201 assert(GET_START_CANARY(stack_abt, n) == START_CANARY_VALUE); 202 assert(GET_END_CANARY(stack_abt, n) == END_CANARY_VALUE); 203 } 204 #if !defined(CFG_WITH_ARM_TRUSTED_FW) 205 for (n = 0; n < ARRAY_SIZE(stack_sm); n++) { 206 assert(GET_START_CANARY(stack_sm, n) == START_CANARY_VALUE); 207 assert(GET_END_CANARY(stack_sm, n) == END_CANARY_VALUE); 208 } 209 #endif 210 #ifndef CFG_WITH_PAGER 211 for (n = 0; n < ARRAY_SIZE(stack_thread); n++) { 212 assert(GET_START_CANARY(stack_thread, n) == START_CANARY_VALUE); 213 assert(GET_END_CANARY(stack_thread, n) == END_CANARY_VALUE); 214 } 215 #endif 216 #endif/*CFG_WITH_STACK_CANARIES*/ 217 } 218 219 static void lock_global(void) 220 { 221 cpu_spin_lock(&thread_global_lock); 222 } 223 224 static void unlock_global(void) 225 { 226 cpu_spin_unlock(&thread_global_lock); 227 } 228 229 #ifdef ARM32 230 uint32_t thread_get_exceptions(void) 231 { 232 uint32_t cpsr = read_cpsr(); 233 234 return (cpsr >> CPSR_F_SHIFT) & THREAD_EXCP_ALL; 235 } 236 237 void thread_set_exceptions(uint32_t exceptions) 238 { 239 uint32_t cpsr = read_cpsr(); 240 241 cpsr &= ~(THREAD_EXCP_ALL << CPSR_F_SHIFT); 242 cpsr |= ((exceptions & THREAD_EXCP_ALL) << CPSR_F_SHIFT); 243 write_cpsr(cpsr); 244 } 245 #endif /*ARM32*/ 246 247 #ifdef ARM64 248 uint32_t thread_get_exceptions(void) 249 { 250 uint32_t daif = read_daif(); 251 252 return (daif >> DAIF_F_SHIFT) & THREAD_EXCP_ALL; 253 } 254 255 void thread_set_exceptions(uint32_t exceptions) 256 { 257 uint32_t daif = read_daif(); 258 259 daif &= ~(THREAD_EXCP_ALL << DAIF_F_SHIFT); 260 daif |= ((exceptions & THREAD_EXCP_ALL) << DAIF_F_SHIFT); 261 write_daif(daif); 262 } 263 #endif /*ARM64*/ 264 265 uint32_t thread_mask_exceptions(uint32_t exceptions) 266 { 267 uint32_t state = thread_get_exceptions(); 268 269 thread_set_exceptions(state | (exceptions & THREAD_EXCP_ALL)); 270 return state; 271 } 272 273 void thread_unmask_exceptions(uint32_t state) 274 { 275 thread_set_exceptions(state & THREAD_EXCP_ALL); 276 } 277 278 279 struct thread_core_local *thread_get_core_local(void) 280 { 281 uint32_t cpu_id = get_core_pos(); 282 283 /* 284 * IRQs must be disabled before playing with core_local since 285 * we otherwise may be rescheduled to a different core in the 286 * middle of this function. 287 */ 288 assert(thread_get_exceptions() & THREAD_EXCP_IRQ); 289 290 assert(cpu_id < CFG_TEE_CORE_NB_CORE); 291 return &thread_core_local[cpu_id]; 292 } 293 294 static void thread_lazy_save_ns_vfp(void) 295 { 296 #ifdef CFG_WITH_VFP 297 struct thread_ctx *thr = threads + thread_get_id(); 298 299 thr->vfp_state.ns_saved = false; 300 #if defined(ARM64) && defined(CFG_WITH_ARM_TRUSTED_FW) 301 /* 302 * ARM TF saves and restores CPACR_EL1, so we must assume NS world 303 * uses VFP and always preserve the register file when secure world 304 * is about to use it 305 */ 306 thr->vfp_state.ns.force_save = true; 307 #endif 308 vfp_lazy_save_state_init(&thr->vfp_state.ns); 309 #endif /*CFG_WITH_VFP*/ 310 } 311 312 static void thread_lazy_restore_ns_vfp(void) 313 { 314 #ifdef CFG_WITH_VFP 315 struct thread_ctx *thr = threads + thread_get_id(); 316 317 assert(!thr->vfp_state.sec_lazy_saved && !thr->vfp_state.sec_saved); 318 vfp_lazy_restore_state(&thr->vfp_state.ns, thr->vfp_state.ns_saved); 319 thr->vfp_state.ns_saved = false; 320 #endif /*CFG_WITH_VFP*/ 321 } 322 323 #ifdef ARM32 324 static void init_regs(struct thread_ctx *thread, 325 struct thread_smc_args *args) 326 { 327 thread->regs.pc = (uint32_t)thread_std_smc_entry; 328 329 /* 330 * Stdcalls starts in SVC mode with masked IRQ, masked Asynchronous 331 * abort and unmasked FIQ. 332 */ 333 thread->regs.cpsr = read_cpsr() & ARM32_CPSR_E; 334 thread->regs.cpsr |= CPSR_MODE_SVC | CPSR_I | CPSR_A; 335 /* Enable thumb mode if it's a thumb instruction */ 336 if (thread->regs.pc & 1) 337 thread->regs.cpsr |= CPSR_T; 338 /* Reinitialize stack pointer */ 339 thread->regs.svc_sp = thread->stack_va_end; 340 341 /* 342 * Copy arguments into context. This will make the 343 * arguments appear in r0-r7 when thread is started. 344 */ 345 thread->regs.r0 = args->a0; 346 thread->regs.r1 = args->a1; 347 thread->regs.r2 = args->a2; 348 thread->regs.r3 = args->a3; 349 thread->regs.r4 = args->a4; 350 thread->regs.r5 = args->a5; 351 thread->regs.r6 = args->a6; 352 thread->regs.r7 = args->a7; 353 } 354 #endif /*ARM32*/ 355 356 #ifdef ARM64 357 static void init_regs(struct thread_ctx *thread, 358 struct thread_smc_args *args) 359 { 360 thread->regs.pc = (uint64_t)thread_std_smc_entry; 361 362 /* 363 * Stdcalls starts in SVC mode with masked IRQ, masked Asynchronous 364 * abort and unmasked FIQ. 365 */ 366 thread->regs.cpsr = SPSR_64(SPSR_64_MODE_EL1, SPSR_64_MODE_SP_EL0, 367 DAIFBIT_IRQ | DAIFBIT_ABT); 368 /* Reinitialize stack pointer */ 369 thread->regs.sp = thread->stack_va_end; 370 371 /* 372 * Copy arguments into context. This will make the 373 * arguments appear in x0-x7 when thread is started. 374 */ 375 thread->regs.x[0] = args->a0; 376 thread->regs.x[1] = args->a1; 377 thread->regs.x[2] = args->a2; 378 thread->regs.x[3] = args->a3; 379 thread->regs.x[4] = args->a4; 380 thread->regs.x[5] = args->a5; 381 thread->regs.x[6] = args->a6; 382 thread->regs.x[7] = args->a7; 383 } 384 #endif /*ARM64*/ 385 386 void thread_init_boot_thread(void) 387 { 388 struct thread_core_local *l = thread_get_core_local(); 389 size_t n; 390 391 for (n = 0; n < CFG_NUM_THREADS; n++) 392 TAILQ_INIT(&threads[n].mutexes); 393 394 for (n = 0; n < CFG_TEE_CORE_NB_CORE; n++) 395 thread_core_local[n].curr_thread = -1; 396 397 l->curr_thread = 0; 398 threads[0].state = THREAD_STATE_ACTIVE; 399 } 400 401 void thread_clr_boot_thread(void) 402 { 403 struct thread_core_local *l = thread_get_core_local(); 404 405 assert(l->curr_thread >= 0 && l->curr_thread < CFG_NUM_THREADS); 406 assert(threads[l->curr_thread].state == THREAD_STATE_ACTIVE); 407 assert(TAILQ_EMPTY(&threads[l->curr_thread].mutexes)); 408 threads[l->curr_thread].state = THREAD_STATE_FREE; 409 l->curr_thread = -1; 410 } 411 412 static void thread_alloc_and_run(struct thread_smc_args *args) 413 { 414 size_t n; 415 struct thread_core_local *l = thread_get_core_local(); 416 bool found_thread = false; 417 418 assert(l->curr_thread == -1); 419 420 lock_global(); 421 422 for (n = 0; n < CFG_NUM_THREADS; n++) { 423 if (threads[n].state == THREAD_STATE_FREE) { 424 threads[n].state = THREAD_STATE_ACTIVE; 425 found_thread = true; 426 break; 427 } 428 } 429 430 unlock_global(); 431 432 if (!found_thread) { 433 args->a0 = TEESMC_RETURN_ETHREAD_LIMIT; 434 return; 435 } 436 437 l->curr_thread = n; 438 439 threads[n].flags = 0; 440 init_regs(threads + n, args); 441 442 /* Save Hypervisor Client ID */ 443 threads[n].hyp_clnt_id = args->a7; 444 445 thread_lazy_save_ns_vfp(); 446 thread_resume(&threads[n].regs); 447 } 448 449 #ifdef ARM32 450 static void copy_a0_to_a3(struct thread_ctx_regs *regs, 451 struct thread_smc_args *args) 452 { 453 /* 454 * Update returned values from RPC, values will appear in 455 * r0-r3 when thread is resumed. 456 */ 457 regs->r0 = args->a0; 458 regs->r1 = args->a1; 459 regs->r2 = args->a2; 460 regs->r3 = args->a3; 461 } 462 #endif /*ARM32*/ 463 464 #ifdef ARM64 465 static void copy_a0_to_a3(struct thread_ctx_regs *regs, 466 struct thread_smc_args *args) 467 { 468 /* 469 * Update returned values from RPC, values will appear in 470 * x0-x3 when thread is resumed. 471 */ 472 regs->x[0] = args->a0; 473 regs->x[1] = args->a1; 474 regs->x[2] = args->a2; 475 regs->x[3] = args->a3; 476 } 477 #endif /*ARM64*/ 478 479 static void thread_resume_from_rpc(struct thread_smc_args *args) 480 { 481 size_t n = args->a3; /* thread id */ 482 struct thread_core_local *l = thread_get_core_local(); 483 uint32_t rv = 0; 484 485 assert(l->curr_thread == -1); 486 487 lock_global(); 488 489 if (n < CFG_NUM_THREADS && 490 threads[n].state == THREAD_STATE_SUSPENDED && 491 args->a7 == threads[n].hyp_clnt_id) 492 threads[n].state = THREAD_STATE_ACTIVE; 493 else 494 rv = TEESMC_RETURN_ERESUME; 495 496 unlock_global(); 497 498 if (rv) { 499 args->a0 = rv; 500 return; 501 } 502 503 l->curr_thread = n; 504 505 if (threads[n].have_user_map) 506 core_mmu_set_user_map(&threads[n].user_map); 507 508 /* 509 * Return from RPC to request service of an IRQ must not 510 * get parameters from non-secure world. 511 */ 512 if (threads[n].flags & THREAD_FLAGS_COPY_ARGS_ON_RETURN) { 513 copy_a0_to_a3(&threads[n].regs, args); 514 threads[n].flags &= ~THREAD_FLAGS_COPY_ARGS_ON_RETURN; 515 } 516 517 thread_lazy_save_ns_vfp(); 518 thread_resume(&threads[n].regs); 519 } 520 521 void thread_handle_fast_smc(struct thread_smc_args *args) 522 { 523 thread_check_canaries(); 524 thread_fast_smc_handler_ptr(args); 525 /* Fast handlers must not unmask any exceptions */ 526 assert(thread_get_exceptions() == THREAD_EXCP_ALL); 527 } 528 529 void thread_handle_std_smc(struct thread_smc_args *args) 530 { 531 thread_check_canaries(); 532 533 if (args->a0 == TEESMC32_CALL_RETURN_FROM_RPC) 534 thread_resume_from_rpc(args); 535 else 536 thread_alloc_and_run(args); 537 } 538 539 /* Helper routine for the assembly function thread_std_smc_entry() */ 540 void __thread_std_smc_entry(struct thread_smc_args *args) 541 { 542 struct thread_ctx *thr = threads + thread_get_id(); 543 544 if (!thr->rpc_arg) { 545 paddr_t parg; 546 void *arg; 547 548 parg = thread_rpc_alloc_arg( 549 TEESMC32_GET_ARG_SIZE(RPC_MAX_PARAMS)); 550 if (!parg || !TEE_ALIGNMENT_IS_OK(parg, struct teesmc32_arg) || 551 core_pa2va(parg, &arg)) { 552 thread_rpc_free_arg(parg); 553 args->a0 = TEESMC_RETURN_ENOMEM; 554 return; 555 } 556 557 thr->rpc_arg = arg; 558 thr->rpc_parg = parg; 559 } 560 561 /* 562 * Take big lock before entering the callback registered in 563 * thread_std_smc_handler_ptr as the callback can reside in the 564 * paged area and the pager can only serve one core at a time. 565 */ 566 thread_take_big_lock(); 567 thread_std_smc_handler_ptr(args); 568 thread_release_big_lock(); 569 } 570 571 void thread_handle_abort(uint32_t abort_type, struct thread_abort_regs *regs) 572 { 573 #ifdef CFG_WITH_VFP 574 struct thread_ctx *thr = threads + thread_get_id(); 575 576 if (vfp_is_enabled()) { 577 vfp_lazy_save_state_init(&thr->vfp_state.sec); 578 thr->vfp_state.sec_lazy_saved = true; 579 } 580 #endif 581 582 thread_abort_handler_ptr(abort_type, regs); 583 584 #ifdef CFG_WITH_VFP 585 assert(!vfp_is_enabled()); 586 if (thr->vfp_state.sec_lazy_saved) { 587 vfp_lazy_restore_state(&thr->vfp_state.sec, 588 thr->vfp_state.sec_saved); 589 thr->vfp_state.sec_saved = false; 590 thr->vfp_state.sec_lazy_saved = false; 591 } 592 #endif 593 } 594 595 void *thread_get_tmp_sp(void) 596 { 597 struct thread_core_local *l = thread_get_core_local(); 598 599 return (void *)l->tmp_stack_va_end; 600 } 601 602 #ifdef ARM64 603 vaddr_t thread_get_saved_thread_sp(void) 604 { 605 struct thread_core_local *l = thread_get_core_local(); 606 int ct = l->curr_thread; 607 608 assert(ct != -1); 609 return threads[ct].kern_sp; 610 } 611 #endif /*ARM64*/ 612 613 void thread_state_free(void) 614 { 615 struct thread_core_local *l = thread_get_core_local(); 616 int ct = l->curr_thread; 617 618 assert(ct != -1); 619 assert(TAILQ_EMPTY(&threads[ct].mutexes)); 620 621 thread_lazy_restore_ns_vfp(); 622 623 lock_global(); 624 625 assert(threads[ct].state == THREAD_STATE_ACTIVE); 626 threads[ct].state = THREAD_STATE_FREE; 627 threads[ct].flags = 0; 628 l->curr_thread = -1; 629 630 unlock_global(); 631 } 632 633 int thread_state_suspend(uint32_t flags, uint32_t cpsr, vaddr_t pc) 634 { 635 struct thread_core_local *l = thread_get_core_local(); 636 int ct = l->curr_thread; 637 638 assert(ct != -1); 639 640 thread_check_canaries(); 641 642 thread_lazy_restore_ns_vfp(); 643 644 lock_global(); 645 646 assert(threads[ct].state == THREAD_STATE_ACTIVE); 647 threads[ct].flags |= flags; 648 threads[ct].regs.cpsr = cpsr; 649 threads[ct].regs.pc = pc; 650 threads[ct].state = THREAD_STATE_SUSPENDED; 651 652 threads[ct].have_user_map = core_mmu_user_mapping_is_active(); 653 if (threads[ct].have_user_map) { 654 core_mmu_get_user_map(&threads[ct].user_map); 655 core_mmu_set_user_map(NULL); 656 } 657 658 659 l->curr_thread = -1; 660 661 unlock_global(); 662 663 return ct; 664 } 665 666 #ifdef ARM32 667 static void set_tmp_stack(struct thread_core_local *l, vaddr_t sp) 668 { 669 l->tmp_stack_va_end = sp; 670 thread_set_irq_sp(sp); 671 thread_set_fiq_sp(sp); 672 } 673 674 static void set_abt_stack(struct thread_core_local *l __unused, vaddr_t sp) 675 { 676 thread_set_abt_sp(sp); 677 } 678 #endif /*ARM32*/ 679 680 #ifdef ARM64 681 static void set_tmp_stack(struct thread_core_local *l, vaddr_t sp) 682 { 683 /* 684 * We're already using the tmp stack when this function is called 685 * so there's no need to assign it to any stack pointer. However, 686 * we'll need to restore it at different times so store it here. 687 */ 688 l->tmp_stack_va_end = sp; 689 } 690 691 static void set_abt_stack(struct thread_core_local *l, vaddr_t sp) 692 { 693 l->abt_stack_va_end = sp; 694 } 695 #endif /*ARM64*/ 696 697 bool thread_init_stack(uint32_t thread_id, vaddr_t sp) 698 { 699 if (thread_id >= CFG_NUM_THREADS) 700 return false; 701 threads[thread_id].stack_va_end = sp; 702 return true; 703 } 704 705 int thread_get_id(void) 706 { 707 /* thread_get_core_local() requires IRQs to be disabled */ 708 uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ); 709 struct thread_core_local *l; 710 int ct; 711 712 l = thread_get_core_local(); 713 ct = l->curr_thread; 714 assert((ct >= 0) && (ct < CFG_NUM_THREADS)); 715 716 thread_unmask_exceptions(exceptions); 717 return ct; 718 } 719 720 static void init_handlers(const struct thread_handlers *handlers) 721 { 722 thread_std_smc_handler_ptr = handlers->std_smc; 723 thread_fast_smc_handler_ptr = handlers->fast_smc; 724 thread_fiq_handler_ptr = handlers->fiq; 725 thread_svc_handler_ptr = handlers->svc; 726 thread_abort_handler_ptr = handlers->abort; 727 thread_cpu_on_handler_ptr = handlers->cpu_on; 728 thread_cpu_off_handler_ptr = handlers->cpu_off; 729 thread_cpu_suspend_handler_ptr = handlers->cpu_suspend; 730 thread_cpu_resume_handler_ptr = handlers->cpu_resume; 731 thread_system_off_handler_ptr = handlers->system_off; 732 thread_system_reset_handler_ptr = handlers->system_reset; 733 } 734 735 736 #ifdef CFG_WITH_PAGER 737 static void init_thread_stacks(void) 738 { 739 size_t n; 740 741 /* 742 * Allocate virtual memory for thread stacks. 743 */ 744 for (n = 0; n < CFG_NUM_THREADS; n++) { 745 tee_mm_entry_t *mm; 746 vaddr_t sp; 747 748 /* Find vmem for thread stack and its protection gap */ 749 mm = tee_mm_alloc(&tee_mm_vcore, 750 SMALL_PAGE_SIZE + STACK_THREAD_SIZE); 751 TEE_ASSERT(mm); 752 753 /* Claim eventual physical page */ 754 tee_pager_add_pages(tee_mm_get_smem(mm), tee_mm_get_size(mm), 755 true); 756 757 /* Realloc both protection vmem and stack vmem separately */ 758 sp = tee_mm_get_smem(mm); 759 tee_mm_free(mm); 760 mm = tee_mm_alloc2(&tee_mm_vcore, sp, SMALL_PAGE_SIZE); 761 TEE_ASSERT(mm); 762 mm = tee_mm_alloc2(&tee_mm_vcore, sp + SMALL_PAGE_SIZE, 763 STACK_THREAD_SIZE); 764 TEE_ASSERT(mm); 765 766 /* init effective stack */ 767 sp = tee_mm_get_smem(mm) + tee_mm_get_bytes(mm); 768 if (!thread_init_stack(n, sp)) 769 panic(); 770 771 /* Add the area to the pager */ 772 tee_pager_add_area(mm, TEE_PAGER_AREA_RW, NULL, NULL); 773 } 774 } 775 #else 776 static void init_thread_stacks(void) 777 { 778 size_t n; 779 780 /* Assign the thread stacks */ 781 for (n = 0; n < CFG_NUM_THREADS; n++) { 782 if (!thread_init_stack(n, GET_STACK(stack_thread[n]))) 783 panic(); 784 } 785 } 786 #endif /*CFG_WITH_PAGER*/ 787 788 void thread_init_primary(const struct thread_handlers *handlers) 789 { 790 /* 791 * The COMPILE_TIME_ASSERT only works in function context. These 792 * checks verifies that the offsets used in assembly code matches 793 * what's used in C code. 794 */ 795 #ifdef ARM32 796 COMPILE_TIME_ASSERT(offsetof(struct thread_svc_regs, r0) == 797 THREAD_SVC_REG_R0_OFFS); 798 COMPILE_TIME_ASSERT(offsetof(struct thread_svc_regs, r1) == 799 THREAD_SVC_REG_R1_OFFS); 800 COMPILE_TIME_ASSERT(offsetof(struct thread_svc_regs, r2) == 801 THREAD_SVC_REG_R2_OFFS); 802 COMPILE_TIME_ASSERT(offsetof(struct thread_svc_regs, r3) == 803 THREAD_SVC_REG_R3_OFFS); 804 COMPILE_TIME_ASSERT(offsetof(struct thread_svc_regs, r4) == 805 THREAD_SVC_REG_R4_OFFS); 806 COMPILE_TIME_ASSERT(offsetof(struct thread_svc_regs, r5) == 807 THREAD_SVC_REG_R5_OFFS); 808 COMPILE_TIME_ASSERT(offsetof(struct thread_svc_regs, r6) == 809 THREAD_SVC_REG_R6_OFFS); 810 COMPILE_TIME_ASSERT(offsetof(struct thread_svc_regs, r7) == 811 THREAD_SVC_REG_R7_OFFS); 812 COMPILE_TIME_ASSERT(offsetof(struct thread_svc_regs, lr) == 813 THREAD_SVC_REG_LR_OFFS); 814 COMPILE_TIME_ASSERT(offsetof(struct thread_svc_regs, spsr) == 815 THREAD_SVC_REG_SPSR_OFFS); 816 #endif /*ARM32*/ 817 #ifdef ARM64 818 /* struct thread_abort_regs */ 819 COMPILE_TIME_ASSERT(offsetof(struct thread_abort_regs, x22) == 820 THREAD_ABT_REG_X_OFFS(22)); 821 COMPILE_TIME_ASSERT(offsetof(struct thread_abort_regs, elr) == 822 THREAD_ABT_REG_ELR_OFFS); 823 COMPILE_TIME_ASSERT(offsetof(struct thread_abort_regs, spsr) == 824 THREAD_ABT_REG_SPSR_OFFS); 825 COMPILE_TIME_ASSERT(offsetof(struct thread_abort_regs, sp_el0) == 826 THREAD_ABT_REG_SP_EL0_OFFS); 827 COMPILE_TIME_ASSERT(sizeof(struct thread_abort_regs) == 828 THREAD_ABT_REGS_SIZE); 829 830 /* struct thread_ctx */ 831 COMPILE_TIME_ASSERT(offsetof(struct thread_ctx, kern_sp) == 832 THREAD_CTX_KERN_SP_OFFSET); 833 COMPILE_TIME_ASSERT(sizeof(struct thread_ctx) == THREAD_CTX_SIZE); 834 835 /* struct thread_ctx_regs */ 836 COMPILE_TIME_ASSERT(offsetof(struct thread_ctx_regs, sp) == 837 THREAD_CTX_REGS_SP_OFFSET); 838 COMPILE_TIME_ASSERT(offsetof(struct thread_ctx_regs, pc) == 839 THREAD_CTX_REGS_PC_OFFSET); 840 COMPILE_TIME_ASSERT(offsetof(struct thread_ctx_regs, cpsr) == 841 THREAD_CTX_REGS_SPSR_OFFSET); 842 COMPILE_TIME_ASSERT(offsetof(struct thread_ctx_regs, x[23]) == 843 THREAD_CTX_REGS_X_OFFSET(23)); 844 COMPILE_TIME_ASSERT(sizeof(struct thread_ctx_regs) == 845 THREAD_CTX_REGS_SIZE); 846 847 /* struct thread_user_mode_rec */ 848 COMPILE_TIME_ASSERT( 849 offsetof(struct thread_user_mode_rec, exit_status0_ptr) == 850 THREAD_USER_MODE_REC_EXIT_STATUS0_PTR_OFFSET); 851 COMPILE_TIME_ASSERT( 852 offsetof(struct thread_user_mode_rec, exit_status1_ptr) == 853 THREAD_USER_MODE_REC_EXIT_STATUS1_PTR_OFFSET); 854 COMPILE_TIME_ASSERT( 855 offsetof(struct thread_user_mode_rec, x[1]) == 856 THREAD_USER_MODE_REC_X_OFFSET(20)); 857 COMPILE_TIME_ASSERT(sizeof(struct thread_user_mode_rec) == 858 THREAD_USER_MODE_REC_SIZE); 859 860 /* struct thread_core_local */ 861 COMPILE_TIME_ASSERT( 862 offsetof(struct thread_core_local, tmp_stack_va_end) == 863 THREAD_CORE_LOCAL_TMP_STACK_VA_END_OFFSET); 864 COMPILE_TIME_ASSERT( 865 offsetof(struct thread_core_local, curr_thread) == 866 THREAD_CORE_LOCAL_CURR_THREAD_OFFSET); 867 COMPILE_TIME_ASSERT( 868 offsetof(struct thread_core_local, flags) == 869 THREAD_CORE_LOCAL_FLAGS_OFFSET); 870 COMPILE_TIME_ASSERT( 871 offsetof(struct thread_core_local, abt_stack_va_end) == 872 THREAD_CORE_LOCAL_ABT_STACK_VA_END_OFFSET); 873 COMPILE_TIME_ASSERT( 874 offsetof(struct thread_core_local, x[3]) == 875 THREAD_CORE_LOCAL_X_OFFSET(3)); 876 COMPILE_TIME_ASSERT(sizeof(struct thread_core_local) == 877 THREAD_CORE_LOCAL_SIZE); 878 879 #endif /*ARM64*/ 880 881 init_handlers(handlers); 882 883 /* Initialize canaries around the stacks */ 884 init_canaries(); 885 886 init_thread_stacks(); 887 } 888 889 static void init_sec_mon(size_t __unused pos) 890 { 891 #if !defined(CFG_WITH_ARM_TRUSTED_FW) 892 /* Initialize secure monitor */ 893 sm_init(GET_STACK(stack_sm[pos])); 894 sm_set_entry_vector(thread_vector_table); 895 #endif 896 } 897 898 void thread_init_per_cpu(void) 899 { 900 size_t pos = get_core_pos(); 901 struct thread_core_local *l = thread_get_core_local(); 902 903 init_sec_mon(pos); 904 905 set_tmp_stack(l, GET_STACK(stack_tmp[pos])); 906 set_abt_stack(l, GET_STACK(stack_abt[pos])); 907 908 thread_init_vbar(); 909 } 910 911 void thread_set_tsd(void *tsd) 912 { 913 /* thread_get_core_local() requires IRQs to be disabled */ 914 uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ); 915 struct thread_core_local *l; 916 int ct; 917 918 l = thread_get_core_local(); 919 ct = l->curr_thread; 920 921 assert(ct != -1); 922 assert(threads[ct].state == THREAD_STATE_ACTIVE); 923 threads[ct].tsd = tsd; 924 925 thread_unmask_exceptions(exceptions); 926 } 927 928 void *thread_get_tsd(void) 929 { 930 /* thread_get_core_local() requires IRQs to be disabled */ 931 uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ); 932 struct thread_core_local *l; 933 int ct; 934 void *tsd; 935 936 l = thread_get_core_local(); 937 ct = l->curr_thread; 938 939 if (ct == -1 || threads[ct].state != THREAD_STATE_ACTIVE) 940 tsd = NULL; 941 else 942 tsd = threads[ct].tsd; 943 944 thread_unmask_exceptions(exceptions); 945 return tsd; 946 } 947 948 struct thread_ctx_regs *thread_get_ctx_regs(void) 949 { 950 struct thread_core_local *l = thread_get_core_local(); 951 952 assert(l->curr_thread != -1); 953 return &threads[l->curr_thread].regs; 954 } 955 956 void thread_set_irq(bool enable) 957 { 958 /* thread_get_core_local() requires IRQs to be disabled */ 959 uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ); 960 struct thread_core_local *l; 961 962 l = thread_get_core_local(); 963 964 assert(l->curr_thread != -1); 965 966 if (enable) { 967 threads[l->curr_thread].flags |= THREAD_FLAGS_IRQ_ENABLE; 968 thread_set_exceptions(exceptions & ~THREAD_EXCP_IRQ); 969 } else { 970 /* 971 * No need to disable IRQ here since it's already disabled 972 * above. 973 */ 974 threads[l->curr_thread].flags &= ~THREAD_FLAGS_IRQ_ENABLE; 975 } 976 } 977 978 void thread_restore_irq(void) 979 { 980 /* thread_get_core_local() requires IRQs to be disabled */ 981 uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ); 982 struct thread_core_local *l; 983 984 l = thread_get_core_local(); 985 986 assert(l->curr_thread != -1); 987 988 if (threads[l->curr_thread].flags & THREAD_FLAGS_IRQ_ENABLE) 989 thread_set_exceptions(exceptions & ~THREAD_EXCP_IRQ); 990 } 991 992 #ifdef CFG_WITH_VFP 993 uint32_t thread_kernel_enable_vfp(void) 994 { 995 uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ); 996 struct thread_ctx *thr = threads + thread_get_id(); 997 998 assert(!vfp_is_enabled()); 999 1000 if (!thr->vfp_state.ns_saved) { 1001 vfp_lazy_save_state_final(&thr->vfp_state.ns); 1002 thr->vfp_state.ns_saved = true; 1003 } else if (thr->vfp_state.sec_lazy_saved && 1004 !thr->vfp_state.sec_saved) { 1005 /* 1006 * This happens when we're handling an abort while the 1007 * thread was using the VFP state. 1008 */ 1009 vfp_lazy_save_state_final(&thr->vfp_state.sec); 1010 thr->vfp_state.sec_saved = true; 1011 } 1012 1013 vfp_enable(); 1014 return exceptions; 1015 } 1016 1017 void thread_kernel_disable_vfp(uint32_t state) 1018 { 1019 uint32_t exceptions; 1020 1021 assert(vfp_is_enabled()); 1022 1023 vfp_disable(); 1024 exceptions = thread_get_exceptions(); 1025 assert(exceptions & THREAD_EXCP_IRQ); 1026 exceptions &= ~THREAD_EXCP_IRQ; 1027 exceptions |= state & THREAD_EXCP_IRQ; 1028 thread_set_exceptions(exceptions); 1029 } 1030 #endif /*CFG_WITH_VFP*/ 1031 1032 void thread_add_mutex(struct mutex *m) 1033 { 1034 struct thread_core_local *l = thread_get_core_local(); 1035 int ct = l->curr_thread; 1036 1037 assert(ct != -1 && threads[ct].state == THREAD_STATE_ACTIVE); 1038 assert(m->owner_id == -1); 1039 m->owner_id = ct; 1040 TAILQ_INSERT_TAIL(&threads[ct].mutexes, m, link); 1041 } 1042 1043 void thread_rem_mutex(struct mutex *m) 1044 { 1045 struct thread_core_local *l = thread_get_core_local(); 1046 int ct = l->curr_thread; 1047 1048 assert(ct != -1 && threads[ct].state == THREAD_STATE_ACTIVE); 1049 assert(m->owner_id == ct); 1050 m->owner_id = -1; 1051 TAILQ_REMOVE(&threads[ct].mutexes, m, link); 1052 } 1053 1054 static bool may_unlock_big_lock(void) 1055 { 1056 uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ); 1057 struct thread_core_local *l = thread_get_core_local(); 1058 int ct = l->curr_thread; 1059 struct mutex *m; 1060 bool have_bl = false; 1061 bool have_other = false; 1062 1063 TAILQ_FOREACH(m, &threads[ct].mutexes, link) { 1064 if (m == &thread_big_lock) 1065 have_bl = true; 1066 else 1067 have_other = true; 1068 } 1069 1070 thread_unmask_exceptions(exceptions); 1071 return have_bl && !have_other; 1072 } 1073 1074 void thread_take_big_lock(void) 1075 { 1076 mutex_lock(&thread_big_lock); 1077 } 1078 1079 void thread_release_big_lock(void) 1080 { 1081 assert(may_unlock_big_lock()); 1082 mutex_unlock(&thread_big_lock); 1083 } 1084 1085 paddr_t thread_rpc_alloc_arg(size_t size) 1086 { 1087 uint32_t rpc_args[THREAD_RPC_NUM_ARGS] = { 1088 TEESMC_RETURN_RPC_ALLOC_ARG, size}; 1089 1090 thread_rpc(rpc_args); 1091 return rpc_args[1]; 1092 } 1093 1094 paddr_t thread_rpc_alloc_payload(size_t size) 1095 { 1096 uint32_t rpc_args[THREAD_RPC_NUM_ARGS] = { 1097 TEESMC_RETURN_RPC_ALLOC_PAYLOAD, size}; 1098 1099 thread_rpc(rpc_args); 1100 return rpc_args[1]; 1101 } 1102 1103 void thread_rpc_free_arg(paddr_t arg) 1104 { 1105 if (arg) { 1106 uint32_t rpc_args[THREAD_RPC_NUM_ARGS] = { 1107 TEESMC_RETURN_RPC_FREE_ARG, arg}; 1108 1109 thread_rpc(rpc_args); 1110 } 1111 } 1112 void thread_rpc_free_payload(paddr_t payload) 1113 { 1114 if (payload) { 1115 uint32_t rpc_args[THREAD_RPC_NUM_ARGS] = { 1116 TEESMC_RETURN_RPC_FREE_PAYLOAD, payload}; 1117 1118 thread_rpc(rpc_args); 1119 } 1120 } 1121 1122 static uint32_t rpc_cmd_nolock(uint32_t cmd, size_t num_params, 1123 struct teesmc32_param *params) 1124 { 1125 uint32_t rpc_args[THREAD_RPC_NUM_ARGS] = { 0 }; 1126 struct thread_ctx *thr = threads + thread_get_id(); 1127 struct teesmc32_arg *arg = thr->rpc_arg; 1128 paddr_t parg = thr->rpc_parg; 1129 const size_t params_size = sizeof(struct teesmc32_param) * num_params; 1130 size_t n; 1131 1132 TEE_ASSERT(arg && parg && num_params <= RPC_MAX_PARAMS); 1133 1134 memset(arg, 0, TEESMC32_GET_ARG_SIZE(RPC_MAX_PARAMS)); 1135 arg->cmd = cmd; 1136 arg->ret = TEE_ERROR_GENERIC; /* in case value isn't updated */ 1137 arg->num_params = num_params; 1138 memcpy(TEESMC32_GET_PARAMS(arg), params, params_size); 1139 1140 rpc_args[0] = TEESMC_RETURN_RPC_CMD; 1141 rpc_args[1] = parg; 1142 thread_rpc(rpc_args); 1143 1144 for (n = 0; n < num_params; n++) { 1145 switch (params[n].attr & TEESMC_ATTR_TYPE_MASK) { 1146 case TEESMC_ATTR_TYPE_VALUE_OUTPUT: 1147 case TEESMC_ATTR_TYPE_VALUE_INOUT: 1148 case TEESMC_ATTR_TYPE_MEMREF_OUTPUT: 1149 case TEESMC_ATTR_TYPE_MEMREF_INOUT: 1150 memcpy(params + n, TEESMC32_GET_PARAMS(arg) + n, 1151 sizeof(struct teesmc32_param)); 1152 break; 1153 default: 1154 break; 1155 } 1156 } 1157 1158 return arg->ret; 1159 } 1160 1161 uint32_t thread_rpc_cmd(uint32_t cmd, size_t num_params, 1162 struct teesmc32_param *params) 1163 { 1164 bool unlock_big_lock = may_unlock_big_lock(); 1165 uint32_t ret; 1166 1167 /* 1168 * If current thread doesn't hold any other mutexes: 1169 * Let other threads get the big lock to do some work while this 1170 * thread is doing some potentially slow RPC in normal world. 1171 */ 1172 if (unlock_big_lock) 1173 mutex_unlock(&thread_big_lock); 1174 1175 ret = rpc_cmd_nolock(cmd, num_params, params); 1176 1177 if (unlock_big_lock) 1178 mutex_lock(&thread_big_lock); 1179 1180 return ret; 1181 } 1182 1183 void thread_optee_rpc_alloc_payload(size_t size, paddr_t *payload, 1184 paddr_t *cookie) 1185 { 1186 uint32_t rpc_args[THREAD_RPC_NUM_ARGS] = { 1187 TEESMC_RETURN_OPTEE_RPC_ALLOC_PAYLOAD, size}; 1188 1189 thread_rpc(rpc_args); 1190 if (payload) 1191 *payload = rpc_args[1]; 1192 if (cookie) 1193 *cookie = rpc_args[2]; 1194 } 1195 1196 void thread_optee_rpc_free_payload(paddr_t cookie) 1197 { 1198 uint32_t rpc_args[THREAD_RPC_NUM_ARGS] ={ 1199 TEESMC_RETURN_OPTEE_RPC_FREE_PAYLOAD, cookie}; 1200 1201 thread_rpc(rpc_args); 1202 } 1203