[Qemu-devel] [[PATCH] 7/7] target-arm: Add WFx instruction trap support

[Greg Bellows <greg.bellows@linaro.org>]

Add support for trapping WFI and WFE instructions to the proper EL when
SCTLR/SCR/HCR settings apply.

Signed-off-by: Greg Bellows <greg.bellows@linaro.org>
---
 target-arm/op_helper.c | 75 +++++++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 71 insertions(+), 4 deletions(-)

diff --git a/target-arm/op_helper.c b/target-arm/op_helper.c
index aa175b5..d7e734d 100644
--- a/target-arm/op_helper.c
+++ b/target-arm/op_helper.c
@@ -209,23 +209,90 @@ uint32_t HELPER(usat16)(CPUARMState *env, uint32_t x, uint32_t shift)
     return res;
 }
 
+static inline uint32_t check_wfx_trap(CPUARMState *env, bool is_wfe)
+{
+    int cur_el = arm_current_el(env), el;
+    uint32_t target_el = 0;
+    uint64_t mask;
+
+    /* Check whether any EL controls above us trap WFx instructions */
+    for (el = cur_el + 1; el <= 3; el++) {
+        switch (el) {
+        case 1:
+            mask = (is_wfe) ? SCTLR_nTWE : SCTLR_nTWI;
+            /* Secure EL1 is only valid in AArch64.  If EL0 is AArch64 then so
+             * must be EL1.
+             */
+            if (arm_el_is_aa64(env, 1)) {
+                if ((env->cp15.sctlr_el[1] & mask) == 0) {
+                    target_el = el;
+                }
+            } else if (arm_feature(env, ARM_FEATURE_V8) &&
+                       !arm_is_secure(env)) {
+                /* SCTLR WFx SCTLR trap bits only exist in ARMv8 */
+                if ((A32_BANKED_CURRENT_REG_GET(env, sctlr) & mask) == 0) {
+                    target_el = el;
+                }
+            }
+            break;
+        case 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) == mask) {
+                    target_el = el;
+                }
+            }
+            break;
+        case 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) == mask) {
+                    target_el = el;
+                }
+            }
+            break;
+        }
+    }
+
+    return target_el;
+}
+
 void HELPER(wfi)(CPUARMState *env)
 {
     CPUState *cs = CPU(arm_env_get_cpu(env));
-
-    cs->exception_index = EXCP_HLT;
-    cs->halted = 1;
+    uint32_t target_el = 0;
+
+    target_el = check_wfx_trap(env, false);
+    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));
+    uint32_t target_el = 0;
 
     /* Don't actually halt the CPU, just yield back to top
      * level loop
      */
-    cs->exception_index = EXCP_YIELD;
+    target_el = check_wfx_trap(env, true);
+    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