6 files changed, 77 insertions, 7 deletions

diff --git a/src/arch/arm/faults.cc b/src/arch/arm/faults.cc
index 3bd9f070c..3c361404e 100644
--- a/src/arch/arm/faults.cc
+++ b/src/arch/arm/faults.cc
@@ -78,6 +78,8 @@ template<> ArmFault::FaultVals ArmFaultVals<FlushPipe>::vals =
 template<> ArmFault::FaultVals ArmFaultVals<ReExec>::vals =
     {"ReExec Flush", 0x00, MODE_SVC, 0, 0, true, true}; // some dummy values
 
+template<> ArmFault::FaultVals ArmFaultVals<ArmSev>::vals =
+    {"ArmSev Flush", 0x00, MODE_SVC, 0, 0, true, true}; // some dummy values
 Addr 
 ArmFault::getVector(ThreadContext *tc)
 {
@@ -127,6 +129,8 @@ ArmFault::invoke(ThreadContext *tc, StaticInstPtr inst)
     cpsr.i = 1;
     cpsr.e = sctlr.ee;
     tc->setMiscReg(MISCREG_CPSR, cpsr);
+    // Make sure mailbox sets to one always
+    tc->setMiscReg(MISCREG_SEV_MAILBOX, 1);
     tc->setIntReg(INTREG_LR, curPc +
             (saved_cpsr.t ? thumbPcOffset() : armPcOffset()));
 
@@ -252,6 +256,18 @@ template void AbortFault<PrefetchAbort>::invoke(ThreadContext *tc,
 template void AbortFault<DataAbort>::invoke(ThreadContext *tc,
                                             StaticInstPtr inst);
 
+void
+ArmSev::invoke(ThreadContext *tc, StaticInstPtr inst) {
+    DPRINTF(Faults, "Invoking ArmSev Fault\n");
+#if FULL_SYSTEM
+    // Set sev_mailbox to 1, clear the pending interrupt from remote
+    // SEV execution and let pipeline continue as pcState is still
+    // valid.
+    tc->setMiscReg(MISCREG_SEV_MAILBOX, 1);
+    tc->getCpuPtr()->clearInterrupt(INT_SEV, 0);
+#endif
+}
+
 // return via SUBS pc, lr, xxx; rfe, movs, ldm
 
 } // namespace ArmISA
diff --git a/src/arch/arm/faults.hh b/src/arch/arm/faults.hh
index 234d8cfec..54edb336b 100644
--- a/src/arch/arm/faults.hh
+++ b/src/arch/arm/faults.hh
@@ -257,6 +257,15 @@ static inline Fault genMachineCheckFault()
     return new Reset();
 }
 
+// A fault that flushes the pipe, excluding the faulting instructions
+class ArmSev : public ArmFaultVals<ArmSev>
+{
+  public:
+    ArmSev () {}
+    void invoke(ThreadContext *tc,
+            StaticInstPtr inst = StaticInst::nullStaticInstPtr);
+};
+
 } // namespace ArmISA
 
 #endif // __ARM_FAULTS_HH__
diff --git a/src/arch/arm/interrupts.hh b/src/arch/arm/interrupts.hh
index 16a5a1f3d..82c0bb713 100644
--- a/src/arch/arm/interrupts.hh
+++ b/src/arch/arm/interrupts.hh
@@ -134,7 +134,8 @@ class Interrupts : public SimObject
         return ((interrupts[INT_IRQ] && !cpsr.i) ||
                 (interrupts[INT_FIQ] && !cpsr.f) ||
                 (interrupts[INT_ABT] && !cpsr.a) ||
-                (interrupts[INT_RST]));
+                (interrupts[INT_RST]) ||
+                (interrupts[INT_SEV]));
     }
 
     /**
@@ -167,6 +168,8 @@ class Interrupts : public SimObject
                     ArmFault::AsynchronousExternalAbort);
         if (interrupts[INT_RST])
            return new Reset;
+        if (interrupts[INT_SEV])
+           return new ArmSev;
 
         panic("intStatus and interrupts not in sync\n");
     }
diff --git a/src/arch/arm/isa/insts/misc.isa b/src/arch/arm/isa/insts/misc.isa
index 4a4f1e314..4d8ea66a2 100644
--- a/src/arch/arm/isa/insts/misc.isa
+++ b/src/arch/arm/isa/insts/misc.isa
@@ -502,20 +502,32 @@ let {{
 
     wfeCode = '''
 #if FULL_SYSTEM
+    // WFE Sleeps if SevMailbox==0 and no unmasked interrupts are pending
     if (SevMailbox == 1) {
         SevMailbox = 0;
         PseudoInst::quiesceSkip(xc->tcBase());
+    } else if (xc->tcBase()->getCpuPtr()->getInterruptController()->checkInterrupts(xc->tcBase())) {
+        PseudoInst::quiesceSkip(xc->tcBase());
     } else {
         PseudoInst::quiesce(xc->tcBase());
     }
 #endif
     '''
+    wfePredFixUpCode = '''
+#if FULL_SYSTEM
+    // WFE is predicated false, reset SevMailbox to reduce spurious sleeps
+    // and SEV interrupts
+    SevMailbox = 1;
+#endif
+    '''
     wfeIop = InstObjParams("wfe", "WfeInst", "PredOp", \
-            { "code" : wfeCode, "predicate_test" : predicateTest },
+            { "code" : wfeCode,
+              "pred_fixup" : wfePredFixUpCode,
+              "predicate_test" : predicateTest },
             ["IsNonSpeculative", "IsQuiesce", "IsSerializeAfter"])
     header_output += BasicDeclare.subst(wfeIop)
     decoder_output += BasicConstructor.subst(wfeIop)
-    exec_output += QuiescePredOpExecute.subst(wfeIop)
+    exec_output += QuiescePredOpExecuteWithFixup.subst(wfeIop)
 
     wfiCode = '''
 #if FULL_SYSTEM
@@ -535,19 +547,20 @@ let {{
     exec_output += QuiescePredOpExecute.subst(wfiIop)
 
     sevCode = '''
-    // Need a way for O3 to not scoreboard these accesses as pipe flushes.
+#if FULL_SYSTEM
     SevMailbox = 1;
     System *sys = xc->tcBase()->getSystemPtr();
     for (int x = 0; x < sys->numContexts(); x++) {
         ThreadContext *oc = sys->getThreadContext(x);
         if (oc == xc->tcBase())
             continue;
-        // Only wake if they were sleeping
+        // Wake CPU with interrupt if they were sleeping
         if (oc->readMiscReg(MISCREG_SEV_MAILBOX) == 0) {
-            oc->setMiscReg(MISCREG_SEV_MAILBOX, 1);
-            PseudoInst::wakeCPU(xc->tcBase(), x);
+            // Post Interrupt and wake cpu if needed
+            oc->getCpuPtr()->postInterrupt(INT_SEV, 0);
         }
     }
+#endif
     '''
     sevIop = InstObjParams("sev", "SevInst", "PredOp", \
             { "code" : sevCode, "predicate_test" : predicateTest },
diff --git a/src/arch/arm/isa/templates/pred.isa b/src/arch/arm/isa/templates/pred.isa
index 04f253ca9..8b7ff69e7 100644
--- a/src/arch/arm/isa/templates/pred.isa
+++ b/src/arch/arm/isa/templates/pred.isa
@@ -205,6 +205,34 @@ def template QuiescePredOpExecute {{
     }
 }};
 
+def template QuiescePredOpExecuteWithFixup {{
+    Fault %(class_name)s::execute(%(CPU_exec_context)s *xc, Trace::InstRecord *traceData) const
+    {
+        Fault fault = NoFault;
+        uint64_t resTemp = 0;
+        resTemp = resTemp;
+        %(op_decl)s;
+        %(op_rd)s;
+
+        if (%(predicate_test)s)
+        {
+            %(code)s;
+            if (fault == NoFault)
+            {
+                %(op_wb)s;
+            }
+        } else {
+            xc->setPredicate(false);
+            %(pred_fixup)s;
+#if FULL_SYSTEM
+            PseudoInst::quiesceSkip(xc->tcBase());
+#endif
+        }
+
+        return fault;
+    }
+}};
+
 def template DataDecode {{
     if (machInst.opcode4 == 0) {
         if (machInst.sField == 0)
diff --git a/src/arch/arm/isa_traits.hh b/src/arch/arm/isa_traits.hh
index c89c9abce..40371e065 100644
--- a/src/arch/arm/isa_traits.hh
+++ b/src/arch/arm/isa_traits.hh
@@ -121,6 +121,7 @@ namespace ArmISA
         INT_ABT,
         INT_IRQ,
         INT_FIQ,
+        INT_SEV, // Special interrupt for recieving SEV's
         NumInterruptTypes
     };
 } // namespace ArmISA