diff --git a/ArmPkg/Library/ArmLib/AArch64/AArch64ArchTimerSupport.S b/ArmPkg/Library/ArmLib/AArch64/AArch64ArchTimerSupport.S
index be6f073c48..acf49c1eda 100644
--- a/ArmPkg/Library/ArmLib/AArch64/AArch64ArchTimerSupport.S
+++ b/ArmPkg/Library/ArmLib/AArch64/AArch64ArchTimerSupport.S
@@ -9,6 +9,41 @@
 
 #include <AsmMacroLib.h>
 
+/* Special accessors for VHE */
+#define cntp_tval_el02          s3_5_c14_c2_0
+#define cntp_ctl_el02           s3_5_c14_c2_1
+#define cntp_cval_el02          s3_5_c14_c2_2
+#define cntv_tval_el02          s3_5_c14_c3_0
+#define cntv_ctl_el02           s3_5_c14_c3_1
+#define cntv_cval_el02          s3_5_c14_c3_2
+
+#define RCNT(reg, sysreg)       rvhe reg, sysreg, sysreg ## 2
+#define wCNT(sysreg, reg, tmp)  wvhe sysreg, reg, sysreg ## 2, tmp
+
+        /* Read from 'altname' when running at EL2+VHE, or 'sysreg' otherwise */
+        .macro  rvhe, reg, sysreg, altname
+        mrs     \reg, CurrentEL
+        tbz     \reg, #3, .L\@
+        mrs     \reg, hcr_el2
+        tbz     \reg, #34, .L\@   // TCR.E2H
+        mrs     \reg, \altname
+        ret
+.L\@:   mrs     \reg, \sysreg
+        ret
+        .endm
+
+        /* Write to 'altname' when running at EL2+VHE, or 'sysreg' otherwise */
+        .macro  wvhe, sysreg, reg, altname, tmp
+        mrs     \tmp, CurrentEL
+        tbz     \tmp, #3, .L\@
+        mrs     \tmp, hcr_el2
+        tbz     \tmp, #34, .L\@   // TCR.E2H
+        msr     \altname, \reg
+        ret
+.L\@:   msr     \sysreg, \reg
+        ret
+        .endm
+
 ASM_FUNC(ArmReadCntFrq)
   mrs   x0, cntfrq_el0           // Read CNTFRQ
   ret
@@ -36,68 +71,55 @@ ASM_FUNC(ArmWriteCntkCtl)
 
 
 ASM_FUNC(ArmReadCntpTval)
-  mrs   x0, cntp_tval_el0        // Read CNTP_TVAL (PL1 physical timer value register)
-  ret
+  RCNT  (x0, cntp_tval_el0)      // Read CNTP_TVAL (PL1 physical timer value register)
 
 
 ASM_FUNC(ArmWriteCntpTval)
-  msr   cntp_tval_el0, x0        // Write to CNTP_TVAL (PL1 physical timer value register)
-  ret
+  wCNT  (cntp_tval_el0, x0, x1)  // Write to CNTP_TVAL (PL1 physical timer value register)
 
 
 ASM_FUNC(ArmReadCntpCtl)
-  mrs   x0, cntp_ctl_el0         // Read CNTP_CTL (PL1 Physical Timer Control Register)
-  ret
+  RCNT  (x0, cntp_ctl_el0)       // Read CNTP_CTL (PL1 Physical Timer Control Register)
 
 
 ASM_FUNC(ArmWriteCntpCtl)
-  msr   cntp_ctl_el0, x0         // Write to  CNTP_CTL (PL1 Physical Timer Control Register)
-  ret
+  wCNT  (cntp_ctl_el0, x0, x1)   // Write to  CNTP_CTL (PL1 Physical Timer Control Register)
 
 
 ASM_FUNC(ArmReadCntvTval)
-  mrs   x0, cntv_tval_el0        // Read CNTV_TVAL (Virtual Timer Value register)
-  ret
+  RCNT  (x0, cntv_tval_el0)      // Read CNTV_TVAL (Virtual Timer Value register)
 
 
 ASM_FUNC(ArmWriteCntvTval)
-  msr   cntv_tval_el0, x0        // Write to CNTV_TVAL (Virtual Timer Value register)
-  ret
+  wCNT  (cntv_tval_el0, x0, x1) // Write to CNTV_TVAL (Virtual Timer Value register)
 
 
 ASM_FUNC(ArmReadCntvCtl)
-  mrs   x0, cntv_ctl_el0         // Read CNTV_CTL (Virtual Timer Control Register)
-  ret
+  RCNT  (x0, cntv_ctl_el0)       // Read CNTV_CTL (Virtual Timer Control Register)
 
 
 ASM_FUNC(ArmWriteCntvCtl)
-  msr   cntv_ctl_el0, x0         // Write to CNTV_CTL (Virtual Timer Control Register)
-  ret
+  wCNT  (cntv_ctl_el0, x0, x1)   // Write to CNTV_CTL (Virtual Timer Control Register)
 
 
 ASM_FUNC(ArmReadCntvCt)
   mrs  x0, cntvct_el0            // Read CNTVCT  (Virtual Count Register)
   ret
 
-
 ASM_FUNC(ArmReadCntpCval)
-  mrs   x0, cntp_cval_el0        // Read CNTP_CTVAL (Physical Timer Compare Value Register)
-  ret
+  RCNT  (x0, cntp_cval_el0)      // Read CNTP_CTVAL (Physical Timer Compare Value Register)
 
 
 ASM_FUNC(ArmWriteCntpCval)
-  msr   cntp_cval_el0, x0        // Write to CNTP_CTVAL (Physical Timer Compare Value Register)
-  ret
+  wCNT  (cntp_cval_el0, x0, x1)  // Write to CNTP_CTVAL (Physical Timer Compare Value Register)
 
 
 ASM_FUNC(ArmReadCntvCval)
-  mrs   x0, cntv_cval_el0        // Read CNTV_CTVAL (Virtual Timer Compare Value Register)
-  ret
+  RCNT  (x0, cntv_cval_el0)      // Read CNTV_CTVAL (Virtual Timer Compare Value Register)
 
 
 ASM_FUNC(ArmWriteCntvCval)
-  msr   cntv_cval_el0, x0        // write to  CNTV_CTVAL (Virtual Timer Compare Value Register)
-  ret
+  wCNT  (cntv_cval_el0, x0, x1)  // write to  CNTV_CTVAL (Virtual Timer Compare Value Register)
 
 
 ASM_FUNC(ArmReadCntvOff)