[U-Boot] [PATCH 15/16] Blackfin: Bf60x: add resume from hibernate

[Bob Liu <lliubbo@gmail.com>]

From: Steven Miao <realmz6@gmail.com>

Add Bf60x resume from hibernate support

Signed-off-by: Steven Miao <realmz6@gmail.com>
Signed-off-by: Sonic Zhang <sonic.zhang@analog.com>
Signed-off-by: Bob Liu <lliubbo@gmail.com>
---
 arch/blackfin/cpu/initcode.c |   46 ++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 46 insertions(+)

diff --git a/arch/blackfin/cpu/initcode.c b/arch/blackfin/cpu/initcode.c
index c68302f..0640c81 100644
--- a/arch/blackfin/cpu/initcode.c
+++ b/arch/blackfin/cpu/initcode.c
@@ -507,6 +507,11 @@ maybe_self_refresh(ADI_BOOT_DATA *bs)
 		return false;
 
 #ifdef __ADSPBF60x__
+	/* resume from hibernate, return false let ddr initialize */
+	if ((bfin_read32(DPM0_STAT) & 0xF0) == 0x50) {
+		serial_putc('b');
+		return false;
+	}
 
 #else /* __ADSPBF60x__ */
 
@@ -821,6 +826,7 @@ program_memory_controller(ADI_BOOT_DATA *bs, bool put_into_srfs)
 	while (!(bfin_read_DMC0_STAT() & DLLCALDONE))
 		continue;
 	serial_putc('!');
+
 #else /* __ADSPBF60x__ */
 
 	/* Program the external memory controller before we come out of
@@ -888,7 +894,46 @@ check_hibernation(ADI_BOOT_DATA *bs, u16 vr_ctl, bool put_into_srfs)
 		return;
 
 	serial_putc('b');
+#ifdef __ADSPBF60x__
+	if (bfin_read32(DPM0_RESTORE0) != 0) {
+		uint32_t reg = bfin_read_DMC0_CTL();
+		reg &= ~0x8;
+		bfin_write_DMC0_CTL(reg);
+
+		while ((bfin_read_DMC0_STAT() & 0x8))
+			continue;
+		while (!(bfin_read_DMC0_STAT() & 0x1))
+			continue;
+
+		serial_putc('z');
+		volatile uint32_t *hibernate_magic = bfin_read32(DPM0_RESTORE4);
+		__builtin_bfin_ssync(); /* make sure memory controller is done */
+		if (hibernate_magic[0] == 0xDEADBEEF) {
+			serial_putc('c');
+
+			__builtin_bfin_ssync();
+
+			bfin_write_EVT15(hibernate_magic[1]);
+			bfin_write_IMASK(EVT_IVG15);
+			__asm__ __volatile__ (
+				/* load reti early to avoid anomaly 281 */
+				"reti = %2;"
+				/* clear hibernate magic */
+				"[%0] = %1;"
+				/* load stack pointer */
+				"SP = [%0 + 8];"
+				/* lower ourselves from reset ivg to ivg15 */
+				"raise 15;"
+				"nop;nop;nop;"
+				"rti;"
+				:
+				: "p"(hibernate_magic), "d"(0x2000 /* jump.s 0 */), "d"(0xffa00000)
+			);
+		}
 
+
+	}
+#else
 	/* Are we coming out of hibernate (suspend to memory) ?
 	 * The memory layout is:
 	 * 0x0: hibernate magic for anomaly 307 (0xDEADBEEF)
@@ -920,6 +965,7 @@ check_hibernation(ADI_BOOT_DATA *bs, u16 vr_ctl, bool put_into_srfs)
 		}
 		serial_putc('d');
 	}
+#endif
 
 	serial_putc('e');
 }
-- 
1.7.9.5