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

[Anshuman Khandual]

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 at linaro.org>
Cc: Mark Rutland <mark.rutland at arm.com>
Cc: Mark Brown <broonie at kernel.org>
Signed-off-by: Anshuman Khandual <anshuman.khandual at arm.com>
---
 arch/aarch64/include/asm/cpu.h | 4 ++++
 arch/aarch64/init.c            | 3 +++
 2 files changed, 7 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..b6f740c 100644
--- a/arch/aarch64/init.c
+++ b/arch/aarch64/init.c
@@ -146,6 +146,9 @@ static void cpu_init_el3(void)
 	if (mrs_field(ID_AA64DFR0_EL1, PMSVER) >= 3)
 		mdcr |= MDCR_EL3_ENPMSN;
 
+	if (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