From 84a2dc4ad9fbd342b4b026305bcf2b0cb15bf3fb Mon Sep 17 00:00:00 2001 From: TuxSH Date: Tue, 14 Jan 2020 00:32:09 +0000 Subject: [PATCH] thermosphere: add fpu regs save/restore --- thermosphere/src/fpu.c | 47 ++++++++++++++++++ thermosphere/src/fpu.h | 30 ++++++++++++ thermosphere/src/fpu_regs_load_store.s | 67 ++++++++++++++++++++++++++ thermosphere/src/irq.c | 2 +- thermosphere/src/types.h | 5 ++ 5 files changed, 150 insertions(+), 1 deletion(-) create mode 100644 thermosphere/src/fpu.c create mode 100644 thermosphere/src/fpu.h create mode 100644 thermosphere/src/fpu_regs_load_store.s diff --git a/thermosphere/src/fpu.c b/thermosphere/src/fpu.c new file mode 100644 index 000000000..3f5b4a43d --- /dev/null +++ b/thermosphere/src/fpu.c @@ -0,0 +1,47 @@ +/* + * Copyright (c) 2019 Atmosphère-NX + * + * This program is free software; you can redistribute it and/or modify it + * under the terms and conditions of the GNU General Public License, + * version 2, as published by the Free Software Foundation. + * + * This program is distributed in the hope it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "fpu.h" +#include "execute_function.h" +#include "core_ctx.h" + +FpuRegisterStorage TEMPORARY g_fpuRegisterStorage[4] = { 0 }; + +// fpu_regs_load_store.s +void fpuLoadRegistersFromStorage(const FpuRegisterStorage *storage); +void fpuStoreRegistersToStorage(FpuRegisterStorage *storage); + +static void fpuDumpRegistersImpl(void *p) +{ + (void)p; + fpuStoreRegistersToStorage(&g_fpuRegisterStorage[currentCoreCtx->coreId]); +} + +static void fpuRestoreRegistersImpl(void *p) +{ + (void)p; + fpuLoadRegistersFromStorage(&g_fpuRegisterStorage[currentCoreCtx->coreId]); +} + +void fpuDumpRegisters(u32 coreList) +{ + executeFunctionOnCores(fpuDumpRegistersImpl, NULL, true, coreList); +} + +void fpuRestoreRegisters(u32 coreList) +{ + executeFunctionOnCores(fpuRestoreRegistersImpl, NULL, true, coreList); +} diff --git a/thermosphere/src/fpu.h b/thermosphere/src/fpu.h new file mode 100644 index 000000000..11afc2a62 --- /dev/null +++ b/thermosphere/src/fpu.h @@ -0,0 +1,30 @@ +/* + * Copyright (c) 2019 Atmosphère-NX + * + * This program is free software; you can redistribute it and/or modify it + * under the terms and conditions of the GNU General Public License, + * version 2, as published by the Free Software Foundation. + * + * This program is distributed in the hope it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include "utils.h" + +typedef struct FpuRegisterStorage { + u128 q[32]; + u64 fpsr; + u64 fpcr; +} FpuRegisterStorage; + +extern FpuRegisterStorage g_fpuRegisterStorage[4]; + +void fpuDumpRegisters(u32 coreList); +void fpuRestoreRegisters(u32 coreList); diff --git a/thermosphere/src/fpu_regs_load_store.s b/thermosphere/src/fpu_regs_load_store.s new file mode 100644 index 000000000..e4b2364e0 --- /dev/null +++ b/thermosphere/src/fpu_regs_load_store.s @@ -0,0 +1,67 @@ +/* + * Copyright (c) 2018-2019 Atmosphère-NX + * + * This program is free software; you can redistribute it and/or modify it + * under the terms and conditions of the GNU General Public License, + * version 2, as published by the Free Software Foundation. + * + * This program is distributed in the hope it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +.macro LDSTORE_QREGS, op + \op q0, q1, [x0], 0x20 + \op q2, q3, [x0], 0x20 + \op q4, q5, [x0], 0x20 + \op q6, q7, [x0], 0x20 + \op q8, q9, [x0], 0x20 + \op q10, q11, [x0], 0x20 + \op q12, q13, [x0], 0x20 + \op q14, q15, [x0], 0x20 + \op q16, q17, [x0], 0x20 + \op q18, q19, [x0], 0x20 + \op q20, q21, [x0], 0x20 + \op q22, q23, [x0], 0x20 + \op q24, q25, [x0], 0x20 + \op q26, q27, [x0], 0x20 + \op q28, q29, [x0], 0x20 + \op q30, q31, [x0], 0x20 +.endm + +.section .text.fpuLoadRegistersFromStorage, "ax", %progbits +.global fpuLoadRegistersFromStorage +.type fpuLoadRegistersFromStorage, %function +.func fpuLoadRegistersFromStorage +.cfi_startproc +fpuLoadRegistersFromStorage: + dmb sy + LDSTORE_QREGS ldp + ldp x1, x2, [x0] + msr fpsr, x1 + msr fpcr, x2 + dsb sy + isb sy + ret +.cfi_endproc +.endfunc + +.section .text.fpuStoreRegistersToStorage, "ax", %progbits +.global fpuStoreRegistersToStorage +.type fpuStoreRegistersToStorage, %function +.func fpuStoreRegistersToStorage +.cfi_startproc + dsb sy + isb sy + LDSTORE_QREGS stp + mrs x1, fpsr + mrs x2, fpcr + stp x1, x2, [x0] + dmb sy + ret +.cfi_endproc +.endfunc diff --git a/thermosphere/src/irq.c b/thermosphere/src/irq.c index eb5544b28..8539b003b 100644 --- a/thermosphere/src/irq.c +++ b/thermosphere/src/irq.c @@ -212,7 +212,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 diff --git a/thermosphere/src/types.h b/thermosphere/src/types.h index 68abc2021..2043306d0 100644 --- a/thermosphere/src/types.h +++ b/thermosphere/src/types.h @@ -23,3 +23,8 @@ typedef volatile s8 vs8; ///< 8-bit volatile signed integer. typedef volatile s16 vs16; ///< 16-bit volatile signed integer. typedef volatile s32 vs32; ///< 32-bit volatile signed integer. typedef volatile s64 vs64; ///< 64-bit volatile signed integer. + +typedef __uint128_t u128; ///< 128-bit unsigned integer. +typedef __int128_t s128; ///< 128-bit signed integer. +typedef volatile u128 vu128; ///< 128-bit volatile unsigned integer. +typedef volatile s128 vs128; ///< 128-bit volatile signed integer.