diff --git a/services/spd/tspd/tspd.mk b/services/spd/tspd/tspd.mk
index cd4b45a..139c7d7 100644
--- a/services/spd/tspd/tspd.mk
+++ b/services/spd/tspd/tspd.mk
@@ -52,3 +52,10 @@
 
 # Let the top-level Makefile know that we intend to build the SP from source
 NEED_BL32		:=	yes
+
+# Flag used to enable routing of non-secure interrupts to EL3 when they are
+# generated while the code is executing in S-EL1/0.
+TSPD_ROUTE_IRQ_TO_EL3	:=	0
+
+$(eval $(call assert_boolean,TSPD_ROUTE_IRQ_TO_EL3))
+$(eval $(call add_define,TSPD_ROUTE_IRQ_TO_EL3))
diff --git a/services/spd/tspd/tspd_main.c b/services/spd/tspd/tspd_main.c
index b8d4569..ee17483 100644
--- a/services/spd/tspd/tspd_main.c
+++ b/services/spd/tspd/tspd_main.c
@@ -48,6 +48,7 @@
 #include <platform.h>
 #include <runtime_svc.h>
 #include <stddef.h>
+#include <string.h>
 #include <tsp.h>
 #include <uuid.h>
 #include "tspd_private.h"
@@ -71,6 +72,24 @@
 
 int32_t tspd_init(void);
 
+uint64_t tspd_handle_sp_preemption(void *handle)
+{
+	cpu_context_t *ns_cpu_context;
+	assert(handle == cm_get_context(SECURE));
+	cm_el1_sysregs_context_save(SECURE);
+	/* Get a reference to the non-secure context */
+	ns_cpu_context = cm_get_context(NON_SECURE);
+	assert(ns_cpu_context);
+
+	/*
+	 * Restore non-secure state. The secure system
+	 * register context will be saved when required.
+	 */
+	cm_el1_sysregs_context_restore(NON_SECURE);
+	cm_set_next_eret_context(NON_SECURE);
+
+	SMC_RET1(ns_cpu_context, SMC_PREEMPTED);
+}
 /*******************************************************************************
  * This function is the handler registered for S-EL1 interrupts by the TSPD. It
  * validates the interrupt and upon success arranges entry into the TSP at
@@ -120,11 +139,16 @@
 						      CTX_SPSR_EL3);
 		tsp_ctx->saved_elr_el3 = SMC_GET_EL3(&tsp_ctx->cpu_ctx,
 						     CTX_ELR_EL3);
+#if TSPD_ROUTE_IRQ_TO_EL3
+		/*Need to save the previously interrupted secure context */
+		memcpy(&tsp_ctx->sp_ctx, &tsp_ctx->cpu_ctx, TSPD_SP_CTX_SIZE);
+#endif
 	}
 
 	cm_el1_sysregs_context_restore(SECURE);
 	cm_set_elr_spsr_el3(SECURE, (uint64_t) &tsp_vectors->fiq_entry,
 		    SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS));
+
 	cm_set_next_eret_context(SECURE);
 
 	/*
@@ -137,6 +161,34 @@
 	SMC_RET2(&tsp_ctx->cpu_ctx, TSP_HANDLE_FIQ_AND_RETURN, read_elr_el3());
 }
 
+#if TSPD_ROUTE_IRQ_TO_EL3
+/*******************************************************************************
+ * This function is the handler registered for S-EL1 interrupts by the TSPD. It
+ * validates the interrupt and upon success arranges entry into the TSP at
+ * 'tsp_fiq_entry()' for handling the interrupt.
+ ******************************************************************************/
+static uint64_t tspd_ns_interrupt_handler(uint32_t id,
+					    uint32_t flags,
+					    void *handle,
+					    void *cookie)
+{
+	/* Check the security state when the exception was generated */
+	assert(get_interrupt_src_ss(flags) == SECURE);
+
+#if IMF_READ_INTERRUPT_ID
+	/* Check the security status of the interrupt */
+	assert(plat_ic_get_interrupt_type(id) == INTR_TYPE_NS);
+#endif
+	/*
+	 * Disable the routing of NS interrupts from secure world to EL3 while
+	 * interrupted on this core.
+	 */
+	disable_intr_rm_local(INTR_TYPE_NS, SECURE);
+
+	return tspd_handle_sp_preemption(handle);
+}
+#endif
+
 /*******************************************************************************
  * Secure Payload Dispatcher setup. The SPD finds out the SP entrypoint and type
  * (aarch32/aarch64) if not already known and initialises the context for entry
@@ -270,21 +322,7 @@
 		if (ns)
 			SMC_RET1(handle, SMC_UNK);
 
-		assert(handle == cm_get_context(SECURE));
-		cm_el1_sysregs_context_save(SECURE);
-		/* Get a reference to the non-secure context */
-		ns_cpu_context = cm_get_context(NON_SECURE);
-		assert(ns_cpu_context);
-
-		/*
-		 * Restore non-secure state. There is no need to save the
-		 * secure system register context since the TSP was supposed
-		 * to preserve it during S-EL1 interrupt handling.
-		 */
-		cm_el1_sysregs_context_restore(NON_SECURE);
-		cm_set_next_eret_context(NON_SECURE);
-
-		SMC_RET1(ns_cpu_context, SMC_PREEMPTED);
+		return tspd_handle_sp_preemption(handle);
 
 	/*
 	 * This function ID is used only by the TSP to indicate that it has
@@ -308,6 +346,14 @@
 			SMC_SET_EL3(&tsp_ctx->cpu_ctx,
 				    CTX_ELR_EL3,
 				    tsp_ctx->saved_elr_el3);
+#if TSPD_ROUTE_IRQ_TO_EL3
+			/*
+			 * Need to restore the previously interrupted
+			 * secure context.
+			 */
+			memcpy(&tsp_ctx->cpu_ctx, &tsp_ctx->sp_ctx,
+				TSPD_SP_CTX_SIZE);
+#endif
 		}
 
 		/* Get a reference to the non-secure context */
@@ -389,6 +435,28 @@
 						flags);
 			if (rc)
 				panic();
+
+#if TSPD_ROUTE_IRQ_TO_EL3
+			/*
+			 * Register an interrupt handler for NS interrupts when
+			 * generated during code executing in secure state are
+			 * routed to EL3.
+			 */
+			flags = 0;
+			set_interrupt_rm_flag(flags, SECURE);
+
+			rc = register_interrupt_type_handler(INTR_TYPE_NS,
+						tspd_ns_interrupt_handler,
+						flags);
+			if (rc)
+				panic();
+
+			/*
+			 * Disable the interrupt NS locally since it will be enabled globally
+			 * within cm_init_context.
+			 */
+			disable_intr_rm_local(INTR_TYPE_NS, SECURE);
+#endif
 		}
 
 
@@ -507,6 +575,13 @@
 				set_std_smc_active_flag(tsp_ctx->state);
 				cm_set_elr_el3(SECURE, (uint64_t)
 						&tsp_vectors->std_smc_entry);
+#if TSPD_ROUTE_IRQ_TO_EL3
+				/*
+				 * Enable the routing of NS interrupts to EL3
+				 * during STD SMC processing on this core.
+				 */
+				enable_intr_rm_local(INTR_TYPE_NS, SECURE);
+#endif
 			}
 
 			cm_el1_sysregs_context_restore(SECURE);
@@ -529,8 +604,18 @@
 			/* Restore non-secure state */
 			cm_el1_sysregs_context_restore(NON_SECURE);
 			cm_set_next_eret_context(NON_SECURE);
-			if (GET_SMC_TYPE(smc_fid) == SMC_TYPE_STD)
+			if (GET_SMC_TYPE(smc_fid) == SMC_TYPE_STD) {
 				clr_std_smc_active_flag(tsp_ctx->state);
+#if TSPD_ROUTE_IRQ_TO_EL3
+				/*
+				 * Disable the routing of NS interrupts to EL3
+				 * after STD SMC processing is finished on this
+				 * core.
+				 */
+				disable_intr_rm_local(INTR_TYPE_NS, SECURE);
+#endif
+			}
+
 			SMC_RET3(ns_cpu_context, x1, x2, x3);
 		}
 
@@ -564,6 +649,15 @@
 		 * We are done stashing the non-secure context. Ask the
 		 * secure payload to do the work now.
 		 */
+#if TSPD_ROUTE_IRQ_TO_EL3
+		/*
+		 * Enable the routing of NS interrupts to EL3 during resumption
+		 * of STD SMC call on this core.
+		 */
+		enable_intr_rm_local(INTR_TYPE_NS, SECURE);
+#endif
+
+
 
 		/* We just need to return to the preempted point in
 		 * TSP and the execution will resume as normal.
diff --git a/services/spd/tspd/tspd_pm.c b/services/spd/tspd/tspd_pm.c
index 17abaea..009ff5f 100644
--- a/services/spd/tspd/tspd_pm.c
+++ b/services/spd/tspd/tspd_pm.c
@@ -133,6 +133,14 @@
 	/* Initialise this cpu's secure context */
 	cm_init_context(mpidr, &tsp_on_entrypoint);
 
+#if TSPD_ROUTE_IRQ_TO_EL3
+	/*
+	 * Disable the NS interrupt locally since it will be enabled globally
+	 * within cm_init_context.
+	 */
+	disable_intr_rm_local(INTR_TYPE_NS, SECURE);
+#endif
+
 	/* Enter the TSP */
 	rc = tspd_synchronous_sp_entry(tsp_ctx);
 
diff --git a/services/spd/tspd/tspd_private.h b/services/spd/tspd/tspd_private.h
index 4d48dbd..5f6fb2b 100644
--- a/services/spd/tspd/tspd_private.h
+++ b/services/spd/tspd/tspd_private.h
@@ -120,6 +120,34 @@
 #define TSPD_C_RT_CTX_SIZE		0x60
 #define TSPD_C_RT_CTX_ENTRIES		(TSPD_C_RT_CTX_SIZE >> DWORD_SHIFT)
 
+/*******************************************************************************
+ * Constants that allow assembler code to preserve caller-saved registers of the
+ * SP context while performing a TSP preemption.
+ * Note: These offsets have to match with the offsets for the corresponding
+ * registers in cpu_context as we are using memcpy to copy the values from
+ * cpu_context to sp_ctx.
+ ******************************************************************************/
+#define TSPD_SP_CTX_X0		0x0
+#define TSPD_SP_CTX_X1		0x8
+#define TSPD_SP_CTX_X2		0x10
+#define TSPD_SP_CTX_X3		0x18
+#define TSPD_SP_CTX_X4		0x20
+#define TSPD_SP_CTX_X5		0x28
+#define TSPD_SP_CTX_X6		0x30
+#define TSPD_SP_CTX_X7		0x38
+#define TSPD_SP_CTX_X8		0x40
+#define TSPD_SP_CTX_X9		0x48
+#define TSPD_SP_CTX_X10		0x50
+#define TSPD_SP_CTX_X11		0x58
+#define TSPD_SP_CTX_X12		0x60
+#define TSPD_SP_CTX_X13		0x68
+#define TSPD_SP_CTX_X14		0x70
+#define TSPD_SP_CTX_X15		0x78
+#define TSPD_SP_CTX_X16		0x80
+#define TSPD_SP_CTX_X17		0x88
+#define TSPD_SP_CTX_SIZE	0x90
+#define TSPD_SP_CTX_ENTRIES		(TSPD_SP_CTX_SIZE >> DWORD_SHIFT)
+
 #ifndef __ASSEMBLY__
 
 #include <cassert.h>
@@ -142,6 +170,17 @@
 CASSERT(TSPD_C_RT_CTX_SIZE == sizeof(c_rt_regs_t),	\
 	assert_spd_c_rt_regs_size_mismatch);
 
+/* SEL1 Secure payload (SP) caller saved register context structure. */
+DEFINE_REG_STRUCT(sp_ctx_regs, TSPD_SP_CTX_ENTRIES);
+
+/*
+ * Compile time assertion to ensure that both the compiler and linker
+ * have the same double word aligned view of the size of the C runtime
+ * register context.
+ */
+CASSERT(TSPD_SP_CTX_SIZE == sizeof(sp_ctx_regs_t),	\
+	assert_spd_sp_regs_size_mismatch);
+
 /*******************************************************************************
  * Structure which helps the SPD to maintain the per-cpu state of the SP.
  * 'saved_spsr_el3' - temporary copy to allow FIQ handling when the TSP has been
@@ -155,6 +194,10 @@
  * 'cpu_ctx'        - space to maintain SP architectural state
  * 'saved_tsp_args' - space to store arguments for TSP arithmetic operations
  *                    which will queried using the TSP_GET_ARGS SMC by TSP.
+ * 'sp_ctx'         - space to save the SEL1 Secure Payload(SP) caller saved
+ *                    register context after it has been preempted by an EL3
+ *                    routed NS interrupt and when a Secure Interrupt is taken
+ *                    to SP.
  ******************************************************************************/
 typedef struct tsp_context {
 	uint64_t saved_elr_el3;
@@ -164,6 +207,9 @@
 	uint64_t c_rt_ctx;
 	cpu_context_t cpu_ctx;
 	uint64_t saved_tsp_args[TSP_NUM_ARGS];
+#if TSPD_ROUTE_IRQ_TO_EL3
+	sp_ctx_regs_t sp_ctx;
+#endif
 } tsp_context_t;
 
 /* Helper macros to store and retrieve tsp args from tsp_context */