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