feat(cm): add tests to validate EL1 regs during context switch

* This patch adds a test to verify the integrity of the el1_context
  registers across world-switch.

* It aims at testing the save and restore functionality provided
  by the EL3 context management library.

* It validates the EL1 ctx register entries after interaction with
  TSP (S-EL1) software.

Change-Id: Id435d9d7699231d66e9e7acdbb3459ec439d2aef
Signed-off-by: Jayanth Dodderi Chidanand <jayanthdodderi.chidanand@arm.com>
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/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/tftf/framework/framework.mk b/tftf/framework/framework.mk
index 0bae3cf..3bff5ed 100644
--- a/tftf/framework/framework.mk
+++ b/tftf/framework/framework.mk
@@ -95,6 +95,12 @@
 	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
+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/tests-cpu-context-mgmt.mk b/tftf/tests/tests-cpu-context-mgmt.mk
new file mode 100644
index 0000000..0919f5f
--- /dev/null
+++ b/tftf/tests/tests-cpu-context-mgmt.mk
@@ -0,0 +1,9 @@
+#
+# Copyright (c) 2024, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+TESTS_SOURCES	+=	$(addprefix tftf/tests/,				\
+		context_mgmt_tests/el1/test_tsp_el1_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..eb86f08
--- /dev/null
+++ b/tftf/tests/tests-cpu-context-mgmt.xml
@@ -0,0 +1,14 @@
+<?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" />
+  </testsuite>
+</testsuites>