pm: a8k: add initial support for suspend to RAM

Add initial support for suspend to RAM feature.
The warm boot mode (recovery) is detected at BLE stage
and the control leaves the BootROM for eliminating
boot device image copy and verification.
Then booting CPU jumps to ATF and continues system restore
bypassing further BootROM stages, and finnaly jumps back to
Linux bypassing the u-boot.

Change-Id: Ifbafd783d5b554bfe50de6c4829e93e40cd28631
Signed-off-by: Konstantin Porotchkin <kostap@marvell.com>
Signed-off-by: Victor Gu <xigu@marvell.com>
Reviewed-on: http://vgitil04.il.marvell.com:8080/40478
Tested-by: iSoC Platform CI <ykjenk@marvell.com>

5 files changed, 359 insertions, 14 deletions

diff --git a/ble/ble_main.c b/ble/ble_main.c
index dd0b5039..ed3cfefe 100644
--- a/ble/ble_main.c
+++ b/ble/ble_main.c
@@ -32,11 +32,14 @@
 ***************************************************************************
 */
 
+#include <arch_helpers.h>
 #include <debug.h>
 #include <console.h>
+#include <mmio.h>
 #include <platform_def.h>
 #include <plat_marvell.h>
 #include <plat_private.h>
+#include <marvell_pm.h>
 #include <string.h>
 
 #define BR_FLAG_SILENT		0x1
@@ -52,13 +55,17 @@ void mailbox_clean(void)
 int  __attribute__ ((section(".entry"))) ble_main(int bootrom_flags)
 {
 	int skip = 0;
+	uintptr_t *mailbox = (void *)PLAT_MARVELL_MAILBOX_BASE;
+
 	/*
 	 * In some situations, like boot from UART, bootrom will
 	 * request to avoid printing to console. in that case don't
 	 * initialize the console and prints will be ignored
 	 */
 	if ((bootrom_flags & BR_FLAG_SILENT) == 0)
-		console_init(PLAT_MARVELL_BOOT_UART_BASE, PLAT_MARVELL_BOOT_UART_CLK_IN_HZ, MARVELL_CONSOLE_BAUDRATE);
+		console_init(PLAT_MARVELL_BOOT_UART_BASE,
+			     PLAT_MARVELL_BOOT_UART_CLK_IN_HZ,
+			     MARVELL_CONSOLE_BAUDRATE);
 
 	NOTICE("Starting binary extension\n");
 
@@ -69,7 +76,6 @@ int  __attribute__ ((section(".entry"))) ble_main(int bootrom_flags)
 	NOTICE("Skip DRAM setup in PALLADIUM mode\n");
 #else
 	ble_plat_setup(&skip);
-
 #endif
 	/* if there's skip image request, bootrom will load from the image
 	 * saved on the next address of the flash
@@ -77,8 +83,36 @@ int  __attribute__ ((section(".entry"))) ble_main(int bootrom_flags)
 	if (skip)
 		return SKIP_IMAGE_CODE;
 
-	/* clean mailbox from garbage data */
-	mailbox_clean();
+	/*
+	 * Check if the mailbox magic number is stored at index MBOX_IDX_MAGIC
+	 * and the suspend to RAM magic number at index MBOX_IDX_SUSPEND_MAGIC.
+	 * If the above is true, this is the recovery from suspend to RAM state.
+	 * In such case the mailbox should remain intact, since it stores the
+	 * warm boot jump address to be used by the ATF in BL31.
+	 * Othervise the mailbox should be cleaned from a garbage data.
+	 */
+	if (mailbox[MBOX_IDX_MAGIC] != MVEBU_MAILBOX_MAGIC_NUM ||
+	    mailbox[MBOX_IDX_SUSPEND_MAGIC] != MVEBU_MAILBOX_SUSPEND_STATE) {
+		NOTICE("Cold boot\n");
+		mailbox_clean();
+	} else {
+		void (*bootrom_exit)(void) =
+			(void (*)(void))mailbox[MBOX_IDX_ROM_EXIT_ADDR];
+
+		INFO("Recovery...\n");
+		/*
+		 * If this is recovery from suspend, two things has to be done:
+		 * 1. Define the DRAM region as executable memory for preparing
+		 *    jump to ATF
+		 * 2. Instead of returning control to the BootROM, invalidate
+		 *    and flush caches, and continue execution at address stored
+		 *    in the mailbox.
+		 * This should be done until the BootROM have a native support
+		 * for the system restore flow.
+		 */
+		ble_prepare_exit();
+		bootrom_exit();
+	}
 
 	return 0;
 }
diff --git a/include/plat/marvell/a8k/common/plat_marvell.h b/include/plat/marvell/a8k/common/plat_marvell.h
index dfc1db19..3cd26565 100644
--- a/include/plat/marvell/a8k/common/plat_marvell.h
+++ b/include/plat/marvell/a8k/common/plat_marvell.h
@@ -126,6 +126,8 @@ void marvell_bl1_setup_mpps(void);
 #endif
 
 const mmap_region_t *plat_marvell_get_mmap(void);
+void ble_prepare_exit(void);
+void exit_bootrom(uintptr_t);
 
 int plat_marvell_early_cpu_powerdown(void);
 #endif /* __PLAT_MARVELL_H__ */
diff --git a/plat/marvell/a8k/common/aarch64/plat_helpers.S b/plat/marvell/a8k/common/aarch64/plat_helpers.S
index 1e396902..5a71c3aa 100644
--- a/plat/marvell/a8k/common/aarch64/plat_helpers.S
+++ b/plat/marvell/a8k/common/aarch64/plat_helpers.S
@@ -33,13 +33,31 @@
  */
 
 #include <asm_macros.S>
+#include <arm_def.h>
 #include <platform_def.h>
 #include <marvell_pm.h>
 
+#define CCU_WIN_CR(x)		(MVEBU_CCU_BASE + (0x10 * x))
+#define	CCU_SRAM_WIN_CR		CCU_WIN_CR(1)
+
+#define	LLC_CR			(MVEBU_LLC_BASE + 0x100)
+#define LLC_TC0_LOCK		(MVEBU_LLC_BASE + 0x920)
+#define	LLC_MNT_IW		(MVEBU_LLC_BASE + 0x77c)
+#define LLC_WAYS		8
+#define	LLC_WAY_MASK		((1 << LLC_WAYS) - 1)
+
 	.globl	plat_secondary_cold_boot_setup
 	.globl	plat_get_my_entrypoint
 	.globl	plat_is_my_cpu_primary
 	.globl  plat_reset_handler
+	.globl disable_mmu_dcache
+	.globl invalidate_dcache_all
+	.globl invalidate_tlb_all
+	.globl platform_unmap_sram
+	.globl disable_sram
+	.globl disable_icache
+	.globl invalidate_icache_all
+	.globl exit_bootrom
 
 	/* -----------------------------------------------------
 	 * void plat_secondary_cold_boot_setup (void);
@@ -136,3 +154,217 @@ func plat_reset_handler
 
         ret
 endfunc plat_reset_handler
+
+/*
+ ***************************************************************************
+ * PM helpers
+ ***************************************************************************
+ */
+
+	/* -----------------------------------------------------
+	 * -----------------------------------------------------
+	 */
+func disable_mmu_dcache
+	/* Disable icache, dcache, and MMU */
+	mrs	x0, sctlr_el3
+	bic	x0, x0, 0x1		/* M bit - MMU */
+	bic	x0, x0, 0x4		/* C bit - Dcache L1 & L2 */
+	msr	sctlr_el3, x0
+	isb
+	b	mmu_off
+mmu_off:
+	ret
+endfunc disable_mmu_dcache
+
+/*
+ * void dcache_maint_level(level)
+ *
+ * clean and invalidate one level cache.
+ *
+ * x0: cache level
+ * x1: 0 flush & invalidate, 1 invalidate only
+ * x2~x9: clobbered
+ */
+func dcache_maint_level
+	lsl	x12, x0, #1
+	msr	csselr_el1, x12		/* select cache level */
+	isb				/* sync change of cssidr_el1 */
+	mrs	x6, ccsidr_el1		/* read the new cssidr_el1 */
+	and	x2, x6, #7		/* x2 <- log2(cache line size)-4 */
+	add	x2, x2, #4		/* x2 <- log2(cache line size) */
+	mov	x3, #0x3ff
+	and	x3, x3, x6, lsr #3	/* x3 <- max number of #ways */
+	clz	w5, w3			/* bit position of #ways */
+	mov	x4, #0x7fff
+	and	x4, x4, x6, lsr #13	/* x4 <- max number of #sets */
+	/* x12 <- cache level << 1 */
+	/* x2 <- line length offset */
+	/* x3 <- number of cache ways - 1 */
+	/* x4 <- number of cache sets - 1 */
+	/* x5 <- bit position of #ways */
+
+loop_set:
+	mov	x6, x3			/* x6 <- working copy of #ways */
+loop_way:
+	lsl	x7, x6, x5
+	orr	x9, x12, x7		/* map way and level to cisw value */
+	lsl	x7, x4, x2
+	orr	x9, x9, x7		/* map set number to cisw value */
+	tbz	w1, #0, 1f
+	dc	isw, x9
+	b	2f
+1:	dc	cisw, x9		/* clean & invalidate by set/way */
+2:	subs	x6, x6, #1		/* decrement the way */
+	b.ge	loop_way
+	subs	x4, x4, #1		/* decrement the set */
+	b.ge	loop_set
+
+	ret
+endfunc dcache_maint_level
+
+/*
+ * void __asm_flush_dcache_all(int invalidate_only)
+ *
+ * x0: 0 flush & invalidate, 1 invalidate only
+ *
+ * clean and invalidate all data cache by SET/WAY.
+ */
+func dcache_maint_all
+	mov	x1, x0
+	dsb	sy
+	mrs	x10, clidr_el1		/* read clidr_el1 */
+	lsr	x11, x10, #24
+	and	x11, x11, #0x7		/* x11 <- loc */
+	cbz	x11, finished		/* if loc is 0, exit */
+	mov	x15, x30
+	mov	x0, #0			/* start flush at cache level 0 */
+	/* x0  <- cache level */
+	/* x10 <- clidr_el1 */
+	/* x11 <- loc */
+	/* x15 <- return address */
+
+loop_level:
+	lsl	x12, x0, #1
+	add	x12, x12, x0		/* x0 <- tripled cache level */
+	lsr	x12, x10, x12
+	and	x12, x12, #7		/* x12 <- cache type */
+	cmp	x12, #2
+	b.lt	skip			/* skip if no cache or icache */
+	bl	dcache_maint_level	/* x1 = 0 flush, 1 invalidate */
+skip:
+	add	x0, x0, #1		/* increment cache level */
+	cmp	x11, x0
+	b.gt	loop_level
+
+	mov	x0, #0
+	msr	csselr_el1, x0		/* resotre csselr_el1 */
+	dsb	sy
+	isb
+	mov	x30, x15
+
+finished:
+	ret
+endfunc dcache_maint_all
+
+	/* -----------------------------------------------------
+	 * -----------------------------------------------------
+	 */
+func invalidate_dcache_all
+	mov	x16, x30
+	mov	x0, #0xffff
+	bl	dcache_maint_all
+	mov	x30, x16
+	ret
+endfunc invalidate_dcache_all
+
+	/* -----------------------------------------------------
+	 * -----------------------------------------------------
+	 */
+func invalidate_tlb_all
+	tlbi	alle3
+	dsb	sy
+	isb
+	ret
+endfunc invalidate_tlb_all
+
+	/* -----------------------------------------------------
+	 * -----------------------------------------------------
+	 */
+func platform_unmap_sram
+	/* Just need to clear the enable bit */
+	ldr     x0, =CCU_SRAM_WIN_CR
+	str     wzr, [x0]
+	ret
+endfunc platform_unmap_sram
+
+	/* -----------------------------------------------------
+	 * -----------------------------------------------------
+	 */
+func disable_sram
+	/* Disable the line lockings. They must be disabled expictly
+	 * or the OS will have problems using the cache */
+	ldr     x1, =LLC_TC0_LOCK
+	str     wzr, [x1]
+
+	/* Invalidate all ways */
+	ldr	w1, =LLC_WAY_MASK
+	ldr	x0, =LLC_MNT_IW
+	str     w1, [x0]
+
+	/* Finally disable LLC */
+	ldr     x0, =LLC_CR
+	str     wzr, [x0]
+
+	ret
+endfunc disable_sram
+
+	/* -----------------------------------------------------
+	 * -----------------------------------------------------
+	 */
+func disable_icache
+	mrs 	x0, sctlr_el3
+	bic	x0, x0, 0x1000	/* I bit - Icache L1 & L2 */
+	msr	sctlr_el3, x0
+	isb
+	ret
+endfunc disable_icache
+
+	/* -----------------------------------------------------
+	 * -----------------------------------------------------
+	 */
+func invalidate_icache_all
+	ic	ialluis
+	isb	sy
+	ret
+endfunc invalidate_icache_all
+
+func exit_bootrom
+	/* Save the system restore address */
+	mov	x28, x0
+
+	/* close the caches and MMU */
+	bl	disable_mmu_dcache
+
+	/*
+	 * There is nothing important in the caches now,
+	 * so invalidate them instead of cleaning.
+	 */
+	bl	invalidate_dcache_all
+	bl	invalidate_tlb_all
+
+	/*
+	 * Clean the memory mapping of SRAM
+	 * the DDR mapping will remain to enable boot image to execute
+	 */
+	bl	platform_unmap_sram
+
+	/* Disable the SRAM */
+	bl	disable_sram
+
+	/* Disable and invalidate icache */
+	bl	disable_icache
+	bl	invalidate_icache_all
+
+	mov	x0, x28
+	br	x0
+endfunc exit_bootrom
diff --git a/plat/marvell/a8k/common/plat_bl31_setup.c b/plat/marvell/a8k/common/plat_bl31_setup.c
index b736830f..ab616a50 100644
--- a/plat/marvell/a8k/common/plat_bl31_setup.c
+++ b/plat/marvell/a8k/common/plat_bl31_setup.c
@@ -36,6 +36,7 @@
 #include <plat_private.h>
 #include <apn806_setup.h>
 #include <cp110_setup.h>
+#include <marvell_pm.h>
 #include <mmio.h>
 #include <mci.h>
 #include <debug.h>
@@ -76,6 +77,8 @@ void marvell_bl31_mss_init(void)
 /* This function overruns the same function in marvell_bl31_setup.c */
 void bl31_plat_arch_setup(void)
 {
+	uintptr_t *mailbox = (void *)PLAT_MARVELL_MAILBOX_BASE;
+
 	/* initiliaze the timer for mdelay/udelay functionality */
 	plat_delay_timer_init();
 
@@ -87,7 +90,9 @@ void bl31_plat_arch_setup(void)
 	 * this will cause an hang in init_rfu
 	 * (after setting the IO windows GCR values).
 	 */
-	marvell_bl31_plat_arch_setup();
+	if (mailbox[MBOX_IDX_MAGIC] != MVEBU_MAILBOX_MAGIC_NUM ||
+	    mailbox[MBOX_IDX_SUSPEND_MAGIC] != MVEBU_MAILBOX_SUSPEND_STATE)
+		marvell_bl31_plat_arch_setup();
 
 	/* configure cp110 for CP0*/
 	cp110_init(0);
@@ -100,5 +105,7 @@ void bl31_plat_arch_setup(void)
 	marvell_bl31_mpp_init();
 
 	/* initialize IPC between MSS and ATF */
-	marvell_bl31_mss_init();
+	if (mailbox[MBOX_IDX_MAGIC] != MVEBU_MAILBOX_MAGIC_NUM ||
+	    mailbox[MBOX_IDX_SUSPEND_MAGIC] != MVEBU_MAILBOX_SUSPEND_STATE)
+		marvell_bl31_mss_init();
 }
diff --git a/plat/marvell/a8k/common/plat_pm.c b/plat/marvell/a8k/common/plat_pm.c
index efed0a83..ccdccc18 100644
--- a/plat/marvell/a8k/common/plat_pm.c
+++ b/plat/marvell/a8k/common/plat_pm.c
@@ -33,10 +33,14 @@
  */
 
 #include <plat_marvell.h>
+#include <platform.h>
+#include <console.h>
 #include <gicv2.h>
 #include <mmio.h>
 #include <debug.h>
 #include <delay_timer.h>
+#include <cache_llc.h>
+#include <marvell_pm.h>
 
 #ifdef SCP_IMAGE
 #include <bakery_lock.h>
@@ -295,6 +299,48 @@ void plat_marvell_system_reset(void)
 	mmio_write_32(MVEBU_RFU_BASE + MVEBU_RFU_GLOBL_SW_RST, 0x0);
 }
 
+/*
+ * This function should be called on restore from
+ * "suspend to RAM" state when the execution flow
+ * has to bypass BootROM image to RAM copy and speed up
+ * the system recovery
+ *
+ */
+static void plat_exit_bootrom(void)
+{
+	exit_bootrom(PLAT_MARVELL_TRUSTED_ROM_BASE);
+}
+
+/*
+ * Send a command to external PMIC to cut off the power rail
+ */
+void plat_marvell_power_suspend_to_ram(void)
+{
+	uintptr_t *mailbox = (void *)PLAT_MARVELL_MAILBOX_BASE;
+
+	INFO("Suspending to RAM\n");
+
+	/* Prevent interrupts from spuriously waking up this cpu */
+	gicv2_cpuif_disable();
+
+	mailbox[MBOX_IDX_SUSPEND_MAGIC] = MVEBU_MAILBOX_SUSPEND_STATE;
+	mailbox[MBOX_IDX_ROM_EXIT_ADDR] = (uintptr_t)&plat_exit_bootrom;
+
+#if PLAT_MARVELL_SHARED_RAM_CACHED
+	flush_dcache_range(PLAT_MARVELL_MAILBOX_BASE +
+			   MBOX_IDX_SUSPEND_MAGIC * sizeof(uintptr_t),
+			   2 * sizeof(uintptr_t));
+#endif
+
+	isb();
+	/*
+	 * Do not halt here!
+	 * The function must return for allowing the caller function
+	 * psci_power_up_finish() to do the proper context saving and
+	 * to release the CPU lock.
+	*/
+}
+
 /*******************************************************************************
  * A8K handler called to check the validity of the power state
  * parameter.
@@ -401,10 +447,7 @@ void a8k_pwr_domain_off(const psci_power_state_t *target_state)
 	/* Prevent interrupts from spuriously waking up this cpu */
 	gicv2_cpuif_disable();
 
-	/*
-	 * pm system synchronization - used to synchronize
-	 * multiple core access to MSS
-	 */
+	/* pm system synchronization - used to synchronize multiple core access to MSS */
 	bakery_lock_get(&pm_sys_lock);
 
 	/* send CPU OFF IPC Message to MSS */
@@ -453,8 +496,8 @@ void a8k_pwr_domain_suspend(const psci_power_state_t *target_state)
 	/* trace message */
 	PM_TRACE(TRACE_PWR_DOMAIN_SUSPEND, idx);
 #else
-	INFO("a8k_pwr_domain_suspend is not supported without SCP\n");
-	return;
+	/* Suspend to RAM */
+	plat_marvell_power_suspend_to_ram();
 #endif /* SCP_IMAGE */
 }
 
@@ -497,8 +540,35 @@ void a8k_pwr_domain_suspend_finish(const psci_power_state_t *target_state)
 	/* trace message */
 	PM_TRACE(TRACE_PWR_DOMAIN_SUSPEND_FINISH, idx);
 #else
-	INFO("a8k_pwr_domain_on_finish is not supported without SCP\n");
-	return;
+	uintptr_t *mailbox = (void *)PLAT_MARVELL_MAILBOX_BASE;
+
+	/* Only primary CPU requres platform init */
+	if (!plat_my_core_pos()) {
+		/* Initialize the console to provide early debug support */
+		console_init(PLAT_MARVELL_BOOT_UART_BASE,
+			     PLAT_MARVELL_BOOT_UART_CLK_IN_HZ,
+			     MARVELL_CONSOLE_BAUDRATE);
+		bl31_plat_arch_setup();
+		marvell_bl31_platform_setup();
+		/*
+		 * Remove suspend to RAM marker from the mailbox
+		 * for treating a regular reset as a cold boot
+		 */
+		mailbox[MBOX_IDX_SUSPEND_MAGIC] = 0;
+		mailbox[MBOX_IDX_ROM_EXIT_ADDR] = 0;
+#if PLAT_MARVELL_SHARED_RAM_CACHED
+		flush_dcache_range(PLAT_MARVELL_MAILBOX_BASE +
+				   MBOX_IDX_SUSPEND_MAGIC * sizeof(uintptr_t),
+				   2 * sizeof(uintptr_t));
+#endif
+		return;
+	}
+
+	/* arch specific configuration */
+	psci_arch_init();
+
+	/* Interrupt initialization */
+	gicv2_cpuif_enable();
 #endif /* SCP_IMAGE */
 }