2 files changed, 39 insertions, 3 deletions
diff --git a/arch/arm/cpu/arm926ejs/spear/spl.c b/arch/arm/cpu/arm926ejs/spear/spl.c
index b004cccafd..b2cacf2d3e 100644
--- a/arch/arm/cpu/arm926ejs/spear/spl.c
+++ b/arch/arm/cpu/arm926ejs/spear/spl.c
@@ -16,6 +16,12 @@
 #include <asm/arch/spr_syscntl.h>
 #include <linux/mtd/st_smi.h>
 
+/* Reserve some space to store the BootROM's stack pointer during SPL operation.
+ * The BSS cannot be used for this purpose because it will be zeroed after
+ * having stored the pointer, so force the location to the data section.
+ */
+u32 bootrom_stash_sp __attribute__((section(".data")));
+
 static void ddr_clock_init(void)
 {
 	struct misc_regs *misc_p = (struct misc_regs *)CONFIG_SPEAR_MISCBASE;
@@ -223,8 +229,9 @@ u32 spl_boot_device(void)
 {
 	u32 mode = 0;
 
-	/* Currently only SNOR is supported as the only */
-	if (snor_boot_selected()) {
+	if (usb_boot_selected()) {
+		mode = BOOT_DEVICE_BOOTROM;
+	} else if (snor_boot_selected()) {
 		/* SNOR-SMI initialization */
 		snor_init();
 
@@ -252,3 +259,27 @@ void board_init_f(ulong dummy)
 	mpmc_init();
 	spear_late_init();
 }
+
+/*
+ * In a few cases (Ethernet, UART or USB boot, we might want to go back into the
+ * BootROM code right after having initialized a few components like the DRAM).
+ * The following function is called from SPL common code (board_init_r).
+ */
+void board_return_to_bootrom(void)
+{
+	/*
+	 * Retrieve the BootROM's stack pointer and jump back to the start of
+	 * the SPL, where we can easily branch back into the BootROM. Don't do
+	 * it right here because SPL might be compiled in Thumb mode while the
+	 * BootROM expects ARM mode.
+	 */
+	asm volatile ("ldr r0, =bootrom_stash_sp;"
+		      "ldr r0, [r0];"
+		      "mov sp, r0;"
+#if defined(CONFIG_SPL_SYS_THUMB_BUILD)
+		      "blx back_to_bootrom;"
+#else
+		      "bl back_to_bootrom;"
+#endif
+		      );
+}
diff --git a/arch/arm/cpu/arm926ejs/spear/start.S b/arch/arm/cpu/arm926ejs/spear/start.S
index 5fb2bd12ec..2cf854eb74 100644
--- a/arch/arm/cpu/arm926ejs/spear/start.S
+++ b/arch/arm/cpu/arm926ejs/spear/start.S
@@ -29,13 +29,17 @@
  */
 
 	.globl	reset
+	.globl	back_to_bootrom
 
 reset:
 	/*
 	* SPL has to return back to BootROM in a few cases (eg. Ethernet boot,
-	* UART boot, USB boot): save registers in BootROM's stack.
+	* UART boot, USB boot): save registers in BootROM's stack and then the
+	* BootROM's stack pointer in the SPL's data section.
 	*/
 	push	{r0-r12,lr}
+	ldr	r0, =bootrom_stash_sp
+	str	sp, [r0]
 
 	/*
 	 * Flush v4 I/D caches
@@ -56,4 +60,5 @@ reset:
 	 */
 	bl	_main	/* _main will call board_init_f */
 
+back_to_bootrom:
 	pop	{r0-r12,pc}