1 files changed, 51 insertions, 7 deletions

diff --git a/src/arch/arm/tlb.cc b/src/arch/arm/tlb.cc
index a499900e0..864f0c28c 100644
--- a/src/arch/arm/tlb.cc
+++ b/src/arch/arm/tlb.cc
@@ -811,7 +811,19 @@ TLB::checkPermissions64(TlbEntry *te, RequestPtr req, Mode mode,
                         "w:%d, x:%d\n", ap, xn, pxn, r, w, x);
 
     if (isStage2) {
-        panic("Virtualization in AArch64 state is not supported yet");
+        assert(ArmSystem::haveVirtualization(tc) && aarch64EL != EL2);
+        // In stage 2 we use the hypervisor access permission bits.
+        // The following permissions are described in ARM DDI 0487A.f
+        // D4-1802
+        uint8_t hap = 0x3 & te->hap;
+        if (is_fetch) {
+            // sctlr.wxn overrides the xn bit
+            grant = !sctlr.wxn && !xn;
+        } else if (is_write) {
+            grant = hap & 0x2;
+        } else { // is_read
+            grant = hap & 0x1;
+        }
     } else {
         switch (aarch64EL) {
           case EL0:
@@ -1233,14 +1245,27 @@ TLB::updateMiscReg(ThreadContext *tc, ArmTranslationType tranType)
             asid = -1;
             break;
         }
+        hcr = tc->readMiscReg(MISCREG_HCR_EL2);
         scr = tc->readMiscReg(MISCREG_SCR_EL3);
         isPriv = aarch64EL != EL0;
-        // @todo: modify this behaviour to support Virtualization in
-        // AArch64
-        vmid           = 0;
-        isHyp          = false;
-        directToStage2 = false;
-        stage2Req      = false;
+        if (haveVirtualization) {
+            vmid           = bits(tc->readMiscReg(MISCREG_VTTBR_EL2), 55, 48);
+            isHyp  =  tranType & HypMode;
+            isHyp &= (tranType & S1S2NsTran) == 0;
+            isHyp &= (tranType & S1CTran)    == 0;
+            // Work out if we should skip the first stage of translation and go
+            // directly to stage 2. This value is cached so we don't have to
+            // compute it for every translation.
+            stage2Req = isStage2 ||
+                        (hcr.vm && !isHyp && !isSecure &&
+                         !(tranType & S1CTran) && (aarch64EL < EL2));
+            directToStage2 = !isStage2 && stage2Req && !sctlr.m;
+        } else {
+            vmid           = 0;
+            isHyp          = false;
+            directToStage2 = false;
+            stage2Req      = false;
+        }
     } else {  // AArch32
         sctlr  = tc->readMiscReg(flattenMiscRegNsBanked(MISCREG_SCTLR, tc,
                                  !isSecure));
@@ -1362,6 +1387,25 @@ TLB::getResultTe(TlbEntry **te, RequestPtr req, ThreadContext *tc, Mode mode,
         TlbEntry *mergeTe)
 {
     Fault fault;
+
+    if (isStage2) {
+        // We are already in the stage 2 TLB. Grab the table entry for stage
+        // 2 only. We are here because stage 1 translation is disabled.
+        TlbEntry *s2Te = NULL;
+        // Get the stage 2 table entry
+        fault = getTE(&s2Te, req, tc, mode, translation, timing, functional,
+                      isSecure, curTranType);
+        // Check permissions of stage 2
+        if ((s2Te != NULL) && (fault = NoFault)) {
+            if(aarch64)
+                fault = checkPermissions64(s2Te, req, mode, tc);
+            else
+                fault = checkPermissions(s2Te, req, mode);
+        }
+        *te = s2Te;
+        return fault;
+    }
+
     TlbEntry *s1Te = NULL;
 
     Addr vaddr_tainted = req->getVaddr();