diff --git a/bl31/context_mgmt.c b/bl31/context_mgmt.c
index eae608c5f906db4ba58faa9f5576fc551514ae56..41418372f3ae0d436dba74aef9fa8f58546b48fd 100644
--- a/bl31/context_mgmt.c
+++ b/bl31/context_mgmt.c
@@ -28,6 +28,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
+#include <arch.h>
#include <arch_helpers.h>
#include <assert.h>
#include <bl_common.h>
@@ -145,10 +146,10 @@ void cm_el1_sysregs_context_restore(uint32_t security_state)
}
/*******************************************************************************
- * 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 @@ void cm_set_el3_eret_context(uint32_t security_state, uint64_t entrypoint,
}
/*******************************************************************************
- * 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;
@@ -183,6 +184,56 @@ void cm_set_el3_elr(uint32_t security_state, uint64_t entrypoint)
write_ctx_reg(state, CTX_ELR_EL3, entrypoint);
}
+/*******************************************************************************
+ * 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);
+
+ /* Ensure that the 'value' is only a bit wide */
+ assert(value <= 1);
+
+ /*
+ * 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
diff --git a/include/bl31/context_mgmt.h b/include/bl31/context_mgmt.h
index d2598eef7740a91e8d9c61f470f8d7eae778698c..ad9d78565023cfe0190d0518c99ae14f2d06a06c 100644
--- a/include/bl31/context_mgmt.h
+++ b/include/bl31/context_mgmt.h
@@ -47,10 +47,13 @@ extern void cm_el1_sysregs_context_save(uint32_t security_state);
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 1c11af3f5879fb9603ba9c8023339616092d1054..68bab36ad3de9fc00ce75f2f193f89a6b418261d 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 f192d348828fbbeff0511e6a5b1a167c75947235..6909cf65e10ac46e584a3fd752241915170481f6 100644
--- a/services/spd/tspd/tspd_main.c
+++ b/services/spd/tspd/tspd_main.c
@@ -282,7 +282,7 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
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 75b4b30081247f5ddb8cd87c36236715d008da1b..d99aa2224cf7d010932300eb75e1a106c603c822 100644
--- a/services/spd/tspd/tspd_pm.c
+++ b/services/spd/tspd/tspd_pm.c
@@ -59,7 +59,7 @@ static int32_t tspd_cpu_off_handler(uint64_t cookie)
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 @@ static void tspd_cpu_suspend_handler(uint64_t power_state)
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 @@ static void tspd_cpu_suspend_finish_handler(uint64_t suspend_level)
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);
/*