diff options
Diffstat (limited to 'bl32')
| -rw-r--r-- | bl32/sp_min/aarch32/entrypoint.S | 68 | ||||
| -rw-r--r-- | bl32/sp_min/sp_min.ld.S | 8 | ||||
| -rw-r--r-- | bl32/sp_min/sp_min_main.c | 7 |
3 files changed, 83 insertions, 0 deletions
diff --git a/bl32/sp_min/aarch32/entrypoint.S b/bl32/sp_min/aarch32/entrypoint.S index 0a684754c..f3a1e440b 100644 --- a/bl32/sp_min/aarch32/entrypoint.S +++ b/bl32/sp_min/aarch32/entrypoint.S @@ -10,6 +10,9 @@ #include <common/runtime_svc.h> #include <context.h> #include <el3_common_macros.S> +#include <lib/el3_runtime/cpu_data.h> +#include <lib/pmf/aarch32/pmf_asm_macros.S> +#include <lib/runtime_instr.h> #include <lib/xlat_tables/xlat_tables_defs.h> #include <smccc_helpers.h> #include <smccc_macros.S> @@ -164,6 +167,20 @@ func sp_min_handle_smc /* 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 @@ func sp_min_handle_smc 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 @@ endfunc sp_min_handle_fiq * 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 @@ func sp_min_warm_entrypoint 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 4559903bf..6997a7fdb 100644 --- a/bl32/sp_min/sp_min.ld.S +++ b/bl32/sp_min/sp_min.ld.S @@ -55,6 +55,14 @@ SECTIONS 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 f39e33b6b..f050160f3 100644 --- a/bl32/sp_min/sp_min_main.c +++ b/bl32/sp_min/sp_min_main.c @@ -19,7 +19,9 @@ #include <context.h> #include <drivers/console.h> #include <lib/el3_runtime/context_mgmt.h> +#include <lib/pmf/pmf.h> #include <lib/psci/psci.h> +#include <lib/runtime_instr.h> #include <lib/utils.h> #include <plat/common/platform.h> #include <platform_sp_min.h> @@ -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]; |