| /* SPDX-License-Identifier: BSD-2-Clause */ |
| /* |
| * Copyright (c) 2019, Linaro Limited |
| */ |
| |
| #include <arm.h> |
| #include <asm.S> |
| #include <generated/asm-defines.h> |
| #include <keep.h> |
| #include <kernel/thread_defs.h> |
| #include <kernel/unwind.h> |
| #include <sm/optee_smc.h> |
| #include <sm/teesmc_opteed.h> |
| #include <sm/teesmc_opteed_macros.h> |
| |
| .arch_extension sec |
| |
| /* |
| * If ASLR is configured the identity mapped code may be mapped at two |
| * locations, the identity location where virtual and physical address is |
| * the same and at the runtime selected location to which OP-TEE has been |
| * relocated. Code executing at a location different compared to the |
| * runtime selected location works OK as long as it doesn't do relative |
| * addressing outside the identity mapped range. To allow relative |
| * addressing this macro jumps to the runtime selected location. |
| * |
| * Note that the identity mapped range and the runtime selected range can |
| * only differ if ASLR is configured. |
| */ |
| .macro readjust_pc |
| #ifdef CFG_CORE_ASLR |
| ldr r12, =1111f |
| bx r12 |
| 1111: |
| #endif |
| .endm |
| |
| LOCAL_FUNC vector_std_smc_entry , : , .identity_map |
| UNWIND( .fnstart) |
| UNWIND( .cantunwind) |
| readjust_pc |
| push {r4-r7} |
| bl thread_handle_std_smc |
| add sp, sp, #(4 * 4) |
| /* |
| * Normally thread_handle_std_smc() should return via |
| * thread_exit(), thread_rpc(), but if thread_handle_std_smc() |
| * hasn't switched stack (error detected) it will do a normal "C" |
| * return. |
| */ |
| mov r1, r0 |
| ldr r0, =TEESMC_OPTEED_RETURN_CALL_DONE |
| smc #0 |
| b . /* SMC should not return */ |
| UNWIND( .fnend) |
| END_FUNC vector_std_smc_entry |
| |
| LOCAL_FUNC vector_fast_smc_entry , : , .identity_map |
| UNWIND( .fnstart) |
| UNWIND( .cantunwind) |
| readjust_pc |
| push {r0-r7} |
| mov r0, sp |
| bl thread_handle_fast_smc |
| pop {r1-r8} |
| ldr r0, =TEESMC_OPTEED_RETURN_CALL_DONE |
| smc #0 |
| b . /* SMC should not return */ |
| UNWIND( .fnend) |
| END_FUNC vector_fast_smc_entry |
| |
| LOCAL_FUNC vector_fiq_entry , : , .identity_map |
| UNWIND( .fnstart) |
| UNWIND( .cantunwind) |
| readjust_pc |
| /* Secure Monitor received a FIQ and passed control to us. */ |
| bl thread_check_canaries |
| bl itr_core_handler |
| ldr r0, =TEESMC_OPTEED_RETURN_FIQ_DONE |
| smc #0 |
| b . /* SMC should not return */ |
| UNWIND( .fnend) |
| END_FUNC vector_fiq_entry |
| |
| LOCAL_FUNC vector_cpu_on_entry , : , .identity_map |
| UNWIND( .fnstart) |
| UNWIND( .cantunwind) |
| /* |
| * We're executing from physical address here while the binary may |
| * have been relocated to a different virtual address. In order to |
| * still read the expected physical memory we must use purely |
| * relative addressing. |
| */ |
| adr r0, .cpu_on_handler_ptr_rel |
| ldr r1, .cpu_on_handler_ptr_rel |
| ldr lr, [r0, r1] |
| adr r2, .boot_mmu_config_rel |
| ldr r3, .boot_mmu_config_rel |
| add r12, r2, r3 |
| ldr r12, [r12, #CORE_MMU_CONFIG_LOAD_OFFSET] |
| sub lr, lr, r12 |
| blx lr |
| /* When cpu_on_handler() returns mmu is enabled */ |
| mov r1, r0 |
| ldr r0, =TEESMC_OPTEED_RETURN_ON_DONE |
| smc #0 |
| b . /* SMC should not return */ |
| .cpu_on_handler_ptr_rel: |
| .word thread_cpu_on_handler_ptr - .cpu_on_handler_ptr_rel |
| .boot_mmu_config_rel: |
| .word boot_mmu_config - .boot_mmu_config_rel |
| UNWIND( .fnend) |
| END_FUNC vector_cpu_on_entry |
| |
| LOCAL_FUNC vector_cpu_off_entry , : , .identity_map |
| UNWIND( .fnstart) |
| UNWIND( .cantunwind) |
| readjust_pc |
| ldr lr, =thread_cpu_off_handler_ptr |
| ldr lr, [lr] |
| blx lr |
| mov r1, r0 |
| ldr r0, =TEESMC_OPTEED_RETURN_OFF_DONE |
| smc #0 |
| b . /* SMC should not return */ |
| UNWIND( .fnend) |
| END_FUNC vector_cpu_off_entry |
| |
| LOCAL_FUNC vector_cpu_suspend_entry , : , .identity_map |
| UNWIND( .fnstart) |
| UNWIND( .cantunwind) |
| readjust_pc |
| ldr lr, =thread_cpu_suspend_handler_ptr |
| ldr lr, [lr] |
| blx lr |
| mov r1, r0 |
| ldr r0, =TEESMC_OPTEED_RETURN_SUSPEND_DONE |
| smc #0 |
| b . /* SMC should not return */ |
| UNWIND( .fnend) |
| END_FUNC vector_cpu_suspend_entry |
| |
| LOCAL_FUNC vector_cpu_resume_entry , : , .identity_map |
| UNWIND( .fnstart) |
| UNWIND( .cantunwind) |
| readjust_pc |
| ldr lr, =thread_cpu_resume_handler_ptr |
| ldr lr, [lr] |
| blx lr |
| mov r1, r0 |
| ldr r0, =TEESMC_OPTEED_RETURN_RESUME_DONE |
| smc #0 |
| b . /* SMC should not return */ |
| UNWIND( .fnend) |
| END_FUNC vector_cpu_resume_entry |
| |
| LOCAL_FUNC vector_system_off_entry , : , .identity_map |
| UNWIND( .fnstart) |
| UNWIND( .cantunwind) |
| readjust_pc |
| ldr lr, =thread_system_off_handler_ptr |
| ldr lr, [lr] |
| blx lr |
| mov r1, r0 |
| ldr r0, =TEESMC_OPTEED_RETURN_SYSTEM_OFF_DONE |
| smc #0 |
| b . /* SMC should not return */ |
| UNWIND( .fnend) |
| END_FUNC vector_system_off_entry |
| |
| LOCAL_FUNC vector_system_reset_entry , : , .identity_map |
| UNWIND( .fnstart) |
| UNWIND( .cantunwind) |
| readjust_pc |
| ldr lr, =thread_system_reset_handler_ptr |
| ldr lr, [lr] |
| blx lr |
| mov r1, r0 |
| ldr r0, =TEESMC_OPTEED_RETURN_SYSTEM_RESET_DONE |
| smc #0 |
| b . /* SMC should not return */ |
| UNWIND( .fnend) |
| END_FUNC vector_system_reset_entry |
| |
| /* |
| * Vector table supplied to ARM Trusted Firmware (ARM-TF) at |
| * initialization. Also used when compiled with the internal monitor, but |
| * the cpu_*_entry and system_*_entry are not used then. |
| * |
| * Note that ARM-TF depends on the layout of this vector table, any change |
| * in layout has to be synced with ARM-TF. |
| */ |
| FUNC thread_vector_table , : , .identity_map |
| UNWIND( .fnstart) |
| UNWIND( .cantunwind) |
| b vector_std_smc_entry |
| b vector_fast_smc_entry |
| b vector_cpu_on_entry |
| b vector_cpu_off_entry |
| b vector_cpu_resume_entry |
| b vector_cpu_suspend_entry |
| b vector_fiq_entry |
| b vector_system_off_entry |
| b vector_system_reset_entry |
| UNWIND( .fnend) |
| END_FUNC thread_vector_table |
| KEEP_PAGER thread_vector_table |
| |
| FUNC thread_std_smc_entry , : |
| UNWIND( .fnstart) |
| UNWIND( .cantunwind) |
| bl __thread_std_smc_entry |
| mov r4, r0 /* Save return value for later */ |
| |
| /* Disable interrupts before switching to temporary stack */ |
| cpsid aif |
| bl thread_get_tmp_sp |
| mov sp, r0 |
| |
| bl thread_state_free |
| |
| ldr r0, =TEESMC_OPTEED_RETURN_CALL_DONE |
| mov r1, r4 |
| mov r2, #0 |
| mov r3, #0 |
| mov r4, #0 |
| smc #0 |
| b . /* SMC should not return */ |
| UNWIND( .fnend) |
| END_FUNC thread_std_smc_entry |
| |
| /* void thread_rpc(uint32_t rv[THREAD_RPC_NUM_ARGS]) */ |
| FUNC thread_rpc , : |
| UNWIND( .fnstart) |
| push {r0, lr} |
| UNWIND( .save {r0, lr}) |
| |
| bl thread_save_state |
| mov r4, r0 /* Save original CPSR */ |
| |
| /* |
| * Switch to temporary stack and SVC mode. Save CPSR to resume into. |
| */ |
| bl thread_get_tmp_sp |
| ldr r5, [sp] /* Get pointer to rv[] */ |
| cps #CPSR_MODE_SVC /* Change to SVC mode */ |
| mov sp, r0 /* Switch to tmp stack */ |
| |
| mov r0, #THREAD_FLAGS_COPY_ARGS_ON_RETURN |
| mov r1, r4 /* CPSR to restore */ |
| ldr r2, =.thread_rpc_return |
| bl thread_state_suspend |
| mov r4, r0 /* Supply thread index */ |
| ldr r0, =TEESMC_OPTEED_RETURN_CALL_DONE |
| ldm r5, {r1-r3} /* Load rv[] into r0-r2 */ |
| smc #0 |
| b . /* SMC should not return */ |
| |
| .thread_rpc_return: |
| /* |
| * At this point has the stack pointer been restored to the value |
| * it had when thread_save_state() was called above. |
| * |
| * Jumps here from thread_resume above when RPC has returned. The |
| * IRQ and FIQ bits are restored to what they where when this |
| * function was originally entered. |
| */ |
| pop {r12, lr} /* Get pointer to rv[] */ |
| stm r12, {r0-r3} /* Store r0-r3 into rv[] */ |
| bx lr |
| UNWIND( .fnend) |
| END_FUNC thread_rpc |
| KEEP_PAGER thread_rpc |
| |
| /* |
| * void thread_foreign_intr_exit(uint32_t thread_index) |
| * |
| * This function is jumped to at the end of macro foreign_intr_handler(). |
| * The current thread as indicated by @thread_index has just been |
| * suspended. The job here is just to inform normal world the thread id to |
| * resume when returning. |
| */ |
| FUNC thread_foreign_intr_exit , : |
| mov r4, r0 |
| ldr r0, =TEESMC_OPTEED_RETURN_CALL_DONE |
| ldr r1, =OPTEE_SMC_RETURN_RPC_FOREIGN_INTR |
| mov r2, #0 |
| mov r3, #0 |
| smc #0 |
| b . /* SMC should not return */ |
| END_FUNC thread_foreign_intr_exit |