diff --git a/bl32/tsp/aarch64/tsp_entrypoint.S b/bl32/tsp/aarch64/tsp_entrypoint.S
index 54276f2..9999c43 100644
--- a/bl32/tsp/aarch64/tsp_entrypoint.S
+++ b/bl32/tsp/aarch64/tsp_entrypoint.S
@@ -39,8 +39,11 @@
 	.globl	tsp_cpu_suspend_entry
 	.globl	tsp_cpu_resume_entry
 	.globl	tsp_fast_smc_entry
+	.globl	tsp_std_smc_entry
 	.globl	tsp_fiq_entry
 
+
+
 	/* ---------------------------------------------
 	 * Populate the params in x0-x7 from the pointer
 	 * to the smc args structure in x0.
@@ -317,8 +320,22 @@
 	 * ---------------------------------------------
 	 */
 func tsp_fast_smc_entry
-	bl	tsp_fast_smc_handler
+	bl	tsp_smc_handler
 	restore_args_call_smc
 tsp_fast_smc_entry_panic:
 	b	tsp_fast_smc_entry_panic
 
+	/*---------------------------------------------
+	 * This entrypoint is used by the TSPD to ask
+	 * the TSP to service a std smc request.
+	 * We will enable preemption during execution
+	 * of tsp_smc_handler.
+	 * ---------------------------------------------
+	 */
+func tsp_std_smc_entry
+	msr	daifclr, #DAIF_FIQ_BIT | DAIF_IRQ_BIT
+	bl	tsp_smc_handler
+	msr	daifset, #DAIF_FIQ_BIT | DAIF_IRQ_BIT
+	restore_args_call_smc
+tsp_std_smc_entry_panic:
+	b	tsp_std_smc_entry_panic
diff --git a/bl32/tsp/aarch64/tsp_exceptions.S b/bl32/tsp/aarch64/tsp_exceptions.S
index ccb4cdd..f84b5e0 100644
--- a/bl32/tsp/aarch64/tsp_exceptions.S
+++ b/bl32/tsp/aarch64/tsp_exceptions.S
@@ -120,7 +120,14 @@
 
 	.align	7
 irq_sp_elx:
-	b	irq_sp_elx
+	save_caller_regs_and_lr
+	/* We just update some statistics in the handler */
+	bl	tsp_irq_received
+	/* Hand over control to the normal world to handle the IRQ */
+	smc	#0
+	/* The resume std smc starts from here */
+	restore_caller_regs_and_lr
+	eret
 	check_vector_size irq_sp_elx
 
 	.align	7
diff --git a/bl32/tsp/tsp_interrupt.c b/bl32/tsp/tsp_interrupt.c
index d5d02c3..5719c06 100644
--- a/bl32/tsp/tsp_interrupt.c
+++ b/bl32/tsp/tsp_interrupt.c
@@ -107,3 +107,18 @@
 
 	return 0;
 }
+
+int32_t tsp_irq_received()
+{
+	uint64_t mpidr = read_mpidr();
+	uint32_t linear_id = platform_get_core_pos(mpidr);
+
+	tsp_stats[linear_id].irq_count++;
+	spin_lock(&console_lock);
+	printf("TSP: cpu 0x%x received irq\n\r", mpidr);
+	INFO("cpu 0x%x: %d irq requests \n",
+	     mpidr, tsp_stats[linear_id].irq_count);
+	spin_unlock(&console_lock);
+
+	return TSP_PREEMPTED;
+}
diff --git a/bl32/tsp/tsp_main.c b/bl32/tsp/tsp_main.c
index ec74db4..1c3f3b9 100644
--- a/bl32/tsp/tsp_main.c
+++ b/bl32/tsp/tsp_main.c
@@ -66,6 +66,7 @@
  * to change.
  ******************************************************************************/
 static const entry_info_t tsp_entry_info = {
+	tsp_std_smc_entry,
 	tsp_fast_smc_entry,
 	tsp_cpu_on_entry,
 	tsp_cpu_off_entry,
@@ -309,9 +310,9 @@
  * TSP fast smc handler. The secure monitor jumps to this function by
  * doing the ERET after populating X0-X7 registers. The arguments are received
  * in the function arguments in order. Once the service is rendered, this
- * function returns to Secure Monitor by raising SMC
+ * function returns to Secure Monitor by raising SMC.
  ******************************************************************************/
-tsp_args_t *tsp_fast_smc_handler(uint64_t func,
+tsp_args_t *tsp_smc_handler(uint64_t func,
 			       uint64_t arg1,
 			       uint64_t arg2,
 			       uint64_t arg3,
@@ -324,18 +325,20 @@
 	uint64_t service_args[2];
 	uint64_t mpidr = read_mpidr();
 	uint32_t linear_id = platform_get_core_pos(mpidr);
+	const char *smc_type;
 
 	/* Update this cpu's statistics */
 	tsp_stats[linear_id].smc_count++;
 	tsp_stats[linear_id].eret_count++;
 
-	printf("SP: cpu 0x%x received fast smc 0x%x\n", read_mpidr(), func);
+	smc_type = ((func >> 31) & 1) == 1 ? "fast" : "standard";
+
+	printf("SP: cpu 0x%x received %s smc 0x%x\n", read_mpidr(), smc_type, func);
 	INFO("cpu 0x%x: %d smcs, %d erets\n", mpidr,
 	     tsp_stats[linear_id].smc_count,
 	     tsp_stats[linear_id].eret_count);
 
 	/* Render secure services and obtain results here */
-
 	results[0] = arg1;
 	results[1] = arg2;
 
@@ -346,20 +349,20 @@
 	tsp_get_magic(service_args);
 
 	/* Determine the function to perform based on the function ID */
-	switch (func) {
-	case TSP_FID_ADD:
+	switch (TSP_BARE_FID(func)) {
+	case TSP_ADD:
 		results[0] += service_args[0];
 		results[1] += service_args[1];
 		break;
-	case TSP_FID_SUB:
+	case TSP_SUB:
 		results[0] -= service_args[0];
 		results[1] -= service_args[1];
 		break;
-	case TSP_FID_MUL:
+	case TSP_MUL:
 		results[0] *= service_args[0];
 		results[1] *= service_args[1];
 		break;
-	case TSP_FID_DIV:
+	case TSP_DIV:
 		results[0] /= service_args[0] ? service_args[0] : 1;
 		results[1] /= service_args[1] ? service_args[1] : 1;
 		break;
@@ -367,9 +370,9 @@
 		break;
 	}
 
-	return set_smc_args(func,
+	return set_smc_args(func, 0,
 			    results[0],
 			    results[1],
-			    0, 0, 0, 0, 0);
+			    0, 0, 0, 0);
 }