feat: add tests to check SCTLR2, THE and D128 sysregs

This patch adds test cases for verifying that the system registers of
FEAT_SCTLR2, FEAT_THE and FEAT_D128 (FEAT_SYSREG128) are working
correctly by performing a series of reads and writes to the registers.

Change-Id: I5c102daa358a7ec5d1801395bc875e9850e83939
Signed-off-by: Igor Podgainõi <igor.podgainoi@arm.com>
Signed-off-by: Manish Pandey <manish.pandey2@arm.com>
diff --git a/include/common/test_helpers.h b/include/common/test_helpers.h
index 8de7a86..fe29ee5 100644
--- a/include/common/test_helpers.h
+++ b/include/common/test_helpers.h
@@ -327,6 +327,30 @@
 		}								\
 	} while (false)
 
+#define SKIP_TEST_IF_SCTLR2_NOT_SUPPORTED()					\
+	do {									\
+		if (!is_feat_sctlr2_supported()) {				\
+			tftf_testcase_printf("FEAT_SCTLR2 not supported\n");	\
+			return TEST_RESULT_SKIPPED;				\
+		}								\
+	} while (false)
+
+#define SKIP_TEST_IF_THE_NOT_SUPPORTED()					\
+	do {									\
+		if (!is_feat_the_supported()) {					\
+			tftf_testcase_printf("FEAT_THE not supported\n");	\
+			return TEST_RESULT_SKIPPED;				\
+		}								\
+	} while (false)
+
+#define SKIP_TEST_IF_D128_NOT_SUPPORTED()					\
+	do {									\
+		if (!is_feat_d128_supported()) {				\
+			tftf_testcase_printf("FEAT_D128 not supported\n");	\
+			return TEST_RESULT_SKIPPED;				\
+		}								\
+	} while (false)
+
 #define SKIP_TEST_IF_RME_NOT_SUPPORTED_OR_RMM_IS_TRP()				\
 	do {									\
 		u_register_t retrmm = 0U;					\
diff --git a/include/lib/aarch64/arch.h b/include/lib/aarch64/arch.h
index 0580f8a..f0c10ef 100644
--- a/include/lib/aarch64/arch.h
+++ b/include/lib/aarch64/arch.h
@@ -432,6 +432,11 @@
 /* ID_AA64MMFR3_EL1 definitions */
 #define ID_AA64MMFR3_EL1			S3_0_C0_C7_3
 
+#define ID_AA64MMFR3_EL1_D128_SHIFT		U(32)
+#define ID_AA64MMFR3_EL1_D128_MASK		ULL(0xf)
+#define ID_AA64MMFR3_EL1_D128_WIDTH		U(4)
+#define ID_AA64MMFR3_EL1_D128_SUPPORTED		ULL(0x1)
+
 #define ID_AA64MMFR3_EL1_S2POE_SHIFT		U(20)
 #define ID_AA64MMFR3_EL1_S2POE_MASK		ULL(0xf)
 #define ID_AA64MMFR3_EL1_S2POE_WIDTH		U(4)
@@ -453,7 +458,9 @@
 #define ID_AA64MMFR3_EL1_S1PIE_SUPPORTED	ULL(0x1)
 
 #define ID_AA64MMFR3_EL1_SCTLRX_SHIFT		U(4)
+#define ID_AA64MMFR3_EL1_SCTLRX_MASK		ULL(0xf)
 #define ID_AA64MMFR3_EL1_SCTLRX_WIDTH		ULL(0x4)
+#define ID_AA64MMFR3_EL1_SCTLR2_SUPPORTED	ULL(0x1)
 
 #define ID_AA64MMFR3_EL1_TCRX_SHIFT		U(0)
 #define ID_AA64MMFR3_EL1_TCRX_MASK		ULL(0xf)
@@ -465,6 +472,11 @@
 #define ID_AA64PFR1_EL1_DF2_WIDTH		U(4)
 #define ID_AA64PFR1_EL1_DF2_MASK		(0xf << ID_AA64PFR1_EL1_DF2_SHIFT)
 
+#define ID_AA64PFR1_EL1_THE_SHIFT		U(48)
+#define ID_AA64PFR1_EL1_THE_MASK		ULL(0xf)
+#define ID_AA64PFR1_EL1_THE_WIDTH		U(4)
+#define ID_AA64PFR1_EL1_THE_SUPPORTED		ULL(1)
+
 #define ID_AA64PFR1_EL1_GCS_SHIFT		U(44)
 #define ID_AA64PFR1_EL1_GCS_MASK		ULL(0xf)
 #define ID_AA64PFR1_EL1_GCS_WIDTH		U(4)
@@ -571,7 +583,8 @@
 #define SCTLR_DSSBS_BIT		(ULL(1) << 44)
 #define SCTLR_RESET_VAL		SCTLR_EL3_RES1
 
-/* SCTLR2_EL1 register definitions */
+/* SCTLR2 register definitions */
+#define SCTLR2_EL2		S3_4_C1_C0_3
 #define SCTLR2_EL1		S3_0_C1_C0_3
 
 #define SCTLR2_NMEA_BIT		(UL(1) << 2)
@@ -1418,6 +1431,12 @@
  ******************************************************************************/
 #define MDSELR_EL1		S2_0_C0_C4_2
 
+/******************************************************************************
+ * Armv8.9 - Translation Hardening Extension Registers
+ ******************************************************************************/
+#define RCWMASK_EL1		S3_0_C13_C0_6
+#define RCWSMASK_EL1		S3_0_C13_C0_3
+
 /*******************************************************************************
  * Armv9.0 - Trace Buffer Extension System Registers
  ******************************************************************************/
diff --git a/include/lib/aarch64/arch_features.h b/include/lib/aarch64/arch_features.h
index 2c9ae80..27f5c82 100644
--- a/include/lib/aarch64/arch_features.h
+++ b/include/lib/aarch64/arch_features.h
@@ -537,4 +537,22 @@
 	return EXTRACT(ID_AA64PFR2_EL1_FPMR, read_id_aa64pfr2_el1())
 		== ID_AA64PFR2_EL1_FPMR_SUPPORTED;
 }
+
+static inline bool is_feat_sctlr2_supported(void)
+{
+	return (((read_id_aa64mmfr3_el1() >> ID_AA64MMFR3_EL1_SCTLRX_SHIFT) &
+		ID_AA64MMFR3_EL1_SCTLRX_MASK) == ID_AA64MMFR3_EL1_SCTLR2_SUPPORTED);
+}
+
+static inline bool is_feat_the_supported(void)
+{
+	return (((read_id_aa64pfr1_el1() >> ID_AA64PFR1_EL1_THE_SHIFT) &
+		ID_AA64PFR1_EL1_THE_MASK) == ID_AA64PFR1_EL1_THE_SUPPORTED);
+}
+
+static inline bool is_feat_d128_supported(void)
+{
+	return (((read_id_aa64mmfr3_el1() >> ID_AA64MMFR3_EL1_D128_SHIFT) &
+		ID_AA64MMFR3_EL1_D128_MASK) == ID_AA64MMFR3_EL1_D128_SUPPORTED);
+}
 #endif /* ARCH_FEATURES_H */
diff --git a/include/lib/aarch64/arch_helpers.h b/include/lib/aarch64/arch_helpers.h
index 0774e7b..35d2454 100644
--- a/include/lib/aarch64/arch_helpers.h
+++ b/include/lib/aarch64/arch_helpers.h
@@ -324,6 +324,7 @@
 DEFINE_SYSREG_RW_FUNCS(sctlr_el3)
 
 DEFINE_RENAME_SYSREG_RW_FUNCS(sctlr2_el1, SCTLR2_EL1)
+DEFINE_RENAME_SYSREG_RW_FUNCS(sctlr2_el2, SCTLR2_EL2)
 
 DEFINE_SYSREG_RW_FUNCS(actlr_el1)
 DEFINE_SYSREG_RW_FUNCS(actlr_el2)
@@ -648,6 +649,10 @@
 DEFINE_RENAME_SYSREG_RW_FUNCS(pir_el2, PIR_EL2)
 DEFINE_RENAME_SYSREG_RW_FUNCS(s2pir_el2, S2PIR_EL2)
 
+/* Armv8.9 Translation Hardening Extension */
+DEFINE_RENAME_SYSREG_RW_FUNCS(rcwmask_el1, RCWMASK_EL1)
+DEFINE_RENAME_SYSREG_RW_FUNCS(rcwsmask_el1, RCWSMASK_EL1)
+
 /* Armv9.4 Guarded Control Stack Extension */
 DEFINE_RENAME_SYSREG_RW_FUNCS(gcscr_el2, GCSCR_EL2)
 DEFINE_RENAME_SYSREG_RW_FUNCS(gcspr_el2, GCSPR_EL2)
diff --git a/include/lib/extensions/sysreg128.h b/include/lib/extensions/sysreg128.h
new file mode 100644
index 0000000..5be2ffc
--- /dev/null
+++ b/include/lib/extensions/sysreg128.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright (c) 2024, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SYSREG128_H
+#define SYSREG128_H
+
+#include <stdint.h>
+
+#ifdef __aarch64__
+
+/* Assembly function prototypes. */
+uint128_t read128_par_el1(void);
+uint128_t read128_ttbr0_el1(void);
+uint128_t read128_ttbr1_el1(void);
+uint128_t read128_ttbr0_el2(void);
+uint128_t read128_ttbr1_el2(void);
+uint128_t read128_vttbr_el2(void);
+uint128_t read128_rcwmask_el1(void);
+uint128_t read128_rcwsmask_el1(void);
+
+void write128_par_el1(uint128_t v);
+void write128_ttbr0_el1(uint128_t v);
+void write128_ttbr1_el1(uint128_t v);
+void write128_ttbr0_el2(uint128_t v);
+void write128_ttbr1_el2(uint128_t v);
+void write128_vttbr_el2(uint128_t v);
+void write128_rcwmask_el1(uint128_t v);
+void write128_rcwsmask_el1(uint128_t v);
+
+#endif /* __aarch64__ */
+
+#endif /* SYSREG128_H */
diff --git a/lib/extensions/sysreg128/aarch64/sysreg128_helpers.S b/lib/extensions/sysreg128/aarch64/sysreg128_helpers.S
new file mode 100644
index 0000000..27db8b1
--- /dev/null
+++ b/lib/extensions/sysreg128/aarch64/sysreg128_helpers.S
@@ -0,0 +1,108 @@
+/*
+ * Copyright (c) 2024, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <asm_macros.S>
+
+#if __aarch64__
+
+    .globl read128_par_el1
+    .globl read128_ttbr0_el1
+    .globl read128_ttbr1_el1
+    .globl read128_ttbr0_el2
+    .globl read128_ttbr1_el2
+    .globl read128_vttbr_el2
+    .globl read128_rcwmask_el1
+    .globl read128_rcwsmask_el1
+    .globl write128_par_el1
+    .globl write128_ttbr0_el1
+    .globl write128_ttbr1_el1
+    .globl write128_ttbr0_el2
+    .globl write128_ttbr1_el2
+    .globl write128_vttbr_el2
+    .globl write128_rcwmask_el1
+    .globl write128_rcwsmask_el1
+
+func read128_par_el1 /* MRRS x0, x1, S3_0_C7_C4_0 */
+    .inst 0xD5787400
+    ret
+endfunc read128_par_el1
+
+func read128_ttbr0_el1 /* MRRS x0, x1, S3_0_C2_C0_0 */
+    .inst 0xD5782000
+    ret
+endfunc read128_ttbr0_el1
+
+func read128_ttbr1_el1 /* MRRS x0, x1, S3_0_C2_C0_1 */
+    .inst 0xD5782020
+    ret
+endfunc read128_ttbr1_el1
+
+func read128_ttbr0_el2 /* MRRS x0, x1, S3_4_C2_C0_0 */
+    .inst 0xD57C2000
+    ret
+endfunc read128_ttbr0_el2
+
+func read128_ttbr1_el2 /* MRRS x0, x1, S3_4_C2_C0_1 */
+    .inst 0xD57C2020
+    ret
+endfunc read128_ttbr1_el2
+
+func read128_vttbr_el2 /* MRRS x0, x1, S3_4_C2_C1_0 */
+    .inst 0xD57C2100
+    ret
+endfunc read128_vttbr_el2
+
+func read128_rcwmask_el1 /* MRRS x0, x1, S3_0_C13_C0_6 */
+    .inst 0xD578D0C0
+    ret
+endfunc read128_rcwmask_el1
+
+func read128_rcwsmask_el1 /* MRRS x0, x1, S3_0_C13_C0_3 */
+    .inst 0xD578D060
+    ret
+endfunc read128_rcwsmask_el1
+
+func write128_par_el1 /* MSRR S3_0_C7_C4_0, x0, x1 */
+    .inst 0xD5587400
+    ret
+endfunc write128_par_el1
+
+func write128_ttbr0_el1 /* MSRR S3_0_C2_C0_0, x0, x1 */
+    .inst 0xD5582000
+    ret
+endfunc write128_ttbr0_el1
+
+func write128_ttbr1_el1 /* MSRR S3_0_C2_C0_1, x0, x1 */
+    .inst 0xD5582020
+    ret
+endfunc write128_ttbr1_el1
+
+func write128_ttbr0_el2 /* MSRR S3_4_C2_C0_0, x0, x1 */
+    .inst 0xD55C2000
+    ret
+endfunc write128_ttbr0_el2
+
+func write128_ttbr1_el2 /* MSRR S3_4_C2_C0_1, x0, x1 */
+    .inst 0xD55C2020
+    ret
+endfunc write128_ttbr1_el2
+
+func write128_vttbr_el2 /* MSRR S3_4_C2_C1_0, x0, x1 */
+    .inst 0xD55C2100
+    ret
+endfunc write128_vttbr_el2
+
+func write128_rcwmask_el1 /* MSRR S3_0_C13_C0_6, x0, x1 */
+    .inst 0xD558D0C0
+    ret
+endfunc write128_rcwmask_el1
+
+func write128_rcwsmask_el1 /* MSRR S3_0_C13_C0_3, x0, x1 */
+    .inst 0xD558D060
+    ret
+endfunc write128_rcwsmask_el1
+
+#endif /* __aarch64__ */
diff --git a/tftf/framework/framework.mk b/tftf/framework/framework.mk
index fc0ccbd..4e27995 100644
--- a/tftf/framework/framework.mk
+++ b/tftf/framework/framework.mk
@@ -92,7 +92,8 @@
 	lib/extensions/sme/aarch64/sme_helpers.S			\
 	lib/extensions/sme/aarch64/sme2_helpers.S			\
 	lib/extensions/sve/aarch64/sve.c				\
-	lib/extensions/sve/aarch64/sve_helpers.S
+	lib/extensions/sve/aarch64/sve_helpers.S			\
+	lib/extensions/sysreg128/aarch64/sysreg128_helpers.S
 endif
 
 ifeq (${ARCH},aarch64)
diff --git a/tftf/tests/extensions/d128/test_d128.c b/tftf/tests/extensions/d128/test_d128.c
new file mode 100644
index 0000000..1e8df0c
--- /dev/null
+++ b/tftf/tests/extensions/d128/test_d128.c
@@ -0,0 +1,68 @@
+/*
+ * Copyright (c) 2024, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/extensions/sysreg128.h>
+#include <test_helpers.h>
+
+#include "./test_d128.h"
+
+/*
+ * @brief Test FEAT_D128 feature support when the extension is enabled.
+ *
+ * The test ensures that access to common system registers PAR_EL1, TTBR0_EL1, TTBR1_EL1,
+ * TTBR0_EL2, VTTBR_EL2 and the VHE register TTBR1_EL2 do not trap to EL3.
+ *
+ * Additonally this test verifies that EL1 registers are properly context switched by
+ * making dummy SMC call to TSPD (running at S-EL1).
+ *
+ * TODO: EL2 registers context switching.
+ *
+ * @return test_result_t
+ */
+test_result_t test_d128_support(void)
+{
+#ifdef __aarch64__
+	SKIP_TEST_IF_D128_NOT_SUPPORTED();
+	SKIP_TEST_IF_TSP_NOT_PRESENT();
+
+	smc_args tsp_svc_params;
+
+	/* Access 128 bit variant of system registers */
+	read128_par_el1();
+	read128_ttbr0_el1();
+	read128_ttbr1_el1();
+	read128_ttbr0_el2();
+	read128_ttbr1_el2();
+	if (is_armv8_1_vhe_present()) {
+		read128_vttbr_el2();
+	}
+
+	/*
+	 * Check that EL1 registers are properly contexted by EL3. Write to the registers
+	 * make dummy SMC call TSP and ensure that values are prorperly contexted.
+	 * NOTE: Ideal way would be to modify these registers in TSP in SMC handler
+	 */
+	write128_par_el1(PAR_EL1_MASK_FULL);
+	write128_ttbr0_el1(TTBR_REG_MASK_FULL);
+	write128_ttbr1_el1(TTBR_REG_MASK_FULL);
+
+	tsp_svc_params.fid = TSP_STD_FID(TSP_ADD);
+	tsp_svc_params.arg1 = 4;
+	tsp_svc_params.arg2 = 6;
+	tftf_smc(&tsp_svc_params);
+
+	if ((read128_par_el1() != PAR_EL1_MASK_FULL) ||
+	    (read128_ttbr0_el1() != TTBR_REG_MASK_FULL) ||
+	    (read128_ttbr1_el1() != TTBR_REG_MASK_FULL)) {
+		NOTICE("Unexpected value of registers after context switch\n");
+		return TEST_RESULT_FAIL;
+	}
+
+	return TEST_RESULT_SUCCESS;
+#else
+	SKIP_TEST_IF_AARCH32();
+#endif /* __aarch64_ */
+}
diff --git a/tftf/tests/extensions/d128/test_d128.h b/tftf/tests/extensions/d128/test_d128.h
new file mode 100644
index 0000000..2e40753
--- /dev/null
+++ b/tftf/tests/extensions/d128/test_d128.h
@@ -0,0 +1,20 @@
+/*
+ * Copyright (c) 2024, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef TEST_D128_H
+#define TEST_D128_H
+
+/* Create Mask setting all used bits except PA */
+#define PAR_EL1_MASK_HIGH		0x0000000000000001
+#define PAR_EL1_MASK_LOW		0xFF00000000000F81
+#define PAR_EL1_MASK_FULL		(((uint128_t)PAR_EL1_MASK_HIGH) << 64U | PAR_EL1_MASK_LOW)
+
+/* Masks created by setting all used bits */
+#define TTBR_REG_MASK_HIGH		0x0000000000FF0000
+#define TTBR_REG_MASK_LOW		0xFFFFFFFFFFFFFFE7
+#define TTBR_REG_MASK_FULL		(((uint128_t)TTBR_REG_MASK_HIGH) << 64U | TTBR_REG_MASK_LOW)
+
+#endif /* TEST_D128_H */
diff --git a/tftf/tests/extensions/sctlr2/test_sctlr2.c b/tftf/tests/extensions/sctlr2/test_sctlr2.c
new file mode 100644
index 0000000..704d19d
--- /dev/null
+++ b/tftf/tests/extensions/sctlr2/test_sctlr2.c
@@ -0,0 +1,52 @@
+/*
+ * Copyright (c) 2024, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <test_helpers.h>
+
+/*
+ * @brief Test FEAT_SCTLR2 support when the extension is enabled.
+ *
+ * Ensure that reading of SCTLR2_EL1 and SCTLR2_EL2 system registers does not
+ * trap to EL3. Also, verify that SCTLR2_EL1 register value is preserved
+ * correctly during world switch to TSP.
+ *
+ * TODO: Test context switching of SCTLR2_EL2
+ *
+ * @return test_result_t
+ */
+test_result_t test_sctlr2_support(void)
+{
+#ifdef __aarch64__
+	SKIP_TEST_IF_SCTLR2_NOT_SUPPORTED();
+	SKIP_TEST_IF_TSP_NOT_PRESENT();
+
+	smc_args tsp_svc_params;
+
+	/* Access the registers */
+	read_sctlr2_el1();
+	read_sctlr2_el2();
+
+	/* Check that SCTLR2_EL1 is properly contexted by EL3. Write to the registers
+	 * make dummy SMC call TSP and ensure that values are prorperly contexted.
+	 * Set EnIDCP128_BIT which is guaranteed to be present if SCTLR2 is present.
+	 */
+	write_sctlr2_el1(read_sctlr2_el1() | SCTLR2_EnIDCP128_BIT);
+
+	tsp_svc_params.fid = TSP_STD_FID(TSP_ADD);
+	tsp_svc_params.arg1 = 4;
+	tsp_svc_params.arg2 = 6;
+	tftf_smc(&tsp_svc_params);
+
+	if ((read_sctlr2_el1() & SCTLR2_EnIDCP128_BIT) == 0) {
+		ERROR("SCTLR2_EL1 unexpected value after context switch\n");
+		return TEST_RESULT_FAIL;
+	}
+
+	return TEST_RESULT_SUCCESS;
+#else
+	SKIP_TEST_IF_AARCH32();
+#endif /* __aarch64_ */
+}
diff --git a/tftf/tests/extensions/the/test_the.c b/tftf/tests/extensions/the/test_the.c
new file mode 100644
index 0000000..b56be93
--- /dev/null
+++ b/tftf/tests/extensions/the/test_the.c
@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2024, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/extensions/sysreg128.h>
+#include <test_helpers.h>
+
+#include "./test_the.h"
+
+/*
+ * @brief Test FEAT_THE support when the extension is enabled.
+ *
+ * Write to the RCWMASK_EL1 and RCWSMASK_EL1 system registers with
+ * a mask of allowed bits in 128-bit mode, if FEAT_D128 is supported,
+ * otherwise in 64-bit mode, perform a dummy SMC call, then read the
+ * registers back.
+ *
+ * The test ensures that accesses to these registers do not trap
+ * to EL3 as well as that their values are preserved correctly.
+ *
+ * @return test_result_t
+ */
+test_result_t test_the_support(void)
+{
+#ifdef __aarch64__
+	SKIP_TEST_IF_THE_NOT_SUPPORTED();
+	SKIP_TEST_IF_TSP_NOT_PRESENT();
+
+	smc_args tsp_svc_params;
+	/* Standard SMC to ADD two numbers */
+	tsp_svc_params.fid = TSP_STD_FID(TSP_ADD);
+	tsp_svc_params.arg1 = 4;
+	tsp_svc_params.arg2 = 6;
+
+	if (is_feat_d128_supported()) {
+		uint128_t reg_read;
+
+		write128_rcwmask_el1(RCWMASK_EL1_MASK_FULL);
+		write128_rcwsmask_el1(RCWSMASK_EL1_MASK_FULL);
+		tftf_smc(&tsp_svc_params);
+
+		reg_read = read128_rcwmask_el1();
+		if (reg_read != RCWMASK_EL1_MASK_FULL) {
+			NOTICE("RCWMASK_EL1 unexpected value after context switch: 0x%llx:0x%llx\n",
+				(uint64_t)(reg_read >> 64U), (uint64_t)reg_read);
+			return TEST_RESULT_FAIL;
+		}
+
+		reg_read = read128_rcwsmask_el1();
+		if (reg_read != RCWSMASK_EL1_MASK_FULL) {
+			NOTICE("RCWSMASK_EL1 unexpected value after context switch: 0x%llx:0x%llx\n",
+				(uint64_t)(reg_read >> 64U), (uint64_t)reg_read);
+			return TEST_RESULT_FAIL;
+		}
+
+		return TEST_RESULT_SUCCESS;
+	}
+
+	uint64_t reg_read;
+
+	write_rcwmask_el1(RCWMASK_EL1_MASK_LOW);
+	write_rcwsmask_el1(RCWSMASK_EL1_MASK_LOW);
+	tftf_smc(&tsp_svc_params);
+
+	reg_read = read_rcwmask_el1();
+	if (reg_read != RCWMASK_EL1_MASK_LOW) {
+		NOTICE("RCWMASK_EL1 unexpected value after context switch: 0x%llx\n", reg_read);
+		return TEST_RESULT_FAIL;
+	}
+
+	reg_read = read_rcwsmask_el1();
+	if (reg_read != RCWSMASK_EL1_MASK_LOW) {
+		NOTICE("RCWSMASK_EL1 unexpected value after context switch: 0x%llx\n", reg_read);
+		return TEST_RESULT_FAIL;
+	}
+
+	return TEST_RESULT_SUCCESS;
+#else
+	SKIP_TEST_IF_AARCH32();
+#endif /* __aarch64_ */
+}
diff --git a/tftf/tests/extensions/the/test_the.h b/tftf/tests/extensions/the/test_the.h
new file mode 100644
index 0000000..92a3768
--- /dev/null
+++ b/tftf/tests/extensions/the/test_the.h
@@ -0,0 +1,20 @@
+/*
+ * Copyright (c) 2024, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef TEST_THE_H
+#define TEST_THE_H
+
+#define RCWMASK_EL1_MASK_HIGH	0x807BE01FF8000000
+#define RCWMASK_EL1_MASK_LOW	0xFFEC00000003FFFE
+#define RCWMASK_EL1_MASK_FULL	\
+    (((uint128_t)RCWMASK_EL1_MASK_HIGH) << 64U | RCWMASK_EL1_MASK_LOW)
+
+#define RCWSMASK_EL1_MASK_HIGH	0x807BE01FF8000000
+#define RCWSMASK_EL1_MASK_LOW	0xFFFC00000003FFFE
+#define RCWSMASK_EL1_MASK_FULL	\
+    (((uint128_t)RCWSMASK_EL1_MASK_HIGH) << 64U | RCWSMASK_EL1_MASK_LOW)
+
+#endif /* TEST_THE_H */
diff --git a/tftf/tests/tests-cpu-extensions.mk b/tftf/tests/tests-cpu-extensions.mk
index a6d6e2a..39783b1 100644
--- a/tftf/tests/tests-cpu-extensions.mk
+++ b/tftf/tests/tests-cpu-extensions.mk
@@ -8,6 +8,7 @@
 	extensions/afp/test_afp.c					\
 	extensions/amu/test_amu.c					\
 	extensions/brbe/test_brbe.c					\
+	extensions/d128/test_d128.c					\
 	extensions/debugv8p9/test_debugv8p9.c				\
 	extensions/ecv/test_ecv.c					\
 	extensions/fgt/test_fgt.c					\
@@ -18,11 +19,13 @@
 	extensions/mte/test_mte.c					\
 	extensions/pauth/test_pauth.c					\
 	extensions/pmuv3/test_pmuv3.c					\
+	extensions/sctlr2/test_sctlr2.c					\
 	extensions/sme/test_sme.c					\
 	extensions/sme/test_sme2.c					\
 	extensions/spe/test_spe.c					\
 	extensions/sve/test_sve.c					\
 	extensions/sys_reg_trace/test_sys_reg_trace.c			\
+	extensions/the/test_the.c					\
 	extensions/trbe/test_trbe.c					\
 	extensions/trf/test_trf.c					\
 	extensions/wfxt/test_wfxt.c					\
diff --git a/tftf/tests/tests-cpu-extensions.xml b/tftf/tests/tests-cpu-extensions.xml
index 0c89840..bc09f37 100644
--- a/tftf/tests/tests-cpu-extensions.xml
+++ b/tftf/tests/tests-cpu-extensions.xml
@@ -40,6 +40,9 @@
     <testcase name="PMUv3 SMC counter preservation" function="test_pmuv3_el3_preserves" />
     <testcase name="LS64 support" function="test_ls64_instructions" />
     <testcase name="LS64-ACCDATA support" function="test_ls64_accdata_sysreg" />
+    <testcase name="SCTLR2 support" function="test_sctlr2_support" />
+    <testcase name="THE support" function="test_the_support" />
+    <testcase name="D128 support" function="test_d128_support" />
   </testsuite>
 
   <testsuite name="ARM_ARCH_SVC" description="Arm Architecture Service tests">