diff --git a/bl1/aarch32/bl1_entrypoint.S b/bl1/aarch32/bl1_entrypoint.S
index e3d915f..39ebcf7 100644
--- a/bl1/aarch32/bl1_entrypoint.S
+++ b/bl1/aarch32/bl1_entrypoint.S
@@ -71,9 +71,21 @@
 	 */
 
 	/*
-	 * MMU needs to be disabled because both BL1 and BL2 execute
+	 * Get the smc_context for next BL image,
+	 * program the gp/system registers and save it in `r4`.
+	 */
+	bl	smc_get_next_ctx
+	mov	r4, r0
+
+	/* Only turn-off MMU if going to secure world */
+	ldr	r5, [r4, #SMC_CTX_SCR]
+	tst	r5, #SCR_NS_BIT
+	bne	skip_mmu_off
+
+	/*
+	 * MMU needs to be disabled because both BL1 and BL2/BL2U execute
 	 * in PL1, and therefore share the same address space.
-	 * BL2 will initialize the address space according to its
+	 * BL2/BL2U will initialize the address space according to its
 	 * own requirement.
 	 */
 	bl	disable_mmu_icache_secure
@@ -81,11 +93,8 @@
 	dsb	sy
 	isb
 
-	/*
-	 * Get the smc_context for next BL image,
-	 * program the gp/system registers and exit
-	 * secure monitor mode
-	 */
-	bl	smc_get_next_ctx
+skip_mmu_off:
+	/* Restore smc_context from `r4` and exit secure monitor mode. */
+	mov	r0, r4
 	monitor_exit
 endfunc bl1_entrypoint
diff --git a/bl1/aarch32/bl1_exceptions.S b/bl1/aarch32/bl1_exceptions.S
index de7ddc5..f73db40 100644
--- a/bl1/aarch32/bl1_exceptions.S
+++ b/bl1/aarch32/bl1_exceptions.S
@@ -8,11 +8,18 @@
 #include <asm_macros.S>
 #include <bl1.h>
 #include <bl_common.h>
+#include <context.h>
+#include <smcc_helpers.h>
+#include <smcc_macros.S>
+#include <xlat_tables.h>
 
 	.globl	bl1_aarch32_smc_handler
 
 
 func bl1_aarch32_smc_handler
+	/* On SMC entry, `sp` points to `smc_ctx_t`. Save `lr`. */
+	str	lr, [sp, #SMC_CTX_LR_MON]
+
 	/* ------------------------------------------------
 	 * SMC in BL1 is handled assuming that the MMU is
 	 * turned off by BL2.
@@ -20,12 +27,12 @@
 	 */
 
 	/* ----------------------------------------------
-	 * Only RUN_IMAGE SMC is supported.
+	 * Detect if this is a RUN_IMAGE or other SMC.
 	 * ----------------------------------------------
 	 */
-	mov	r8, #BL1_SMC_RUN_IMAGE
-	cmp	r8, r0
-	blne	report_exception
+	mov	lr, #BL1_SMC_RUN_IMAGE
+	cmp	lr, r0
+	bne	smc_handler
 
 	/* ------------------------------------------------
 	 * Make sure only Secure world reaches here.
@@ -70,3 +77,76 @@
 	ldm	r8, {r0, r1, r2, r3}
 	eret
 endfunc bl1_aarch32_smc_handler
+
+	/* -----------------------------------------------------
+	 * Save Secure/Normal world context and jump to
+	 * BL1 SMC handler.
+	 * -----------------------------------------------------
+	 */
+func smc_handler
+	/* -----------------------------------------------------
+	 * Save the GP registers.
+	 * -----------------------------------------------------
+	 */
+	smcc_save_gp_mode_regs
+
+	/*
+	 * `sp` still points to `smc_ctx_t`. Save it to a register
+	 * and restore the C runtime stack pointer to `sp`.
+	 */
+	mov	r6, sp
+	ldr	sp, [r6, #SMC_CTX_SP_MON]
+
+	ldr	r0, [r6, #SMC_CTX_SCR]
+	and	r7, r0, #SCR_NS_BIT		/* flags */
+
+	/* Switch to Secure Mode */
+	bic	r0, #SCR_NS_BIT
+	stcopr	r0, SCR
+	isb
+
+	/* If caller is from Secure world then turn on the MMU */
+	tst	r7, #SCR_NS_BIT
+	bne	skip_mmu_on
+
+	/* Turn on the MMU */
+	mov	r0, #DISABLE_DCACHE
+	bl	enable_mmu_secure
+
+	/* Enable the data cache. */
+	ldcopr	r9, SCTLR
+	orr	r9, r9, #SCTLR_C_BIT
+	stcopr	r9, SCTLR
+	isb
+
+skip_mmu_on:
+	/* Prepare arguments for BL1 SMC wrapper. */
+	ldr	r0, [r6, #SMC_CTX_GPREG_R0]	/* smc_fid */
+	mov	r1, #0				/* cookie */
+	mov	r2, r6				/* handle */
+	mov	r3, r7				/* flags */
+	bl	bl1_smc_wrapper
+
+	/* Get the smc_context for next BL image */
+	bl	smc_get_next_ctx
+	mov	r4, r0
+
+	/* Only turn-off MMU if going to secure world */
+	ldr	r5, [r4, #SMC_CTX_SCR]
+	tst	r5, #SCR_NS_BIT
+	bne	skip_mmu_off
+
+	/* Disable the MMU */
+	bl	disable_mmu_icache_secure
+	stcopr	r0, TLBIALL
+	dsb	sy
+	isb
+
+skip_mmu_off:
+	/* -----------------------------------------------------
+	 * Do the transition to next BL image.
+	 * -----------------------------------------------------
+	 */
+	mov	r0, r4
+	monitor_exit
+endfunc smc_handler