[U-Boot] [PATCH] arm/ls102xa: Add STANDBY implement based on PSCI

[Dongsheng Wang]

From: Wang Dongsheng <dongsheng.wang at freescale.com>

Base on PSCI services, implement CPU_SUSPEND for ls102xa platform.

Signed-off-by: Wang Dongsheng <dongsheng.wang at freescale.com>

diff --git a/arch/arm/cpu/armv7/ls102xa/psci.S b/arch/arm/cpu/armv7/ls102xa/psci.S
index cf5cd48..1bc7e45 100644
--- a/arch/arm/cpu/armv7/ls102xa/psci.S
+++ b/arch/arm/cpu/armv7/ls102xa/psci.S
@@ -14,10 +14,13 @@
 
 #define SCFG_CORE0_SFT_RST      0x130
 #define SCFG_CORESRENCR         0x204
+#define SCFG_CLUSTERPMCR	0x904
 
 #define DCFG_CCSR_BRR           0x0E4
 #define DCFG_CCSR_SCRATCHRW1    0x200
 
+#define RCPM_POWMGTCSR		0x130
+
 	.pushsection ._secure.text, "ax"
 
 	.arch_extension sec
@@ -111,6 +114,41 @@ psci_cpu_off:
 1:	wfi
 	b	1b
 
+.globl psci_cpu_suspend
+psci_cpu_suspend:
+	push	{lr}
+
+	@ Get Power State
+	mov	r0, r2, lsr #16
+	ands	r0, r0, #1
+	bne	out
+
+	@ STANDBY Power State
+	@ Get RCPM base address
+	movw	r0, #(CONFIG_SYS_FSL_RCPM_ADDR & 0xffff)
+	movt	r0, #(CONFIG_SYS_FSL_RCPM_ADDR >> 16)
+
+	@ Get POWMGTCSR
+	ldr	r1, [r0, #RCPM_POWMGTCSR]
+	rev	r1, r1
+
+	movw	r2, #0
+	movt	r2, #(1 << 4)
+
+	orr	r1, r1, r2
+	rev	r1, r1
+	str     r1, [r0, #RCPM_POWMGTCSR]
+
+	dsb
+	wfi
+
+out:
+	@ Return
+	mov	r0, #ARM_PSCI_RET_SUCCESS
+
+	pop	{lr}
+	bx	lr
+
 .globl	psci_arch_init
 psci_arch_init:
 	mov	r6, lr
@@ -121,6 +159,6 @@ psci_arch_init:
 
 	bx	r6
 
-	.globl psci_text_end
+.globl psci_text_end
 psci_text_end:
 	.popsection
diff --git a/arch/arm/include/asm/arch-ls102xa/config.h b/arch/arm/include/asm/arch-ls102xa/config.h
index c55cdef..e314c35 100644
--- a/arch/arm/include/asm/arch-ls102xa/config.h
+++ b/arch/arm/include/asm/arch-ls102xa/config.h
@@ -32,6 +32,7 @@
 #define CONFIG_SYS_FSL_SERDES_ADDR		(CONFIG_SYS_IMMR + 0x00ea0000)
 #define CONFIG_SYS_FSL_GUTS_ADDR		(CONFIG_SYS_IMMR + 0x00ee0000)
 #define CONFIG_SYS_FSL_LS1_CLK_ADDR		(CONFIG_SYS_IMMR + 0x00ee1000)
+#define CONFIG_SYS_FSL_RCPM_ADDR		(CONFIG_SYS_IMMR + 0x00ee2000)
 #define CONFIG_SYS_NS16550_COM1			(CONFIG_SYS_IMMR + 0x011c0500)
 #define CONFIG_SYS_NS16550_COM2			(CONFIG_SYS_IMMR + 0x011d0500)
 #define CONFIG_SYS_DCU_ADDR			(CONFIG_SYS_IMMR + 0x01ce0000)
-- 
2.1.0.27.g96db324