diff --git a/include/lib/extensions/spe.h b/include/lib/extensions/spe.h index c6e44f96b..4801a2206 100644 --- a/include/lib/extensions/spe.h +++ b/include/lib/extensions/spe.h @@ -12,16 +12,20 @@ #if ENABLE_SPE_FOR_NS void spe_enable(cpu_context_t *ctx); +void spe_disable(cpu_context_t *ctx); void spe_init_el2_unused(void); -void spe_disable(void); +void spe_stop(void); #else static inline void spe_enable(cpu_context_t *ctx) { } +static inline void spe_disable(cpu_context_t *ctx) +{ +} static inline void spe_init_el2_unused(void) { } -static inline void spe_disable(void) +static inline void spe_stop(void) { } #endif /* ENABLE_SPE_FOR_NS */ diff --git a/include/lib/extensions/trbe.h b/include/lib/extensions/trbe.h index 5db331672..2c488e097 100644 --- a/include/lib/extensions/trbe.h +++ b/include/lib/extensions/trbe.h @@ -10,9 +10,13 @@ #include #if ENABLE_TRBE_FOR_NS +void trbe_disable(cpu_context_t *ctx); void trbe_enable(cpu_context_t *ctx); void trbe_init_el2_unused(void); #else +static inline void trbe_disable(cpu_context_t *ctx) +{ +} static inline void trbe_enable(cpu_context_t *ctx) { } diff --git a/lib/extensions/spe/spe.c b/lib/extensions/spe/spe.c index c6076fe1e..d6532224a 100644 --- a/lib/extensions/spe/spe.c +++ b/lib/extensions/spe/spe.c @@ -52,6 +52,27 @@ void spe_enable(cpu_context_t *ctx) write_ctx_reg(state, CTX_MDCR_EL3, mdcr_el3_val); } +void spe_disable(cpu_context_t *ctx) +{ + el3_state_t *state = get_el3state_ctx(ctx); + u_register_t mdcr_el3_val = read_ctx_reg(state, CTX_MDCR_EL3); + + /* + * MDCR_EL3.NSPB: Clear these bits to disable SPE feature, as it was enabled + * for Non-secure state only. After clearing these bits Secure state owns + * the Profiling Buffer and accesses to Statistical Profiling and Profiling + * Buffer control registers at EL2 and EL1 generate Trap exceptions to EL3 + * + * MDCR_EL3.NSPBE: Don't care as it was cleared during spe_enable and setting + * this to 1 does not make sense as NSPBE{1} and NSPB{0b0x} is RESERVED. + * + * MDCR_EL3.EnPMSN (ARM v8.7): Clear the bit to trap access of PMSNEVFR_EL1 + * from EL2/EL1 to EL3. + */ + mdcr_el3_val &= ~(MDCR_NSPB(MDCR_NSPB_EL1) | MDCR_EnPMSN_BIT); + write_ctx_reg(state, CTX_MDCR_EL3, mdcr_el3_val); +} + void spe_init_el2_unused(void) { uint64_t v; @@ -70,7 +91,7 @@ void spe_init_el2_unused(void) write_mdcr_el2(v); } -void spe_disable(void) +void spe_stop(void) { uint64_t v; diff --git a/lib/extensions/trbe/trbe.c b/lib/extensions/trbe/trbe.c index 915773420..8c1c42180 100644 --- a/lib/extensions/trbe/trbe.c +++ b/lib/extensions/trbe/trbe.c @@ -39,6 +39,25 @@ void trbe_enable(cpu_context_t *ctx) write_ctx_reg(state, CTX_MDCR_EL3, mdcr_el3_val); } +void trbe_disable(cpu_context_t *ctx) +{ + el3_state_t *state = get_el3state_ctx(ctx); + u_register_t mdcr_el3_val = read_ctx_reg(state, CTX_MDCR_EL3); + + /* + * MDCR_EL3.NSTBE = 0b0 + * Trace Buffer owning Security state is secure state. If FEAT_RME + * is not implemented, this field is RES0. + * + * MDCR_EL3.NSTB = 0b00 + * Clear these bits to disable access of trace buffer control registers + * from lower ELs in any security state. + */ + mdcr_el3_val &= ~(MDCR_NSTB(MDCR_NSTB_EL1)); + mdcr_el3_val &= ~(MDCR_NSTBE_BIT); + write_ctx_reg(state, CTX_MDCR_EL3, mdcr_el3_val); +} + void trbe_init_el2_unused(void) { /* diff --git a/lib/psci/psci_common.c b/lib/psci/psci_common.c index 60449f646..9f0b19089 100644 --- a/lib/psci/psci_common.c +++ b/lib/psci/psci_common.c @@ -1303,7 +1303,7 @@ void psci_do_manage_extensions(void) * before exiting coherency. */ if (is_feat_spe_supported()) { - spe_disable(); + spe_stop(); } }