diff --git a/bl31/aarch64/bl31_entrypoint.S b/bl31/aarch64/bl31_entrypoint.S index 74b0993..665a05e 100644 --- a/bl31/aarch64/bl31_entrypoint.S +++ b/bl31/aarch64/bl31_entrypoint.S @@ -9,7 +9,7 @@ #include #include #include -#include +#include #include #include diff --git a/bl32/sp_min/aarch32/entrypoint.S b/bl32/sp_min/aarch32/entrypoint.S index 0a68475..f3a1e44 100644 --- a/bl32/sp_min/aarch32/entrypoint.S +++ b/bl32/sp_min/aarch32/entrypoint.S @@ -10,6 +10,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -164,6 +167,20 @@ /* On SMC entry, `sp` points to `smc_ctx_t`. Save `lr`. */ str lr, [sp, #SMC_CTX_LR_MON] +#if ENABLE_RUNTIME_INSTRUMENTATION + /* + * Read the timestamp value and store it on top of the C runtime stack. + * The value will be saved to the per-cpu data once the C stack is + * available, as a valid stack is needed to call _cpu_data() + */ + strd r0, r1, [sp, #SMC_CTX_GPREG_R0] + ldcopr16 r0, r1, CNTPCT_64 + ldr lr, [sp, #SMC_CTX_SP_MON] + strd r0, r1, [lr, #-8]! + str lr, [sp, #SMC_CTX_SP_MON] + ldrd r0, r1, [sp, #SMC_CTX_GPREG_R0] +#endif + smccc_save_gp_mode_regs clrex_on_monitor_entry @@ -175,6 +192,23 @@ mov r2, sp /* handle */ ldr sp, [r2, #SMC_CTX_SP_MON] +#if ENABLE_RUNTIME_INSTRUMENTATION + /* Save handle to a callee saved register */ + mov r6, r2 + + /* + * Restore the timestamp value and store it in per-cpu data. The value + * will be extracted from per-cpu data by the C level SMC handler and + * saved to the PMF timestamp region. + */ + ldrd r4, r5, [sp], #8 + bl _cpu_data + strd r4, r5, [r0, #CPU_DATA_PMF_TS0_OFFSET] + + /* Restore handle */ + mov r2, r6 +#endif + ldr r0, [r2, #SMC_CTX_SCR] and r3, r0, #SCR_NS_BIT /* flags */ @@ -239,6 +273,16 @@ * The Warm boot entrypoint for SP_MIN. */ func sp_min_warm_entrypoint +#if ENABLE_RUNTIME_INSTRUMENTATION + /* + * This timestamp update happens with cache off. The next + * timestamp collection will need to do cache maintenance prior + * to timestamp update. + */ + pmf_calc_timestamp_addr rt_instr_svc, RT_INSTR_EXIT_HW_LOW_PWR + ldcopr16 r2, r3, CNTPCT_64 + strd r2, r3, [r0] +#endif /* * On the warm boot path, most of the EL3 initialisations performed by * 'el3_entrypoint_common' must be skipped: @@ -295,6 +339,30 @@ bl smc_get_next_ctx /* r0 points to `smc_ctx_t` */ /* The PSCI cpu_context registers have been copied to `smc_ctx_t` */ + +#if ENABLE_RUNTIME_INSTRUMENTATION + /* Save smc_ctx_t */ + mov r5, r0 + + pmf_calc_timestamp_addr rt_instr_svc, RT_INSTR_EXIT_PSCI + mov r4, r0 + + /* + * Invalidate before updating timestamp to ensure previous timestamp + * updates on the same cache line with caches disabled are properly + * seen by the same core. Without the cache invalidate, the core might + * write into a stale cache line. + */ + mov r1, #PMF_TS_SIZE + bl inv_dcache_range + + ldcopr16 r0, r1, CNTPCT_64 + strd r0, r1, [r4] + + /* Restore smc_ctx_t */ + mov r0, r5 +#endif + b sp_min_exit endfunc sp_min_warm_entrypoint diff --git a/bl32/sp_min/sp_min.ld.S b/bl32/sp_min/sp_min.ld.S index 4559903..6997a7f 100644 --- a/bl32/sp_min/sp_min.ld.S +++ b/bl32/sp_min/sp_min.ld.S @@ -55,6 +55,14 @@ KEEP(*(rt_svc_descs)) __RT_SVC_DESCS_END__ = .; +#if ENABLE_PMF + /* Ensure 4-byte alignment for descriptors and ensure inclusion */ + . = ALIGN(4); + __PMF_SVC_DESCS_START__ = .; + KEEP(*(pmf_svc_descs)) + __PMF_SVC_DESCS_END__ = .; +#endif /* ENABLE_PMF */ + /* * Ensure 4-byte alignment for cpu_ops so that its fields are also * aligned. Also ensure cpu_ops inclusion. diff --git a/bl32/sp_min/sp_min_main.c b/bl32/sp_min/sp_min_main.c index f39e33b..f050160 100644 --- a/bl32/sp_min/sp_min_main.c +++ b/bl32/sp_min/sp_min_main.c @@ -19,7 +19,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -28,6 +30,11 @@ #include "sp_min_private.h" +#if ENABLE_RUNTIME_INSTRUMENTATION +PMF_REGISTER_SERVICE_SMC(rt_instr_svc, PMF_RT_INSTR_SVC_ID, + RT_INSTR_TOTAL_IDS, PMF_STORE_ENABLE) +#endif + /* Pointers to per-core cpu contexts */ static void *sp_min_cpu_ctx_ptr[PLATFORM_CORE_COUNT]; diff --git a/include/lib/pmf/aarch32/pmf_asm_macros.S b/include/lib/pmf/aarch32/pmf_asm_macros.S new file mode 100644 index 0000000..1dbb408 --- /dev/null +++ b/include/lib/pmf/aarch32/pmf_asm_macros.S @@ -0,0 +1,28 @@ +/* + * Copyright (c) 2019, Arm Limited. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#ifndef PMF_ASM_MACROS_S +#define PMF_ASM_MACROS_S + +#define PMF_TS_SIZE 8 + + /* + * This macro calculates the address of the per-cpu timestamp + * for the given service name and local timestamp id. + * Clobbers: r0 - r4 + */ + .macro pmf_calc_timestamp_addr _name, _tid + mov r4, lr + bl plat_my_core_pos + mov lr, r4 + ldr r1, =__PERCPU_TIMESTAMP_SIZE__ + mov r2, #(\_tid * PMF_TS_SIZE) + mla r0, r0, r1, r2 + ldr r1, =pmf_ts_mem_\_name + add r0, r0, r1 + .endm + +#endif /* PMF_ASM_MACROS_S */ diff --git a/include/lib/pmf/aarch64/pmf_asm_macros.S b/include/lib/pmf/aarch64/pmf_asm_macros.S new file mode 100644 index 0000000..5f3e6b7 --- /dev/null +++ b/include/lib/pmf/aarch64/pmf_asm_macros.S @@ -0,0 +1,30 @@ +/* + * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#ifndef PMF_ASM_MACROS_S +#define PMF_ASM_MACROS_S + +#define PMF_TS_SIZE 8 + + /* + * This macro calculates the address of the per-cpu timestamp + * for the given service name and local timestamp id. + * Clobbers: x0 - x9 + */ + .macro pmf_calc_timestamp_addr _name, _tid + mov x9, x30 + bl plat_my_core_pos + mov x30, x9 + adr x2, __PMF_PERCPU_TIMESTAMP_END__ + adr x1, __PMF_TIMESTAMP_START__ + sub x1, x2, x1 + mov x2, #(\_tid * PMF_TS_SIZE) + madd x0, x0, x1, x2 + adr x1, pmf_ts_mem_\_name + add x0, x0, x1 + .endm + +#endif /* PMF_ASM_MACROS_S */ diff --git a/include/lib/pmf/pmf_asm_macros.S b/include/lib/pmf/pmf_asm_macros.S deleted file mode 100644 index 5f3e6b7..0000000 --- a/include/lib/pmf/pmf_asm_macros.S +++ /dev/null @@ -1,30 +0,0 @@ -/* - * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef PMF_ASM_MACROS_S -#define PMF_ASM_MACROS_S - -#define PMF_TS_SIZE 8 - - /* - * This macro calculates the address of the per-cpu timestamp - * for the given service name and local timestamp id. - * Clobbers: x0 - x9 - */ - .macro pmf_calc_timestamp_addr _name, _tid - mov x9, x30 - bl plat_my_core_pos - mov x30, x9 - adr x2, __PMF_PERCPU_TIMESTAMP_END__ - adr x1, __PMF_TIMESTAMP_START__ - sub x1, x2, x1 - mov x2, #(\_tid * PMF_TS_SIZE) - madd x0, x0, x1, x2 - adr x1, pmf_ts_mem_\_name - add x0, x0, x1 - .endm - -#endif /* PMF_ASM_MACROS_S */ diff --git a/plat/arm/common/aarch64/execution_state_switch.c b/plat/arm/common/aarch64/execution_state_switch.c new file mode 100644 index 0000000..8835fa1 --- /dev/null +++ b/plat/arm/common/aarch64/execution_state_switch.c @@ -0,0 +1,181 @@ +/* + * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +/* + * Handle SMC from a lower exception level to switch its execution state + * (either from AArch64 to AArch32, or vice versa). + * + * smc_fid: + * SMC function ID - either ARM_SIP_SVC_STATE_SWITCH_64 or + * ARM_SIP_SVC_STATE_SWITCH_32. + * pc_hi, pc_lo: + * PC upon re-entry to the calling exception level; width dependent on the + * calling exception level. + * cookie_hi, cookie_lo: + * Opaque pointer pairs received from the caller to pass it back, upon + * re-entry. + * handle: + * Handle to saved context. + */ +int arm_execution_state_switch(unsigned int smc_fid, + uint32_t pc_hi, + uint32_t pc_lo, + uint32_t cookie_hi, + uint32_t cookie_lo, + void *handle) +{ + bool caller_64, thumb = false, from_el2; + unsigned int el, endianness; + u_register_t spsr, pc, scr, sctlr; + entry_point_info_t ep; + cpu_context_t *ctx = (cpu_context_t *) handle; + el3_state_t *el3_ctx = get_el3state_ctx(ctx); + + /* Validate supplied entry point */ + pc = (u_register_t) (((uint64_t) pc_hi << 32) | pc_lo); + if (arm_validate_ns_entrypoint(pc) != 0) + goto invalid_param; + + /* That the SMC originated from NS is already validated by the caller */ + + /* + * Disallow state switch if any of the secondaries have been brought up. + */ + if (psci_secondaries_brought_up() != 0) + goto exec_denied; + + spsr = read_ctx_reg(el3_ctx, CTX_SPSR_EL3); + caller_64 = (GET_RW(spsr) == MODE_RW_64); + + if (caller_64) { + /* + * If the call originated from AArch64, expect 32-bit pointers when + * switching to AArch32. + */ + if ((pc_hi != 0U) || (cookie_hi != 0U)) + goto invalid_param; + + pc = pc_lo; + + /* Instruction state when entering AArch32 */ + thumb = (pc & 1U) != 0U; + } else { + /* Construct AArch64 PC */ + pc = (((u_register_t) pc_hi) << 32) | pc_lo; + } + + /* Make sure PC is 4-byte aligned, except for Thumb */ + if (((pc & 0x3U) != 0U) && !thumb) + goto invalid_param; + + /* + * EL3 controls register width of the immediate lower EL only. Expect + * this request from EL2/Hyp unless: + * + * - EL2 is not implemented; + * - EL2 is implemented, but was disabled. This can be inferred from + * SCR_EL3.HCE. + */ + from_el2 = caller_64 ? (GET_EL(spsr) == MODE_EL2) : + (GET_M32(spsr) == MODE32_hyp); + scr = read_ctx_reg(el3_ctx, CTX_SCR_EL3); + if (!from_el2) { + /* The call is from NS privilege level other than HYP */ + + /* + * Disallow switching state if there's a Hypervisor in place; + * this request must be taken up with the Hypervisor instead. + */ + if ((scr & SCR_HCE_BIT) != 0U) + goto exec_denied; + } + + /* + * Return to the caller using the same endianness. Extract + * endianness bit from the respective system control register + * directly. + */ + sctlr = from_el2 ? read_sctlr_el2() : read_sctlr_el1(); + endianness = ((sctlr & SCTLR_EE_BIT) != 0U) ? 1U : 0U; + + /* Construct SPSR for the exception state we're about to switch to */ + if (caller_64) { + unsigned long long impl; + + /* + * Switching from AArch64 to AArch32. Ensure this CPU implements + * the target EL in AArch32. + */ + impl = from_el2 ? el_implemented(2) : el_implemented(1); + if (impl != EL_IMPL_A64_A32) + goto exec_denied; + + /* Return to the equivalent AArch32 privilege level */ + el = from_el2 ? MODE32_hyp : MODE32_svc; + spsr = SPSR_MODE32((u_register_t) el, + thumb ? SPSR_T_THUMB : SPSR_T_ARM, + endianness, DISABLE_ALL_EXCEPTIONS); + } else { + /* + * Switching from AArch32 to AArch64. Since it's not possible to + * implement an EL as AArch32-only (from which this call was + * raised), it's safe to assume AArch64 is also implemented. + */ + el = from_el2 ? MODE_EL2 : MODE_EL1; + spsr = SPSR_64((u_register_t) el, MODE_SP_ELX, + DISABLE_ALL_EXCEPTIONS); + } + + /* + * Use the context management library to re-initialize the existing + * context with the execution state flipped. Since the library takes + * entry_point_info_t pointer as the argument, construct a dummy one + * with PC, state width, endianness, security etc. appropriately set. + * Other entries in the entry point structure are irrelevant for + * purpose. + */ + zeromem(&ep, sizeof(ep)); + ep.pc = pc; + ep.spsr = (uint32_t) spsr; + SET_PARAM_HEAD(&ep, PARAM_EP, VERSION_1, + ((unsigned int) ((endianness != 0U) ? EP_EE_BIG : + EP_EE_LITTLE) + | NON_SECURE | EP_ST_DISABLE)); + + /* + * Re-initialize the system register context, and exit EL3 as if for the + * first time. State switch is effectively a soft reset of the + * calling EL. + */ + cm_init_my_context(&ep); + cm_prepare_el3_exit(NON_SECURE); + + /* + * State switch success. The caller of SMC wouldn't see the SMC + * returning. Instead, execution starts at the supplied entry point, + * with context pointers populated in registers 0 and 1. + */ + SMC_RET2(handle, cookie_hi, cookie_lo); + +invalid_param: + SMC_RET1(handle, STATE_SW_E_PARAM); + +exec_denied: + /* State switch denied */ + SMC_RET1(handle, STATE_SW_E_DENIED); +} diff --git a/plat/arm/common/arm_common.mk b/plat/arm/common/arm_common.mk index acc3797..cda39b7 100644 --- a/plat/arm/common/arm_common.mk +++ b/plat/arm/common/arm_common.mk @@ -215,12 +215,17 @@ BL31_SOURCES += plat/arm/common/arm_bl31_setup.c \ plat/arm/common/arm_pm.c \ plat/arm/common/arm_topology.c \ - plat/arm/common/execution_state_switch.c \ plat/common/plat_psci_common.c ifeq (${ENABLE_PMF}, 1) -BL31_SOURCES += plat/arm/common/arm_sip_svc.c \ +ifeq (${ARCH}, aarch64) +BL31_SOURCES += plat/arm/common/aarch64/execution_state_switch.c\ + plat/arm/common/arm_sip_svc.c \ lib/pmf/pmf_smc.c +else +BL32_SOURCES += plat/arm/common/arm_sip_svc.c \ + lib/pmf/pmf_smc.c +endif endif ifeq (${EL3_EXCEPTION_HANDLING},1) diff --git a/plat/arm/common/arm_sip_svc.c b/plat/arm/common/arm_sip_svc.c index 3d308a3..a61f5f8 100644 --- a/plat/arm/common/arm_sip_svc.c +++ b/plat/arm/common/arm_sip_svc.c @@ -1,5 +1,5 @@ /* - * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved. + * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved. * * SPDX-License-Identifier: BSD-3-Clause */ @@ -50,23 +50,22 @@ switch (smc_fid) { case ARM_SIP_SVC_EXE_STATE_SWITCH: { - u_register_t pc; - + /* Execution state can be switched only if EL3 is AArch64 */ +#ifdef __aarch64__ /* Allow calls from non-secure only */ if (!is_caller_non_secure(flags)) SMC_RET1(handle, STATE_SW_E_DENIED); - /* Validate supplied entry point */ - pc = (u_register_t) ((x1 << 32) | (uint32_t) x2); - if (arm_validate_ns_entrypoint(pc) != 0) - SMC_RET1(handle, STATE_SW_E_PARAM); - /* * Pointers used in execution state switch are all 32 bits wide */ return (uintptr_t) arm_execution_state_switch(smc_fid, (uint32_t) x1, (uint32_t) x2, (uint32_t) x3, (uint32_t) x4, handle); +#else + /* State switch denied */ + SMC_RET1(handle, STATE_SW_E_DENIED); +#endif /* __aarch64__ */ } case ARM_SIP_SVC_CALL_COUNT: diff --git a/plat/arm/common/execution_state_switch.c b/plat/arm/common/execution_state_switch.c deleted file mode 100644 index 00ac16e..0000000 --- a/plat/arm/common/execution_state_switch.c +++ /dev/null @@ -1,179 +0,0 @@ -/* - * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -/* - * Handle SMC from a lower exception level to switch its execution state - * (either from AArch64 to AArch32, or vice versa). - * - * smc_fid: - * SMC function ID - either ARM_SIP_SVC_STATE_SWITCH_64 or - * ARM_SIP_SVC_STATE_SWITCH_32. - * pc_hi, pc_lo: - * PC upon re-entry to the calling exception level; width dependent on the - * calling exception level. - * cookie_hi, cookie_lo: - * Opaque pointer pairs received from the caller to pass it back, upon - * re-entry. - * handle: - * Handle to saved context. - */ -int arm_execution_state_switch(unsigned int smc_fid, - uint32_t pc_hi, - uint32_t pc_lo, - uint32_t cookie_hi, - uint32_t cookie_lo, - void *handle) -{ - /* Execution state can be switched only if EL3 is AArch64 */ -#ifdef __aarch64__ - bool caller_64, thumb = false, from_el2; - unsigned int el, endianness; - u_register_t spsr, pc, scr, sctlr; - entry_point_info_t ep; - cpu_context_t *ctx = (cpu_context_t *) handle; - el3_state_t *el3_ctx = get_el3state_ctx(ctx); - - /* That the SMC originated from NS is already validated by the caller */ - - /* - * Disallow state switch if any of the secondaries have been brought up. - */ - if (psci_secondaries_brought_up() != 0) - goto exec_denied; - - spsr = read_ctx_reg(el3_ctx, CTX_SPSR_EL3); - caller_64 = (GET_RW(spsr) == MODE_RW_64); - - if (caller_64) { - /* - * If the call originated from AArch64, expect 32-bit pointers when - * switching to AArch32. - */ - if ((pc_hi != 0U) || (cookie_hi != 0U)) - goto invalid_param; - - pc = pc_lo; - - /* Instruction state when entering AArch32 */ - thumb = (pc & 1U) != 0U; - } else { - /* Construct AArch64 PC */ - pc = (((u_register_t) pc_hi) << 32) | pc_lo; - } - - /* Make sure PC is 4-byte aligned, except for Thumb */ - if (((pc & 0x3U) != 0U) && !thumb) - goto invalid_param; - - /* - * EL3 controls register width of the immediate lower EL only. Expect - * this request from EL2/Hyp unless: - * - * - EL2 is not implemented; - * - EL2 is implemented, but was disabled. This can be inferred from - * SCR_EL3.HCE. - */ - from_el2 = caller_64 ? (GET_EL(spsr) == MODE_EL2) : - (GET_M32(spsr) == MODE32_hyp); - scr = read_ctx_reg(el3_ctx, CTX_SCR_EL3); - if (!from_el2) { - /* The call is from NS privilege level other than HYP */ - - /* - * Disallow switching state if there's a Hypervisor in place; - * this request must be taken up with the Hypervisor instead. - */ - if ((scr & SCR_HCE_BIT) != 0U) - goto exec_denied; - } - - /* - * Return to the caller using the same endianness. Extract - * endianness bit from the respective system control register - * directly. - */ - sctlr = from_el2 ? read_sctlr_el2() : read_sctlr_el1(); - endianness = ((sctlr & SCTLR_EE_BIT) != 0U) ? 1U : 0U; - - /* Construct SPSR for the exception state we're about to switch to */ - if (caller_64) { - unsigned long long impl; - - /* - * Switching from AArch64 to AArch32. Ensure this CPU implements - * the target EL in AArch32. - */ - impl = from_el2 ? el_implemented(2) : el_implemented(1); - if (impl != EL_IMPL_A64_A32) - goto exec_denied; - - /* Return to the equivalent AArch32 privilege level */ - el = from_el2 ? MODE32_hyp : MODE32_svc; - spsr = SPSR_MODE32((u_register_t) el, - thumb ? SPSR_T_THUMB : SPSR_T_ARM, - endianness, DISABLE_ALL_EXCEPTIONS); - } else { - /* - * Switching from AArch32 to AArch64. Since it's not possible to - * implement an EL as AArch32-only (from which this call was - * raised), it's safe to assume AArch64 is also implemented. - */ - el = from_el2 ? MODE_EL2 : MODE_EL1; - spsr = SPSR_64((u_register_t) el, MODE_SP_ELX, - DISABLE_ALL_EXCEPTIONS); - } - - /* - * Use the context management library to re-initialize the existing - * context with the execution state flipped. Since the library takes - * entry_point_info_t pointer as the argument, construct a dummy one - * with PC, state width, endianness, security etc. appropriately set. - * Other entries in the entry point structure are irrelevant for - * purpose. - */ - zeromem(&ep, sizeof(ep)); - ep.pc = pc; - ep.spsr = (uint32_t) spsr; - SET_PARAM_HEAD(&ep, PARAM_EP, VERSION_1, - ((unsigned int) ((endianness != 0U) ? EP_EE_BIG : - EP_EE_LITTLE) - | NON_SECURE | EP_ST_DISABLE)); - - /* - * Re-initialize the system register context, and exit EL3 as if for the - * first time. State switch is effectively a soft reset of the - * calling EL. - */ - cm_init_my_context(&ep); - cm_prepare_el3_exit(NON_SECURE); - - /* - * State switch success. The caller of SMC wouldn't see the SMC - * returning. Instead, execution starts at the supplied entry point, - * with context pointers populated in registers 0 and 1. - */ - SMC_RET2(handle, cookie_hi, cookie_lo); - -invalid_param: - SMC_RET1(handle, STATE_SW_E_PARAM); - -exec_denied: -#endif /* __aarch64__ */ - /* State switch denied */ - SMC_RET1(handle, STATE_SW_E_DENIED); -}