1 files changed, 29 insertions, 6 deletions

diff --git a/src/arch/arm/locked_mem.hh b/src/arch/arm/locked_mem.hh
index f2601f00c..24c78e721 100644
--- a/src/arch/arm/locked_mem.hh
+++ b/src/arch/arm/locked_mem.hh
@@ -53,6 +53,8 @@
  */
 
 #include "arch/arm/miscregs.hh"
+#include "arch/arm/isa_traits.hh"
+#include "debug/LLSC.hh"
 #include "mem/packet.hh"
 #include "mem/request.hh"
 
@@ -62,31 +64,48 @@ template <class XC>
 inline void
 handleLockedSnoop(XC *xc, PacketPtr pkt, Addr cacheBlockMask)
 {
+    DPRINTF(LLSC,"%s:  handleing snoop for address: %#x locked: %d\n",
+            xc->getCpuPtr()->name(),pkt->getAddr(),
+            xc->readMiscReg(MISCREG_LOCKFLAG));
     if (!xc->readMiscReg(MISCREG_LOCKFLAG))
         return;
 
     Addr locked_addr = xc->readMiscReg(MISCREG_LOCKADDR) & cacheBlockMask;
+    // If no caches are attached, the snoop address always needs to be masked
     Addr snoop_addr = pkt->getAddr() & cacheBlockMask;
 
-    if (locked_addr == snoop_addr)
+    DPRINTF(LLSC,"%s:  handleing snoop for address: %#x locked addr: %#x\n",
+            xc->getCpuPtr()->name(),snoop_addr, locked_addr);
+    if (locked_addr == snoop_addr) {
+        DPRINTF(LLSC,"%s: address match, clearing lock and signaling sev\n",
+                xc->getCpuPtr()->name());
         xc->setMiscReg(MISCREG_LOCKFLAG, false);
+        // Implement ARMv8 WFE/SEV semantics
+        xc->setMiscReg(MISCREG_SEV_MAILBOX, true);
+        xc->getCpuPtr()->wakeup();
+    }
 }
 
 template <class XC>
 inline void
-handleLockedSnoopHit(XC *xc)
+handleLockedRead(XC *xc, Request *req)
 {
+    xc->setMiscReg(MISCREG_LOCKADDR, req->getPaddr());
+    xc->setMiscReg(MISCREG_LOCKFLAG, true);
+    DPRINTF(LLSC,"%s: Placing address %#x in monitor\n", xc->getCpuPtr()->name(),
+                 req->getPaddr());
 }
 
 template <class XC>
 inline void
-handleLockedRead(XC *xc, Request *req)
+handleLockedSnoopHit(XC *xc)
 {
-    xc->setMiscReg(MISCREG_LOCKADDR, req->getPaddr());
-    xc->setMiscReg(MISCREG_LOCKFLAG, true);
+    DPRINTF(LLSC,"%s:  handling snoop lock hit address: %#x\n",
+            xc->getCpuPtr()->name(), xc->readMiscReg(MISCREG_LOCKADDR));
+        xc->setMiscReg(MISCREG_LOCKFLAG, false);
+        xc->setMiscReg(MISCREG_SEV_MAILBOX, true);
 }
 
-
 template <class XC>
 inline bool
 handleLockedWrite(XC *xc, Request *req, Addr cacheBlockMask)
@@ -94,6 +113,8 @@ handleLockedWrite(XC *xc, Request *req, Addr cacheBlockMask)
     if (req->isSwap())
         return true;
 
+    DPRINTF(LLSC,"%s: handling locked write for  address %#x in monitor\n",
+            xc->getCpuPtr()->name(), req->getPaddr());
     // Verify that the lock flag is still set and the address
     // is correct
     bool lock_flag = xc->readMiscReg(MISCREG_LOCKFLAG);
@@ -103,6 +124,8 @@ handleLockedWrite(XC *xc, Request *req, Addr cacheBlockMask)
         // don't even bother sending to memory system
         req->setExtraData(0);
         xc->setMiscReg(MISCREG_LOCKFLAG, false);
+        DPRINTF(LLSC,"%s: clearing lock flag in handle locked write\n",
+                xc->getCpuPtr()->name());
         // the rest of this code is not architectural;
         // it's just a debugging aid to help detect
         // livelock by warning on long sequences of failed