From 067770334e9190ee176af2d588b1ea10127ae70e Mon Sep 17 00:00:00 2001
From: TuxSH <1922548+TuxSH@users.noreply.github.com>
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.