[PATCH linux-evl v2 2/2] riscv: Add FPU register test support

From: shannmu <[email protected]>

Define evl_set_fpregs() and evl_check_fpregs(), allowing low-level
manipulation and validation of FPU register state.

Signed-off-by: Tobias Schaffner <[email protected]>
---
 arch/riscv/include/asm/evl/fptest.h      |  13 ++-
 arch/riscv/include/uapi/asm/evl/fptest.h | 133 ++++++++++++++++++++++-
 2 files changed, 137 insertions(+), 9 deletions(-)

diff --git a/arch/riscv/include/asm/evl/fptest.h b/arch/riscv/include/asm/evl/fptest.h
index 987246edac69..d9a4ae9cbde8 100644
--- a/arch/riscv/include/asm/evl/fptest.h
+++ b/arch/riscv/include/asm/evl/fptest.h
@@ -3,20 +3,27 @@
 #define _EVL_RISCV_ASM_FPTEST_H
 
 #include <linux/cpufeature.h>
+#include <asm/fpu.h>
 #include <uapi/asm/evl/fptest.h>
 
 static inline bool evl_begin_fpu(void)
 {
-	return false;
+	kernel_fpu_begin();
+
+	return true;
 }
 
-static inline void evl_end_fpu(void) { }
+static inline void evl_end_fpu(void)
+{
+	kernel_fpu_end();
+}
 
 static inline u32 evl_detect_fpu(void)
 {
 	u32 features = 0;
 
-	/* TODO: Check if we have any FPU features. Not using any for now. */
+	if (has_fpu())
+		return features = evl_riscv_fpsimd;
 
 	return features;
 }
diff --git a/arch/riscv/include/uapi/asm/evl/fptest.h b/arch/riscv/include/uapi/asm/evl/fptest.h
index 9169fc5c1b50..7be41bc4e453 100644
--- a/arch/riscv/include/uapi/asm/evl/fptest.h
+++ b/arch/riscv/include/uapi/asm/evl/fptest.h
@@ -1,17 +1,138 @@
 /*
- * SPDX-License-Identifier: GPL-2.0
+ * SPDX-License-Identifier: GPL-2.0 WITH Linux-syscall-note
  *
- * Copyright (C) 2024 Tobias Schaffner <[email protected]>.
+ * Derived from Xenomai Cobalt, https://xenomai.org/
+ * Copyright (C) 2006 Gilles Chanteperdrix <[email protected]>.
  */
 #ifndef _EVL_RISCV_ASM_UAPI_FPTEST_H
 #define _EVL_RISCV_ASM_UAPI_FPTEST_H
 
 #include <linux/types.h>
 
-#define evl_riscv_fp  0x1
+#define evl_riscv_fpsimd  0x1
+#define evl_riscv_sve	 0x2
 
-/* TODO: Implement fptest */
-#define evl_set_fpregs(__features, __val)
-#define evl_check_fpregs(__features, __val, __bad) 0
+/*
+ * CAUTION: keep this code strictly inlined in macros: we don't want
+ * GCC to apply any callee-saved logic to fpsimd registers in
+ * evl_set_fpregs() before evl_check_fpregs() can verify their
+ * contents, but we still want GCC to know about the registers we have
+ * clobbered.
+ */
+
+#define evl_set_fpregs(__features, __val)				\
+	do {								\
+		unsigned int __i;					\
+		__u64 __e[32];						\
+									\
+		if (__features & evl_riscv_fpsimd) {			\
+			for (__i = 0; __i < 32; __i++)			\
+				__e[__i] = (__val);			\
+			__asm__ __volatile__(				\
+				".option push\n"			\
+				".option arch, +d\n"			\
+				"fld  f0, 0(%0)  \n"			\
+				"fld  f1, 8(%0)  \n"			\
+				"fld  f2, 16(%0) \n"			\
+				"fld  f3, 24(%0) \n"			\
+				"fld  f4, 32(%0) \n"			\
+				"fld  f5, 40(%0) \n"			\
+				"fld  f6, 48(%0) \n"			\
+				"fld  f7, 56(%0) \n"			\
+				"fld  f8, 64(%0) \n"			\
+				"fld  f9, 72(%0) \n"			\
+				"fld  f10, 80(%0) \n"			\
+				"fld  f11, 88(%0) \n"			\
+				"fld  f12, 96(%0) \n"			\
+				"fld  f13, 104(%0) \n"			\
+				"fld  f14, 112(%0) \n"			\
+				"fld  f15, 120(%0) \n"			\
+				"fld  f16, 128(%0) \n"			\
+				"fld  f17, 136(%0) \n"			\
+				"fld  f18, 144(%0) \n"			\
+				"fld  f19, 152(%0) \n"			\
+				"fld  f20, 160(%0) \n"			\
+				"fld  f21, 168(%0) \n"			\
+				"fld  f22, 176(%0) \n"			\
+				"fld  f23, 184(%0) \n"			\
+				"fld  f24, 192(%0) \n"			\
+				"fld  f25, 200(%0) \n"			\
+				"fld  f26, 208(%0) \n"			\
+				"fld  f27, 216(%0) \n"			\
+				"fld  f28, 224(%0) \n"			\
+				"fld  f29, 232(%0) \n"			\
+				"fld  f30, 240(%0) \n"			\
+				"fld  f31, 248(%0) \n"			\
+				: /* No outputs. */			\
+				: "r"(&__e[0])				\
+				: "f0", "f1", "f2", "f3", "f4", "f5",	\
+				  "f6", "f7", "f8", "f9", "f10", "f11",	\
+				  "f12", "f13", "f14", "f15", "f16",	\
+				  "f17", "f18", "f19", "f20", "f21",	\
+				  "f22", "f23", "f24", "f25", "f26",	\
+				  "f27", "f28", "f29", "f30", "f31",	\
+				  "memory");				\
+		}							\
+	} while (0)
+
+
+#define evl_check_fpregs(__features, __val, __bad)			\
+	({								\
+		unsigned int __result = (__val), __i;			\
+		__u64 __e[32];						\
+									\
+		if (__features & evl_riscv_fpsimd) {			\
+			for (__i = 0; __i < 32; __i++)			\
+				__e[__i] = __val;			\
+									\
+			__asm__ __volatile__(				\
+				".option push\n"			\
+				".option arch, +d\n"			\
+				"fsd f0, 0(%0)   \n"			\
+				"fsd f1, 8(%0)   \n"			\
+				"fsd f2, 16(%0)  \n"			\
+				"fsd f3, 24(%0)  \n"			\
+				"fsd f4, 32(%0)  \n"			\
+				"fsd f5, 40(%0)  \n"			\
+				"fsd f6, 48(%0)  \n"			\
+				"fsd f7, 56(%0)  \n"			\
+				"fsd f8, 64(%0)  \n"			\
+				"fsd f9, 72(%0)  \n"			\
+				"fsd f10, 80(%0) \n"			\
+				"fsd f11, 88(%0) \n"			\
+				"fsd f12, 96(%0) \n"			\
+				"fsd f13, 104(%0)\n"			\
+				"fsd f14, 112(%0)\n"			\
+				"fsd f15, 120(%0)\n"			\
+				"fsd f16, 128(%0)\n"			\
+				"fsd f17, 136(%0)\n"			\
+				"fsd f18, 144(%0)\n"			\
+				"fsd f19, 152(%0)\n"			\
+				"fsd f20, 160(%0)\n"			\
+				"fsd f21, 168(%0)\n"			\
+				"fsd f22, 176(%0)\n"			\
+				"fsd f23, 184(%0)\n"			\
+				"fsd f24, 192(%0)\n"			\
+				"fsd f25, 200(%0)\n"			\
+				"fsd f26, 208(%0)\n"			\
+				"fsd f27, 216(%0)\n"			\
+				"fsd f28, 224(%0)\n"			\
+				"fsd f29, 232(%0)\n"			\
+				"fsd f30, 240(%0)\n"			\
+				"fsd f31, 248(%0)\n"			\
+				: /* No outputs */			\
+				: "r"(&__e[0])				\
+				: "memory");				\
+									\
+			for (__i = 0; __i < 32; __i++) {		\
+				if (__e[__i] != __val) {		\
+					__result = __e[__i];		\
+					(__bad) = __i;			\
+					break;				\
+				}					\
+			}						\
+		}							\
+		__result;						\
+	})
 
 #endif /* !_EVL_RISCV_ASM_UAPI_FPTEST_H */
-- 
2.43.0