thermosphere: rework fpu register handling

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

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 fpuLoadRegistersFromCache(const FpuRegisterCache *cache);
-void fpuStoreRegistersToStorage(FpuRegisterStorage *storage);
+void fpuStoreRegistersToCache(FpuRegisterCache *cache);
 
-static void fpuDumpRegistersImpl(void *p)
+FpuRegisterCache *fpuGetRegisterCache(void)
 {
-    (void)p;
+    return &g_fpuRegisterCache[currentCoreCtx->coreId];
-    fpuStoreRegistersToStorage(&g_fpuRegisterStorage[currentCoreCtx->coreId]);
 }
 
-static void fpuRestoreRegistersImpl(void *p)
+FpuRegisterCache *fpuReadRegisters(void)
 {
-    (void)p;
+    FpuRegisterCache *cache = &g_fpuRegisterCache[currentCoreCtx->coreId];
-    fpuLoadRegistersFromStorage(&g_fpuRegisterStorage[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;
 }

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);
+FpuRegisterCache *fpuGetRegisterCache(void);
-void fpuRestoreRegisters(u32 coreList);
+FpuRegisterCache *fpuReadRegisters(void);
+void fpuCommitRegisters(void);
+void fpuCleanInvalidateRegisterCache(void);

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

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();
 }