diff --git a/arch/arm/cpu/armv7/start.S b/arch/arm/cpu/armv7/start.S
index 098a83ab7145bd324e6db9c20e0a5aea95c70970..9b49ece2d650e199a1ed3415ed2ae47794f20fab 100644
--- a/arch/arm/cpu/armv7/start.S
+++ b/arch/arm/cpu/armv7/start.S
@@ -54,8 +54,7 @@ save_boot_params_ret:
  * (OMAP4 spl TEXT_BASE is not 32 byte aligned.
  * Continue to use ROM code vector only in OMAP4 spl)
  */
-#if !(defined(CONFIG_OMAP44XX) && defined(CONFIG_SPL_BUILD)) && \
-		!defined(CONFIG_SPL_FEL)
+#if !(defined(CONFIG_OMAP44XX) && defined(CONFIG_SPL_BUILD))
 	/* Set V=0 in CP15 SCTLR register - for VBAR to point to vector */
 	mrc	p15, 0, r0, c1, c0, 0	@ Read CP15 SCTLR Register
 	bic	r0, #CR_V		@ V = 0
@@ -68,9 +67,7 @@ save_boot_params_ret:
 
 	/* the mask ROM code should have PLL and others stable */
 #ifndef CONFIG_SKIP_LOWLEVEL_INIT
-#ifndef CONFIG_SPL_FEL
 	bl	cpu_init_cp15
-#endif
 	bl	cpu_init_crit
 #endif
 
diff --git a/arch/arm/cpu/armv7/sunxi/board.c b/arch/arm/cpu/armv7/sunxi/board.c
index b7492ac1fe5e5f5f7552a19b003cbd5dc3da5d60..c02c0150960c2be2ee2324c680d86d258f09b4f2 100644
--- a/arch/arm/cpu/armv7/sunxi/board.c
+++ b/arch/arm/cpu/armv7/sunxi/board.c
@@ -30,6 +30,10 @@
 struct fel_stash {
 	uint32_t sp;
 	uint32_t lr;
+	uint32_t cpsr;
+	uint32_t sctlr;
+	uint32_t vbar;
+	uint32_t cr;
 };
 
 struct fel_stash fel_stash __attribute__((section(".data")));
@@ -108,15 +112,34 @@ void s_init(void)
  */
 u32 spl_boot_device(void)
 {
+#ifdef CONFIG_SPL_FEL
 	/*
-	 * Have we been asked to return to the FEL portion of the boot ROM?
-	 * TODO: We need a more robust test here, or bracket this with
-	 * #ifdef CONFIG_SPL_FEL.
+	 * This is the legacy compile time configuration for a special FEL
+	 * enabled build. It has many restrictions and can only boot over USB.
 	 */
-	if (fel_stash.lr >= 0xffff0000 && fel_stash.lr < 0xffff4000)
+	return BOOT_DEVICE_BOARD;
+#else
+	/*
+	 * When booting from the SD card, the "eGON.BT0" signature is expected
+	 * to be found in memory at the address 0x0004 (see the "mksunxiboot"
+	 * tool, which generates this header).
+	 *
+	 * When booting in the FEL mode over USB, this signature is patched in
+	 * memory and replaced with something else by the 'fel' tool. This other
+	 * signature is selected in such a way, that it can't be present in a
+	 * valid bootable SD card image (because the BROM would refuse to
+	 * execute the SPL in this case).
+	 *
+	 * This branch is just making a decision at runtime whether to load
+	 * the main u-boot binary from the SD card (if the "eGON.BT0" signature
+	 * is found) or return to the FEL code in the BROM to wait and receive
+	 * the main u-boot binary over USB.
+	 */
+	if (readl(4) == 0x4E4F4765 && readl(8) == 0x3054422E) /* eGON.BT0 */
+		return BOOT_DEVICE_MMC1;
+	else
 		return BOOT_DEVICE_BOARD;
-
-	return BOOT_DEVICE_MMC1;
+#endif
 }
 
 /* No confirmation data available in SPL yet. Hardcode bootmode */
diff --git a/arch/arm/cpu/armv7/sunxi/fel_utils.S b/arch/arm/cpu/armv7/sunxi/fel_utils.S
index 0c1de52df889b242e645536f7db9532f4cb0e9dd..bf0033552d23b34b079e3055c3c062981b25b15e 100644
--- a/arch/arm/cpu/armv7/sunxi/fel_utils.S
+++ b/arch/arm/cpu/armv7/sunxi/fel_utils.S
@@ -15,11 +15,28 @@ ENTRY(save_boot_params)
 	ldr	r0, =fel_stash
 	str	sp, [r0, #0]
 	str	lr, [r0, #4]
+	mrs	lr, cpsr		@ Read CPSR
+	str	lr, [r0, #8]
+	mrc	p15, 0, lr, c1, c0, 0	@ Read CP15 SCTLR Register
+	str	lr, [r0, #12]
+	mrc	p15, 0, lr, c12, c0, 0	@ Read VBAR
+	str	lr, [r0, #16]
+	mrc	p15, 0, lr, c1, c0, 0	@ Read CP15 Control Register
+	str	lr, [r0, #20]
 	b	save_boot_params_ret
 ENDPROC(save_boot_params)
 
 ENTRY(return_to_fel)
 	mov	sp, r0
 	mov	lr, r1
+	ldr	r0, =fel_stash
+	ldr	r1, [r0, #20]
+	mcr	p15, 0, r1, c1, c0, 0	@ Write CP15 Control Register
+	ldr	r1, [r0, #16]
+	mcr	p15, 0, r1, c12, c0, 0	@ Write VBAR
+	ldr	r1, [r0, #12]
+	mcr	p15, 0, r1, c1, c0, 0	@ Write CP15 SCTLR Register
+	ldr	r1, [r0, #8]
+	msr	cpsr, r1		@ Write CPSR
 	bx	lr
 ENDPROC(return_to_fel)