[boot-wrapper PATCH V2] aarch64: Enable access into FEAT_SPE_FDS register from EL2 and below

[Anshuman Khandual <anshuman.khandual@arm.com>]

FEAT_SPE_FDS adds system register PMSDSFR_EL1. But accessing that system
register from EL2 and below exception levels, will trap into EL3 unless
MDCR_EL3.EnPMS3 is set.

Enable access to FEAT_SPE_FDS registers when they are implemented.

Cc: James Clark <james.clark@linaro.org>
Cc: Mark Rutland <mark.rutland@arm.com>
Signed-off-by: Anshuman Khandual <anshuman.khandual@arm.com>
---
Changes in V2:

- Check for FEAT_SPE implementation before accessing PMSIDR_EL1 register

Changes in V1:

https://lore.kernel.org/all/20250604114604.629782-1-anshuman.khandual@arm.com/

 arch/aarch64/include/asm/cpu.h | 4 ++++
 arch/aarch64/init.c            | 9 +++++++++
 2 files changed, 13 insertions(+)

diff --git a/arch/aarch64/include/asm/cpu.h b/arch/aarch64/include/asm/cpu.h
index 2b3a659..ac50474 100644
--- a/arch/aarch64/include/asm/cpu.h
+++ b/arch/aarch64/include/asm/cpu.h
@@ -55,6 +55,7 @@
 #define MDCR_EL3_NSTB_NS_NOTRAP			(UL(3) << 24)
 #define MDCR_EL3_SBRBE_NOTRAP_NOPROHIBIT	(UL(3) << 32)
 #define MDCR_EL3_ENPMSN				BIT(36)
+#define MDCR_EL3_ENPMS3				BIT(42)
 #define MDCR_EL3_EBWE				BIT(43)
 #define MDCR_EL3_EnPM2				BIT(7)
 
@@ -185,6 +186,9 @@
 
 #define SCTLR_EL1_CP15BEN	(1 << 5)
 
+#define PMSIDR_EL1		s3_0_c9_c9_7
+#define PMSIDR_EL1_FDS		BIT(7)
+
 #ifdef KERNEL_32
 /*
  * When booting a 32-bit kernel, EL1 uses AArch32 and registers which are
diff --git a/arch/aarch64/init.c b/arch/aarch64/init.c
index e1640a9..df43d2d 100644
--- a/arch/aarch64/init.c
+++ b/arch/aarch64/init.c
@@ -146,6 +146,15 @@ static void cpu_init_el3(void)
 	if (mrs_field(ID_AA64DFR0_EL1, PMSVER) >= 3)
 		mdcr |= MDCR_EL3_ENPMSN;
 
+	/*
+	 * PMSIDR_EL1 register is present, only when FEAT_SPE
+	 * feature is implemeneted. Otherwise direct accesses
+	 * to PMSIDR_EL1 are UNDEFINED.
+	 */
+	if ((mrs_field(ID_AA64DFR0_EL1, PMSVER) >= 1) &&
+	    (mrs_field(PMSIDR_EL1, FDS)))
+		mdcr |= MDCR_EL3_ENPMS3;
+
 	if (mrs_field(ID_AA64DFR0_EL1, TRACEBUFFER))
 		mdcr |= MDCR_EL3_NSTB_NS_NOTRAP;
 
-- 
2.25.1