diff --git a/bl31/bl31_traps.c b/bl31/bl31_traps.c
index 2cfe14a..d14a91e 100644
--- a/bl31/bl31_traps.c
+++ b/bl31/bl31_traps.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2022, ARM Limited. All rights reserved.
+ * Copyright (c) 2022-2024, Arm Limited. All rights reserved.
  * Copyright (c) 2023, NVIDIA Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
@@ -7,8 +7,11 @@
  * Dispatch synchronous system register traps from lower ELs.
  */
 
+#include <arch_features.h>
+#include <arch_helpers.h>
 #include <bl31/sync_handle.h>
 #include <context.h>
+#include <lib/el3_runtime/context_mgmt.h>
 
 int handle_sysreg_trap(uint64_t esr_el3, cpu_context_t *ctx)
 {
@@ -28,3 +31,205 @@
 
 	return TRAP_RET_UNHANDLED;
 }
+
+static bool is_tge_enabled(void)
+{
+	u_register_t hcr_el2 = read_hcr_el2();
+
+	return ((read_feat_vhe_id_field() != 0U) && ((hcr_el2 & HCR_TGE_BIT) != 0U));
+}
+
+/*
+ * This function is to ensure that undef injection does not happen into
+ * non-existent S-EL2. This could happen when trap happens from S-EL{1,0}
+ * and non-secure world is running with TGE bit set, considering EL3 does
+ * not save/restore EL2 registers if only one world has EL2 enabled.
+ * So reading hcr_el2.TGE would give NS world value.
+ */
+static bool is_secure_trap_without_sel2(u_register_t scr)
+{
+	return ((scr & (SCR_NS_BIT | SCR_EEL2_BIT)) == 0);
+}
+
+static unsigned int target_el(unsigned int from_el, u_register_t scr)
+{
+	if (from_el > MODE_EL1) {
+		return from_el;
+	} else if (is_tge_enabled() && !is_secure_trap_without_sel2(scr)) {
+		return MODE_EL2;
+	} else {
+		return MODE_EL1;
+	}
+}
+
+static u_register_t get_elr_el3(u_register_t spsr_el3, u_register_t vbar, unsigned int target_el)
+{
+	unsigned int outgoing_el = GET_EL(spsr_el3);
+	u_register_t elr_el3 = 0;
+
+	if (outgoing_el == target_el) {
+		/*
+		 * Target EL is either EL1 or EL2, lsb can tell us the SPsel
+		 *  Thread mode  : 0
+		 *  Handler mode : 1
+		 */
+		if ((spsr_el3 & (MODE_SP_MASK << MODE_SP_SHIFT)) == MODE_SP_ELX) {
+			elr_el3 = vbar + CURRENT_EL_SPX;
+		} else {
+			elr_el3 = vbar + CURRENT_EL_SP0;
+		}
+	} else {
+		/* Vector address for Lower EL using Aarch64 */
+		elr_el3 = vbar + LOWER_EL_AARCH64;
+	}
+
+	return elr_el3;
+}
+
+/*
+ * Explicitly create all bits of SPSR to get PSTATE at exception return.
+ *
+ * The code is based on "Aarch64.exceptions.takeexception" described in
+ * DDI0602 revision 2023-06.
+ * "https://developer.arm.com/documentation/ddi0602/2023-06/Shared-Pseudocode/
+ * aarch64-exceptions-takeexception"
+ *
+ * NOTE: This piece of code must be reviewed every release to ensure that
+ * we keep up with new ARCH features which introduces a new SPSR bit.
+ */
+static u_register_t create_spsr(u_register_t old_spsr, unsigned int target_el)
+{
+	u_register_t new_spsr = 0;
+	u_register_t sctlr;
+
+	/* Set M bits for target EL in AArch64 mode, also get sctlr */
+	if (target_el == MODE_EL2) {
+		sctlr = read_sctlr_el2();
+		new_spsr |= (SPSR_M_AARCH64 << SPSR_M_SHIFT) | SPSR_M_EL2H;
+	} else {
+		sctlr = read_sctlr_el1();
+		new_spsr |= (SPSR_M_AARCH64 << SPSR_M_SHIFT) | SPSR_M_EL1H;
+	}
+
+	/* Mask all exceptions, update DAIF bits */
+	new_spsr |= SPSR_DAIF_MASK << SPSR_DAIF_SHIFT;
+
+	/* If FEAT_BTI is present, clear BTYPE bits */
+	new_spsr |= old_spsr & (SPSR_BTYPE_MASK_AARCH64 << SPSR_BTYPE_SHIFT_AARCH64);
+	if (is_armv8_5_bti_present()) {
+		new_spsr &= ~(SPSR_BTYPE_MASK_AARCH64 << SPSR_BTYPE_SHIFT_AARCH64);
+	}
+
+	/* If SSBS is implemented, take the value from SCTLR.DSSBS */
+	new_spsr |= old_spsr & SPSR_SSBS_BIT_AARCH64;
+	if (is_feat_ssbs_present()) {
+		if ((sctlr & SCTLR_DSSBS_BIT) != 0U) {
+			new_spsr |= SPSR_SSBS_BIT_AARCH64;
+		} else {
+			new_spsr &= ~SPSR_SSBS_BIT_AARCH64;
+		}
+	}
+
+	/* If FEAT_NMI is implemented, ALLINT = !(SCTLR.SPINTMASK) */
+	new_spsr |= old_spsr & SPSR_ALLINT_BIT_AARCH64;
+	if (is_feat_nmi_present()) {
+		if ((sctlr & SCTLR_SPINTMASK_BIT) != 0U) {
+			new_spsr &= ~SPSR_ALLINT_BIT_AARCH64;
+		} else {
+			new_spsr |= SPSR_ALLINT_BIT_AARCH64;
+		}
+	}
+
+	/* Clear PSTATE.IL bit explicitly */
+	new_spsr &= ~SPSR_IL_BIT;
+
+	/* Clear PSTATE.SS bit explicitly */
+	new_spsr &= ~SPSR_SS_BIT;
+
+	/* Update PSTATE.PAN bit */
+	new_spsr |= old_spsr & SPSR_PAN_BIT;
+	if (is_feat_pan_present() &&
+	    ((target_el == MODE_EL1) || ((target_el == MODE_EL2) && is_tge_enabled())) &&
+	    ((sctlr & SCTLR_SPAN_BIT) == 0U)) {
+	    new_spsr |= SPSR_PAN_BIT;
+	}
+
+	/* Clear UAO bit if FEAT_UAO is present */
+	new_spsr |= old_spsr & SPSR_UAO_BIT_AARCH64;
+	if (is_feat_uao_present()) {
+		new_spsr &= ~SPSR_UAO_BIT_AARCH64;
+	}
+
+	/* DIT bits are unchanged */
+	new_spsr |= old_spsr & SPSR_DIT_BIT;
+
+	/* If FEAT_MTE2 is implemented mask tag faults by setting TCO bit */
+	new_spsr |= old_spsr & SPSR_TCO_BIT_AARCH64;
+	if (read_feat_mte_id_field() >= MTE_IMPLEMENTED_ELX) {
+		new_spsr |= SPSR_TCO_BIT_AARCH64;
+	}
+
+	/* NZCV bits are unchanged */
+	new_spsr |= old_spsr & SPSR_NZCV;
+
+	/* If FEAT_EBEP is present set PM bit */
+	new_spsr |= old_spsr & SPSR_PM_BIT_AARCH64;
+	if (is_feat_ebep_present()) {
+		new_spsr |= SPSR_PM_BIT_AARCH64;
+	}
+
+	/* If FEAT_SEBEP is present clear PPEND bit */
+	new_spsr |= old_spsr & SPSR_PPEND_BIT;
+	if (is_feat_sebep_present()) {
+		new_spsr &= ~SPSR_PPEND_BIT;
+	}
+
+	/* If FEAT_GCS is present, update EXLOCK bit */
+	new_spsr |= old_spsr & SPSR_EXLOCK_BIT_AARCH64;
+	if (is_feat_gcs_present()) {
+		u_register_t gcscr;
+		if (target_el == MODE_EL2) {
+			gcscr = read_gcscr_el2();
+		} else {
+			gcscr = read_gcscr_el1();
+		}
+		new_spsr |= (gcscr & GCSCR_EXLOCK_EN_BIT) ? SPSR_EXLOCK_BIT_AARCH64 : 0;
+	}
+
+	return new_spsr;
+}
+
+/*
+ * Handler for injecting Undefined exception to lower EL which is caused by
+ * lower EL accessing system registers of which (old)EL3 firmware is unaware.
+ *
+ * This is a safety net to avoid EL3 panics caused by system register access
+ * that triggers an exception syndrome EC=0x18.
+ */
+void inject_undef64(cpu_context_t *ctx)
+{
+	u_register_t esr = (EC_UNKNOWN << ESR_EC_SHIFT) | ESR_IL_BIT;
+	el3_state_t *state = get_el3state_ctx(ctx);
+	u_register_t elr_el3 = read_ctx_reg(state, CTX_ELR_EL3);
+	u_register_t old_spsr = read_ctx_reg(state, CTX_SPSR_EL3);
+	u_register_t scr_el3 = read_ctx_reg(state, CTX_SCR_EL3);
+	u_register_t new_spsr = 0;
+	unsigned int to_el = target_el(GET_EL(old_spsr), scr_el3);
+
+	if (to_el == MODE_EL2) {
+		write_elr_el2(elr_el3);
+		elr_el3 = get_elr_el3(old_spsr, read_vbar_el2(), to_el);
+		write_esr_el2(esr);
+		write_spsr_el2(old_spsr);
+	} else {
+		write_elr_el1(elr_el3);
+		elr_el3 = get_elr_el3(old_spsr, read_vbar_el1(), to_el);
+		write_esr_el1(esr);
+		write_spsr_el1(old_spsr);
+	}
+
+	new_spsr = create_spsr(old_spsr, to_el);
+
+	write_ctx_reg(state, CTX_SPSR_EL3, new_spsr);
+	write_ctx_reg(state, CTX_ELR_EL3, elr_el3);
+}