diff --git a/bl31/context_mgmt.c b/bl31/context_mgmt.c index eae608c..bdb8513 100644 --- a/bl31/context_mgmt.c +++ b/bl31/context_mgmt.c @@ -28,6 +28,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include #include #include #include @@ -145,10 +146,10 @@ } /******************************************************************************* - * This function function populates 'cpu_context' pertaining to the given - * security state with the entrypoint, SPSR and SCR values so that an ERET from - * this securit state correctly restores corresponding values to drop the CPU to - * the next exception level + * This function populates 'cpu_context' pertaining to the given security state + * with the entrypoint, SPSR and SCR values so that an ERET from this security + * state correctly restores corresponding values to drop the CPU to the next + * exception level ******************************************************************************/ void cm_set_el3_eret_context(uint32_t security_state, uint64_t entrypoint, uint32_t spsr, uint32_t scr) @@ -167,10 +168,10 @@ } /******************************************************************************* - * This function function populates ELR_EL3 member of 'cpu_context' pertaining - * to the given security state with the given entrypoint + * This function populates ELR_EL3 member of 'cpu_context' pertaining to the + * given security state with the given entrypoint ******************************************************************************/ -void cm_set_el3_elr(uint32_t security_state, uint64_t entrypoint) +void cm_set_elr_el3(uint32_t security_state, uint64_t entrypoint) { cpu_context_t *ctx; el3_state_t *state; @@ -184,6 +185,53 @@ } /******************************************************************************* + * This function updates a single bit in the SCR_EL3 member of the 'cpu_context' + * pertaining to the given security state using the value and bit position + * specified in the parameters. It preserves all other bits. + ******************************************************************************/ +void cm_write_scr_el3_bit(uint32_t security_state, + uint32_t bit_pos, + uint32_t value) +{ + cpu_context_t *ctx; + el3_state_t *state; + uint32_t scr_el3; + + ctx = cm_get_context(read_mpidr(), security_state); + assert(ctx); + + /* Ensure that the bit position is a valid one */ + assert((1 << bit_pos) & SCR_VALID_BIT_MASK); + + /* + * Get the SCR_EL3 value from the cpu context, clear the desired bit + * and set it to its new value. + */ + state = get_el3state_ctx(ctx); + scr_el3 = read_ctx_reg(state, CTX_SCR_EL3); + scr_el3 &= ~(1 << bit_pos); + scr_el3 |= value << bit_pos; + write_ctx_reg(state, CTX_SCR_EL3, scr_el3); +} + +/******************************************************************************* + * This function retrieves SCR_EL3 member of 'cpu_context' pertaining to the + * given security state. + ******************************************************************************/ +uint32_t cm_get_scr_el3(uint32_t security_state) +{ + cpu_context_t *ctx; + el3_state_t *state; + + ctx = cm_get_context(read_mpidr(), security_state); + assert(ctx); + + /* Populate EL3 state so that ERET jumps to the correct entry */ + state = get_el3state_ctx(ctx); + return read_ctx_reg(state, CTX_SCR_EL3); +} + +/******************************************************************************* * This function is used to program the context that's used for exception * return. This initializes the SP_EL3 to a pointer to a 'cpu_context' set for * the required security state diff --git a/include/bl31/context_mgmt.h b/include/bl31/context_mgmt.h index d2598ee..ad9d785 100644 --- a/include/bl31/context_mgmt.h +++ b/include/bl31/context_mgmt.h @@ -47,10 +47,13 @@ extern void cm_el1_sysregs_context_restore(uint32_t security_state); extern void cm_set_el3_eret_context(uint32_t security_state, uint64_t entrypoint, uint32_t spsr, uint32_t scr); -extern void cm_set_el3_elr(uint32_t security_state, uint64_t entrypoint); +extern void cm_set_elr_el3(uint32_t security_state, uint64_t entrypoint); +extern void cm_write_scr_el3_bit(uint32_t security_state, + uint32_t bit_pos, + uint32_t value); extern void cm_set_next_eret_context(uint32_t security_state); extern void cm_init_pcpu_ptr_cache(); extern void cm_set_pcpu_ptr_cache(const void *pcpu_ptr); extern void *cm_get_pcpu_ptr_cache(void); - +extern uint32_t cm_get_scr_el3(uint32_t security_state); #endif /* __CM_H__ */ diff --git a/include/lib/aarch64/arch.h b/include/lib/aarch64/arch.h index d7e65b3..3a00f6e 100644 --- a/include/lib/aarch64/arch.h +++ b/include/lib/aarch64/arch.h @@ -148,6 +148,7 @@ #define SCR_FIQ_BIT (1 << 2) #define SCR_IRQ_BIT (1 << 1) #define SCR_NS_BIT (1 << 0) +#define SCR_VALID_BIT_MASK 0x2f8f /* HCR definitions */ #define HCR_RW_BIT (1ull << 31) diff --git a/services/spd/tspd/tspd_main.c b/services/spd/tspd/tspd_main.c index 6200d57..06e21c4 100644 --- a/services/spd/tspd/tspd_main.c +++ b/services/spd/tspd/tspd_main.c @@ -295,7 +295,7 @@ assert(&tsp_ctx->cpu_ctx == cm_get_context(mpidr, SECURE)); set_aapcs_args7(&tsp_ctx->cpu_ctx, smc_fid, x1, x2, 0, 0, 0, 0, 0); - cm_set_el3_elr(SECURE, (uint64_t) tsp_entry_info->fast_smc_entry); + cm_set_elr_el3(SECURE, (uint64_t) tsp_entry_info->fast_smc_entry); cm_el1_sysregs_context_restore(SECURE); cm_set_next_eret_context(SECURE); diff --git a/services/spd/tspd/tspd_pm.c b/services/spd/tspd/tspd_pm.c index 75b4b30..d99aa22 100644 --- a/services/spd/tspd/tspd_pm.c +++ b/services/spd/tspd/tspd_pm.c @@ -59,7 +59,7 @@ assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_ON); /* Program the entry point and enter the TSP */ - cm_set_el3_elr(SECURE, (uint64_t) tsp_entry_info->cpu_off_entry); + cm_set_elr_el3(SECURE, (uint64_t) tsp_entry_info->cpu_off_entry); rc = tspd_synchronous_sp_entry(tsp_ctx); /* @@ -96,7 +96,7 @@ write_ctx_reg(get_gpregs_ctx(&tsp_ctx->cpu_ctx), CTX_GPREG_X0, power_state); - cm_set_el3_elr(SECURE, (uint64_t) tsp_entry_info->cpu_suspend_entry); + cm_set_elr_el3(SECURE, (uint64_t) tsp_entry_info->cpu_suspend_entry); rc = tspd_synchronous_sp_entry(tsp_ctx); /* @@ -165,7 +165,7 @@ write_ctx_reg(get_gpregs_ctx(&tsp_ctx->cpu_ctx), CTX_GPREG_X0, suspend_level); - cm_set_el3_elr(SECURE, (uint64_t) tsp_entry_info->cpu_resume_entry); + cm_set_elr_el3(SECURE, (uint64_t) tsp_entry_info->cpu_resume_entry); rc = tspd_synchronous_sp_entry(tsp_ctx); /*