Merge changes from topic "jc/context_mgmt_tests"
* changes:
feat(cm): add test to validate EL2 regs during context switch
feat(cm): add el2-ctx registers helper macros
feat(cm): add tests to validate EL1 regs during context switch
feat(cm): add el1-ctx register helper macros
diff --git a/include/lib/aarch64/arch.h b/include/lib/aarch64/arch.h
index a641052..cfc573c 100644
--- a/include/lib/aarch64/arch.h
+++ b/include/lib/aarch64/arch.h
@@ -104,7 +104,6 @@
#define ICC_EOIR0_EL1 S3_0_C12_C8_1
#define ICC_EOIR1_EL1 S3_0_C12_C12_1
#define ICC_SGI0R_EL1 S3_0_C12_C11_7
-
#define ICV_CTRL_EL1 S3_0_C12_C12_4
#define ICV_IAR1_EL1 S3_0_C12_C12_0
#define ICV_IGRPEN1_EL1 S3_0_C12_C12_7
@@ -112,6 +111,29 @@
#define ICV_PMR_EL1 S3_0_C4_C6_0
/*******************************************************************************
+ * Definitions for EL2 system registers.
+ ******************************************************************************/
+#define CNTPOFF_EL2 S3_4_C14_C0_6
+#define HDFGRTR2_EL2 S3_4_C3_C1_0
+#define HDFGWTR2_EL2 S3_4_C3_C1_1
+#define HFGRTR2_EL2 S3_4_C3_C1_2
+#define HFGWTR2_EL2 S3_4_C3_C1_3
+#define HDFGRTR_EL2 S3_4_C3_C1_4
+#define HDFGWTR_EL2 S3_4_C3_C1_5
+#define HAFGRTR_EL2 S3_4_C3_C1_6
+#define HFGITR2_EL2 S3_4_C3_C1_7
+#define HFGITR_EL2 S3_4_C1_C1_6
+#define HFGRTR_EL2 S3_4_C1_C1_4
+#define HFGWTR_EL2 S3_4_C1_C1_5
+#define ICH_HCR_EL2 S3_4_C12_C11_0
+#define ICH_VMCR_EL2 S3_4_C12_C11_7
+#define VNCR_EL2 S3_4_C2_C2_0
+#define PMSCR_EL2 S3_4_C9_C9_0
+#define TFSR_EL2 S3_4_C5_C6_0
+#define CONTEXTIDR_EL2 S3_4_C13_C0_1
+#define TTBR1_EL2 S3_4_C2_C0_1
+
+/*******************************************************************************
* Generic timer memory mapped registers & offsets
******************************************************************************/
#define CNTCR_OFF U(0x000)
@@ -384,7 +406,8 @@
#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)
-
+#define ID_AA64MMFR1_EL1_VHE_SHIFT ULL(8)
+#define ID_AA64MMFR1_EL1_VHE_MASK ULL(0xf)
/* ID_AA64MMFR2_EL1 definitions */
#define ID_AA64MMFR2_EL1 S3_0_C0_C7_2
@@ -395,41 +418,54 @@
#define ID_AA64MMFR2_EL1_CNP_SHIFT U(0)
#define ID_AA64MMFR2_EL1_CNP_MASK ULL(0xf)
-/* ID_AA64MMFR3_EL1 definitions */
-#define ID_AA64MMFR3_EL1 S3_0_C0_C7_3
+#define ID_AA64MMFR2_EL1_NV_SHIFT U(24)
+#define ID_AA64MMFR2_EL1_NV_MASK ULL(0xf)
+#define NV2_IMPLEMENTED ULL(0x2)
-#define ID_AA64MMFR3_TCR2_SHIFT U(0)
-#define ID_AA64MMFR3_TCR2_MASK ULL(0xf)
+/* ID_AA64MMFR3_EL1 definitions */
+#define ID_AA64MMFR3_EL1 S3_0_C0_C7_3
+
+#define ID_AA64MMFR3_EL1_S2POE_SHIFT U(20)
+#define ID_AA64MMFR3_EL1_S2POE_MASK ULL(0xf)
+#define ID_AA64MMFR3_EL1_S2POE_WIDTH U(4)
+#define ID_AA64MMFR3_EL1_S2POE_SUPPORTED ULL(0x1)
+
+#define ID_AA64MMFR3_EL1_S1POE_SHIFT U(16)
+#define ID_AA64MMFR3_EL1_S1POE_MASK ULL(0xf)
+#define ID_AA64MMFR3_EL1_S1POE_WIDTH U(4)
+#define ID_AA64MMFR3_EL1_S1POE_SUPPORTED ULL(0x1)
+
+#define ID_AA64MMFR3_EL1_S2PIE_SHIFT U(12)
+#define ID_AA64MMFR3_EL1_S2PIE_MASK ULL(0xf)
+#define ID_AA64MMFR3_EL1_S2PIE_WIDTH U(4)
+#define ID_AA64MMFR3_EL1_S2PIE_SUPPORTED ULL(0x1)
+
+#define ID_AA64MMFR3_EL1_S1PIE_SHIFT U(8)
+#define ID_AA64MMFR3_EL1_S1PIE_MASK ULL(0xf)
+#define ID_AA64MMFR3_EL1_S1PIE_WIDTH U(4)
+#define ID_AA64MMFR3_EL1_S1PIE_SUPPORTED ULL(0x1)
+
+#define ID_AA64MMFR3_EL1_TCRX_SHIFT U(0)
+#define ID_AA64MMFR3_EL1_TCRX_MASK ULL(0xf)
+#define ID_AA64MMFR3_EL1_TCRX_WIDTH U(4)
+#define ID_AA64MMFR3_EL1_TCR2_SUPPORTED ULL(0x1)
/* ID_AA64PFR1_EL1 definitions */
-#define ID_AA64PFR1_EL1_SSBS_SHIFT U(4)
-#define ID_AA64PFR1_EL1_SSBS_MASK ULL(0xf)
+#define ID_AA64PFR1_EL1_GCS_SHIFT U(44)
+#define ID_AA64PFR1_EL1_GCS_MASK ULL(0xf)
+#define ID_AA64PFR1_EL1_GCS_WIDTH U(4)
+#define ID_AA64PFR1_EL1_GCS_SUPPORTED ULL(1)
-#define SSBS_UNAVAILABLE ULL(0) /* No architectural SSBS support */
-
-#define ID_AA64PFR1_EL1_BT_SHIFT U(0)
-#define ID_AA64PFR1_EL1_BT_MASK ULL(0xf)
-
-#define BTI_IMPLEMENTED ULL(1) /* The BTI mechanism is implemented */
-
-#define ID_AA64PFR1_EL1_MTE_SHIFT U(8)
-#define ID_AA64PFR1_EL1_MTE_MASK ULL(0xf)
-
-#define ID_AA64PFR1_EL1_RNDR_TRAP_SHIFT U(28)
-#define ID_AA64PFR1_EL1_RNDR_TRAP_MASK ULL(0xf)
-
-#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_MASK ULL(0xf)
#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 */
+#define ID_AA64PFR1_EL1_RNDR_TRAP_SHIFT U(28)
+#define ID_AA64PFR1_EL1_RNDR_TRAP_MASK ULL(0xf)
+#define ID_AA64PFR1_EL1_RNG_TRAP_SUPPORTED ULL(0x1)
+#define ID_AA64PFR1_EL1_RNG_TRAP_NOT_SUPPORTED ULL(0x0)
#define ID_AA64PFR1_EL1_SME_SHIFT U(24)
#define ID_AA64PFR1_EL1_SME_MASK ULL(0xf)
@@ -438,13 +474,28 @@
#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_MPAM_FRAC_SHIFT U(16)
+#define ID_AA64PFR1_MPAM_FRAC_MASK ULL(0xf)
+
#define ID_AA64PFR1_RAS_FRAC_SHIFT U(12)
+#define ID_AA64PFR1_RAS_FRAC_MASK ULL(0xf)
#define ID_AA64PFR1_RAS_FRAC_WIDTH U(4)
#define ID_AA64PFR1_RASV1P1_SUPPORTED ULL(0x1)
-#define ID_AA64PFR1_MPAM_FRAC_SHIFT U(16)
-#define ID_AA64PFR1_MPAM_FRAC_MASK ULL(0xf)
+#define ID_AA64PFR1_EL1_MTE_SHIFT U(8)
+#define ID_AA64PFR1_EL1_MTE_MASK ULL(0xf)
+#define ID_AA64PFR1_EL1_MTE_WIDTH U(4)
+#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 */
+
+#define ID_AA64PFR1_EL1_SSBS_SHIFT U(4)
+#define ID_AA64PFR1_EL1_SSBS_MASK ULL(0xf)
+#define SSBS_UNAVAILABLE ULL(0) /* No architectural SSBS support */
+
+#define ID_AA64PFR1_EL1_BT_SHIFT U(0)
+#define ID_AA64PFR1_EL1_BT_MASK ULL(0xf)
+#define BTI_IMPLEMENTED ULL(1) /* The BTI mechanism is implemented */
/* ID_PFR1_EL1 definitions */
#define ID_PFR1_VIRTEXT_SHIFT U(12)
@@ -1456,5 +1507,33 @@
#define HFGWTR_EL2_FEAT_PAUTH_MASK ULL(0x1f0)
#define HFGWTR_EL2_NON_FEAT_DEPENDENT_MASK ULL(0x7f2903380b)
+/*******************************************************************************
+ * Permission indirection and overlay Registers
+ ******************************************************************************/
+#define PIRE0_EL2 S3_4_C10_C2_2
+#define PIR_EL2 S3_4_C10_C2_3
+#define POR_EL2 S3_4_C10_C2_4
+#define S2PIR_EL2 S3_4_C10_C2_5
+#define PIRE0_EL1 S3_0_C10_C2_2
+#define PIR_EL1 S3_0_C10_C2_3
+#define POR_EL1 S3_0_C10_C2_4
+#define S2POR_EL1 S3_0_C10_C2_5
+
+/*******************************************************************************
+ * FEAT_GCS - Guarded Control Stack Registers
+ ******************************************************************************/
+#define GCSCR_EL2 S3_4_C2_C5_0
+#define GCSPR_EL2 S3_4_C2_C5_1
+#define GCSCR_EL1 S3_0_C2_C5_0
+#define GCSCRE0_EL1 S3_0_C2_C5_2
+#define GCSPR_EL1 S3_0_C2_C5_1
+#define GCSPR_EL0 S3_3_C2_C5_1
+
+/*******************************************************************************
+ * Realm management extension register definitions
+ ******************************************************************************/
+#define SCXTNUM_EL2 S3_4_C13_C0_7
+#define SCXTNUM_EL1 S3_0_C13_C0_7
+#define SCXTNUM_EL0 S3_3_C13_C0_7
#endif /* ARCH_H */
diff --git a/include/lib/aarch64/arch_features.h b/include/lib/aarch64/arch_features.h
index 8a6e4b7..c7d824a 100644
--- a/include/lib/aarch64/arch_features.h
+++ b/include/lib/aarch64/arch_features.h
@@ -28,6 +28,12 @@
(id_aa64mmfr1_pan <= ID_AA64MMFR1_EL1_PAN3_SUPPORTED);
}
+static inline bool is_armv8_1_vhe_present(void)
+{
+ return ((read_id_aa64mmfr1_el1() >> ID_AA64MMFR1_EL1_VHE_SHIFT) &
+ ID_AA64MMFR1_EL1_VHE_MASK) == 1U;
+}
+
static inline bool is_armv8_2_pan2_present(void)
{
u_register_t id_aa64mmfr1_pan =
@@ -114,12 +120,24 @@
(read_id_aa64isar2_el1() & mask_id_aa64isar2)) != 0U;
}
+static inline bool is_armv8_4_amuv1_present(void)
+{
+ return ((read_id_aa64pfr0_el1() >> ID_AA64PFR0_AMU_SHIFT) &
+ ID_AA64PFR0_AMU_MASK) == 1U;
+}
+
static inline bool is_armv8_4_dit_present(void)
{
return ((read_id_aa64pfr0_el1() >> ID_AA64PFR0_DIT_SHIFT) &
ID_AA64PFR0_DIT_MASK) == 1U;
}
+static inline bool is_armv8_4_nv2_present(void)
+{
+ return ((read_id_aa64mmfr2_el1() >> ID_AA64MMFR2_EL1_NV_SHIFT) &
+ ID_AA64MMFR2_EL1_NV_MASK) == NV2_IMPLEMENTED;
+}
+
static inline bool is_armv8_4_ttst_present(void)
{
return ((read_id_aa64mmfr2_el1() >> ID_AA64MMFR2_EL1_ST_SHIFT) &
@@ -258,8 +276,8 @@
static inline bool is_feat_tcr2_supported(void)
{
- return (((read_id_aa64mmfr3_el1() >> ID_AA64MMFR3_TCR2_SHIFT) &
- ID_AA64MMFR3_TCR2_MASK) != 0);
+ return (((read_id_aa64mmfr3_el1() >> ID_AA64MMFR3_EL1_TCRX_SHIFT) &
+ ID_AA64MMFR3_EL1_TCRX_MASK) >= ID_AA64MMFR3_EL1_TCR2_SUPPORTED);
}
static inline bool get_feat_pmuv3_supported(void)
@@ -394,7 +412,7 @@
static inline bool is_feat_ras_present(void)
{
return EXTRACT(ID_AA64PFR0_RAS, read_id_aa64pfr0_el1())
- == ID_AA64PFR0_RAS_SUPPORTED;
+ >= ID_AA64PFR0_RAS_SUPPORTED;
}
static inline bool is_feat_rasv1p1_present(void)
@@ -462,4 +480,49 @@
>= ID_AA64DFR0_TRACEBUFFER_SUPPORTED;
}
+static inline bool is_feat_gcs_present(void)
+{
+ return EXTRACT(ID_AA64PFR1_EL1_GCS, read_id_aa64pfr1_el1())
+ >= ID_AA64PFR1_EL1_GCS_SUPPORTED;
+}
+
+static inline bool is_feat_s1poe_present(void)
+{
+ return EXTRACT(ID_AA64MMFR3_EL1_S1POE, read_id_aa64mmfr3_el1())
+ >= ID_AA64MMFR3_EL1_S1POE_SUPPORTED;
+}
+
+static inline bool is_feat_s2poe_present(void)
+{
+ return EXTRACT(ID_AA64MMFR3_EL1_S2POE, read_id_aa64mmfr3_el1())
+ >= ID_AA64MMFR3_EL1_S2POE_SUPPORTED;
+}
+
+static inline bool is_feat_s1pie_present(void)
+{
+ return EXTRACT(ID_AA64MMFR3_EL1_S1PIE, read_id_aa64mmfr3_el1())
+ >= ID_AA64MMFR3_EL1_S1PIE_SUPPORTED;
+}
+
+static inline bool is_feat_s2pie_present(void)
+{
+ return EXTRACT(ID_AA64MMFR3_EL1_S2PIE, read_id_aa64mmfr3_el1())
+ >= ID_AA64MMFR3_EL1_S2PIE_SUPPORTED;
+}
+
+static inline bool is_feat_sxpoe_present(void)
+{
+ return is_feat_s1poe_present() || is_feat_s2poe_present();
+}
+
+static inline bool is_feat_sxpie_present(void)
+{
+ return is_feat_s1pie_present() || is_feat_s2pie_present();
+}
+
+static inline bool is_feat_mte2_present(void)
+{
+ return EXTRACT(ID_AA64PFR1_EL1_MTE, read_id_aa64pfr1_el1())
+ >= MTE_IMPLEMENTED_ELX;
+}
#endif /* ARCH_FEATURES_H */
diff --git a/include/lib/aarch64/arch_helpers.h b/include/lib/aarch64/arch_helpers.h
index 744bbe6..5cb28cb 100644
--- a/include/lib/aarch64/arch_helpers.h
+++ b/include/lib/aarch64/arch_helpers.h
@@ -367,6 +367,7 @@
DEFINE_SYSREG_RW_FUNCS(ttbr0_el3)
DEFINE_SYSREG_RW_FUNCS(ttbr1_el1)
+DEFINE_RENAME_SYSREG_RW_FUNCS(ttbr1_el2, TTBR1_EL2)
DEFINE_SYSREG_RW_FUNCS(vttbr_el2)
@@ -388,6 +389,24 @@
DEFINE_SYSREG_READ_FUNC(cntvct_el0)
DEFINE_SYSREG_RW_FUNCS(cnthctl_el2)
+DEFINE_SYSREG_RW_FUNCS(csselr_el1)
+DEFINE_SYSREG_RW_FUNCS(sp_el1)
+DEFINE_SYSREG_RW_FUNCS(tpidr_el0)
+DEFINE_SYSREG_RW_FUNCS(tpidr_el1)
+DEFINE_SYSREG_RW_FUNCS(tpidrro_el0)
+DEFINE_SYSREG_RW_FUNCS(contextidr_el1)
+DEFINE_SYSREG_RW_FUNCS(mdccint_el1)
+DEFINE_SYSREG_RW_FUNCS(mdscr_el1)
+DEFINE_SYSREG_RW_FUNCS(spsr_abt)
+DEFINE_SYSREG_RW_FUNCS(spsr_und)
+DEFINE_SYSREG_RW_FUNCS(spsr_irq)
+DEFINE_SYSREG_RW_FUNCS(spsr_fiq)
+DEFINE_SYSREG_RW_FUNCS(dacr32_el2)
+DEFINE_SYSREG_RW_FUNCS(ifsr32_el2)
+DEFINE_SYSREG_RW_FUNCS(cntv_ctl_el0)
+DEFINE_SYSREG_RW_FUNCS(cntv_cval_el0)
+DEFINE_SYSREG_RW_FUNCS(cntkctl_el1)
+
#define get_cntp_ctl_enable(x) (((x) >> CNTP_CTL_ENABLE_SHIFT) & \
CNTP_CTL_ENABLE_MASK)
#define get_cntp_ctl_imask(x) (((x) >> CNTP_CTL_IMASK_SHIFT) & \
@@ -494,11 +513,16 @@
DEFINE_RENAME_SYSREG_RW_FUNCS(amcntenclr1_el0, AMCNTENCLR1_EL0)
DEFINE_RENAME_SYSREG_RW_FUNCS(amcntenset1_el0, AMCNTENSET1_EL0)
+/* Armv8.4 Memory Partitioning and Monitoring Extension Registers */
DEFINE_RENAME_SYSREG_READ_FUNC(mpamidr_el1, MPAMIDR_EL1)
DEFINE_RENAME_SYSREG_RW_FUNCS(mpam3_el3, MPAM3_EL3)
DEFINE_RENAME_SYSREG_RW_FUNCS(mpam2_el2, MPAM2_EL2)
DEFINE_RENAME_SYSREG_RW_FUNCS(mpamhcr_el2, MPAMHCR_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(scxtnum_el2, SCXTNUM_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(scxtnum_el1, SCXTNUM_EL1)
+DEFINE_RENAME_SYSREG_RW_FUNCS(scxtnum_el0, SCXTNUM_EL0)
+
/* Static profiling control registers */
DEFINE_RENAME_SYSREG_RW_FUNCS(pmscr_el1, PMSCR_EL1)
DEFINE_RENAME_SYSREG_RW_FUNCS(pmsevfr_el1, PMSEVFR_EL1)
@@ -538,6 +562,7 @@
/* Armv8.2 Registers */
DEFINE_RENAME_SYSREG_READ_FUNC(id_aa64mmfr2_el1, ID_AA64MMFR2_EL1)
+DEFINE_RENAME_SYSREG_READ_FUNC(id_aa64mmfr3_el1, ID_AA64MMFR3_EL1)
/* Armv8.3 Pointer Authentication Registers */
/* Instruction keys A and B */
@@ -563,6 +588,7 @@
DEFINE_RENAME_SYSREG_RW_FUNCS(tfsr_el1, TFSR_EL1)
DEFINE_RENAME_SYSREG_RW_FUNCS(rgsr_el1, RGSR_EL1)
DEFINE_RENAME_SYSREG_RW_FUNCS(gcr_el1, GCR_EL1)
+DEFINE_RENAME_SYSREG_RW_FUNCS(tfsr_el2, TFSR_EL2)
/* Armv8.4 Data Independent Timing */
DEFINE_RENAME_SYSREG_RW_FUNCS(dit, DIT)
@@ -573,6 +599,7 @@
DEFINE_RENAME_SYSREG_RW_FUNCS(hfgitr_el2, HFGITR_EL2)
DEFINE_RENAME_SYSREG_RW_FUNCS(hdfgrtr_el2, HDFGRTR_EL2)
DEFINE_RENAME_SYSREG_RW_FUNCS(hdfgwtr_el2, HDFGWTR_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(hafgrtr_el2, HAFGRTR_EL2)
/* Armv8.9 Fine Grained Virtualization Traps 2 Registers */
DEFINE_RENAME_SYSREG_RW_FUNCS(hfgrtr2_el2, HFGRTR2_EL2)
@@ -607,6 +634,21 @@
DEFINE_RENAME_SYSREG_RW_FUNCS(trfcr_el1, TRFCR_EL1)
DEFINE_RENAME_SYSREG_RW_FUNCS(trfcr_el2, TRFCR_EL2)
+/* Armv8.4 Enhanced Nested Virtualization */
+DEFINE_RENAME_SYSREG_RW_FUNCS(vncr_el2, VNCR_EL2)
+
+/* Armv8.9 Stage 1/2 Permission Overlays */
+DEFINE_RENAME_SYSREG_RW_FUNCS(por_el2, POR_EL2)
+
+/* Armv8.9 Stage 1/2 Permission Indirections */
+DEFINE_RENAME_SYSREG_RW_FUNCS(pire0_el2, PIRE0_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(pir_el2, PIR_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(s2pir_el2, S2PIR_EL2)
+
+/* Armv9.4 Guarded Control Stack Extension */
+DEFINE_RENAME_SYSREG_RW_FUNCS(gcscr_el2, GCSCR_EL2)
+DEFINE_RENAME_SYSREG_RW_FUNCS(gcspr_el2, GCSPR_EL2)
+
/* Trace System Registers */
DEFINE_RENAME_SYSREG_RW_FUNCS(trcauxctlr, TRCAUXCTLR)
DEFINE_RENAME_SYSREG_RW_FUNCS(trcrsr, TRCRSR)
@@ -639,8 +681,45 @@
/* ID_PFR2_EL1 */
DEFINE_RENAME_SYSREG_READ_FUNC(id_pfr2_el1, ID_PFR2_EL1)
-/* ID_AA64MMFR3_EL1 */
-DEFINE_RENAME_SYSREG_READ_FUNC(id_aa64mmfr3_el1, ID_AA64MMFR3_EL1)
+/* FEAT_SxPIE Registers */
+DEFINE_RENAME_SYSREG_RW_FUNCS(pire0_el1, PIRE0_EL1)
+DEFINE_RENAME_SYSREG_RW_FUNCS(pir_el1, PIR_EL1)
+
+/* Armv8.2 RAS Registers */
+DEFINE_RENAME_SYSREG_RW_FUNCS(disr_el1, DISR_EL1)
+
+/* FEAT_SxPOE Registers */
+DEFINE_RENAME_SYSREG_RW_FUNCS(por_el1, POR_EL1)
+DEFINE_RENAME_SYSREG_RW_FUNCS(s2por_el1, S2POR_EL1)
+
+/* FEAT_GCS Registers */
+DEFINE_RENAME_SYSREG_RW_FUNCS(gcscr_el1, GCSCR_EL1)
+DEFINE_RENAME_SYSREG_RW_FUNCS(gcscre0_el1, GCSCRE0_EL1)
+DEFINE_RENAME_SYSREG_RW_FUNCS(gcspr_el1, GCSPR_EL1)
+DEFINE_RENAME_SYSREG_RW_FUNCS(gcspr_el0, GCSPR_EL0)
+
+/* CONTEXTIDR_EL2 */
+DEFINE_RENAME_SYSREG_RW_FUNCS(contextidr_el2, CONTEXTIDR_EL2)
+
+/* Reliability, Availability, Serviceability (RAS) */
+DEFINE_SYSREG_RW_FUNCS(vdisr_el2)
+DEFINE_SYSREG_RW_FUNCS(vsesr_el2)
+
+DEFINE_SYSREG_RW_FUNCS(dbgvcr32_el2)
+DEFINE_SYSREG_RW_FUNCS(hacr_el2)
+DEFINE_SYSREG_RW_FUNCS(hpfar_el2)
+DEFINE_SYSREG_RW_FUNCS(ich_hcr_el2)
+DEFINE_SYSREG_RW_FUNCS(ich_vmcr_el2)
+DEFINE_SYSREG_RW_FUNCS(tpidr_el2)
+DEFINE_SYSREG_RW_FUNCS(vtcr_el2)
+
+static inline u_register_t read_sp(void)
+{
+ u_register_t v;
+ __asm__ volatile ("mov %0, sp" : "=r" (v));
+
+ return v;
+}
#define IS_IN_EL(x) \
(GET_EL(read_CurrentEl()) == MODE_EL##x)
diff --git a/include/lib/context_mgmt/context_el1.h b/include/lib/context_mgmt/context_el1.h
new file mode 100644
index 0000000..6b516da
--- /dev/null
+++ b/include/lib/context_mgmt/context_el1.h
@@ -0,0 +1,176 @@
+/*
+ * Copyright (c) 2024, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef CONTEXT_EL1_H
+#define CONTEXT_EL1_H
+
+#include <stdint.h>
+#include <stdio.h>
+#include <tftf_lib.h>
+
+#define NS_CORRUPT_EL1_REGS 1
+#define NS_RESTORE_EL1_REGS 0
+/**
+ * Structure template to define individual EL1 register.
+ */
+typedef struct el1_reg {
+ char *reg_name;
+ uint64_t reg_value;
+} el1_reg_t;
+
+/*******************************************************************************
+ * EL1 Registers:
+ * AArch64 EL1 system registers which are intended to be saved and restored as
+ * part of context management library during world switch.
+ ******************************************************************************/
+
+typedef struct el1_common_regs {
+ el1_reg_t spsr_el1;
+ el1_reg_t elr_el1;
+ el1_reg_t sctlr_el1;
+ el1_reg_t tcr_el1;
+ el1_reg_t cpacr_el1;
+ el1_reg_t csselr_el1;
+ el1_reg_t sp_el1;
+ el1_reg_t esr_el1;
+ el1_reg_t ttbr0_el1;
+ el1_reg_t ttbr1_el1;
+ el1_reg_t mair_el1;
+ el1_reg_t amair_el1;
+ el1_reg_t actlr_el1;
+ el1_reg_t tpidr_el1;
+ el1_reg_t tpidr_el0;
+ el1_reg_t tpidrro_el0;
+ el1_reg_t par_el1;
+ el1_reg_t far_el1;
+ el1_reg_t afsr0_el1;
+ el1_reg_t afsr1_el1;
+ el1_reg_t contextidr_el1;
+ el1_reg_t vbar_el1;
+ el1_reg_t mdccint_el1;
+ el1_reg_t mdscr_el1;
+} el1_common_regs_t;
+
+typedef struct el1_aarch32_regs {
+ el1_reg_t spsr_abt;
+ el1_reg_t spsr_und;
+ el1_reg_t spsr_irq;
+ el1_reg_t spsr_fiq;
+ el1_reg_t dacr32_el2;
+ el1_reg_t ifsr32_el2;
+} el1_aarch32_regs_t;
+
+typedef struct el1_arch_timer_regs {
+ el1_reg_t cntp_ctl_el0;
+ el1_reg_t cntp_cval_el0;
+ el1_reg_t cntv_ctl_el0;
+ el1_reg_t cntv_cval_el0;
+ el1_reg_t cntkctl_el1;
+} el1_arch_timer_regs_t;
+
+typedef struct el1_mte2_regs {
+ el1_reg_t tfsre0_el1;
+ el1_reg_t tfsr_el1;
+ el1_reg_t rgsr_el1;
+ el1_reg_t gcr_el1;
+} el1_mte2_regs_t;
+
+typedef struct el1_ras_regs {
+ el1_reg_t disr_el1;
+} el1_ras_regs_t;
+
+typedef struct el1_s1pie_regs {
+ el1_reg_t pire0_el1;
+ el1_reg_t pir_el1;
+} el1_s1pie_regs_t;
+
+typedef struct el1_s1poe_regs {
+ el1_reg_t por_el1;
+} el1_s1poe_regs_t;
+
+typedef struct el1_s2poe_regs {
+ el1_reg_t s2por_el1;
+} el1_s2poe_regs_t;
+
+typedef struct el1_tcr2_regs {
+ el1_reg_t tcr2_el1;
+} el1_tcr2_regs_t;
+
+typedef struct el1_trf_regs {
+ el1_reg_t trfcr_el1;
+} el1_trf_regs_t;
+
+typedef struct el1_csv2_2_regs {
+ el1_reg_t scxtnum_el0;
+ el1_reg_t scxtnum_el1;
+} el1_csv2_2_regs_t;
+
+typedef struct el1_gcs_regs {
+ el1_reg_t gcscr_el1;
+ el1_reg_t gcscre0_el1;
+ el1_reg_t gcspr_el1;
+ el1_reg_t gcspr_el0;
+} el1_gcs_regs_t;
+typedef struct el1_ctx_regs {
+ el1_common_regs_t common;
+ el1_aarch32_regs_t el1_aarch32;
+ el1_arch_timer_regs_t arch_timer;
+ el1_mte2_regs_t mte2;
+ el1_ras_regs_t ras;
+ el1_s1pie_regs_t s1pie;
+ el1_s1poe_regs_t s1poe;
+ el1_s2poe_regs_t s2poe;
+ el1_tcr2_regs_t tcr2;
+ el1_trf_regs_t trf;
+ el1_csv2_2_regs_t csv2_2;
+ el1_gcs_regs_t gcs;
+} el1_ctx_regs_t;
+
+/*
+ * Helper macros to access and print members of the el1_ctx_regs_t structure.
+ */
+
+#define PRINT_CTX_MEM_SEPARATOR() \
+ printf("+-----------------------+--------------------+\n"); \
+
+#define PRINT_CTX_MEMBER(feat, reg) \
+ { \
+ printf("| %-15s | 0x%016llx |\n", \
+ (feat->reg).reg_name, (feat->reg).reg_value); \
+ PRINT_CTX_MEM_SEPARATOR(); \
+ }
+
+#define write_el1_ctx_reg(feat, reg, name, val) \
+{ \
+ (feat->reg).reg_name = name; \
+ (feat->reg).reg_value = (uint64_t)val; \
+}
+
+/* Macros to access members of the 'el1_ctx_regs_t' structure */
+#define get_el1_common_regs_ctx(h) (&((el1_ctx_regs_t *) h)->common)
+#define get_el1_aarch32_regs_ctx(h) (&((el1_ctx_regs_t *) h)->el1_aarch32)
+#define get_el1_arch_timer_regs_ctx(h) (&((el1_ctx_regs_t *) h)->arch_timer)
+#define get_el1_mte2_regs_ctx(h) (&((el1_ctx_regs_t *) h)->mte2)
+#define get_el1_ras_regs_ctx(h) (&((el1_ctx_regs_t *) h)->ras)
+#define get_el1_s1pie_regs_ctx(h) (&((el1_ctx_regs_t *) h)->s1pie)
+#define get_el1_s1poe_regs_ctx(h) (&((el1_ctx_regs_t *) h)->s1poe)
+#define get_el1_s2poe_regs_ctx(h) (&((el1_ctx_regs_t *) h)->s2poe)
+#define get_el1_tcr2_regs_ctx(h) (&((el1_ctx_regs_t *) h)->tcr2)
+#define get_el1_trf_regs_ctx(h) (&((el1_ctx_regs_t *) h)->trf)
+#define get_el1_csv2_2_regs_ctx(h) (&((el1_ctx_regs_t *) h)->csv2_2)
+#define get_el1_gcs_regs_ctx(h) (&((el1_ctx_regs_t *) h)->gcs)
+
+/**
+ * --------------------------------------
+ * EL1 context accessor public functions.
+ * --------------------------------------
+ */
+void print_el1_sysregs_context(const el1_ctx_regs_t *el1_ctx);
+void save_el1_sysregs_context(const el1_ctx_regs_t *el1_ctx);
+void modify_el1_context_sysregs(const el1_ctx_regs_t *el1_ctx, const bool modify_option);
+bool compare_el1_contexts(const el1_ctx_regs_t *el1_ctx1, const el1_ctx_regs_t *el1_ctx2);
+
+#endif /* CONTEXT_EL1_H */
diff --git a/include/lib/context_mgmt/context_el2.h b/include/lib/context_mgmt/context_el2.h
new file mode 100644
index 0000000..3c659e3
--- /dev/null
+++ b/include/lib/context_mgmt/context_el2.h
@@ -0,0 +1,218 @@
+/*
+ * Copyright (c) 2024, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef CONTEXT_EL2_H
+#define CONTEXT_EL2_H
+
+#include <stdint.h>
+
+#include <arch_features.h>
+#include <arch_helpers.h>
+#include <debug.h>
+
+/**
+ * Register mask for EL2 corruption test.
+ */
+#define REG_CORRUPTION_MASK ULL(0xffffffffffffffff)
+
+/**
+ * Bit masks for sensitive fields of various registers.
+ */
+#define SCTLR_EL2_EE ULL(0x2000000)
+#define TTBR1_EL2_ASID ULL(0xffff000000000000)
+#define TCR2_EL2_POE ULL(0x8)
+#define GCSCR_EL2_PCRSEL ULL(0x1)
+
+/**
+ * Enum of the supported operations for the el2_modify_registers API.
+ */
+typedef enum {
+ CORRUPT_REGS,
+ RESTORE_REGS
+} el2_modify_operation_t;
+
+/*******************************************************************************
+ * EL2 Registers:
+ * AArch64 EL2 system register context structure for preserving the
+ * architectural state during world switches.
+ ******************************************************************************/
+typedef struct el2_common_regs {
+ uint64_t actlr_el2;
+ uint64_t afsr0_el2;
+ uint64_t afsr1_el2;
+ uint64_t amair_el2;
+ uint64_t cnthctl_el2;
+ uint64_t cntvoff_el2;
+ uint64_t cptr_el2;
+ uint64_t dbgvcr32_el2;
+ uint64_t elr_el2;
+ uint64_t esr_el2;
+ uint64_t far_el2;
+ uint64_t hacr_el2;
+ uint64_t hcr_el2;
+ uint64_t hpfar_el2;
+ uint64_t hstr_el2;
+ uint64_t icc_sre_el2;
+ uint64_t ich_hcr_el2;
+ uint64_t ich_vmcr_el2;
+ uint64_t mair_el2;
+ uint64_t mdcr_el2;
+ uint64_t pmscr_el2;
+ uint64_t sctlr_el2;
+ uint64_t spsr_el2;
+ uint64_t sp_el2;
+ uint64_t tcr_el2;
+ uint64_t tpidr_el2;
+ uint64_t ttbr0_el2;
+ uint64_t vbar_el2;
+ uint64_t vmpidr_el2;
+ uint64_t vpidr_el2;
+ uint64_t vtcr_el2;
+ uint64_t vttbr_el2;
+} el2_common_regs_t;
+
+typedef struct el2_mte2_regs {
+ uint64_t tfsr_el2;
+} el2_mte2_regs_t;
+
+typedef struct el2_fgt_regs {
+ uint64_t hdfgrtr_el2;
+ uint64_t hafgrtr_el2;
+ uint64_t hdfgwtr_el2;
+ uint64_t hfgitr_el2;
+ uint64_t hfgrtr_el2;
+ uint64_t hfgwtr_el2;
+} el2_fgt_regs_t;
+
+typedef struct el2_fgt2_regs {
+ uint64_t hdfgrtr2_el2;
+ uint64_t hdfgwtr2_el2;
+ uint64_t hfgitr2_el2;
+ uint64_t hfgrtr2_el2;
+ uint64_t hfgwtr2_el2;
+} el2_fgt2_regs_t;
+
+typedef struct el2_ecv_regs {
+ uint64_t cntpoff_el2;
+} el2_ecv_regs_t;
+
+typedef struct el2_vhe_regs {
+ uint64_t contextidr_el2;
+ uint64_t ttbr1_el2;
+} el2_vhe_regs_t;
+
+typedef struct el2_ras_regs {
+ uint64_t vdisr_el2;
+ uint64_t vsesr_el2;
+} el2_ras_regs_t;
+
+typedef struct el2_neve_regs {
+ uint64_t vncr_el2;
+} el2_neve_regs_t;
+
+typedef struct el2_trf_regs {
+ uint64_t trfcr_el2;
+} el2_trf_regs_t;
+
+typedef struct el2_csv2_regs {
+ uint64_t scxtnum_el2;
+} el2_csv2_regs_t;
+
+typedef struct el2_hcx_regs {
+ uint64_t hcrx_el2;
+} el2_hcx_regs_t;
+
+typedef struct el2_tcr2_regs {
+ uint64_t tcr2_el2;
+} el2_tcr2_regs_t;
+
+typedef struct el2_sxpoe_regs {
+ uint64_t por_el2;
+} el2_sxpoe_regs_t;
+
+typedef struct el2_sxpie_regs {
+ uint64_t pire0_el2;
+ uint64_t pir_el2;
+} el2_sxpie_regs_t;
+
+typedef struct el2_s2pie_regs {
+ uint64_t s2pir_el2;
+} el2_s2pie_regs_t;
+
+typedef struct el2_gcs_regs {
+ uint64_t gcscr_el2;
+ uint64_t gcspr_el2;
+} el2_gcs_regs_t;
+
+typedef struct el2_mpam_regs {
+ uint64_t mpam2_el2;
+ uint64_t mpamhcr_el2;
+ uint64_t mpamvpm0_el2;
+ uint64_t mpamvpm1_el2;
+ uint64_t mpamvpm2_el2;
+ uint64_t mpamvpm3_el2;
+ uint64_t mpamvpm4_el2;
+ uint64_t mpamvpm5_el2;
+ uint64_t mpamvpm6_el2;
+ uint64_t mpamvpm7_el2;
+ uint64_t mpamvpmv_el2;
+} el2_mpam_regs_t;
+
+typedef struct el2_sysregs {
+ el2_common_regs_t common;
+
+ el2_mte2_regs_t mte2;
+ el2_fgt_regs_t fgt;
+ el2_fgt2_regs_t fgt2;
+ el2_ecv_regs_t ecv;
+ el2_vhe_regs_t vhe;
+ el2_ras_regs_t ras;
+ el2_neve_regs_t neve;
+ el2_trf_regs_t trf;
+ el2_csv2_regs_t csv2;
+ el2_hcx_regs_t hcx;
+ el2_tcr2_regs_t tcr2;
+ el2_sxpoe_regs_t sxpoe;
+ el2_sxpie_regs_t sxpie;
+ el2_s2pie_regs_t s2pie;
+ el2_gcs_regs_t gcs;
+ el2_mpam_regs_t mpam;
+} el2_sysregs_t;
+
+/******************************************************************************/
+/**
+ * --------------------------
+ * EL2 context helper macros.
+ * --------------------------
+ */
+#define EL2_SAVE_CTX_REG(ctx, reg_member) \
+ ctx->reg_member = read_##reg_member()
+
+#define EL2_SAVE_CTX_SP(ctx) \
+ do { \
+ if (read_spsel() == 1) \
+ ctx->sp_el2 = read_sp(); \
+ } while (0)
+
+#define EL2_WRITE_MASK_CTX_REG(ctx, reg_member, mask) \
+ write_##reg_member(ctx->reg_member | (mask))
+
+#define EL2_PRINT_CTX_HEADING(heading) \
+ INFO("\t%s:\n", heading)
+
+#define EL2_PRINT_CTX_MEMBER(ctx, reg_name, reg_member) \
+ INFO("\t-- %s: 0x%llx\n", reg_name, ctx->reg_member)
+
+/**
+ * --------------------------------------
+ * EL2 context accessor public functions.
+ * --------------------------------------
+ */
+void el2_save_registers(el2_sysregs_t *ctx);
+void el2_modify_registers(const el2_sysregs_t *ctx, const el2_modify_operation_t op);
+void el2_dump_register_context(const char *ctx_name, const el2_sysregs_t *ctx);
+
+#endif /* CONTEXT_EL2_H */
diff --git a/include/runtime_services/secure_el1_payloads/tsp.h b/include/runtime_services/secure_el1_payloads/tsp.h
index 19db911..db52e6b 100644
--- a/include/runtime_services/secure_el1_payloads/tsp.h
+++ b/include/runtime_services/secure_el1_payloads/tsp.h
@@ -1,5 +1,5 @@
/*
- * Copyright (c) 2018-2022, Arm Limited. All rights reserved.
+ * Copyright (c) 2018-2024, Arm Limited. All rights reserved.
*
* SPDX-License-Identifier: BSD-3-Clause
*/
@@ -27,12 +27,13 @@
* Identifiers for various TSP services. Corresponding function IDs (whether
* fast or standard) are generated by macros defined below
*/
-#define TSP_ADD 0x2000
-#define TSP_SUB 0x2001
-#define TSP_MUL 0x2002
-#define TSP_DIV 0x2003
-#define TSP_HANDLE_SEL1_INTR_AND_RETURN 0x2004
-#define TSP_CHECK_DIT 0x2005
+#define TSP_ADD 0x2000
+#define TSP_SUB 0x2001
+#define TSP_MUL 0x2002
+#define TSP_DIV 0x2003
+#define TSP_HANDLE_SEL1_INTR_AND_RETURN 0x2004
+#define TSP_CHECK_DIT 0x2005
+#define TSP_MODIFY_EL1_CTX 0x2006
/*
* Identify a TSP service from function ID filtering the last 16 bits from the
diff --git a/lib/context_mgmt/aarch64/context_el1.c b/lib/context_mgmt/aarch64/context_el1.c
new file mode 100644
index 0000000..6d500aa
--- /dev/null
+++ b/lib/context_mgmt/aarch64/context_el1.c
@@ -0,0 +1,535 @@
+/*
+ * Copyright (c) 2024, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch_features.h>
+#include <arch_helpers.h>
+#include <lib/context_mgmt/context_el1.h>
+
+#define EL1_CTX_CORRUPT_MASK ULL(0xffff0000)
+#define EL1_CTX_RESTORE_MASK ULL(0x0)
+
+/**
+ * ---------------------------------------------------------------
+ * Private Helper functions to save EL1 system context registers.
+ * ---------------------------------------------------------------
+ */
+static void save_el1_ctx_common_regs(const el1_ctx_regs_t *ctx)
+{
+ el1_common_regs_t *el1_common = get_el1_common_regs_ctx(ctx);
+
+ write_el1_ctx_reg(el1_common, spsr_el1, "spsr_el1", read_spsr_el1());
+ write_el1_ctx_reg(el1_common, elr_el1, "elr_el1", read_elr_el1());
+ write_el1_ctx_reg(el1_common, sctlr_el1, "sctlr_el1", read_sctlr_el1());
+ write_el1_ctx_reg(el1_common, tcr_el1, "tcr_el1", read_tcr_el1());
+ write_el1_ctx_reg(el1_common, cpacr_el1, "cpacr_el1", read_cpacr_el1());
+ write_el1_ctx_reg(el1_common, csselr_el1, "csselr_el1", read_csselr_el1());
+ write_el1_ctx_reg(el1_common, sp_el1, "sp_el1", read_sp_el1());
+ write_el1_ctx_reg(el1_common, esr_el1, "esr_el1", read_esr_el1());
+ write_el1_ctx_reg(el1_common, ttbr0_el1, "ttbr0_el1", read_ttbr0_el1());
+ write_el1_ctx_reg(el1_common, ttbr1_el1, "ttbr1_el1", read_ttbr1_el1());
+ write_el1_ctx_reg(el1_common, mair_el1, "mair_el1", read_mair_el1());
+ write_el1_ctx_reg(el1_common, amair_el1, "amair_el1", read_amair_el1());
+ write_el1_ctx_reg(el1_common, actlr_el1, "actlr_el1", read_actlr_el1());
+ write_el1_ctx_reg(el1_common, tpidr_el1, "tpidr_el1", read_tpidr_el1());
+ write_el1_ctx_reg(el1_common, tpidr_el0, "tpidr_el0", read_tpidr_el0());
+ write_el1_ctx_reg(el1_common, tpidrro_el0, "tpidrro_el0", read_tpidrro_el0());
+ write_el1_ctx_reg(el1_common, par_el1, "par_el1", read_par_el1());
+ write_el1_ctx_reg(el1_common, far_el1, "far_el1", read_far_el1());
+ write_el1_ctx_reg(el1_common, afsr0_el1, "afsr0_el1", read_afsr0_el1());
+ write_el1_ctx_reg(el1_common, afsr1_el1, "afsr1_el1", read_afsr1_el1());
+ write_el1_ctx_reg(el1_common, contextidr_el1, "contextidr_el1", read_contextidr_el1());
+ write_el1_ctx_reg(el1_common, vbar_el1, "vbar_el1", read_vbar_el1());
+ write_el1_ctx_reg(el1_common, mdccint_el1, "mdccint_el1", read_mdccint_el1());
+ write_el1_ctx_reg(el1_common, mdscr_el1, "mdscr_el1", read_mdscr_el1());
+}
+
+static void save_el1_ctx_aarch32_regs(const el1_ctx_regs_t *ctx)
+{
+#if CTX_INCLUDE_AARCH32_REGS
+ el1_aarch32_regs_t *el1_aarch32 = get_el1_aarch32_regs_ctx(ctx);
+
+ write_el1_ctx_reg(el1_aarch32, spsr_abt, "spsr_abt", read_spsr_abt());
+ write_el1_ctx_reg(el1_aarch32, spsr_und, "spsr_und", read_spsr_und());
+ write_el1_ctx_reg(el1_aarch32, spsr_irq, "spsr_irq", read_spsr_irq());
+ write_el1_ctx_reg(el1_aarch32, spsr_fiq, "spsr_fiq", read_spsr_fiq());
+ write_el1_ctx_reg(el1_aarch32, dacr32_el2, "dacr32_el2", read_dacr32_el2());
+ write_el1_ctx_reg(el1_aarch32, ifsr32_el2, "ifsr32_el2", read_ifsr32_el2());
+#endif
+}
+
+static void save_el1_ctx_timer_regs(const el1_ctx_regs_t *ctx)
+{
+ el1_arch_timer_regs_t *el1_arch_timer = get_el1_arch_timer_regs_ctx(ctx);
+
+ write_el1_ctx_reg(el1_arch_timer, cntp_ctl_el0, "cntp_ctl_el0", read_cntp_ctl_el0());
+ write_el1_ctx_reg(el1_arch_timer, cntp_cval_el0, "cntp_cval_el0", read_cntp_cval_el0());
+ write_el1_ctx_reg(el1_arch_timer, cntv_ctl_el0, "cntv_ctl_el0", read_cntv_ctl_el0());
+ write_el1_ctx_reg(el1_arch_timer, cntv_cval_el0, "cntv_cval_el0", read_cntv_cval_el0());
+ write_el1_ctx_reg(el1_arch_timer, cntkctl_el1, "cntkctl_el1", read_cntkctl_el1());
+}
+
+static void save_el1_ctx_mte2_regs(const el1_ctx_regs_t *ctx)
+{
+ if (is_feat_mte2_present()) {
+ el1_mte2_regs_t *el1_mte2 = get_el1_mte2_regs_ctx(ctx);
+
+ write_el1_ctx_reg(el1_mte2, tfsre0_el1, "tfsre0_el1", read_tfsre0_el1());
+ write_el1_ctx_reg(el1_mte2, tfsr_el1, "tfsr_el1", read_tfsr_el1());
+ write_el1_ctx_reg(el1_mte2, rgsr_el1, "rgsr_el1", read_rgsr_el1());
+ write_el1_ctx_reg(el1_mte2, gcr_el1, "gcr_el1", read_gcr_el1());
+ }
+}
+
+static void save_el1_ctx_ras_regs(const el1_ctx_regs_t *ctx)
+{
+ if (is_feat_ras_present()) {
+ el1_ras_regs_t *el1_ras = get_el1_ras_regs_ctx(ctx);
+
+ write_el1_ctx_reg(el1_ras, disr_el1, "disr_el1", read_disr_el1());
+ }
+}
+
+static void save_el1_ctx_s1pie_regs(const el1_ctx_regs_t *ctx)
+{
+ if (is_feat_s1pie_present()) {
+ el1_s1pie_regs_t *el1_s1pie = get_el1_s1pie_regs_ctx(ctx);
+ write_el1_ctx_reg(el1_s1pie, pire0_el1, "pire0_el1", read_pire0_el1());
+ write_el1_ctx_reg(el1_s1pie, pir_el1, "pir_el1", read_pir_el1());
+ }
+}
+
+static void save_el1_ctx_s1poe_regs(const el1_ctx_regs_t *ctx)
+{
+ if (is_feat_s1poe_present()) {
+ el1_s1poe_regs_t *el1_s1poe = get_el1_s1poe_regs_ctx(ctx);
+
+ write_el1_ctx_reg(el1_s1poe, por_el1, "por_el1", read_por_el1());
+ }
+}
+
+static void save_el1_ctx_s2poe_regs(const el1_ctx_regs_t *ctx)
+{
+ if (is_feat_s2poe_present()) {
+ el1_s2poe_regs_t *el1_s2poe = get_el1_s2poe_regs_ctx(ctx);
+
+ write_el1_ctx_reg(el1_s2poe, s2por_el1, "s2por_el1", read_s2por_el1());
+ }
+}
+
+static void save_el1_ctx_tcr2_regs(const el1_ctx_regs_t *ctx)
+{
+ if (is_feat_tcr2_supported()) {
+ el1_tcr2_regs_t *el1_tcr2 = get_el1_tcr2_regs_ctx(ctx);
+
+ write_el1_ctx_reg(el1_tcr2, tcr2_el1, "tcr2_el1", read_tcr2_el1());
+ }
+}
+
+static void save_el1_ctx_trf_regs(const el1_ctx_regs_t *ctx)
+{
+ if (get_armv8_4_trf_support()) {
+ el1_trf_regs_t *el1_trf = get_el1_trf_regs_ctx(ctx);
+
+ write_el1_ctx_reg(el1_trf, trfcr_el1, "trfcr_el1", read_trfcr_el1());
+ }
+}
+
+static void save_el1_ctx_csv2_2_regs(const el1_ctx_regs_t *ctx)
+{
+ if (is_feat_csv2_2_present()) {
+ el1_csv2_2_regs_t *el1_csv2_2 = get_el1_csv2_2_regs_ctx(ctx);
+
+ write_el1_ctx_reg(el1_csv2_2, scxtnum_el0, "scxtnum_el0", read_scxtnum_el0());
+ write_el1_ctx_reg(el1_csv2_2, scxtnum_el1, "scxtnum_el1", read_scxtnum_el1());
+ }
+}
+
+static void save_el1_ctx_gcs_regs(const el1_ctx_regs_t *ctx)
+{
+ if (is_feat_gcs_present()) {
+ el1_gcs_regs_t *el1_gcs = get_el1_gcs_regs_ctx(ctx);
+
+ write_el1_ctx_reg(el1_gcs, gcscr_el1, "gcscr_el1", read_gcscr_el1());
+ write_el1_ctx_reg(el1_gcs, gcscre0_el1, "gcscre0_el1", read_gcscre0_el1());
+ write_el1_ctx_reg(el1_gcs, gcspr_el1, "gcspr_el1", read_gcspr_el1());
+ write_el1_ctx_reg(el1_gcs, gcspr_el0, "gcspr_el0", read_gcspr_el0());
+ }
+}
+
+/**
+ * ------------------------------------------------------------------------
+ * Private Helper functions to modify/restore EL1 system context registers.
+ * ------------------------------------------------------------------------
+ */
+static void write_el1_ctx_common_regs(const el1_ctx_regs_t *ctx, uint64_t reg_mask)
+{
+ el1_common_regs_t *el1_common = get_el1_common_regs_ctx(ctx);
+
+ write_spsr_el1((el1_common->spsr_el1.reg_value) | reg_mask);
+ write_elr_el1((el1_common->elr_el1.reg_value) | reg_mask);
+ write_sctlr_el1((el1_common->sctlr_el1.reg_value) | reg_mask);
+ write_tcr_el1((el1_common->tcr_el1.reg_value) | reg_mask);
+ write_cpacr_el1((el1_common->cpacr_el1.reg_value) | reg_mask);
+ write_csselr_el1((el1_common->csselr_el1.reg_value) | reg_mask);
+ write_sp_el1((el1_common->sp_el1.reg_value) | reg_mask);
+ write_esr_el1((el1_common->esr_el1.reg_value) | reg_mask);
+ write_ttbr0_el1((el1_common->ttbr0_el1.reg_value) | reg_mask);
+ write_ttbr1_el1((el1_common->ttbr1_el1.reg_value) | reg_mask);
+ write_mair_el1((el1_common->mair_el1.reg_value) | reg_mask);
+ write_amair_el1((el1_common->amair_el1.reg_value) | reg_mask);
+ write_actlr_el1((el1_common->actlr_el1.reg_value) | reg_mask);
+ write_tpidr_el1((el1_common->tpidr_el1.reg_value) | reg_mask);
+ write_tpidr_el0((el1_common->tpidr_el0.reg_value) | reg_mask);
+ write_tpidrro_el0((el1_common->tpidrro_el0.reg_value) | reg_mask);
+ write_par_el1((el1_common->par_el1.reg_value) | reg_mask);
+ write_far_el1((el1_common->far_el1.reg_value) | reg_mask);
+ write_afsr0_el1((el1_common->afsr0_el1.reg_value) | reg_mask);
+ write_afsr1_el1((el1_common->afsr1_el1.reg_value) | reg_mask);
+ write_contextidr_el1((el1_common->contextidr_el1.reg_value) | reg_mask);
+ write_vbar_el1((el1_common->vbar_el1.reg_value) | reg_mask);
+ write_mdccint_el1((el1_common->mdccint_el1.reg_value) | reg_mask);
+ write_mdscr_el1((el1_common->mdscr_el1.reg_value) | reg_mask);
+}
+
+static void write_el1_ctx_aarch32_regs(const el1_ctx_regs_t *ctx, uint64_t reg_mask)
+{
+#if CTX_INCLUDE_AARCH32_REGS
+ el1_aarch32_regs_t *el1_aarch32 = get_el1_aarch32_regs_ctx(ctx);
+
+ write_spsr_abt((el1_aarch32->spsr_abt.reg_value) | reg_mask);
+ write_spsr_und((el1_aarch32->spsr_und.reg_value) | reg_mask);
+ write_spsr_irq((el1_aarch32->spsr_irq.reg_value) | reg_mask);
+ write_spsr_fiq((el1_aarch32->spsr_fiq.reg_value) | reg_mask);
+ write_dacr32_el2((el1_aarch32->dacr32_el2.reg_value) | reg_mask);
+ write_ifsr32_el2((el1_aarch32->ifsr32_el2.reg_value) | reg_mask);
+#endif
+}
+
+static void write_el1_ctx_timer_regs(const el1_ctx_regs_t *ctx, uint64_t reg_mask)
+{
+ el1_arch_timer_regs_t *el1_arch_timer = get_el1_arch_timer_regs_ctx(ctx);
+
+ write_cntp_ctl_el0((el1_arch_timer->cntp_ctl_el0.reg_value) | reg_mask);
+ write_cntp_cval_el0((el1_arch_timer->cntp_cval_el0.reg_value) | reg_mask);
+ write_cntv_ctl_el0((el1_arch_timer->cntv_ctl_el0.reg_value) | reg_mask);
+ write_cntv_cval_el0((el1_arch_timer->cntv_cval_el0.reg_value) | reg_mask);
+ write_cntkctl_el1((el1_arch_timer->cntkctl_el1.reg_value) | reg_mask);
+}
+
+static void write_el1_ctx_mte2_regs(const el1_ctx_regs_t *ctx, uint64_t reg_mask)
+{
+ if (is_feat_mte2_present()) {
+ el1_mte2_regs_t *el1_mte2 = get_el1_mte2_regs_ctx(ctx);
+
+ write_tfsre0_el1((el1_mte2->tfsre0_el1.reg_value) | reg_mask);
+ write_tfsr_el1((el1_mte2->tfsr_el1.reg_value) | reg_mask);
+ write_rgsr_el1((el1_mte2->rgsr_el1.reg_value) | reg_mask);
+ write_gcr_el1((el1_mte2->gcr_el1.reg_value) | reg_mask);
+ }
+}
+
+static void write_el1_ctx_ras_regs(const el1_ctx_regs_t *ctx, uint64_t reg_mask)
+{
+ if (is_feat_ras_present()) {
+ el1_ras_regs_t *el1_ras = get_el1_ras_regs_ctx(ctx);
+
+ write_disr_el1((el1_ras->disr_el1.reg_value) | reg_mask);
+ }
+}
+
+static void write_el1_ctx_s1pie_regs(const el1_ctx_regs_t *ctx, uint64_t reg_mask)
+{
+ if (is_feat_s1pie_present()) {
+ el1_s1pie_regs_t *el1_s1pie = get_el1_s1pie_regs_ctx(ctx);
+
+ write_pire0_el1((el1_s1pie->pire0_el1.reg_value) | reg_mask);
+ write_pir_el1((el1_s1pie->pir_el1.reg_value) | reg_mask);
+ }
+}
+
+static void write_el1_ctx_s1poe_regs(const el1_ctx_regs_t *ctx, uint64_t reg_mask)
+{
+ if (is_feat_s1poe_present()) {
+ el1_s1poe_regs_t *el1_s1poe = get_el1_s1poe_regs_ctx(ctx);
+
+ write_por_el1((el1_s1poe->por_el1.reg_value) | reg_mask);
+ }
+}
+
+static void write_el1_ctx_s2poe_regs(const el1_ctx_regs_t *ctx, uint64_t reg_mask)
+{
+ if (is_feat_s2poe_present()) {
+ el1_s2poe_regs_t *el1_s2poe = get_el1_s2poe_regs_ctx(ctx);
+
+ write_s2por_el1((el1_s2poe->s2por_el1.reg_value) | reg_mask);
+ }
+}
+
+static void write_el1_ctx_tcr2_regs(const el1_ctx_regs_t *ctx, uint64_t reg_mask)
+{
+ if (is_feat_tcr2_supported()) {
+ el1_tcr2_regs_t *el1_tcr2 = get_el1_tcr2_regs_ctx(ctx);
+
+ write_tcr2_el1((el1_tcr2->tcr2_el1.reg_value) | reg_mask);
+ }
+}
+
+static void write_el1_ctx_trf_regs(const el1_ctx_regs_t *ctx, uint64_t reg_mask)
+{
+ if (get_armv8_4_trf_support()) {
+ el1_trf_regs_t *el1_trf = get_el1_trf_regs_ctx(ctx);
+
+ write_trfcr_el1((el1_trf->trfcr_el1.reg_value) | reg_mask);
+ }
+}
+
+static void write_el1_ctx_csv2_2_regs(const el1_ctx_regs_t *ctx, uint64_t reg_mask)
+{
+ if (is_feat_csv2_2_present()) {
+ el1_csv2_2_regs_t *el1_csv2_2 = get_el1_csv2_2_regs_ctx(ctx);
+
+ write_scxtnum_el0((el1_csv2_2->scxtnum_el0.reg_value) | reg_mask);
+ write_scxtnum_el1((el1_csv2_2->scxtnum_el1.reg_value) | reg_mask);
+ }
+}
+
+static void write_el1_ctx_gcs_regs(const el1_ctx_regs_t *ctx, uint64_t reg_mask)
+{
+ if (is_feat_gcs_present()) {
+ el1_gcs_regs_t *el1_gcs = get_el1_gcs_regs_ctx(ctx);
+
+ write_gcscr_el1((el1_gcs->gcscr_el1.reg_value) | reg_mask);
+ write_gcscre0_el1((el1_gcs->gcscre0_el1.reg_value) | reg_mask);
+ write_gcspr_el1((el1_gcs->gcspr_el1.reg_value) | reg_mask);
+ write_gcspr_el0((el1_gcs->gcspr_el0.reg_value) | reg_mask);
+ }
+}
+
+/**
+ * ------------------------------------------------------------------
+ * Private helper functions to Print each sub structure of the main
+ * EL1 system context structure.
+ * ------------------------------------------------------------------
+ */
+static void print_el1_ctx_common_sysregs(const el1_ctx_regs_t *el1_ctx)
+{
+ el1_common_regs_t *el1_common = get_el1_common_regs_ctx(el1_ctx);
+
+ PRINT_CTX_MEMBER(el1_common, spsr_el1);
+ PRINT_CTX_MEMBER(el1_common, elr_el1);
+ PRINT_CTX_MEMBER(el1_common, sctlr_el1);
+ PRINT_CTX_MEMBER(el1_common, tcr_el1);
+ PRINT_CTX_MEMBER(el1_common, cpacr_el1);
+ PRINT_CTX_MEMBER(el1_common, csselr_el1);
+ PRINT_CTX_MEMBER(el1_common, sp_el1);
+ PRINT_CTX_MEMBER(el1_common, esr_el1);
+ PRINT_CTX_MEMBER(el1_common, ttbr0_el1);
+ PRINT_CTX_MEMBER(el1_common, ttbr1_el1);
+ PRINT_CTX_MEMBER(el1_common, mair_el1);
+ PRINT_CTX_MEMBER(el1_common, amair_el1);
+ PRINT_CTX_MEMBER(el1_common, actlr_el1);
+ PRINT_CTX_MEMBER(el1_common, tpidr_el1);
+ PRINT_CTX_MEMBER(el1_common, tpidr_el0);
+ PRINT_CTX_MEMBER(el1_common, tpidrro_el0);
+ PRINT_CTX_MEMBER(el1_common, par_el1);
+ PRINT_CTX_MEMBER(el1_common, far_el1);
+ PRINT_CTX_MEMBER(el1_common, afsr0_el1);
+ PRINT_CTX_MEMBER(el1_common, afsr1_el1);
+ PRINT_CTX_MEMBER(el1_common, contextidr_el1);
+ PRINT_CTX_MEMBER(el1_common, vbar_el1);
+ PRINT_CTX_MEMBER(el1_common, mdccint_el1);
+ PRINT_CTX_MEMBER(el1_common, mdscr_el1);
+}
+
+static void print_el1_ctx_aarch32_sysregs(const el1_ctx_regs_t *el1_ctx)
+{
+#if CTX_INCLUDE_AARCH32_REGS
+ el1_aarch32_regs_t *el1_aarch32 = get_el1_aarch32_regs_ctx(el1_ctx);
+
+ PRINT_CTX_MEMBER(el1_aarch32, spsr_abt);
+ PRINT_CTX_MEMBER(el1_aarch32, spsr_und);
+ PRINT_CTX_MEMBER(el1_aarch32, spsr_irq);
+ PRINT_CTX_MEMBER(el1_aarch32, spsr_fiq);
+ PRINT_CTX_MEMBER(el1_aarch32, dacr32_el2);
+ PRINT_CTX_MEMBER(el1_aarch32, ifsr32_el2);
+#endif
+}
+
+static void print_el1_ctx_timer_sysregs(const el1_ctx_regs_t *el1_ctx)
+{
+ el1_arch_timer_regs_t *el1_arch_timer = get_el1_arch_timer_regs_ctx(el1_ctx);
+
+ PRINT_CTX_MEMBER(el1_arch_timer, cntp_ctl_el0);
+ PRINT_CTX_MEMBER(el1_arch_timer, cntp_cval_el0);
+ PRINT_CTX_MEMBER(el1_arch_timer, cntv_ctl_el0);
+ PRINT_CTX_MEMBER(el1_arch_timer, cntv_cval_el0);
+ PRINT_CTX_MEMBER(el1_arch_timer, cntkctl_el1);
+}
+
+static void print_el1_ctx_mte2_sysregs(const el1_ctx_regs_t *el1_ctx)
+{
+ el1_mte2_regs_t *el1_mte2 = get_el1_mte2_regs_ctx(el1_ctx);
+
+ PRINT_CTX_MEMBER(el1_mte2, tfsre0_el1);
+ PRINT_CTX_MEMBER(el1_mte2, tfsr_el1);
+ PRINT_CTX_MEMBER(el1_mte2, rgsr_el1);
+ PRINT_CTX_MEMBER(el1_mte2, gcr_el1);
+}
+
+static void print_el1_ctx_ras_sysregs(const el1_ctx_regs_t *el1_ctx)
+{
+ el1_ras_regs_t *el1_ras = get_el1_ras_regs_ctx(el1_ctx);
+
+ PRINT_CTX_MEMBER(el1_ras, disr_el1);
+}
+
+static void print_el1_ctx_s1pie_sysregs(const el1_ctx_regs_t *el1_ctx)
+{
+ el1_s1pie_regs_t *el1_s1pie = get_el1_s1pie_regs_ctx(el1_ctx);
+
+ PRINT_CTX_MEMBER(el1_s1pie, pire0_el1);
+ PRINT_CTX_MEMBER(el1_s1pie, pir_el1);
+}
+
+static void print_el1_ctx_s1poe_sysregs(const el1_ctx_regs_t *el1_ctx)
+{
+ el1_s1poe_regs_t *el1_s1poe = get_el1_s1poe_regs_ctx(el1_ctx);
+
+ PRINT_CTX_MEMBER(el1_s1poe, por_el1);
+}
+
+static void print_el1_ctx_s2poe_sysregs(const el1_ctx_regs_t *el1_ctx)
+{
+ el1_s2poe_regs_t *el1_s2poe = get_el1_s2poe_regs_ctx(el1_ctx);
+
+ PRINT_CTX_MEMBER(el1_s2poe, s2por_el1);
+
+}
+
+static void print_el1_ctx_tcr2_sysregs(const el1_ctx_regs_t *el1_ctx)
+{
+ el1_tcr2_regs_t *el1_tcr2 = get_el1_tcr2_regs_ctx(el1_ctx);
+
+ PRINT_CTX_MEMBER(el1_tcr2, tcr2_el1);
+
+}
+
+static void print_el1_ctx_trf_sysregs(const el1_ctx_regs_t *el1_ctx)
+{
+ el1_trf_regs_t *el1_trf = get_el1_trf_regs_ctx(el1_ctx);
+
+ PRINT_CTX_MEMBER(el1_trf, trfcr_el1);
+}
+
+static void print_el1_ctx_csv2_2_sysregs(const el1_ctx_regs_t *el1_ctx)
+{
+ el1_csv2_2_regs_t *el1_csv2_2 = get_el1_csv2_2_regs_ctx(el1_ctx);
+
+ PRINT_CTX_MEMBER(el1_csv2_2, scxtnum_el0);
+ PRINT_CTX_MEMBER(el1_csv2_2, scxtnum_el1);
+}
+
+static void print_el1_ctx_gcs_sysregs(const el1_ctx_regs_t *el1_ctx)
+{
+ el1_gcs_regs_t *el1_gcs = get_el1_gcs_regs_ctx(el1_ctx);
+
+ PRINT_CTX_MEMBER(el1_gcs, gcscr_el1);
+ PRINT_CTX_MEMBER(el1_gcs, gcscre0_el1);
+ PRINT_CTX_MEMBER(el1_gcs, gcspr_el1);
+ PRINT_CTX_MEMBER(el1_gcs, gcspr_el0);
+}
+
+/**
+ * Public Function: save_el1_sysregs_context.
+ *
+ * @brief: To read the EL1 registers and save it into the context structure.
+ * @param: EL1 system register context struct(el1_ctx_regs_t)
+ */
+void save_el1_sysregs_context(const el1_ctx_regs_t *el1_ctx)
+{
+ save_el1_ctx_common_regs(el1_ctx);
+ save_el1_ctx_aarch32_regs(el1_ctx);
+ save_el1_ctx_timer_regs(el1_ctx);
+ save_el1_ctx_mte2_regs(el1_ctx);
+ save_el1_ctx_ras_regs(el1_ctx);
+ save_el1_ctx_s1pie_regs(el1_ctx);
+ save_el1_ctx_s1poe_regs(el1_ctx);
+ save_el1_ctx_s2poe_regs(el1_ctx);
+ save_el1_ctx_tcr2_regs(el1_ctx);
+ save_el1_ctx_trf_regs(el1_ctx);
+ save_el1_ctx_csv2_2_regs(el1_ctx);
+ save_el1_ctx_gcs_regs(el1_ctx);
+}
+
+/**
+ * Public Function: modify_el1_context_sysregs.
+ *
+ * @brief: To modify the EL1 registers
+ * @param: EL1 system register context struct(el1_ctx_regs_t)
+ */
+void modify_el1_context_sysregs(const el1_ctx_regs_t *el1_ctx, const bool modify_option)
+{
+ uint64_t mask;
+ if (modify_option == NS_CORRUPT_EL1_REGS)
+ mask = EL1_CTX_CORRUPT_MASK;
+ else
+ mask = EL1_CTX_RESTORE_MASK;
+
+ write_el1_ctx_common_regs(el1_ctx, mask);
+ write_el1_ctx_aarch32_regs(el1_ctx, mask);
+ write_el1_ctx_timer_regs(el1_ctx, mask);
+ write_el1_ctx_mte2_regs(el1_ctx, mask);
+ write_el1_ctx_ras_regs(el1_ctx, mask);
+ write_el1_ctx_s1pie_regs(el1_ctx, mask);
+ write_el1_ctx_s1poe_regs(el1_ctx, mask);
+ write_el1_ctx_s2poe_regs(el1_ctx, mask);
+ write_el1_ctx_tcr2_regs(el1_ctx, mask);
+ write_el1_ctx_trf_regs(el1_ctx, mask);
+ write_el1_ctx_csv2_2_regs(el1_ctx, mask);
+ write_el1_ctx_gcs_regs(el1_ctx, mask);
+}
+
+/**
+ * Public Function: print_el1_sysregs_context
+ *
+ * @brief: To print all the members of the EL1 context structure.
+ * @param: EL1 system register context struct pointer (el1_ctx_regs_t *)
+ */
+void print_el1_sysregs_context(const el1_ctx_regs_t *el1_ctx)
+{
+ PRINT_CTX_MEM_SEPARATOR();
+ printf("| EL1 Context Registers | Value |\n");
+ PRINT_CTX_MEM_SEPARATOR();
+ print_el1_ctx_common_sysregs(el1_ctx);
+ print_el1_ctx_aarch32_sysregs(el1_ctx);
+ print_el1_ctx_timer_sysregs(el1_ctx);
+ print_el1_ctx_mte2_sysregs(el1_ctx);
+ print_el1_ctx_ras_sysregs(el1_ctx);
+ print_el1_ctx_s1pie_sysregs(el1_ctx);
+ print_el1_ctx_s1poe_sysregs(el1_ctx);
+ print_el1_ctx_s2poe_sysregs(el1_ctx);
+ print_el1_ctx_tcr2_sysregs(el1_ctx);
+ print_el1_ctx_trf_sysregs(el1_ctx);
+ print_el1_ctx_csv2_2_sysregs(el1_ctx);
+ print_el1_ctx_gcs_sysregs(el1_ctx);
+ printf("\n");
+}
+
+/**
+ * Public Function: compare the EL1 context structures.
+ *
+ * @brief: To compare two explicit EL1 context structure values and return
+ * the status as (TRUE) if equal or (FALSE) if not-equal.
+ * @param: EL1 system register context struct pointers
+ * ( el1_ctx_regs_t *, el1_ctx_regs_t *)
+ */
+bool compare_el1_contexts(const el1_ctx_regs_t *el1_ctx1, const el1_ctx_regs_t *el1_ctx2)
+{
+ if (!memcmp(el1_ctx1, el1_ctx2, sizeof(el1_ctx_regs_t)))
+ return true;
+ else
+ return false;
+}
diff --git a/lib/context_mgmt/aarch64/context_el2.c b/lib/context_mgmt/aarch64/context_el2.c
new file mode 100644
index 0000000..3049274
--- /dev/null
+++ b/lib/context_mgmt/aarch64/context_el2.c
@@ -0,0 +1,599 @@
+/*
+ * Copyright (c) 2024, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/context_mgmt/context_el2.h>
+
+/**
+ * Internal functions to save groups of EL2 registers.
+ */
+
+static void el2_save_common_registers(el2_common_regs_t *ctx)
+{
+ EL2_SAVE_CTX_REG(ctx, actlr_el2);
+ EL2_SAVE_CTX_REG(ctx, afsr0_el2);
+ EL2_SAVE_CTX_REG(ctx, afsr1_el2);
+ EL2_SAVE_CTX_REG(ctx, amair_el2);
+ EL2_SAVE_CTX_REG(ctx, cnthctl_el2);
+ EL2_SAVE_CTX_REG(ctx, cntvoff_el2);
+ EL2_SAVE_CTX_REG(ctx, cptr_el2);
+ /* Accessing DBGVCR32_EL2 may crash the test, so it is omitted. */
+ EL2_SAVE_CTX_REG(ctx, elr_el2);
+ EL2_SAVE_CTX_REG(ctx, esr_el2);
+ EL2_SAVE_CTX_REG(ctx, far_el2);
+ EL2_SAVE_CTX_REG(ctx, hacr_el2);
+ EL2_SAVE_CTX_REG(ctx, hcr_el2);
+ EL2_SAVE_CTX_REG(ctx, hpfar_el2);
+ EL2_SAVE_CTX_REG(ctx, hstr_el2);
+ EL2_SAVE_CTX_REG(ctx, icc_sre_el2);
+ EL2_SAVE_CTX_REG(ctx, ich_hcr_el2);
+ EL2_SAVE_CTX_REG(ctx, ich_vmcr_el2);
+ EL2_SAVE_CTX_REG(ctx, mair_el2);
+ EL2_SAVE_CTX_REG(ctx, mdcr_el2);
+ EL2_SAVE_CTX_REG(ctx, pmscr_el2);
+ EL2_SAVE_CTX_REG(ctx, sctlr_el2);
+ EL2_SAVE_CTX_REG(ctx, spsr_el2);
+ EL2_SAVE_CTX_SP(ctx);
+ EL2_SAVE_CTX_REG(ctx, tcr_el2);
+ EL2_SAVE_CTX_REG(ctx, tpidr_el2);
+ EL2_SAVE_CTX_REG(ctx, ttbr0_el2);
+ EL2_SAVE_CTX_REG(ctx, vbar_el2);
+ EL2_SAVE_CTX_REG(ctx, vmpidr_el2);
+ EL2_SAVE_CTX_REG(ctx, vpidr_el2);
+ EL2_SAVE_CTX_REG(ctx, vtcr_el2);
+ EL2_SAVE_CTX_REG(ctx, vttbr_el2);
+}
+
+static void el2_save_mte2_registers(el2_mte2_regs_t *ctx)
+{
+ if (get_armv8_5_mte_support() == 2) {
+ EL2_SAVE_CTX_REG(ctx, tfsr_el2);
+ }
+}
+
+static void el2_save_fgt_registers(el2_fgt_regs_t *ctx)
+{
+ if (is_armv8_6_fgt_present()) {
+ EL2_SAVE_CTX_REG(ctx, hdfgrtr_el2);
+ if (is_armv8_4_amuv1_present())
+ EL2_SAVE_CTX_REG(ctx, hafgrtr_el2);
+ EL2_SAVE_CTX_REG(ctx, hdfgwtr_el2);
+ EL2_SAVE_CTX_REG(ctx, hfgitr_el2);
+ EL2_SAVE_CTX_REG(ctx, hfgrtr_el2);
+ EL2_SAVE_CTX_REG(ctx, hfgwtr_el2);
+ }
+}
+
+static void el2_save_fgt2_registers(el2_fgt2_regs_t *ctx)
+{
+ if (is_armv8_9_fgt2_present()) {
+ EL2_SAVE_CTX_REG(ctx, hdfgrtr2_el2);
+ EL2_SAVE_CTX_REG(ctx, hdfgwtr2_el2);
+ EL2_SAVE_CTX_REG(ctx, hfgitr2_el2);
+ EL2_SAVE_CTX_REG(ctx, hfgrtr2_el2);
+ EL2_SAVE_CTX_REG(ctx, hfgwtr2_el2);
+ }
+}
+
+static void el2_save_ecv_registers(el2_ecv_regs_t *ctx)
+{
+ if (get_armv8_6_ecv_support() == ID_AA64MMFR0_EL1_ECV_SELF_SYNCH) {
+ EL2_SAVE_CTX_REG(ctx, cntpoff_el2);
+ }
+}
+
+static void el2_save_vhe_registers(el2_vhe_regs_t *ctx)
+{
+ if (is_armv8_1_vhe_present()) {
+ EL2_SAVE_CTX_REG(ctx, contextidr_el2);
+ EL2_SAVE_CTX_REG(ctx, ttbr1_el2);
+ }
+}
+
+static void el2_save_ras_registers(el2_ras_regs_t *ctx)
+{
+ if (is_feat_ras_present() || is_feat_rasv1p1_present()) {
+ EL2_SAVE_CTX_REG(ctx, vdisr_el2);
+ EL2_SAVE_CTX_REG(ctx, vsesr_el2);
+ }
+}
+
+static void el2_save_neve_registers(el2_neve_regs_t *ctx)
+{
+ if (is_armv8_4_nv2_present()) {
+ EL2_SAVE_CTX_REG(ctx, vncr_el2);
+ }
+}
+
+static void el2_save_trf_registers(el2_trf_regs_t *ctx)
+{
+ if (get_armv8_4_trf_support()) {
+ EL2_SAVE_CTX_REG(ctx, trfcr_el2);
+ }
+}
+
+static void el2_save_csv2_registers(el2_csv2_regs_t *ctx)
+{
+ if (is_feat_csv2_2_present()) {
+ EL2_SAVE_CTX_REG(ctx, scxtnum_el2);
+ }
+}
+
+static void el2_save_hcx_registers(el2_hcx_regs_t *ctx)
+{
+ if (get_feat_hcx_support()) {
+ EL2_SAVE_CTX_REG(ctx, hcrx_el2);
+ }
+}
+
+static void el2_save_tcr2_registers(el2_tcr2_regs_t *ctx)
+{
+ if (is_feat_tcr2_supported()) {
+ EL2_SAVE_CTX_REG(ctx, tcr2_el2);
+ }
+}
+
+static void el2_save_sxpoe_registers(el2_sxpoe_regs_t *ctx)
+{
+ if (is_feat_sxpoe_present()) {
+ EL2_SAVE_CTX_REG(ctx, por_el2);
+ }
+}
+
+static void el2_save_sxpie_registers(el2_sxpie_regs_t *ctx)
+{
+ if (is_feat_sxpie_present()) {
+ EL2_SAVE_CTX_REG(ctx, pire0_el2);
+ EL2_SAVE_CTX_REG(ctx, pir_el2);
+ }
+}
+
+static void el2_save_s2pie_registers(el2_s2pie_regs_t *ctx)
+{
+ if (is_feat_s2pie_present()) {
+ EL2_SAVE_CTX_REG(ctx, s2pir_el2);
+ }
+}
+
+static void el2_save_gcs_registers(el2_gcs_regs_t *ctx)
+{
+ if (is_feat_gcs_present()) {
+ EL2_SAVE_CTX_REG(ctx, gcscr_el2);
+ EL2_SAVE_CTX_REG(ctx, gcspr_el2);
+ }
+}
+
+static void el2_save_mpam_registers(el2_mpam_regs_t *ctx)
+{
+ if (is_feat_mpam_supported()) {
+ EL2_SAVE_CTX_REG(ctx, mpam2_el2);
+ /**
+ * Registers MPAMHCR_EL2, MPAMVPM0_EL2, MPAMVPM1_EL2, MPAMVPM2_EL2,
+ * MPAMVPM3_EL2, MPAMVPM4_EL2, MPAMVPM5_EL2, MPAMVPM6_EL2, MPAMVPM7_EL2
+ * and MPAMVPMV_EL2 are not included, because an exception is raised
+ * upon accessing them.
+ */
+ }
+}
+
+/******************************************************************************/
+
+/**
+ * Internal functions to corrupt/restore groups of EL2 registers.
+ */
+
+static void el2_write_common_registers_with_mask(const el2_common_regs_t *ctx, uint64_t or_mask)
+{
+ EL2_WRITE_MASK_CTX_REG(ctx, actlr_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, afsr0_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, afsr1_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, amair_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, cnthctl_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, cntvoff_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, cptr_el2, or_mask);
+ /* Accessing DBGVCR32_EL2 may crash the test, so it is omitted. */
+ EL2_WRITE_MASK_CTX_REG(ctx, elr_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, esr_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, far_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, hacr_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, hcr_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, hpfar_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, hstr_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, icc_sre_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, ich_hcr_el2, or_mask);
+ /* Unable to restore ICH_VMCR_EL2 to original value after modification, so it is omitted. */
+ EL2_WRITE_MASK_CTX_REG(ctx, mair_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, mdcr_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, pmscr_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, sctlr_el2, (or_mask & ~SCTLR_EL2_EE));
+ EL2_WRITE_MASK_CTX_REG(ctx, spsr_el2, or_mask);
+ /* The stack pointer should not be modified, because the control flow depends on it. */
+ /* Masking TCR_EL2 may crash the test, so it is omitted. */
+ EL2_WRITE_MASK_CTX_REG(ctx, tpidr_el2, or_mask);
+ /* Masking TTBR0_EL2 may crash the test, so it is omitted. */
+ EL2_WRITE_MASK_CTX_REG(ctx, vbar_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, vmpidr_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, vpidr_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, vtcr_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, vttbr_el2, or_mask);
+}
+
+static void el2_write_mte2_registers_with_mask(const el2_mte2_regs_t *ctx, uint64_t or_mask)
+{
+ if (get_armv8_5_mte_support() == 2) {
+ EL2_WRITE_MASK_CTX_REG(ctx, tfsr_el2, or_mask);
+ }
+}
+
+static void el2_write_fgt_registers_with_mask(const el2_fgt_regs_t *ctx, uint64_t or_mask)
+{
+ if (is_armv8_6_fgt_present()) {
+ EL2_WRITE_MASK_CTX_REG(ctx, hdfgrtr_el2, or_mask);
+ if (is_armv8_4_amuv1_present())
+ EL2_WRITE_MASK_CTX_REG(ctx, hafgrtr_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, hdfgwtr_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, hfgitr_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, hfgrtr_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, hfgwtr_el2, or_mask);
+ }
+}
+
+static void el2_write_fgt2_registers_with_mask(const el2_fgt2_regs_t *ctx, uint64_t or_mask)
+{
+ if (is_armv8_9_fgt2_present()) {
+ EL2_WRITE_MASK_CTX_REG(ctx, hdfgrtr2_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, hdfgwtr2_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, hfgitr2_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, hfgrtr2_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, hfgwtr2_el2, or_mask);
+ }
+}
+
+static void el2_write_ecv_registers_with_mask(const el2_ecv_regs_t *ctx, uint64_t or_mask)
+{
+ if (get_armv8_6_ecv_support() == ID_AA64MMFR0_EL1_ECV_SELF_SYNCH) {
+ EL2_WRITE_MASK_CTX_REG(ctx, cntpoff_el2, or_mask);
+ }
+}
+
+static void el2_write_vhe_registers_with_mask(const el2_vhe_regs_t *ctx, uint64_t or_mask)
+{
+ if (is_armv8_1_vhe_present()) {
+ EL2_WRITE_MASK_CTX_REG(ctx, contextidr_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, ttbr1_el2, (or_mask & ~TTBR1_EL2_ASID));
+ }
+}
+
+static void el2_write_ras_registers_with_mask(const el2_ras_regs_t *ctx, uint64_t or_mask)
+{
+ if (is_feat_ras_present() || is_feat_rasv1p1_present()) {
+ EL2_WRITE_MASK_CTX_REG(ctx, vdisr_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, vsesr_el2, or_mask);
+ }
+}
+
+static void el2_write_neve_registers_with_mask(const el2_neve_regs_t *ctx, uint64_t or_mask)
+{
+ if (is_armv8_4_nv2_present()) {
+ EL2_WRITE_MASK_CTX_REG(ctx, vncr_el2, or_mask);
+ }
+}
+
+static void el2_write_trf_registers_with_mask(const el2_trf_regs_t *ctx, uint64_t or_mask)
+{
+ if (get_armv8_4_trf_support()) {
+ EL2_WRITE_MASK_CTX_REG(ctx, trfcr_el2, or_mask);
+ }
+}
+
+static void el2_write_csv2_registers_with_mask(const el2_csv2_regs_t *ctx, uint64_t or_mask)
+{
+ if (is_feat_csv2_2_present()) {
+ EL2_WRITE_MASK_CTX_REG(ctx, scxtnum_el2, or_mask);
+ }
+}
+
+static void el2_write_hcx_registers_with_mask(const el2_hcx_regs_t *ctx, uint64_t or_mask)
+{
+ if (get_feat_hcx_support()) {
+ EL2_WRITE_MASK_CTX_REG(ctx, hcrx_el2, or_mask);
+ }
+}
+
+static void el2_write_tcr2_registers_with_mask(const el2_tcr2_regs_t *ctx, uint64_t or_mask)
+{
+ if (is_feat_tcr2_supported()) {
+ EL2_WRITE_MASK_CTX_REG(ctx, tcr2_el2, (or_mask & ~TCR2_EL2_POE));
+ }
+}
+
+static void el2_write_sxpoe_registers_with_mask(const el2_sxpoe_regs_t *ctx, uint64_t or_mask)
+{
+ if (is_feat_sxpoe_present()) {
+ EL2_WRITE_MASK_CTX_REG(ctx, por_el2, or_mask);
+ }
+}
+
+static void el2_write_sxpie_registers_with_mask(const el2_sxpie_regs_t *ctx, uint64_t or_mask)
+{
+ if (is_feat_sxpie_present()) {
+ EL2_WRITE_MASK_CTX_REG(ctx, pire0_el2, or_mask);
+ EL2_WRITE_MASK_CTX_REG(ctx, pir_el2, or_mask);
+ }
+}
+
+static void el2_write_s2pie_registers_with_mask(const el2_s2pie_regs_t *ctx, uint64_t or_mask)
+{
+ if (is_feat_s2pie_present()) {
+ EL2_WRITE_MASK_CTX_REG(ctx, s2pir_el2, or_mask);
+ }
+}
+
+static void el2_write_gcs_registers_with_mask(const el2_gcs_regs_t *ctx, uint64_t or_mask)
+{
+ if (is_feat_gcs_present()) {
+ EL2_WRITE_MASK_CTX_REG(ctx, gcscr_el2, (or_mask & ~GCSCR_EL2_PCRSEL));
+ EL2_WRITE_MASK_CTX_REG(ctx, gcspr_el2, or_mask);
+ }
+}
+
+/**
+ * Register MPAM2_EL2 is not included, because its contents may change
+ * before context switching occurs.
+ *
+ * Registers MPAMHCR_EL2, MPAMVPM0_EL2, MPAMVPM1_EL2, MPAMVPM2_EL2,
+ * MPAMVPM3_EL2, MPAMVPM4_EL2, MPAMVPM5_EL2, MPAMVPM6_EL2, MPAMVPM7_EL2
+ * and MPAMVPMV_EL2 are not included, because an exception is raised
+ * upon accessing them.
+ */
+
+/******************************************************************************/
+
+/**
+ * Internal functions to dump the saved EL2 register context.
+ */
+
+static void el2_dump_common_register_context(const el2_common_regs_t *ctx)
+{
+ EL2_PRINT_CTX_HEADING("Common registers");
+ EL2_PRINT_CTX_MEMBER(ctx, "ACTLR_EL2", actlr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "AFSR0_EL2", afsr0_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "AFSR1_EL2", afsr1_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "AMAIR_EL2", amair_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "CNTHCTL_EL2", cnthctl_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "CNTVOFF_EL2", cntvoff_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "CPTR_EL2", cptr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "ELR_EL2", elr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "ESR_EL2", esr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "FAR_EL2", far_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "HACR_EL2", hacr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "HCR_EL2", hcr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "HPFAR_EL2", hpfar_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "HSTR_EL2", hstr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "ICC_SRE_EL2", icc_sre_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "ICH_HCR_EL2", ich_hcr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "ICH_VMCR_EL2", ich_vmcr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "MAIR_EL2", mair_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "MDCR_EL2", mdcr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "PMSCR_EL2", pmscr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "SCTLR_EL2", sctlr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "SPSR_EL2", spsr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "SP_EL2", sp_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "TCR_EL2", tcr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "TPIDR_EL2", tpidr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "TTBR0_EL2", ttbr0_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "VBAR_EL2", vbar_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "VMPIDR_EL2", vmpidr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "VPIDR_EL2", vpidr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "VTCR_EL2", vtcr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "VTTBR_EL2", vttbr_el2);
+ INFO("\n");
+}
+
+static void el2_dump_mte2_register_context(const el2_mte2_regs_t *ctx)
+{
+ EL2_PRINT_CTX_HEADING("MTE2 registers");
+ EL2_PRINT_CTX_MEMBER(ctx, "TFSR_EL2", tfsr_el2);
+ INFO("\n");
+}
+
+static void el2_dump_fgt_register_context(const el2_fgt_regs_t *ctx)
+{
+ EL2_PRINT_CTX_HEADING("FGT registers");
+ EL2_PRINT_CTX_MEMBER(ctx, "HDFGRTR_EL2", hdfgrtr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "HAFGRTR_EL2", hafgrtr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "HDFGWTR_EL2", hdfgwtr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "HFGITR_EL2", hfgitr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "HFGRTR_EL2", hfgrtr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "HFGWTR_EL2", hfgwtr_el2);
+ INFO("\n");
+}
+
+static void el2_dump_fgt2_register_context(const el2_fgt2_regs_t *ctx)
+{
+ EL2_PRINT_CTX_HEADING("FGT2 registers");
+ EL2_PRINT_CTX_MEMBER(ctx, "HDFGRTR2_EL2", hdfgrtr2_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "HDFGWTR2_EL2", hdfgwtr2_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "HFGITR2_EL2", hfgitr2_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "HFGRTR2_EL2", hfgrtr2_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "HFGWTR2_EL2", hfgwtr2_el2);
+ INFO("\n");
+}
+
+static void el2_dump_ecv_register_context(const el2_ecv_regs_t *ctx)
+{
+ EL2_PRINT_CTX_HEADING("ECV registers");
+ EL2_PRINT_CTX_MEMBER(ctx, "CNTPOFF_EL2", cntpoff_el2);
+ INFO("\n");
+}
+
+static void el2_dump_vhe_register_context(const el2_vhe_regs_t *ctx)
+{
+ EL2_PRINT_CTX_HEADING("VHE registers");
+ EL2_PRINT_CTX_MEMBER(ctx, "CONTEXTIDR_EL2", contextidr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "TTBR1_EL2", ttbr1_el2);
+ INFO("\n");
+}
+
+static void el2_dump_ras_register_context(const el2_ras_regs_t *ctx)
+{
+ EL2_PRINT_CTX_HEADING("RAS registers");
+ EL2_PRINT_CTX_MEMBER(ctx, "VDISR_EL2", vdisr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "VSESR_EL2", vsesr_el2);
+ INFO("\n");
+}
+
+static void el2_dump_neve_register_context(const el2_neve_regs_t *ctx)
+{
+ EL2_PRINT_CTX_HEADING("NEVE registers");
+ EL2_PRINT_CTX_MEMBER(ctx, "VNCR_EL2", vncr_el2);
+ INFO("\n");
+}
+
+static void el2_dump_trf_register_context(const el2_trf_regs_t *ctx)
+{
+ EL2_PRINT_CTX_HEADING("TRF registers");
+ EL2_PRINT_CTX_MEMBER(ctx, "TRFCR_EL2", trfcr_el2);
+ INFO("\n");
+}
+
+static void el2_dump_csv2_register_context(const el2_csv2_regs_t *ctx)
+{
+ EL2_PRINT_CTX_HEADING("CSV2 registers");
+ EL2_PRINT_CTX_MEMBER(ctx, "SCXTNUM_EL2", scxtnum_el2);
+ INFO("\n");
+}
+
+static void el2_dump_hcx_register_context(const el2_hcx_regs_t *ctx)
+{
+ EL2_PRINT_CTX_HEADING("HCX registers");
+ EL2_PRINT_CTX_MEMBER(ctx, "HCRX_EL2", hcrx_el2);
+ INFO("\n");
+}
+
+static void el2_dump_tcr2_register_context(const el2_tcr2_regs_t *ctx)
+{
+ EL2_PRINT_CTX_HEADING("TCR2 registers");
+ EL2_PRINT_CTX_MEMBER(ctx, "TCR2_EL2", tcr2_el2);
+ INFO("\n");
+}
+
+static void el2_dump_sxpoe_register_context(const el2_sxpoe_regs_t *ctx)
+{
+ EL2_PRINT_CTX_HEADING("SxPOE registers");
+ EL2_PRINT_CTX_MEMBER(ctx, "POR_EL2", por_el2);
+ INFO("\n");
+}
+
+static void el2_dump_sxpie_register_context(const el2_sxpie_regs_t *ctx)
+{
+ EL2_PRINT_CTX_HEADING("SxPIE registers");
+ EL2_PRINT_CTX_MEMBER(ctx, "PIRE0_EL2", pire0_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "PIR_EL2", pir_el2);
+ INFO("\n");
+}
+
+static void el2_dump_s2pie_register_context(const el2_s2pie_regs_t *ctx)
+{
+ EL2_PRINT_CTX_HEADING("S2PIE registers");
+ EL2_PRINT_CTX_MEMBER(ctx, "S2PIR_EL2", s2pir_el2);
+ INFO("\n");
+}
+
+static void el2_dump_gcs_register_context(const el2_gcs_regs_t *ctx)
+{
+ EL2_PRINT_CTX_HEADING("GCS registers");
+ EL2_PRINT_CTX_MEMBER(ctx, "GCSCR_EL2", gcscr_el2);
+ EL2_PRINT_CTX_MEMBER(ctx, "GCSPR_EL2", gcspr_el2);
+ INFO("\n");
+}
+
+static void el2_dump_mpam_register_context(const el2_mpam_regs_t *ctx)
+{
+ EL2_PRINT_CTX_HEADING("MPAM registers");
+ EL2_PRINT_CTX_MEMBER(ctx, "MPAM2_EL2", mpam2_el2);
+ INFO("\n");
+}
+
+/******************************************************************************/
+
+/**
+ * Public function definitions.
+ */
+
+/**
+ * Read EL2 system registers and save the values into the given context pointer.
+ */
+void el2_save_registers(el2_sysregs_t *ctx)
+{
+ el2_save_common_registers(&ctx->common);
+ el2_save_mte2_registers(&ctx->mte2);
+ el2_save_fgt_registers(&ctx->fgt);
+ el2_save_fgt2_registers(&ctx->fgt2);
+ el2_save_ecv_registers(&ctx->ecv);
+ el2_save_vhe_registers(&ctx->vhe);
+ el2_save_ras_registers(&ctx->ras);
+ el2_save_neve_registers(&ctx->neve);
+ el2_save_trf_registers(&ctx->trf);
+ el2_save_csv2_registers(&ctx->csv2);
+ el2_save_hcx_registers(&ctx->hcx);
+ el2_save_tcr2_registers(&ctx->tcr2);
+ el2_save_sxpoe_registers(&ctx->sxpoe);
+ el2_save_sxpie_registers(&ctx->sxpie);
+ el2_save_s2pie_registers(&ctx->s2pie);
+ el2_save_gcs_registers(&ctx->gcs);
+ el2_save_mpam_registers(&ctx->mpam);
+}
+
+/**
+ * Modify EL2 system registers from a given context pointer
+ * by applying the appropriate OR mask depending on the operation.
+ */
+void el2_modify_registers(const el2_sysregs_t *ctx, const el2_modify_operation_t op)
+{
+ uint64_t or_mask = (op == CORRUPT_REGS) ? REG_CORRUPTION_MASK : 0;
+ el2_write_common_registers_with_mask(&ctx->common, or_mask);
+ el2_write_mte2_registers_with_mask(&ctx->mte2, or_mask);
+ el2_write_fgt_registers_with_mask(&ctx->fgt, or_mask);
+ el2_write_fgt2_registers_with_mask(&ctx->fgt2, or_mask);
+ el2_write_ecv_registers_with_mask(&ctx->ecv, or_mask);
+ el2_write_vhe_registers_with_mask(&ctx->vhe, or_mask);
+ el2_write_ras_registers_with_mask(&ctx->ras, or_mask);
+ el2_write_neve_registers_with_mask(&ctx->neve, or_mask);
+ el2_write_trf_registers_with_mask(&ctx->trf, or_mask);
+ el2_write_csv2_registers_with_mask(&ctx->csv2, or_mask);
+ el2_write_hcx_registers_with_mask(&ctx->hcx, or_mask);
+ el2_write_tcr2_registers_with_mask(&ctx->tcr2, or_mask);
+ el2_write_sxpoe_registers_with_mask(&ctx->sxpoe, or_mask);
+ el2_write_sxpie_registers_with_mask(&ctx->sxpie, or_mask);
+ el2_write_s2pie_registers_with_mask(&ctx->s2pie, or_mask);
+ el2_write_gcs_registers_with_mask(&ctx->gcs, or_mask);
+ /* There is nothing to do in regards to MPAM registers. */
+}
+
+/**
+ * Dump (print out) the EL2 system registers from a given context pointer.
+ */
+void el2_dump_register_context(const char *ctx_name, const el2_sysregs_t *ctx)
+{
+ INFO("%s:\n", ctx_name);
+ el2_dump_common_register_context(&ctx->common);
+ el2_dump_mte2_register_context(&ctx->mte2);
+ el2_dump_fgt_register_context(&ctx->fgt);
+ el2_dump_fgt2_register_context(&ctx->fgt2);
+ el2_dump_ecv_register_context(&ctx->ecv);
+ el2_dump_vhe_register_context(&ctx->vhe);
+ el2_dump_ras_register_context(&ctx->ras);
+ el2_dump_neve_register_context(&ctx->neve);
+ el2_dump_trf_register_context(&ctx->trf);
+ el2_dump_csv2_register_context(&ctx->csv2);
+ el2_dump_hcx_register_context(&ctx->hcx);
+ el2_dump_tcr2_register_context(&ctx->tcr2);
+ el2_dump_sxpoe_register_context(&ctx->sxpoe);
+ el2_dump_sxpie_register_context(&ctx->sxpie);
+ el2_dump_s2pie_register_context(&ctx->s2pie);
+ el2_dump_gcs_register_context(&ctx->gcs);
+ el2_dump_mpam_register_context(&ctx->mpam);
+}
diff --git a/tftf/framework/framework.mk b/tftf/framework/framework.mk
index 0bae3cf..fc0ccbd 100644
--- a/tftf/framework/framework.mk
+++ b/tftf/framework/framework.mk
@@ -95,6 +95,13 @@
lib/extensions/sve/aarch64/sve_helpers.S
endif
+ifeq (${ARCH},aarch64)
+# Context Management Library support files
+FRAMEWORK_SOURCES += \
+ lib/context_mgmt/aarch64/context_el1.c \
+ lib/context_mgmt/aarch64/context_el2.c
+endif
+
TFTF_LINKERFILE := tftf/framework/tftf.ld.S
diff --git a/tftf/tests/context_mgmt_tests/el1/test_tsp_el1_context_mgmt.c b/tftf/tests/context_mgmt_tests/el1/test_tsp_el1_context_mgmt.c
new file mode 100644
index 0000000..78a6416
--- /dev/null
+++ b/tftf/tests/context_mgmt_tests/el1/test_tsp_el1_context_mgmt.c
@@ -0,0 +1,126 @@
+/*
+ * Copyright (c) 2024, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <lib/context_mgmt/context_el1.h>
+#include <test_helpers.h>
+#include <tftf_lib.h>
+
+#define TSP_MODIFY_WITH_DUMMY_VALUE 1
+#define TSP_RESTORE_WITH_DEFAULT 0
+
+#ifdef __aarch64__
+/**
+ * Global place holders for the entire test
+ */
+static el1_ctx_regs_t el1_ctx_original = {0};
+static el1_ctx_regs_t el1_ctx_before_smc = {0};
+static el1_ctx_regs_t el1_ctx_after_eret = {0};
+#endif
+
+/**
+ * Public Function: test_tsp_el1_ctx_regs_ctx_mgmt.
+ *
+ * @Test_Aim: This Test aims to validate EL1 context registers are restored to
+ * correct values upon returning from context switch triggered by an Standard
+ * SMC to TSP. This will ensure the context-management library at EL3 precisely
+ * saves and restores the appropriate world specific context during world switch
+ * and prevents data leakage in all cases.
+ */
+test_result_t test_tsp_el1_regs_ctx_mgmt(void)
+{
+ SKIP_TEST_IF_AARCH32();
+
+#ifdef __aarch64__
+ smc_args tsp_svc_params;
+ smc_ret_values tsp_result = {0};
+ test_result_t ret_value;
+ bool result;
+
+ SKIP_TEST_IF_TSP_NOT_PRESENT();
+
+ /**
+ * 1. Read the values of EL1 registers and save them into the
+ * global buffer before the world switch.
+ * This will be safe copy of registers which will be restored
+ * back at the end of the test.
+ */
+ save_el1_sysregs_context(&el1_ctx_original);
+
+ /**
+ * 2. Modify/Corrupt the EL1 registers with some dummy values.
+ * This will be essential for the test to ensure registers are
+ * accessed and modified in both the security states, which will
+ * be an ideal usecase in realtime.
+ */
+ modify_el1_context_sysregs(&el1_ctx_original, NS_CORRUPT_EL1_REGS);
+
+ /**
+ * 3. Read the values of EL1 registers again after modification
+ * and save them into an other buffer before the world switch.
+ * This will be used for comparison at the later stage on ERET.
+ */
+ save_el1_sysregs_context(&el1_ctx_before_smc);
+
+ /* 4. Standard SMC to TSP, to modify EL1 registers in secure world */
+ tsp_svc_params.fid = TSP_STD_FID(TSP_MODIFY_EL1_CTX);
+ tsp_svc_params.arg1 = TSP_MODIFY_WITH_DUMMY_VALUE;
+ tsp_result = tftf_smc(&tsp_svc_params);
+
+ /* Check the result of the TSP_MODIFY_EL1_CTX STD_SMC call */
+ if (tsp_result.ret0 != 0) {
+ ERROR("TSP_MODIFY_EL1_CTX returned wrong result: "
+ "got %d expected: 0 \n",
+ (unsigned int)tsp_result.ret0);
+ return TEST_RESULT_FAIL;
+ }
+
+ /**
+ * 5. Read the values of EL1 registers and save them into
+ * the global buffer after the ERET.
+ */
+ save_el1_sysregs_context(&el1_ctx_after_eret);
+
+ /* 6. Validate the EL1 contexts */
+ result = compare_el1_contexts(&el1_ctx_before_smc, &el1_ctx_after_eret);
+
+ if (result) {
+ INFO("EL1 context is safely handled by the "
+ "EL3 Ctx-management Library\n");
+ ret_value = TEST_RESULT_SUCCESS;
+ } else {
+ ERROR("EL1 context is corrupted during world switch\n");
+ ret_value = TEST_RESULT_FAIL;
+
+ /* Print the saved EL1 context before world switch. */
+ INFO("EL1 context registers list before SMC in NWd:\n");
+ print_el1_sysregs_context(&el1_ctx_before_smc);
+
+ /* Print the saved EL1 context after ERET */
+ INFO("EL1 context registers list after ERET in NWd:\n");
+ print_el1_sysregs_context(&el1_ctx_after_eret);
+ }
+
+ /**
+ * 7. Restore the EL1 registers to their original value in order
+ * to avoid any unintended crash for next tests.
+ */
+ modify_el1_context_sysregs(&el1_ctx_original, NS_RESTORE_EL1_REGS);
+
+ /* 8. Standard SMC to TSP, to restore EL1 regs in secure world */
+ tsp_svc_params.fid = TSP_STD_FID(TSP_MODIFY_EL1_CTX);
+ tsp_svc_params.arg1 = TSP_RESTORE_WITH_DEFAULT;
+ tsp_result = tftf_smc(&tsp_svc_params);
+
+ /* Check the result of the TSP_MODIFY_EL1_CTX STD_SMC call */
+ if (tsp_result.ret0 != 0) {
+ ERROR("TSP_MODIFY_EL1_CTX returned wrong result: "
+ "got %d expected: 0 \n",
+ (unsigned int)tsp_result.ret0);
+ return TEST_RESULT_FAIL;
+ }
+ return ret_value;
+
+#endif /* __aarch64__ */
+}
diff --git a/tftf/tests/context_mgmt_tests/el2/test_spm_el2_context_mgmt.c b/tftf/tests/context_mgmt_tests/el2/test_spm_el2_context_mgmt.c
new file mode 100644
index 0000000..457e650
--- /dev/null
+++ b/tftf/tests/context_mgmt_tests/el2/test_spm_el2_context_mgmt.c
@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2024 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <cactus_test_cmds.h>
+#include <ffa_endpoints.h>
+#include <lib/context_mgmt/context_el2.h>
+#include <spm_test_helpers.h>
+#include <test_helpers.h>
+#include <tftf.h>
+#include <tftf_lib.h>
+
+static const struct ffa_uuid expected_sp_uuids[] = {
+ {PRIMARY_UUID}, {SECONDARY_UUID}, {TERTIARY_UUID}
+};
+
+/*
+ * This test aims to validate EL2 system registers are restored to previously saved
+ * values upon returning from context switch triggered by an FF-A direct
+ * message request to an SP.
+ */
+test_result_t test_spm_el2_regs_ctx_mgmt(void)
+{
+ struct ffa_value ret;
+ el2_sysregs_t ctx_original = {0}, ctx_expected = {0}, ctx_actual = {0};
+
+ /* Check SPMC has ffa_version and expected FFA endpoints are deployed. */
+ CHECK_SPMC_TESTING_SETUP(1, 0, expected_sp_uuids);
+
+ /* Save the original values of EL2 system registers. */
+ el2_save_registers(&ctx_original);
+
+ /* Corrupt the EL2 system registers and save the expected values. */
+ el2_modify_registers(&ctx_original, CORRUPT_REGS);
+ el2_save_registers(&ctx_expected);
+
+ /* Send a message to SP1 through direct messaging. */
+ ret = cactus_echo_send_cmd(HYP_ID, SP_ID(1), ECHO_VAL1);
+
+ if (cactus_get_response(ret) != CACTUS_SUCCESS ||
+ cactus_echo_get_val(ret) != ECHO_VAL1 ||
+ !is_ffa_direct_response(ret)) {
+ return TEST_RESULT_FAIL;
+ }
+
+ /* Save the modified values of EL2 system registers after the FF-A command. */
+ el2_save_registers(&ctx_actual);
+
+ /* Restore the EL2 system registers to their original state. */
+ el2_modify_registers(&ctx_original, RESTORE_REGS);
+
+ if (memcmp(&ctx_expected, &ctx_actual, sizeof(el2_sysregs_t))) {
+ ERROR("Expected vs actual EL2 register contexts differ.\n\n");
+ el2_dump_register_context("Expected", &ctx_expected);
+ el2_dump_register_context("Actual", &ctx_actual);
+ return TEST_RESULT_FAIL;
+ }
+
+ return TEST_RESULT_SUCCESS;
+}
diff --git a/tftf/tests/tests-cpu-context-mgmt.mk b/tftf/tests/tests-cpu-context-mgmt.mk
new file mode 100644
index 0000000..c94dc37
--- /dev/null
+++ b/tftf/tests/tests-cpu-context-mgmt.mk
@@ -0,0 +1,12 @@
+#
+# Copyright (c) 2024, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+include tftf/tests/tests-spm.mk
+
+TESTS_SOURCES += $(addprefix tftf/tests/, \
+ context_mgmt_tests/el1/test_tsp_el1_context_mgmt.c \
+ context_mgmt_tests/el2/test_spm_el2_context_mgmt.c \
+)
diff --git a/tftf/tests/tests-cpu-context-mgmt.xml b/tftf/tests/tests-cpu-context-mgmt.xml
new file mode 100644
index 0000000..abdf5d9
--- /dev/null
+++ b/tftf/tests/tests-cpu-context-mgmt.xml
@@ -0,0 +1,16 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<!--
+ Copyright (c) 2024, Arm Limited. All rights reserved.
+
+ SPDX-License-Identifier: BSD-3-Clause
+-->
+
+<testsuites>
+ <testsuite name="CPU Context Management" description="Validate CPU context switch between NWd and SWd">
+ <testcase name="Test EL1 regs preserved across context switch with TSP"
+ function="test_tsp_el1_regs_ctx_mgmt" />
+ <testcase name="Test EL2 regs preserved across context switch with SPM"
+ function="test_spm_el2_regs_ctx_mgmt" />
+ </testsuite>
+</testsuites>