5 files changed, 29 insertions, 6 deletions

diff --git a/src/arch/arm/interrupts.hh b/src/arch/arm/interrupts.hh
index 884bb7b9f..16a5a1f3d 100644
--- a/src/arch/arm/interrupts.hh
+++ b/src/arch/arm/interrupts.hh
@@ -137,6 +137,19 @@ class Interrupts : public SimObject
                 (interrupts[INT_RST]));
     }
 
+    /**
+     * Check the raw interrupt state.
+     * This function is used to check if a wfi operation should sleep. If there
+     * is an interrupt pending, even if it's masked, wfi doesn't sleep.
+     * @return any interrupts pending
+     */
+    bool
+    checkRaw() const
+    {
+        return intStatus;
+    }
+
+
     Fault
     getInterrupt(ThreadContext *tc)
     {
diff --git a/src/arch/arm/isa/insts/data.isa b/src/arch/arm/isa/insts/data.isa
index e8012ff89..5b1526e41 100644
--- a/src/arch/arm/isa/insts/data.isa
+++ b/src/arch/arm/isa/insts/data.isa
@@ -247,6 +247,7 @@ let {{
             NextJazelle = ((CPSR)newCpsr).j;
             NextItState = ((((CPSR)newCpsr).it2 << 2) & 0xFC)
                 | (((CPSR)newCpsr).it1 & 0x3);
+            SevMailbox = 1;
             '''
             buildImmDataInst(mnem + 's', code, flagType,
                              suffix = "ImmPclr", buildCc = False,
diff --git a/src/arch/arm/isa/insts/macromem.isa b/src/arch/arm/isa/insts/macromem.isa
index 28b140b93..67d8da572 100644
--- a/src/arch/arm/isa/insts/macromem.isa
+++ b/src/arch/arm/isa/insts/macromem.isa
@@ -96,6 +96,7 @@ let {{
         IWNPC = cSwap(%s, cpsr.e) | ((Spsr & 0x20) ? 1 : 0);
         NextItState = ((((CPSR)Spsr).it2 << 2) & 0xFC)
                 | (((CPSR)Spsr).it1 & 0x3);
+        SevMailbox = 1;
     '''
 
     microLdrRetUopIop = InstObjParams('ldr_ret_uop', 'MicroLdrRetUop',
diff --git a/src/arch/arm/isa/insts/misc.isa b/src/arch/arm/isa/insts/misc.isa
index 7333faef0..a9a375213 100644
--- a/src/arch/arm/isa/insts/misc.isa
+++ b/src/arch/arm/isa/insts/misc.isa
@@ -483,11 +483,10 @@ let {{
 
     wfeCode = '''
 #if FULL_SYSTEM
-    if (SevMailbox) {
+    if (SevMailbox == 1) {
         SevMailbox = 0;
         PseudoInst::quiesceSkip(xc->tcBase());
-    }
-    else {
+    } else {
         PseudoInst::quiesce(xc->tcBase());
     }
 #endif
@@ -501,7 +500,12 @@ let {{
 
     wfiCode = '''
 #if FULL_SYSTEM
-    PseudoInst::quiesce(xc->tcBase());
+    // WFI doesn't sleep if interrupts are pending (masked or not)
+    if (xc->tcBase()->getCpuPtr()->getInterruptController()->checkRaw()) {
+        PseudoInst::quiesceSkip(xc->tcBase());
+    } else {
+        PseudoInst::quiesce(xc->tcBase());
+    }
 #endif
     '''
     wfiIop = InstObjParams("wfi", "WfiInst", "PredOp", \
@@ -517,8 +521,12 @@ let {{
     System *sys = xc->tcBase()->getSystemPtr();
     for (int x = 0; x < sys->numContexts(); x++) {
         ThreadContext *oc = sys->getThreadContext(x);
-        if (oc != xc->tcBase()) {
+        if (oc == xc->tcBase())
+            continue;
+        // Only wake if they were sleeping
+        if (oc->readMiscReg(MISCREG_SEV_MAILBOX) == 0) {
             oc->setMiscReg(MISCREG_SEV_MAILBOX, 1);
+            PseudoInst::wakeCPU(xc->tcBase(), x);
         }
     }
     '''
diff --git a/src/arch/arm/system.hh b/src/arch/arm/system.hh
index 8711e7cce..e63d5c0ed 100644
--- a/src/arch/arm/system.hh
+++ b/src/arch/arm/system.hh
@@ -53,7 +53,7 @@
 
 class ArmSystem : public System
 {
-  private:
+  protected:
     /**
      * PC based event to skip the dprink() call and emulate its
      * functionality