Merge changes from topic "jpc/el3-initialize-hfg-el2-regs"

* changes:
  refactor(fgt): modify FEAT_FGT test to check for init values
  feat(cpufeat): add feat detection helpers

4 files changed, 379 insertions, 46 deletions

diff --git a/include/lib/aarch64/arch.h b/include/lib/aarch64/arch.h
index a37e230..739cd26 100644
--- a/include/lib/aarch64/arch.h
+++ b/include/lib/aarch64/arch.h
@@ -128,35 +128,50 @@
 #define DCCSW			U(0x2)
 
 /* ID_AA64PFR0_EL1 definitions */
-#define ID_AA64PFR0_EL0_SHIFT	U(0)
-#define ID_AA64PFR0_EL1_SHIFT	U(4)
-#define ID_AA64PFR0_EL2_SHIFT	U(8)
-#define ID_AA64PFR0_EL3_SHIFT	U(12)
-#define ID_AA64PFR0_AMU_SHIFT	U(44)
-#define ID_AA64PFR0_AMU_LENGTH	U(4)
-#define ID_AA64PFR0_AMU_MASK	ULL(0xf)
-#define ID_AA64PFR0_AMU_NOT_SUPPORTED	U(0x0)
-#define ID_AA64PFR0_AMU_V1	U(0x1)
-#define ID_AA64PFR0_AMU_V1P1	U(0x2)
-#define ID_AA64PFR0_ELX_MASK	ULL(0xf)
-#define ID_AA64PFR0_SVE_SHIFT	U(32)
-#define ID_AA64PFR0_SVE_WIDTH	U(4)
-#define ID_AA64PFR0_SVE_MASK	ULL(0xf)
-#define ID_AA64PFR0_SVE_LENGTH	U(4)
-#define ID_AA64PFR0_MPAM_SHIFT	U(40)
-#define ID_AA64PFR0_MPAM_MASK	ULL(0xf)
-#define ID_AA64PFR0_DIT_SHIFT	U(48)
-#define ID_AA64PFR0_DIT_MASK	ULL(0xf)
-#define ID_AA64PFR0_DIT_LENGTH	U(4)
-#define ID_AA64PFR0_DIT_SUPPORTED	U(1)
-#define ID_AA64PFR0_CSV2_SHIFT	U(56)
-#define ID_AA64PFR0_CSV2_MASK	ULL(0xf)
-#define ID_AA64PFR0_CSV2_LENGTH	U(4)
+#define ID_AA64PFR0_EL0_SHIFT			U(0)
+#define ID_AA64PFR0_EL1_SHIFT			U(4)
+#define ID_AA64PFR0_EL2_SHIFT			U(8)
+#define ID_AA64PFR0_EL3_SHIFT			U(12)
+#define ID_AA64PFR0_AMU_SHIFT			U(44)
+#define ID_AA64PFR0_AMU_LENGTH			U(4)
+#define ID_AA64PFR0_AMU_MASK			ULL(0xf)
+#define ID_AA64PFR0_AMU_NOT_SUPPORTED		U(0x0)
+#define ID_AA64PFR0_AMU_V1			U(0x1)
+#define ID_AA64PFR0_AMU_V1P1			U(0x2)
+#define ID_AA64PFR0_ELX_MASK			ULL(0xf)
+#define ID_AA64PFR0_SVE_SHIFT			U(32)
+#define ID_AA64PFR0_SVE_WIDTH			U(4)
+#define ID_AA64PFR0_SVE_MASK			ULL(0xf)
+#define ID_AA64PFR0_SVE_LENGTH			U(4)
+#define ID_AA64PFR0_MPAM_SHIFT			U(40)
+#define ID_AA64PFR0_MPAM_MASK			ULL(0xf)
+#define ID_AA64PFR0_DIT_SHIFT			U(48)
+#define ID_AA64PFR0_DIT_MASK			ULL(0xf)
+#define ID_AA64PFR0_DIT_LENGTH			U(4)
+#define ID_AA64PFR0_DIT_SUPPORTED		U(1)
+#define ID_AA64PFR0_CSV2_SHIFT			U(56)
+#define ID_AA64PFR0_CSV2_MASK			ULL(0xf)
+#define ID_AA64PFR0_CSV2_WIDTH			U(4)
+#define ID_AA64PFR0_CSV2_NOT_SUPPORTED		ULL(0x0)
+#define ID_AA64PFR0_CSV2_SUPPORTED		ULL(0x1)
+#define ID_AA64PFR0_CSV2_2_SUPPORTED		ULL(0x2)
 #define ID_AA64PFR0_FEAT_RME_SHIFT		U(52)
 #define ID_AA64PFR0_FEAT_RME_MASK		ULL(0xf)
 #define ID_AA64PFR0_FEAT_RME_LENGTH		U(4)
 #define ID_AA64PFR0_FEAT_RME_NOT_SUPPORTED	U(0)
 #define ID_AA64PFR0_FEAT_RME_V1			U(1)
+#define ID_AA64PFR0_RAS_MASK			ULL(0xf)
+#define ID_AA64PFR0_RAS_SHIFT			U(28)
+#define ID_AA64PFR0_RAS_WIDTH			U(4)
+#define ID_AA64PFR0_RAS_NOT_SUPPORTED		ULL(0x0)
+#define ID_AA64PFR0_RAS_SUPPORTED		ULL(0x1)
+#define ID_AA64PFR0_RASV1P1_SUPPORTED		ULL(0x2)
+#define ID_AA64PFR0_GIC_SHIFT			U(24)
+#define ID_AA64PFR0_GIC_WIDTH			U(4)
+#define ID_AA64PFR0_GIC_MASK			ULL(0xf)
+#define ID_AA64PFR0_GIC_NOT_SUPPORTED		ULL(0x0)
+#define ID_AA64PFR0_GICV3_GICV4_SUPPORTED	ULL(0x1)
+#define ID_AA64PFR0_GICV4_1_SUPPORTED		ULL(0x2)
 
 /* ID_AA64DFR0_EL1.PMS definitions (for ARMv8.2+) */
 #define ID_AA64DFR0_PMS_SHIFT		U(32)
@@ -214,24 +229,46 @@
 #define EL_IMPL_A64ONLY		ULL(1)
 #define EL_IMPL_A64_A32		ULL(2)
 
-#define ID_AA64PFR0_GIC_SHIFT	U(24)
-#define ID_AA64PFR0_GIC_WIDTH	U(4)
-#define ID_AA64PFR0_GIC_MASK	ULL(0xf)
+/* ID_AA64ISAR0_EL1 definitions */
+#define ID_AA64ISAR0_EL1			S3_0_C0_C6_0
+#define ID_AA64ISAR0_TLB_MASK			ULL(0xf)
+#define ID_AA64ISAR0_TLB_SHIFT			U(56)
+#define ID_AA64ISAR0_TLB_WIDTH			U(4)
+#define ID_AA64ISAR0_TLBIRANGE_SUPPORTED	ULL(0x2)
+#define ID_AA64ISAR0_TLB_NOT_SUPPORTED		ULL(0)
 
 /* ID_AA64ISAR1_EL1 definitions */
-#define ID_AA64ISAR1_EL1	S3_0_C0_C6_1
-#define ID_AA64ISAR1_GPI_SHIFT	U(28)
-#define ID_AA64ISAR1_GPI_WIDTH	U(4)
-#define ID_AA64ISAR1_GPI_MASK	ULL(0xf)
-#define ID_AA64ISAR1_GPA_SHIFT	U(24)
-#define ID_AA64ISAR1_GPA_WIDTH	U(4)
-#define ID_AA64ISAR1_GPA_MASK	ULL(0xf)
-#define ID_AA64ISAR1_API_SHIFT	U(8)
-#define ID_AA64ISAR1_API_WIDTH	U(4)
-#define ID_AA64ISAR1_API_MASK	ULL(0xf)
-#define ID_AA64ISAR1_APA_SHIFT	U(4)
-#define ID_AA64ISAR1_APA_WIDTH	U(4)
-#define ID_AA64ISAR1_APA_MASK	ULL(0xf)
+#define ID_AA64ISAR1_EL1			S3_0_C0_C6_1
+#define ID_AA64ISAR1_GPI_SHIFT			U(28)
+#define ID_AA64ISAR1_GPI_WIDTH			U(4)
+#define ID_AA64ISAR1_GPI_MASK			ULL(0xf)
+#define ID_AA64ISAR1_GPA_SHIFT			U(24)
+#define ID_AA64ISAR1_GPA_WIDTH			U(4)
+#define ID_AA64ISAR1_GPA_MASK			ULL(0xf)
+#define ID_AA64ISAR1_API_SHIFT			U(8)
+#define ID_AA64ISAR1_API_WIDTH			U(4)
+#define ID_AA64ISAR1_API_MASK			ULL(0xf)
+#define ID_AA64ISAR1_APA_SHIFT			U(4)
+#define ID_AA64ISAR1_APA_WIDTH			U(4)
+#define ID_AA64ISAR1_APA_MASK			ULL(0xf)
+#define ID_AA64ISAR1_SPECRES_MASK		ULL(0xf)
+#define ID_AA64ISAR1_SPECRES_SHIFT		U(40)
+#define ID_AA64ISAR1_SPECRES_WIDTH		U(4)
+#define ID_AA64ISAR1_SPECRES_NOT_SUPPORTED	ULL(0x0)
+#define ID_AA64ISAR1_SPECRES_SUPPORTED		ULL(0x1)
+#define ID_AA64ISAR1_DPB_MASK			ULL(0xf)
+#define ID_AA64ISAR1_DPB_SHIFT			U(0)
+#define ID_AA64ISAR1_DPB_WIDTH			U(4)
+#define ID_AA64ISAR1_DPB_NOT_SUPPORTED		ULL(0x0)
+#define ID_AA64ISAR1_DPB_SUPPORTED		ULL(0x1)
+#define ID_AA64ISAR1_DPB2_SUPPORTED		ULL(0x2)
+#define ID_AA64ISAR1_LS64_MASK			ULL(0xf)
+#define ID_AA64ISAR1_LS64_SHIFT			U(60)
+#define ID_AA64ISAR1_LS64_WIDTH			U(4)
+#define ID_AA64ISAR1_LS64_NOT_SUPPORTED		ULL(0x0)
+#define ID_AA64ISAR1_LS64_SUPPORTED		ULL(0x1)
+#define ID_AA64ISAR1_LS64_V_SUPPORTED		ULL(0x2)
+#define ID_AA64ISAR1_LS64_ACCDATA_SUPPORTED	ULL(0x3)
 
 /* ID_AA64ISAR2_EL1 definitions */
 #define ID_AA64ISAR2_EL1		S3_0_C0_C6_2
@@ -312,7 +349,7 @@
 /* ID_AA64MMFR1_EL1 definitions */
 #define ID_AA64MMFR1_EL1_PAN_SHIFT		U(20)
 #define ID_AA64MMFR1_EL1_PAN_MASK		ULL(0xf)
-#define ID_AA64MMFR1_EL1_PAN_NOT_SUPPORTED	ULL(0x0)
+#define ID_AA64MMFR1_EL1_PAN_WIDTH		U(4)
 #define ID_AA64MMFR1_EL1_PAN_SUPPORTED		ULL(0x1)
 #define ID_AA64MMFR1_EL1_PAN2_SUPPORTED		ULL(0x2)
 #define ID_AA64MMFR1_EL1_PAN3_SUPPORTED		ULL(0x3)
@@ -323,6 +360,12 @@
 #define ID_AA64MMFR1_EL1_AFP_SHIFT		U(44)
 #define ID_AA64MMFR1_EL1_AFP_MASK		ULL(0xf)
 #define ID_AA64MMFR1_EL1_AFP_SUPPORTED		ULL(0x1)
+#define ID_AA64MMFR1_EL1_LO_SHIFT		U(16)
+#define ID_AA64MMFR1_EL1_LO_MASK		ULL(0xf)
+#define ID_AA64MMFR1_EL1_LO_WIDTH		U(4)
+#define ID_AA64MMFR1_EL1_LOR_NOT_SUPPORTED	ULL(0x0)
+#define ID_AA64MMFR1_EL1_LOR_SUPPORTED		ULL(0x1)
+
 
 /* ID_AA64MMFR2_EL1 definitions */
 #define ID_AA64MMFR2_EL1		S3_0_C0_C7_2
@@ -353,6 +396,12 @@
 #define ID_AA64PFR1_EL1_RNG_TRAP_SUPPORTED	ULL(0x1)
 #define ID_AA64PFR1_EL1_RNG_TRAP_NOT_SUPPORTED	ULL(0x0)
 
+#define ID_AA64PFR1_CSV2_FRAC_MASK		ULL(0xf)
+#define ID_AA64PFR1_CSV2_FRAC_SHIFT		U(32)
+#define ID_AA64PFR1_CSV2_FRAC_WIDTH		U(4)
+#define ID_AA64PFR1_CSV2_1P1_SUPPORTED		ULL(0x1)
+#define ID_AA64PFR1_CSV2_1P2_SUPPORTED		ULL(0x2)
+
 #define MTE_UNIMPLEMENTED	ULL(0)
 #define MTE_IMPLEMENTED_EL0	ULL(1)	/* MTE is only implemented at EL0 */
 #define MTE_IMPLEMENTED_ELX	ULL(2)	/* MTE is implemented at all ELs */
@@ -363,6 +412,11 @@
 #define ID_AA64PFR1_EL1_SME_SUPPORTED		ULL(0x1)
 #define ID_AA64PFR1_EL1_SME2_SUPPORTED		ULL(0x2)
 
+#define ID_AA64PFR1_RAS_FRAC_MASK		ULL(0xf)
+#define ID_AA64PFR1_RAS_FRAC_SHIFT		U(12)
+#define ID_AA64PFR1_RAS_FRAC_WIDTH		U(4)
+#define ID_AA64PFR1_RASV1P1_SUPPORTED		ULL(0x1)
+
 /* ID_PFR1_EL1 definitions */
 #define ID_PFR1_VIRTEXT_SHIFT	U(12)
 #define ID_PFR1_VIRTEXT_MASK	U(0xf)
@@ -1269,4 +1323,59 @@
 #define HCRX_EL2_EnAS0_BIT	(UL(1) << 0)
 #define HCRX_EL2_INIT_VAL	ULL(0x0)
 
+/*******************************************************************************
+ * PFR0_EL1 - Definitions for AArch32 Processor Feature Register 0
+ ******************************************************************************/
+#define ID_PFR0_EL1				S3_0_C0_C1_0
+#define ID_PFR0_EL1_RAS_MASK			ULL(0xf)
+#define ID_PFR0_EL1_RAS_SHIFT			U(28)
+#define ID_PFR0_EL1_RAS_WIDTH			U(4)
+#define ID_PFR0_EL1_RAS_SUPPORTED		ULL(0x1)
+#define ID_PFR0_EL1_RASV1P1_SUPPORTED		ULL(0x2)
+
+/*******************************************************************************
+ * PFR2_EL1 - Definitions for AArch32 Processor Feature Register 2
+ ******************************************************************************/
+#define ID_PFR2_EL1				S3_0_C0_C3_4
+#define ID_PFR2_EL1_RAS_FRAC_MASK		ULL(0xf)
+#define ID_PFR2_EL1_RAS_FRAC_SHIFT		U(8)
+#define ID_PFR2_EL1_RAS_FRAC_WIDTH		U(4)
+#define ID_PFR2_EL1_RASV1P1_SUPPORTED		ULL(0x1)
+
+/*******************************************************************************
+ * FEAT_FGT - Definitions for Fine-Grained Trap registers
+ ******************************************************************************/
+#define HFGITR_EL2_INIT_VAL			ULL(0x180000000000000)
+#define HFGITR_EL2_FEAT_BRBE_MASK		ULL(0x180000000000000)
+#define HFGITR_EL2_FEAT_SPECRES_MASK		ULL(0x7000000000000)
+#define HFGITR_EL2_FEAT_TLBIRANGE_MASK		ULL(0x3fc00000000)
+#define HFGITR_EL2_FEAT_TLBIRANGE_TLBIOS_MASK	ULL(0xf000000)
+#define HFGITR_EL2_FEAT_TLBIOS_MASK		ULL(0xfc0000)
+#define HFGITR_EL2_FEAT_PAN2_MASK		ULL(0x30000)
+#define HFGITR_EL2_FEAT_DPB2_MASK		ULL(0x200)
+#define HFGITR_EL2_NON_FEAT_DEPENDENT_MASK	ULL(0x78fc03f000fdff)
+
+#define HFGRTR_EL2_INIT_VAL			ULL(0xc4000000000000)
+#define HFGRTR_EL2_FEAT_SME_MASK		ULL(0xc0000000000000)
+#define HFGRTR_EL2_FEAT_LS64_ACCDATA_MASK	ULL(0x4000000000000)
+#define HFGRTR_EL2_FEAT_RAS_MASK		ULL(0x27f0000000000)
+#define HFGRTR_EL2_FEAT_RASV1P1_MASK		ULL(0x1800000000000)
+#define HFGRTR_EL2_FEAT_GICV3_MASK		ULL(0x800000000)
+#define HFGRTR_EL2_FEAT_CSV2_2_CSV2_1P2_MASK	ULL(0xc0000000)
+#define HFGRTR_EL2_FEAT_LOR_MASK		ULL(0xf80000)
+#define HFGRTR_EL2_FEAT_PAUTH_MASK		ULL(0x1f0)
+#define HFGRTR_EL2_NON_FEAT_DEPENDENT_MASK	ULL(0x7f3f07fe0f)
+
+#define HFGWTR_EL2_INIT_VAL			ULL(0xc4000000000000)
+#define HFGWTR_EL2_FEAT_SME_MASK		ULL(0xc0000000000000)
+#define HFGWTR_EL2_FEAT_LS64_ACCDATA_MASK	ULL(0x4000000000000)
+#define HFGWTR_EL2_FEAT_RAS_MASK		ULL(0x23a0000000000)
+#define HFGWTR_EL2_FEAT_RASV1P1_MASK		ULL(0x1800000000000)
+#define HFGWTR_EL2_FEAT_GICV3_MASK		ULL(0x8000000000)
+#define HFGWTR_EL2_FEAT_CSV2_2_CSV2_1P2_MASK	ULL(0xc0000000)
+#define HFGWTR_EL2_FEAT_LOR_MASK		ULL(0xf80000)
+#define HFGWTR_EL2_FEAT_PAUTH_MASK		ULL(0x1f0)
+#define HFGWTR_EL2_NON_FEAT_DEPENDENT_MASK	ULL(0x7f2903380b)
+
+
 #endif /* ARCH_H */
diff --git a/include/lib/aarch64/arch_features.h b/include/lib/aarch64/arch_features.h
index ee14d67..85f8952 100644
--- a/include/lib/aarch64/arch_features.h
+++ b/include/lib/aarch64/arch_features.h
@@ -22,8 +22,18 @@ static inline bool is_armv7_gentimer_present(void)
 
 static inline bool is_armv8_1_pan_present(void)
 {
-	return ((read_id_aa64mmfr1_el1() >> ID_AA64MMFR1_EL1_PAN_SHIFT) &
-		ID_AA64MMFR1_EL1_PAN_MASK) != 0U;
+	u_register_t id_aa64mmfr1_pan =
+		EXTRACT(ID_AA64MMFR1_EL1_PAN, read_id_aa64mmfr1_el1());
+	return (id_aa64mmfr1_pan >= ID_AA64MMFR1_EL1_PAN_SUPPORTED) &&
+		(id_aa64mmfr1_pan <= ID_AA64MMFR1_EL1_PAN3_SUPPORTED);
+}
+
+static inline bool is_armv8_2_pan2_present(void)
+{
+	u_register_t id_aa64mmfr1_pan =
+		EXTRACT(ID_AA64MMFR1_EL1_PAN, read_id_aa64mmfr1_el1());
+	return (id_aa64mmfr1_pan >= ID_AA64MMFR1_EL1_PAN2_SUPPORTED) &&
+		(id_aa64mmfr1_pan <= ID_AA64MMFR1_EL1_PAN3_SUPPORTED);
 }
 
 static inline bool is_armv8_2_sve_present(void)
@@ -289,4 +299,111 @@ static inline bool is_feat_52b_on_4k_2_supported(void)
 			&& (is_feat_52b_on_4k_supported() == true)));
 }
 
+static inline bool is_feat_specres_present(void)
+{
+	return EXTRACT(ID_AA64ISAR1_SPECRES, read_id_aa64isar1_el1())
+		== ID_AA64ISAR1_SPECRES_SUPPORTED;
+}
+
+static inline bool is_feat_tlbirange_present(void)
+{
+	return EXTRACT(ID_AA64ISAR0_TLB, read_id_aa64isar0_el1())
+		== ID_AA64ISAR0_TLBIRANGE_SUPPORTED;
+}
+
+static inline bool is_feat_tlbios_present(void)
+{
+	return EXTRACT(ID_AA64ISAR0_TLB, read_id_aa64isar0_el1())
+		!= ID_AA64ISAR0_TLB_NOT_SUPPORTED;
+}
+
+static inline bool is_feat_dpb_present(void)
+{
+	return EXTRACT(ID_AA64ISAR1_DPB, read_id_aa64isar1_el1())
+		>= ID_AA64ISAR1_DPB_SUPPORTED;
+}
+
+static inline bool is_feat_dpb2_present(void)
+{
+	return EXTRACT(ID_AA64ISAR1_DPB, read_id_aa64isar1_el1())
+		>= ID_AA64ISAR1_DPB2_SUPPORTED;
+}
+
+static inline bool is_feat_ls64_present(void)
+{
+	return EXTRACT(ID_AA64ISAR1_LS64, read_id_aa64isar1_el1())
+		>= ID_AA64ISAR1_LS64_SUPPORTED;
+}
+
+static inline bool is_feat_ls64_v_present(void)
+{
+	return EXTRACT(ID_AA64ISAR1_LS64, read_id_aa64isar1_el1())
+		>= ID_AA64ISAR1_LS64_V_SUPPORTED;
+}
+
+static inline bool is_feat_ls64_accdata_present(void)
+{
+	return EXTRACT(ID_AA64ISAR1_LS64, read_id_aa64isar1_el1())
+		>= ID_AA64ISAR1_LS64_ACCDATA_SUPPORTED;
+}
+
+static inline bool is_feat_ras_present(void)
+{
+	return EXTRACT(ID_AA64PFR0_RAS, read_id_aa64pfr0_el1())
+		== ID_AA64PFR0_RAS_SUPPORTED;
+}
+
+static inline bool is_feat_rasv1p1_present(void)
+{
+	return (EXTRACT(ID_AA64PFR0_RAS, read_id_aa64pfr0_el1())
+		== ID_AA64PFR0_RASV1P1_SUPPORTED)
+		|| (is_feat_ras_present() &&
+			(EXTRACT(ID_AA64PFR1_RAS_FRAC, read_id_aa64pfr1_el1())
+				== ID_AA64PFR1_RASV1P1_SUPPORTED))
+		|| (EXTRACT(ID_PFR0_EL1_RAS, read_id_pfr0_el1())
+			== ID_PFR0_EL1_RASV1P1_SUPPORTED)
+		|| ((EXTRACT(ID_PFR0_EL1_RAS, read_id_pfr0_el1())
+			== ID_PFR0_EL1_RAS_SUPPORTED) &&
+			(EXTRACT(ID_PFR2_EL1_RAS_FRAC, read_id_pfr2_el1())
+				== ID_PFR2_EL1_RASV1P1_SUPPORTED));
+}
+
+static inline bool is_feat_gicv3_gicv4_present(void)
+{
+	return EXTRACT(ID_AA64PFR0_GIC, read_id_aa64pfr0_el1())
+		== ID_AA64PFR0_GICV3_GICV4_SUPPORTED;
+}
+
+static inline bool is_feat_csv2_present(void)
+{
+	return EXTRACT(ID_AA64PFR0_CSV2, read_id_aa64pfr0_el1())
+		== ID_AA64PFR0_CSV2_SUPPORTED;
+}
+
+static inline bool is_feat_csv2_2_present(void)
+{
+	return EXTRACT(ID_AA64PFR0_CSV2, read_id_aa64pfr0_el1())
+		== ID_AA64PFR0_CSV2_2_SUPPORTED;
+}
+
+static inline bool is_feat_csv2_1p1_present(void)
+{
+	return is_feat_csv2_present() &&
+		(EXTRACT(ID_AA64PFR1_CSV2_FRAC, read_id_aa64pfr1_el1())
+			== ID_AA64PFR1_CSV2_1P1_SUPPORTED);
+}
+
+static inline bool is_feat_csv2_1p2_present(void)
+{
+	return is_feat_csv2_present() &&
+		(EXTRACT(ID_AA64PFR1_CSV2_FRAC, read_id_aa64pfr1_el1())
+			== ID_AA64PFR1_CSV2_1P2_SUPPORTED);
+}
+
+static inline bool is_feat_lor_present(void)
+{
+	return EXTRACT(ID_AA64MMFR1_EL1_LO, read_id_aa64mmfr1_el1())
+		!= ID_AA64MMFR1_EL1_LOR_NOT_SUPPORTED;
+}
+
 #endif /* ARCH_FEATURES_H */
diff --git a/include/lib/aarch64/arch_helpers.h b/include/lib/aarch64/arch_helpers.h
index 77dc881..39461d5 100644
--- a/include/lib/aarch64/arch_helpers.h
+++ b/include/lib/aarch64/arch_helpers.h
@@ -186,11 +186,13 @@ void disable_mmu_icache(void);
 
 DEFINE_SYSREG_RW_FUNCS(par_el1)
 DEFINE_SYSREG_READ_FUNC(id_pfr1_el1)
+DEFINE_SYSREG_READ_FUNC(id_aa64isar0_el1)
 DEFINE_SYSREG_READ_FUNC(id_aa64isar1_el1)
 DEFINE_SYSREG_READ_FUNC(id_aa64pfr0_el1)
 DEFINE_SYSREG_READ_FUNC(id_aa64pfr1_el1)
 DEFINE_SYSREG_READ_FUNC(id_aa64dfr0_el1)
 DEFINE_SYSREG_READ_FUNC(id_afr0_el1)
+DEFINE_SYSREG_READ_FUNC(id_pfr0_el1)
 DEFINE_SYSREG_READ_FUNC(CurrentEl)
 DEFINE_SYSREG_READ_FUNC(ctr_el0)
 DEFINE_SYSREG_RW_FUNCS(daif)
@@ -618,6 +620,9 @@ DEFINE_RENAME_SYSREG_RW_FUNCS(fpcr, FPCR)
 /* ID_AA64ISAR2_EL1 */
 DEFINE_RENAME_SYSREG_READ_FUNC(id_aa64isar2_el1, ID_AA64ISAR2_EL1)
 
+/* ID_PFR2_EL1 */
+DEFINE_RENAME_SYSREG_READ_FUNC(id_pfr2_el1, ID_PFR2_EL1)
+
 #define IS_IN_EL(x) \
 	(GET_EL(read_CurrentEl()) == MODE_EL##x)
 
diff --git a/tftf/tests/extensions/fgt/test_fgt.c b/tftf/tests/extensions/fgt/test_fgt.c
index 6213d4b..5d9600d 100644
--- a/tftf/tests/extensions/fgt/test_fgt.c
+++ b/tftf/tests/extensions/fgt/test_fgt.c
@@ -10,6 +10,38 @@
 #include <string.h>
 #include <arch_helpers.h>
 
+#ifdef __aarch64__
+
+static bool is_init_val_set(u_register_t reg, u_register_t init_val,
+	u_register_t feat_mask)
+{
+	return (reg & feat_mask) == (init_val & feat_mask);
+}
+
+#define CHECK_FEAT_TRAP_INITIALIZED(_reg, _REG, _feat_check, _FEAT)	\
+do {									\
+	if (_feat_check() != 0) {					\
+		if (is_init_val_set(_reg, _REG ## _INIT_VAL,		\
+			_REG ## _ ## FEAT_ ## _FEAT ## _MASK) == 0) {	\
+			return TEST_RESULT_FAIL;			\
+		}							\
+	}								\
+} while (false);
+
+#define CHECK_FEAT_TRAP_INITIALIZED2(_reg, _REG, _feat_check, _FEAT,	\
+	_feat2_check, _FEAT2, _op)					\
+do {									\
+	if ((_feat_check() != 0) _op (_feat2_check() != 0)) {		\
+		if (is_init_val_set(_reg, _REG ## _INIT_VAL, _REG ## _	\
+			## FEAT_ ## _FEAT ## _ ## _FEAT2 ## _MASK)	\
+			== 0) {						\
+			return TEST_RESULT_FAIL;			\
+		}							\
+	}								\
+} while (false);
+
+#endif
+
 /*
  * TF-A is expected to allow access to ARMv8.6-FGT system registers from EL2.
  * Reading these registers causes a trap to EL3 and crash when TF-A has not
@@ -21,12 +53,82 @@ test_result_t test_fgt_enabled(void)
 
 #ifdef __aarch64__
 	SKIP_TEST_IF_FGT_NOT_SUPPORTED();
-	read_hfgrtr_el2();
-	read_hfgwtr_el2();
-	read_hfgitr_el2();
+
+	u_register_t hfgitr_el2 = read_hfgitr_el2();
+	u_register_t hfgrtr_el2 = read_hfgrtr_el2();
+	u_register_t hfgwtr_el2 = read_hfgwtr_el2();
+
+	/*
+	 * The following registers are not supposed to be consumed, but
+	 * are read to test their presence when FEAT_FGT is supported.
+	 */
 	read_hdfgrtr_el2();
 	read_hdfgwtr_el2();
 
+	CHECK_FEAT_TRAP_INITIALIZED(hfgitr_el2, HFGITR_EL2,		\
+		get_feat_brbe_support, BRBE)
+	CHECK_FEAT_TRAP_INITIALIZED(hfgitr_el2, HFGITR_EL2,		\
+		is_feat_specres_present, SPECRES)
+	CHECK_FEAT_TRAP_INITIALIZED(hfgitr_el2, HFGITR_EL2,		\
+		is_feat_tlbirange_present, TLBIRANGE)
+	CHECK_FEAT_TRAP_INITIALIZED2(hfgitr_el2, HFGITR_EL2,		\
+		is_feat_tlbirange_present, TLBIRANGE,			\
+		is_feat_tlbios_present, TLBIOS, &&)
+	CHECK_FEAT_TRAP_INITIALIZED(hfgitr_el2, HFGITR_EL2,		\
+		is_feat_tlbios_present, TLBIOS)
+	CHECK_FEAT_TRAP_INITIALIZED(hfgitr_el2, HFGITR_EL2,		\
+		is_armv8_2_pan2_present, PAN2)
+	CHECK_FEAT_TRAP_INITIALIZED(hfgitr_el2, HFGITR_EL2,		\
+		is_feat_dpb2_present, DPB2)
+	if (is_init_val_set(hfgitr_el2, HFGITR_EL2_INIT_VAL,
+		HFGITR_EL2_NON_FEAT_DEPENDENT_MASK) == 0) {
+		return TEST_RESULT_FAIL;
+	}
+
+	CHECK_FEAT_TRAP_INITIALIZED(hfgrtr_el2, HFGRTR_EL2,		\
+		is_feat_sme_supported, SME)
+	CHECK_FEAT_TRAP_INITIALIZED(hfgrtr_el2, HFGRTR_EL2,		\
+		is_feat_ls64_accdata_present, LS64_ACCDATA)
+	CHECK_FEAT_TRAP_INITIALIZED(hfgrtr_el2, HFGRTR_EL2,		\
+		is_feat_ras_present, RAS)
+	CHECK_FEAT_TRAP_INITIALIZED(hfgrtr_el2, HFGRTR_EL2,		\
+		is_feat_rasv1p1_present, RASV1P1)
+	CHECK_FEAT_TRAP_INITIALIZED(hfgrtr_el2, HFGRTR_EL2,		\
+		is_feat_gicv3_gicv4_present, GICV3)
+	CHECK_FEAT_TRAP_INITIALIZED2(hfgrtr_el2, HFGRTR_EL2,		\
+		is_feat_csv2_2_present, CSV2_2,				\
+		is_feat_csv2_1p2_present, CSV2_1P2, ||)
+	CHECK_FEAT_TRAP_INITIALIZED(hfgrtr_el2, HFGRTR_EL2,		\
+		is_feat_lor_present, LOR)
+	CHECK_FEAT_TRAP_INITIALIZED(hfgrtr_el2, HFGRTR_EL2,		\
+		is_armv8_3_pauth_apa_api_apa3_present, PAUTH)
+	if (is_init_val_set(hfgrtr_el2, HFGRTR_EL2_INIT_VAL,
+		HFGRTR_EL2_NON_FEAT_DEPENDENT_MASK) == 0) {
+		return TEST_RESULT_FAIL;
+	}
+
+	CHECK_FEAT_TRAP_INITIALIZED(hfgwtr_el2, HFGWTR_EL2,		\
+		is_feat_sme_supported, SME)
+	CHECK_FEAT_TRAP_INITIALIZED(hfgwtr_el2, HFGWTR_EL2,		\
+		is_feat_ls64_accdata_present, LS64_ACCDATA);
+	CHECK_FEAT_TRAP_INITIALIZED(hfgwtr_el2, HFGWTR_EL2,		\
+		is_feat_ras_present, RAS);
+	CHECK_FEAT_TRAP_INITIALIZED(hfgwtr_el2, HFGWTR_EL2,		\
+		is_feat_rasv1p1_present, RASV1P1);
+	CHECK_FEAT_TRAP_INITIALIZED(hfgwtr_el2, HFGWTR_EL2,		\
+		is_feat_gicv3_gicv4_present, GICV3);
+	CHECK_FEAT_TRAP_INITIALIZED2(hfgwtr_el2, HFGWTR_EL2,		\
+		is_feat_csv2_2_present, CSV2_2,				\
+		is_feat_csv2_1p2_present, CSV2_1P2, ||)
+	CHECK_FEAT_TRAP_INITIALIZED(hfgwtr_el2, HFGWTR_EL2,		\
+		is_feat_lor_present, LOR)
+	CHECK_FEAT_TRAP_INITIALIZED(hfgwtr_el2, HFGWTR_EL2,		\
+		is_armv8_3_pauth_apa_api_apa3_present, PAUTH)
+	if (is_init_val_set(hfgwtr_el2, HFGWTR_EL2_INIT_VAL,		\
+		HFGWTR_EL2_NON_FEAT_DEPENDENT_MASK) == 0) {
+		return TEST_RESULT_FAIL;
+	}
+
 	return TEST_RESULT_SUCCESS;
 #endif	/* __aarch64__ */
 }