3 files changed, 15 insertions, 5 deletions

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);
         }
     }
     '''