[U-Boot] [PATCH 2/2] arm/ls102xa: Add PSCI support for ls102xa

[Dongsheng Wang]

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

Base on PSCI services, implement CPU_ON/CPU_OFF for ls102xa platform.

Tested on LS1021AQDS, LS1021ATWR.
Test CPU hotplug times: 60K
Test kernel boot times: 1.2K

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

diff --git a/arch/arm/cpu/armv7/ls102xa/Makefile b/arch/arm/cpu/armv7/ls102xa/Makefile
index 2e6a207..2d55782 100644
--- a/arch/arm/cpu/armv7/ls102xa/Makefile
+++ b/arch/arm/cpu/armv7/ls102xa/Makefile
@@ -12,3 +12,7 @@ obj-y	+= fsl_epu.o
 obj-$(CONFIG_OF_LIBFDT) += fdt.o
 obj-$(CONFIG_SYS_HAS_SERDES) += fsl_ls1_serdes.o ls102xa_serdes.o
 obj-$(CONFIG_SPL) += spl.o
+
+ifdef CONFIG_ARMV7_PSCI
+obj-y  += psci.o
+endif
diff --git a/arch/arm/cpu/armv7/ls102xa/psci.S b/arch/arm/cpu/armv7/ls102xa/psci.S
new file mode 100644
index 0000000..cf5cd48
--- /dev/null
+++ b/arch/arm/cpu/armv7/ls102xa/psci.S
@@ -0,0 +1,126 @@
+/*
+ * Copyright 2015 Freescale Semiconductor, Inc.
+ * Author: Wang Dongsheng <dongsheng.wang at freescale.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <config.h>
+#include <linux/linkage.h>
+
+#include <asm/armv7.h>
+#include <asm/arch-armv7/generictimer.h>
+#include <asm/psci.h>
+
+#define SCFG_CORE0_SFT_RST      0x130
+#define SCFG_CORESRENCR         0x204
+
+#define DCFG_CCSR_BRR           0x0E4
+#define DCFG_CCSR_SCRATCHRW1    0x200
+
+	.pushsection ._secure.text, "ax"
+
+	.arch_extension sec
+
+#define	ONE_MS		(GENERIC_TIMER_CLK / 1000)
+#define	RESET_WAIT	(30 * ONE_MS)
+
+	@ r1 = target CPU
+	@ r2 = target PC
+.globl	psci_cpu_on
+psci_cpu_on:
+	push	{lr}
+
+	@ Clear and Get the correct CPU number
+	@ r1 = 0xf01
+	and	r1, r1, #0xff
+
+	mov	r0, r1
+	bl	psci_get_cpu_stack_top
+	str	r2, [r0]
+	dsb
+
+	@ Get DCFG base address
+	movw	r4, #(CONFIG_SYS_FSL_GUTS_ADDR & 0xffff)
+	movt	r4, #(CONFIG_SYS_FSL_GUTS_ADDR >> 16)
+
+	@ Detect target CPU state
+	ldr	r2, [r4, #DCFG_CCSR_BRR]
+	rev	r2, r2
+	lsr	r2, r2, r1
+	ands	r2, r2, #1
+	beq	holdoff_release
+
+	@ Reset target CPU
+	@ Get SCFG base address
+	movw	r0, #(CONFIG_SYS_FSL_SCFG_ADDR & 0xffff)
+	movt	r0, #(CONFIG_SYS_FSL_SCFG_ADDR >> 16)
+
+	@ Enable CORE Soft Reset
+	movw	r5, #0
+	movt	r5, #(1 << 15)
+	rev	r5, r5
+	str	r5, [r0, #SCFG_CORESRENCR]
+
+	@ Get CPUx offset register
+	mov	r6, #0x4
+	mul	r6, r6, r1
+	add	r2, r0, r6
+
+	@ Do reset on target CPU
+	movw	r5, #0
+	movt	r5, #(1 << 15)
+	rev	r5, r5
+	str	r5, [r2, #SCFG_CORE0_SFT_RST]
+
+	@ Wait target CPU up
+	timer_wait	r2, RESET_WAIT
+
+	@ Disable CORE soft reset
+	mov	r5, #0
+	str	r5, [r0, #SCFG_CORESRENCR]
+
+holdoff_release:
+	@ Release on target CPU
+	ldr	r2, [r4, #DCFG_CCSR_BRR]
+	mov	r6, #1
+	lsl	r6, r6, r1	@ 32 bytes per CPU
+
+	rev	r6, r6
+	orr	r2, r2, r6
+	str	r2, [r4, #DCFG_CCSR_BRR]
+
+	@ Set secondary boot entry
+	ldr	r6, =psci_cpu_entry
+	rev	r6, r6
+	str	r6, [r4, #DCFG_CCSR_SCRATCHRW1]
+
+	isb
+	dsb
+
+	@ Return
+	mov	r0, #ARM_PSCI_RET_SUCCESS
+
+	pop	{lr}
+	bx	lr
+
+.globl	psci_cpu_off
+psci_cpu_off:
+	bl	psci_cpu_off_common
+
+1:	wfi
+	b	1b
+
+.globl	psci_arch_init
+psci_arch_init:
+	mov	r6, lr
+
+	bl	psci_get_cpu_id
+	bl	psci_get_cpu_stack_top
+	mov	sp, r0
+
+	bx	r6
+
+	.globl psci_text_end
+psci_text_end:
+	.popsection
diff --git a/include/configs/ls1021aqds.h b/include/configs/ls1021aqds.h
index ca913b0..7232cd9 100644
--- a/include/configs/ls1021aqds.h
+++ b/include/configs/ls1021aqds.h
@@ -11,6 +11,8 @@
 
 #define CONFIG_LS102XA
 
+#define CONFIG_ARMV7_PSCI
+
 #define CONFIG_SYS_GENERIC_BOARD
 
 #define CONFIG_DISPLAY_CPUINFO
diff --git a/include/configs/ls1021atwr.h b/include/configs/ls1021atwr.h
index 6b6f2ba..b618be5 100644
--- a/include/configs/ls1021atwr.h
+++ b/include/configs/ls1021atwr.h
@@ -11,6 +11,8 @@
 
 #define CONFIG_LS102XA
 
+#define CONFIG_ARMV7_PSCI
+
 #define CONFIG_SYS_GENERIC_BOARD
 
 #define CONFIG_DISPLAY_CPUINFO
-- 
2.1.0.27.g96db324