From 97c4595a3a8adf4a2fc021d3c917d00f382ca669 Mon Sep 17 00:00:00 2001
From: TuxSH <1922548+TuxSH@users.noreply.github.com>
Date: Sat, 25 Jan 2020 20:16:10 +0000
Subject: [PATCH] thermosphere: rework fpu register handling

---
 thermosphere/src/exceptions.c          |  3 ++
 thermosphere/src/fpu.c                 | 40 +++++++++++++++++---------
 thermosphere/src/fpu.h                 | 14 +++++----
 thermosphere/src/fpu_regs_load_store.s |  4 +--
 thermosphere/src/main.c                |  5 ++++
 5 files changed, 45 insertions(+), 21 deletions(-)

diff --git a/thermosphere/src/exceptions.c b/thermosphere/src/exceptions.c
index 75c04de62..266197c1c 100644
--- a/thermosphere/src/exceptions.c
+++ b/thermosphere/src/exceptions.c
@@ -27,6 +27,8 @@
 #include "debug_pause.h"
 #include "timer.h"
 
+#include "fpu.h"
+
 bool spsrEvaluateConditionCode(u64 spsr, u32 conditionCode)
 {
     if (conditionCode == 14) {
@@ -121,6 +123,7 @@ void exceptionReturnPreprocess(ExceptionStackFrame *frame)
         // Were we paused & are we about to return to the guest?
         exceptionEnterInterruptibleHypervisorCode();
         debugPauseWaitAndUpdateSingleStep();
+        fpuCleanInvalidateRegisterCache();
     }
 
     // Update virtual counter
diff --git a/thermosphere/src/fpu.c b/thermosphere/src/fpu.c
index 3f5b4a43d..7721342d4 100644
--- a/thermosphere/src/fpu.c
+++ b/thermosphere/src/fpu.c
@@ -15,33 +15,45 @@
  */
 
 #include "fpu.h"
-#include "execute_function.h"
 #include "core_ctx.h"
 
-FpuRegisterStorage TEMPORARY g_fpuRegisterStorage[4] = { 0 };
+static FpuRegisterCache TEMPORARY g_fpuRegisterCache[4] = { 0 };
 
 // fpu_regs_load_store.s
-void fpuLoadRegistersFromStorage(const FpuRegisterStorage *storage);
-void fpuStoreRegistersToStorage(FpuRegisterStorage *storage);
+void fpuLoadRegistersFromCache(const FpuRegisterCache *cache);
+void fpuStoreRegistersToCache(FpuRegisterCache *cache);
 
-static void fpuDumpRegistersImpl(void *p)
+FpuRegisterCache *fpuGetRegisterCache(void)
 {
-    (void)p;
-    fpuStoreRegistersToStorage(&g_fpuRegisterStorage[currentCoreCtx->coreId]);
+    return &g_fpuRegisterCache[currentCoreCtx->coreId];
 }
 
-static void fpuRestoreRegistersImpl(void *p)
+FpuRegisterCache *fpuReadRegisters(void)
 {
-    (void)p;
-    fpuLoadRegistersFromStorage(&g_fpuRegisterStorage[currentCoreCtx->coreId]);
+    FpuRegisterCache *cache = &g_fpuRegisterCache[currentCoreCtx->coreId];
+    if (!cache->valid) {
+        fpuStoreRegistersToCache(cache);
+        cache->valid = true;
+    }
+    return cache;
 }
 
-void fpuDumpRegisters(u32 coreList)
+void fpuCommitRegisters(void)
 {
-    executeFunctionOnCores(fpuDumpRegistersImpl, NULL, true, coreList);
+    FpuRegisterCache *cache = &g_fpuRegisterCache[currentCoreCtx->coreId];
+    cache->dirty = true;
+
+    // Because the caller rewrote the entire cache in the event it didn't read it before:
+    cache->valid = true;
 }
 
-void fpuRestoreRegisters(u32 coreList)
+void fpuCleanInvalidateRegisterCache(void)
 {
-    executeFunctionOnCores(fpuRestoreRegistersImpl, NULL, true, coreList);
+    FpuRegisterCache *cache = &g_fpuRegisterCache[currentCoreCtx->coreId];
+    if (cache->dirty) {
+        fpuLoadRegistersFromCache(cache);
+        cache->dirty = false;
+    }
+
+    cache->valid = false;
 }
diff --git a/thermosphere/src/fpu.h b/thermosphere/src/fpu.h
index 11afc2a62..18769d265 100644
--- a/thermosphere/src/fpu.h
+++ b/thermosphere/src/fpu.h
@@ -18,13 +18,17 @@
 
 #include "utils.h"
 
-typedef struct FpuRegisterStorage {
+typedef struct FpuRegisterCache {
     u128    q[32];
     u64     fpsr;
     u64     fpcr;
-} FpuRegisterStorage;
+    bool    valid;
+    bool    dirty;
+} FpuRegisterCache;
 
-extern FpuRegisterStorage g_fpuRegisterStorage[4];
+// Only for the current core:
 
-void fpuDumpRegisters(u32 coreList);
-void fpuRestoreRegisters(u32 coreList);
+FpuRegisterCache *fpuGetRegisterCache(void);
+FpuRegisterCache *fpuReadRegisters(void);
+void fpuCommitRegisters(void);
+void fpuCleanInvalidateRegisterCache(void);
diff --git a/thermosphere/src/fpu_regs_load_store.s b/thermosphere/src/fpu_regs_load_store.s
index 0959c1cb9..1f12ed6c9 100644
--- a/thermosphere/src/fpu_regs_load_store.s
+++ b/thermosphere/src/fpu_regs_load_store.s
@@ -35,7 +35,7 @@
     \op     q30, q31, [x0], 0x20
 .endm
 
-FUNCTION fpuLoadRegistersFromStorage
+FUNCTION fpuLoadRegistersFromCache
     dmb     ish
     LDSTORE_QREGS ldp
     ldp     x1, x2, [x0]
@@ -46,7 +46,7 @@ FUNCTION fpuLoadRegistersFromStorage
     ret
 END_FUNCTION
 
-FUNCTION fpuStoreRegistersToStorage
+FUNCTION fpuStoreRegistersToCache
     dsb     ish
     isb
     LDSTORE_QREGS stp
diff --git a/thermosphere/src/main.c b/thermosphere/src/main.c
index 76dea50c2..56f915299 100644
--- a/thermosphere/src/main.c
+++ b/thermosphere/src/main.c
@@ -15,6 +15,7 @@
 #include "irq.h"
 #include "transport_interface.h"
 #include "guest_memory.h"
+#include "fpu.h"
 
 #include "memory_map.h"
 #include "mmu.h"
@@ -128,4 +129,8 @@ void thermosphereMain(ExceptionStackFrame *frame, u64 pct)
     frame->elr_el2      = currentCoreCtx->kernelEntrypoint;
     frame->x[0]         = currentCoreCtx->kernelArgument;
     frame->cntpct_el0   = pct;
+
+    // Initialize FPU registers -- no need to memset, the regcaches are in .tempbss
+    fpuCommitRegisters();
+    fpuCleanInvalidateRegisterCache();
 }