feat(fpmr): disable FPMR trap

This patch enables support of FEAT_FPMR by enabling access
to FPMR register. It achieves it by setting the EnFPM bit of
SCR_EL3. This feature is currently enabled for NS world only.

Reference:
https://developer.arm.com/documentation/109697/2024_09/
Feature-descriptions/The-Armv9-5-architecture-extension?lang=en

Change-Id: I580c409b9b22f8ead0737502280fb9093a3d5dd2
Signed-off-by: Arvind Ram Prakash <arvind.ramprakash@arm.com>
diff --git a/include/arch/aarch64/arch.h b/include/arch/aarch64/arch.h
index 737d07a..3a7e2eb 100644
--- a/include/arch/aarch64/arch.h
+++ b/include/arch/aarch64/arch.h
@@ -465,6 +465,11 @@
 #define ID_AA64PFR2_EL1_MTEFAR_SHIFT		U(8)
 #define ID_AA64PFR2_EL1_MTEFAR_MASK		ULL(0xf)
 
+#define ID_AA64PFR2_EL1_FPMR_SHIFT		U(32)
+#define ID_AA64PFR2_EL1_FPMR_MASK		ULL(0xf)
+
+#define FPMR_IMPLEMENTED			ULL(0x1)
+
 #define VDISR_EL2				S3_4_C12_C1_1
 #define VSESR_EL2				S3_4_C5_C2_3
 
@@ -605,6 +610,7 @@
 #define SCR_NSE_SHIFT		U(62)
 #define SCR_FGTEN2_BIT		(UL(1) << 59)
 #define SCR_NSE_BIT		(ULL(1) << SCR_NSE_SHIFT)
+#define SCR_EnFPM_BIT		(ULL(1) << 50)
 #define SCR_GPF_BIT		(UL(1) << 48)
 #define SCR_D128En_BIT		(UL(1) << 47)
 #define SCR_TWEDEL_SHIFT	U(30)
@@ -1522,6 +1528,11 @@
  ******************************************************************************/
 #define CLUSTERPWRDN_EL1	S3_0_c15_c3_6
 
+/*******************************************************************************
+ * FEAT_FPMR - Floating point Mode Register
+ ******************************************************************************/
+#define FPMR			S3_3_C4_C4_2
+
 /* CLUSTERPWRDN_EL1 register definitions */
 #define DSU_CLUSTER_PWR_OFF	0
 #define DSU_CLUSTER_PWR_ON	1
diff --git a/include/arch/aarch64/arch_features.h b/include/arch/aarch64/arch_features.h
index 59188da..8bdc13c 100644
--- a/include/arch/aarch64/arch_features.h
+++ b/include/arch/aarch64/arch_features.h
@@ -142,6 +142,8 @@
  * +----------------------------+
  * |	FEAT_LS64_ACCDATA	|
  * +----------------------------+
+ * |	FEAT_FPMR		|
+ * +----------------------------+
  */
 
 __attribute__((always_inline))
@@ -284,6 +286,12 @@
 		     ID_AA64MMFR3_EL1_D128_MASK, D128_IMPLEMENTED,
 		     ENABLE_FEAT_D128)
 
+/* FEAT_FPMR */
+CREATE_FEATURE_FUNCS(feat_fpmr, id_aa64pfr2_el1, ID_AA64PFR2_EL1_FPMR_SHIFT,
+		     ID_AA64PFR2_EL1_FPMR_MASK, FPMR_IMPLEMENTED,
+		     ENABLE_FEAT_FPMR)
+
+
 __attribute__((always_inline))
 static inline bool is_feat_sxpie_supported(void)
 {
diff --git a/include/arch/aarch64/arch_helpers.h b/include/arch/aarch64/arch_helpers.h
index 119c428..3f3f14d 100644
--- a/include/arch/aarch64/arch_helpers.h
+++ b/include/arch/aarch64/arch_helpers.h
@@ -718,6 +718,8 @@
 DEFINE_RENAME_SYSREG_RW_FUNCS(gptbr_el3, GPTBR_EL3)
 DEFINE_RENAME_SYSREG_RW_FUNCS(gpccr_el3, GPCCR_EL3)
 
+DEFINE_RENAME_SYSREG_RW_FUNCS(fpmr, FPMR)
+
 #define IS_IN_EL(x) \
 	(GET_EL(read_CurrentEl()) == MODE_EL##x)