arch-arm: Fixed WFE/WFI trapping behaviour

This patch fixes the WFx trapping behaviour by introducing the arm arm
v8 pseudocode functions: checkForWFxTrap32 and checkForWFxTrap64

Change-Id: I3db0d78b5c4ad46860e6d199c2f2fc7b41842840
Signed-off-by: Giacomo Travaglini <giacomo.travaglini@arm.com>
Reviewed-by: Andreas Sandberg <andreas.sandberg@arm.com>
Reviewed-on: https://gem5-review.googlesource.com/6622
Maintainer: Andreas Sandberg <andreas.sandberg@arm.com>

3 files changed, 164 insertions, 33 deletions

diff --git a/src/arch/arm/insts/static_inst.cc b/src/arch/arm/insts/static_inst.cc
index 8501715d5..eeda3ad4f 100644
--- a/src/arch/arm/insts/static_inst.cc
+++ b/src/arch/arm/insts/static_inst.cc
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2010-2014, 2016 ARM Limited
+ * Copyright (c) 2010-2014, 2016-2017 ARM Limited
  * Copyright (c) 2013 Advanced Micro Devices, Inc.
  * All rights reserved
  *
@@ -735,6 +735,129 @@ ArmStaticInst::checkAdvSIMDOrFPEnabled32(ThreadContext *tc,
     return NoFault;
 }
 
+inline bool
+ArmStaticInst::isWFxTrapping(ThreadContext *tc,
+                             ExceptionLevel tgtEl,
+                             bool isWfe) const
+{
+    bool trap = false;
+    SCTLR sctlr = ((SCTLR)tc->readMiscReg(MISCREG_SCTLR_EL1));
+    HCR hcr = ((HCR)tc->readMiscReg(MISCREG_HCR_EL2));
+    SCR scr = ((SCR)tc->readMiscReg(MISCREG_SCR_EL3));
+
+    switch (tgtEl) {
+      case EL1:
+        trap = isWfe? !sctlr.ntwe : !sctlr.ntwi;
+        break;
+      case EL2:
+        trap = isWfe? hcr.twe : hcr.twi;
+        break;
+      case EL3:
+        trap = isWfe? scr.twe : scr.twi;
+        break;
+      default:
+        break;
+    }
+
+    return trap;
+}
+
+Fault
+ArmStaticInst::checkForWFxTrap32(ThreadContext *tc,
+                                 ExceptionLevel targetEL,
+                                 bool isWfe) const
+{
+    // Check if target exception level is implemented.
+    assert(ArmSystem::haveEL(tc, targetEL));
+
+    // Check for routing to AArch64: this happens if the
+    // target exception level (where the trap will be handled)
+    // is using aarch64
+    if (ELIs64(tc, targetEL)) {
+        return checkForWFxTrap64(tc, targetEL, isWfe);
+    }
+
+    // Check if processor needs to trap at selected exception level
+    bool trap = isWFxTrapping(tc, targetEL, isWfe);
+
+    if (trap) {
+        uint32_t iss = isWfe? 0x1E00001 : /* WFE Instruction syndrome */
+                              0x1E00000;  /* WFI Instruction syndrome */
+        switch (targetEL) {
+          case EL1:
+            return std::make_shared<UndefinedInstruction>(
+                machInst, iss,
+                EC_TRAPPED_WFI_WFE, mnemonic);
+          case EL2:
+            return std::make_shared<HypervisorTrap>(machInst, iss,
+                                                    EC_TRAPPED_WFI_WFE);
+          case EL3:
+            return std::make_shared<SecureMonitorTrap>(machInst, iss,
+                                                       EC_TRAPPED_WFI_WFE);
+          default:
+            panic("Unrecognized Exception Level: %d\n", targetEL);
+        }
+    }
+
+    return NoFault;
+}
+
+Fault
+ArmStaticInst::checkForWFxTrap64(ThreadContext *tc,
+                                 ExceptionLevel targetEL,
+                                 bool isWfe) const
+{
+    // Check if target exception level is implemented.
+    assert(ArmSystem::haveEL(tc, targetEL));
+
+    // Check if processor needs to trap at selected exception level
+    bool trap = isWFxTrapping(tc, targetEL, isWfe);
+
+    if (trap) {
+        uint32_t iss = isWfe? 0x1E00001 : /* WFE Instruction syndrome */
+                              0x1E00000;  /* WFI Instruction syndrome */
+        switch (targetEL) {
+          case EL1:
+            return std::make_shared<SupervisorTrap>(machInst, iss,
+                                                    EC_TRAPPED_WFI_WFE);
+          case EL2:
+            return std::make_shared<HypervisorTrap>(machInst, iss,
+                                                    EC_TRAPPED_WFI_WFE);
+          case EL3:
+            return std::make_shared<SecureMonitorTrap>(machInst, iss,
+                                                       EC_TRAPPED_WFI_WFE);
+          default:
+            panic("Unrecognized Exception Level: %d\n", targetEL);
+        }
+    }
+
+    return NoFault;
+}
+
+Fault
+ArmStaticInst::trapWFx(ThreadContext *tc,
+                       CPSR cpsr, SCR scr,
+                       bool isWfe) const
+{
+    Fault fault = NoFault;
+    if (cpsr.el == EL0) {
+        fault = checkForWFxTrap32(tc, EL1, isWfe);
+    }
+
+    if ((fault == NoFault) &&
+        ArmSystem::haveEL(tc, EL2) && !inSecureState(scr, cpsr) &&
+        ((cpsr.el == EL0) || (cpsr.el == EL1))) {
+
+        fault = checkForWFxTrap32(tc, EL2, isWfe);
+    }
+
+    if ((fault == NoFault) &&
+        ArmSystem::haveEL(tc, EL3) && cpsr.el != EL3) {
+        fault = checkForWFxTrap32(tc, EL3, isWfe);
+    }
+
+    return fault;
+}
 
 static uint8_t
 getRestoredITBits(ThreadContext *tc, CPSR spsr)
diff --git a/src/arch/arm/insts/static_inst.hh b/src/arch/arm/insts/static_inst.hh
index 7ca1a84b6..9aed77c21 100644
--- a/src/arch/arm/insts/static_inst.hh
+++ b/src/arch/arm/insts/static_inst.hh
@@ -364,6 +364,11 @@ class ArmStaticInst : public StaticInst
                                                       mnemonic, true);
     }
 
+    // Utility function used by checkForWFxTrap32 and checkForWFxTrap64
+    // Returns true if processor has to trap a WFI/WFE instruction.
+    bool isWFxTrapping(ThreadContext *tc,
+                       ExceptionLevel targetEL, bool isWfe) const;
+
     /**
      * Trap an access to Advanced SIMD or FP registers due to access
      * control bits.
@@ -406,6 +411,29 @@ class ArmStaticInst : public StaticInst
                                     bool fpexc_check, bool advsimd) const;
 
     /**
+     * Check if WFE/WFI instruction execution in aarch32 should be trapped.
+     *
+     * See aarch32/exceptions/traps/AArch32.checkForWFxTrap in the
+     * ARM ARM psueodcode library.
+     */
+    Fault checkForWFxTrap32(ThreadContext *tc,
+                            ExceptionLevel tgtEl, bool isWfe) const;
+
+    /**
+     * Check if WFE/WFI instruction execution in aarch64 should be trapped.
+     *
+     * See aarch64/exceptions/traps/AArch64.checkForWFxTrap in the
+     * ARM ARM psueodcode library.
+     */
+    Fault checkForWFxTrap64(ThreadContext *tc,
+                            ExceptionLevel tgtEl, bool isWfe) const;
+
+    /**
+     * WFE/WFI trapping helper function.
+     */
+    Fault trapWFx(ThreadContext *tc, CPSR cpsr, SCR scr, bool isWfe) const;
+
+    /**
      * Get the new PSTATE from a SPSR register in preparation for an
      * exception return.
      *
diff --git a/src/arch/arm/isa/insts/misc.isa b/src/arch/arm/isa/insts/misc.isa
index ef0a6709e..ae8474f79 100644
--- a/src/arch/arm/isa/insts/misc.isa
+++ b/src/arch/arm/isa/insts/misc.isa
@@ -670,10 +670,8 @@ let {{
     exec_output += PredOpExecute.subst(yieldIop)
 
     wfeCode = '''
-    HCR  hcr  = Hcr;
     CPSR cpsr = Cpsr;
     SCR  scr  = Scr64;
-    SCTLR sctlr = Sctlr;
 
     // WFE Sleeps if SevMailbox==0 and no unmasked interrupts are pending,
     ThreadContext *tc = xc->tcBase();
@@ -683,22 +681,13 @@ let {{
     } else if (tc->getCpuPtr()->getInterruptController(
                 tc->threadId())->checkInterrupts(tc)) {
         PseudoInst::quiesceSkip(tc);
-    } else if (cpsr.el == EL0 && !sctlr.ntwe) {
-        PseudoInst::quiesceSkip(tc);
-        fault = std::make_shared<SupervisorTrap>(machInst, 0x1E00001,
-                                                 EC_TRAPPED_WFI_WFE);
-    } else if (ArmSystem::haveVirtualization(tc) &&
-               !inSecureState(scr, cpsr) && (cpsr.mode != MODE_HYP) &&
-               hcr.twe) {
-        PseudoInst::quiesceSkip(tc);
-        fault = std::make_shared<HypervisorTrap>(machInst, 0x1E00001,
-                                                 EC_TRAPPED_WFI_WFE);
-    } else if (ArmSystem::haveSecurity(tc) && cpsr.el != EL3 && scr.twe) {
-        PseudoInst::quiesceSkip(tc);
-        fault = std::make_shared<SecureMonitorTrap>(machInst, 0x1E00001,
-                                                    EC_TRAPPED_WFI_WFE);
     } else {
-        PseudoInst::quiesce(tc);
+        fault = trapWFx(tc, cpsr, scr, true);
+        if (fault == NoFault) {
+            PseudoInst::quiesce(tc);
+        } else {
+            PseudoInst::quiesceSkip(tc);
+        }
     }
     '''
     wfePredFixUpCode = '''
@@ -720,28 +709,19 @@ let {{
     HCR  hcr  = Hcr;
     CPSR cpsr = Cpsr;
     SCR  scr  = Scr64;
-    SCTLR sctlr = Sctlr;
 
     // WFI doesn't sleep if interrupts are pending (masked or not)
     ThreadContext *tc = xc->tcBase();
     if (tc->getCpuPtr()->getInterruptController(
                 tc->threadId())->checkWfiWake(hcr, cpsr, scr)) {
         PseudoInst::quiesceSkip(tc);
-    } else if (cpsr.el == EL0 && !sctlr.ntwi) {
-        PseudoInst::quiesceSkip(tc);
-        fault = std::make_shared<SupervisorTrap>(machInst, 0x1E00000,
-                                                 EC_TRAPPED_WFI_WFE);
-    } else if (ArmSystem::haveVirtualization(tc) && hcr.twi &&
-               (cpsr.mode != MODE_HYP) && !inSecureState(scr, cpsr)) {
-        PseudoInst::quiesceSkip(tc);
-        fault = std::make_shared<HypervisorTrap>(machInst, 0x1E00000,
-                                                 EC_TRAPPED_WFI_WFE);
-    } else if (ArmSystem::haveSecurity(tc) && cpsr.el != EL3 && scr.twi) {
-        PseudoInst::quiesceSkip(tc);
-        fault = std::make_shared<SecureMonitorTrap>(machInst, 0x1E00000,
-                                                    EC_TRAPPED_WFI_WFE);
     } else {
-        PseudoInst::quiesce(tc);
+        fault = trapWFx(tc, cpsr, scr, false);
+        if (fault == NoFault) {
+            PseudoInst::quiesce(tc);
+        } else {
+            PseudoInst::quiesceSkip(tc);
+        }
     }
     tc->getCpuPtr()->clearInterrupt(tc->threadId(), INT_ABT, 0);
     '''