1 files changed, 21 insertions, 14 deletions
diff --git a/UefiCpuPkg/CpuTimerDxeRiscV64/Timer.c b/UefiCpuPkg/CpuTimerDxeRiscV64/Timer.c
index db153f715e..287e7e1330 100644
--- a/UefiCpuPkg/CpuTimerDxeRiscV64/Timer.c
+++ b/UefiCpuPkg/CpuTimerDxeRiscV64/Timer.c
@@ -40,7 +40,8 @@ STATIC EFI_TIMER_NOTIFY  mTimerNotifyFunction;
 //
 // The current period of the timer interrupt
 //
-STATIC UINT64  mTimerPeriod = 0;
+STATIC UINT64  mTimerPeriod     = 0;
+STATIC UINT64  mLastPeriodStart = 0;
 
 /**
   Timer Interrupt Handler.
@@ -56,25 +57,33 @@ TimerInterruptHandler (
   )
 {
   EFI_TPL  OriginalTPL;
-  UINT64   RiscvTimer;
+  UINT64   PeriodStart;
+
+  PeriodStart = RiscVReadTimer ();
 
   OriginalTPL = gBS->RaiseTPL (TPL_HIGH_LEVEL);
   if (mTimerNotifyFunction != NULL) {
-    mTimerNotifyFunction (mTimerPeriod);
+    //
+    // For any number of reasons, the ticks could be coming
+    // in slower than mTimerPeriod. For example, some code
+    // is doing a *lot* of stuff inside an EFI_TPL_HIGH
+    // critical section, and this should not cause the EFI
+    // time to increment slower. So when we take an interrupt,
+    // account for the actual time passed.
+    //
+    mTimerNotifyFunction (PeriodStart - mLastPeriodStart);
   }
 
-  RiscVDisableTimerInterrupt (); // Disable SMode timer int
-  RiscVClearPendingTimerInterrupt ();
   if (mTimerPeriod == 0) {
+    RiscVDisableTimerInterrupt ();
     gBS->RestoreTPL (OriginalTPL);
-    RiscVDisableTimerInterrupt (); // Disable SMode timer int
     return;
   }
 
-  RiscvTimer               = RiscVReadTimer ();
-  SbiSetTimer (RiscvTimer += mTimerPeriod);
-  gBS->RestoreTPL (OriginalTPL);
+  mLastPeriodStart          = PeriodStart;
+  SbiSetTimer (PeriodStart += mTimerPeriod);
   RiscVEnableTimerInterrupt (); // enable SMode timer int
+  gBS->RestoreTPL (OriginalTPL);
 }
 
 /**
@@ -154,8 +163,6 @@ TimerDriverSetTimerPeriod (
   IN UINT64                   TimerPeriod
   )
 {
-  UINT64  RiscvTimer;
-
   DEBUG ((DEBUG_INFO, "TimerDriverSetTimerPeriod(0x%lx)\n", TimerPeriod));
 
   if (TimerPeriod == 0) {
@@ -164,9 +171,9 @@ TimerDriverSetTimerPeriod (
     return EFI_SUCCESS;
   }
 
-  mTimerPeriod = TimerPeriod / 10; // convert unit from 100ns to 1us
-  RiscvTimer   = RiscVReadTimer ();
-  SbiSetTimer (RiscvTimer + mTimerPeriod);
+  mTimerPeriod     = TimerPeriod / 10; // convert unit from 100ns to 1us
+  mLastPeriodStart = RiscVReadTimer ();
+  SbiSetTimer (mLastPeriodStart + mTimerPeriod);
 
   mCpu->EnableInterrupt (mCpu);
   RiscVEnableTimerInterrupt (); // enable SMode timer int