Add support for trapping WFI and WFE instructions to the proper EL when
SCTLR/SCR/HCR settings apply.
Signed-off-by: Greg Bellows <greg.bell...@linaro.org>
---
v1 -> v2
- Replace check loop with simpler if checks.
- Changed WFx syncdrome function to take bool
- Changed return of uint32_t to int
- Added cdditional comments.
---
target-arm/internals.h | 2 +-
target-arm/op_helper.c | 76 ++++++++++++++++++++++++++++++++++++++++++++++++--
2 files changed, 74 insertions(+), 4 deletions(-)
diff --git a/target-arm/internals.h b/target-arm/internals.h
index de0a9c1..8e208f7 100644
--- a/target-arm/internals.h
+++ b/target-arm/internals.h
@@ -347,7 +347,7 @@ static inline uint32_t syn_breakpoint(int same_el)
| ARM_EL_IL | 0x22;
}
-static inline uint32_t syn_wfx(int cv, int cond, int ti)
+static inline uint32_t syn_wfx(int cv, int cond, bool ti)
{
return (EC_WFX_TRAP << ARM_EL_EC_SHIFT) |
(cv << 24) | (cond << 20) | ti;
diff --git a/target-arm/op_helper.c b/target-arm/op_helper.c
index 971edc7..43aa8ef 100644
--- a/target-arm/op_helper.c
+++ b/target-arm/op_helper.c
@@ -223,23 +223,93 @@ uint32_t HELPER(usat16)(CPUARMState *env, uint32_t x,
uint32_t shift)
return res;
}
+/* Function checks whether WFx (WFI/WFE) instructions are set-up to be trapped.
+ * The function returns the target EL (1-3) if the instruction is to be trapped
+ * otherwise it returns 0 indicating it is not trapped.
+ */
+static inline int check_wfx_trap(CPUARMState *env, bool is_wfe)
+{
+ int cur_el = arm_current_el(env);
+ uint64_t mask;
+
+ /* If we are currently in EL0 then we need to check if SCTLR is set up for
+ * WFx instructions being trapped to EL1.
+ */
+ if (cur_el < 1) {
+ mask = is_wfe ? SCTLR_nTWE : SCTLR_nTWI;
+ if (arm_el_is_aa64(env, 1)) {
+ if (!(env->cp15.sctlr_el[1] & mask)) {
+ return 1;
+ }
+ } else if (arm_feature(env, ARM_FEATURE_V8)) {
+ /* SCTLR WFx SCTLR trap bits only exist in ARMv8 */
+ if (!(A32_BANKED_CURRENT_REG_GET(env, sctlr) & mask)) {
+ return 1;
+ }
+ }
+ }
+
+ /* We are not trapping to EL1 at this point.
+ * Check if we are trapping WFx to EL2 it present.
+ */
+ if (cur_el < 2) {
+ if (arm_feature(env, ARM_FEATURE_EL2) && !arm_is_secure(env)) {
+ mask = (is_wfe) ? HCR_TWE : HCR_TWI;
+ if (env->cp15.hcr_el2 & mask) {
+ return 2;
+ }
+ }
+ }
+
+ /* We are not trapping to EL1 or EL2 at this point.
+ * Check if we are trapping WFx to EL3 if present.
+ */
+ if (cur_el < 3) {
+ if (arm_feature(env, ARM_FEATURE_EL3) &&
+ arm_feature(env, ARM_FEATURE_V8)) {
+ mask = (is_wfe) ? SCR_TWE : SCR_TWI;
+ if (env->cp15.scr_el3 & mask) {
+ return 3;
+ }
+ }
+ }
+
+ return 0;
+}
+
void HELPER(wfi)(CPUARMState *env)
{
CPUState *cs = CPU(arm_env_get_cpu(env));
+ int target_el = check_wfx_trap(env, false);
- cs->exception_index = EXCP_HLT;
- cs->halted = 1;
+ if (target_el) {
+ cs->exception_index = EXCP_UDEF;
+ env->exception.syndrome = syn_wfx(1, 0xe, false);
+ env->exception.target_el = target_el;
+ env->pc -= 4;
+ } else {
+ cs->exception_index = EXCP_HLT;
+ cs->halted = 1;
+ }
cpu_loop_exit(cs);
}
void HELPER(wfe)(CPUARMState *env)
{
CPUState *cs = CPU(arm_env_get_cpu(env));
+ int target_el = check_wfx_trap(env, true);
/* Don't actually halt the CPU, just yield back to top
* level loop
*/
- cs->exception_index = EXCP_YIELD;
+ if (target_el) {
+ cs->exception_index = EXCP_UDEF;
+ env->exception.syndrome = syn_wfx(1, 0xe, true);
+ env->exception.target_el = target_el;
+ env->pc -= 4;
+ } else {
+ cs->exception_index = EXCP_YIELD;
+ }
cpu_loop_exit(cs);
}
--
1.8.3.2
