From aca1e86f45971060dfe00daaa7c714f25a5dcb19 Mon Sep 17 00:00:00 2001
From: TuxSH <tuxsh@sfr.fr>
Date: Sat, 1 Feb 2020 22:09:07 +0000
Subject: [PATCH] thermosphere: fix bug in exceptionReturnPreprocess

---
 thermosphere/src/exceptions.c | 31 +++++++++++++++++--------------
 thermosphere/src/irq.c        |  2 +-
 2 files changed, 18 insertions(+), 15 deletions(-)

diff --git a/thermosphere/src/exceptions.c b/thermosphere/src/exceptions.c
index c92194f29..8d107ff21 100644
--- a/thermosphere/src/exceptions.c
+++ b/thermosphere/src/exceptions.c
@@ -127,21 +127,24 @@ void exceptionEntryPostprocess(ExceptionStackFrame *frame, bool isLowerEl)
 // Called on exception return (avoids overflowing a vector section)
 void exceptionReturnPreprocess(ExceptionStackFrame *frame)
 {
-    if (currentCoreCtx->wasPaused && frame == currentCoreCtx->guestFrame) {
-        // Were we paused & are we about to return to the guest?
-        exceptionEnterInterruptibleHypervisorCode();
-        while (!debugManagerHandlePause());
-        fpuCleanInvalidateRegisterCache();
-    }
-
-    // Update virtual counter
-    currentCoreCtx->totalTimeInHypervisor += timerGetSystemTick() - frame->cntpct_el0;
-    SET_SYSREG(cntvoff_el2, currentCoreCtx->totalTimeInHypervisor);
-
     if (frame == currentCoreCtx->guestFrame) {
-        // Restore interrupt mask
-        SET_SYSREG(cntp_ctl_el0, frame->cntp_ctl_el0);
-        SET_SYSREG(cntv_ctl_el0, frame->cntv_ctl_el0);
+        if (currentCoreCtx->wasPaused) { 
+            // Were we paused & are we about to return to the guest?
+            exceptionEnterInterruptibleHypervisorCode();
+            while (!debugManagerHandlePause());
+            fpuCleanInvalidateRegisterCache();
+        }
+
+        // Update virtual counter
+        u64 ticksNow = timerGetSystemTick();
+        currentCoreCtx->totalTimeInHypervisor += ticksNow - frame->cntpct_el0;
+        SET_SYSREG(cntvoff_el2, currentCoreCtx->totalTimeInHypervisor);
+
+        if (frame == currentCoreCtx->guestFrame) {
+            // Restore interrupt mask
+            SET_SYSREG(cntp_ctl_el0, frame->cntp_ctl_el0);
+            SET_SYSREG(cntv_ctl_el0, frame->cntv_ctl_el0);
+        }
     }
 }
 
diff --git a/thermosphere/src/irq.c b/thermosphere/src/irq.c
index bc73f6156..9f1b69141 100644
--- a/thermosphere/src/irq.c
+++ b/thermosphere/src/irq.c
@@ -206,7 +206,7 @@ void handleIrqException(ExceptionStackFrame *frame, bool isLowerEl, bool isA32)
     u32 irqId = iar & 0x3FF;
     u32 srcCore = (iar >> 10) & 7;
 
-    DEBUG("EL2 [core %d]: Received irq %x\n", (int)currentCoreCtx->coreId, irqId);
+    //DEBUG("EL2 [core %d]: Received irq %x\n", (int)currentCoreCtx->coreId, irqId);
 
     if (irqId == GIC_IRQID_SPURIOUS) {
         // Spurious interrupt received