ARMv7: PSCI: ls102xa: add more PSCI v1.0 functions implemention

This patch implements PSCI functions for ls102xa SoC following PSCI v1.0, they are as the list: psci_version, psci_features, psci_cpu_suspend, psci_affinity_info, psci_system_reset, psci_system_off. Tested on LS1021aQDS, LS1021aTWR. Signed-off-by: Wang Dongsheng <dongsheng.wang@nxp.com> Signed-off-by: Hongbo Zhang <hongbo.zhang@nxp.com> Reviewed-by: York Sun <york.sun@nxp.com>

ARMv7: PSCI: ls102xa: add more PSCI v1.0 functions implemention
This patch implements PSCI functions for ls102xa SoC following PSCI v1.0, they are as the list: psci_version, psci_features, psci_cpu_suspend, psci_affinity_info, psci_system_reset, psci_system_off. Tested on LS1021aQDS, LS1021aTWR. Signed-off-by: Wang Dongsheng <dongsheng.wang@nxp.com> Signed-off-by: Hongbo Zhang <hongbo.zhang@nxp.com> Reviewed-by: York Sun <york.sun@nxp.com>
Hongbo Zhang · York Sun
1 parent 7e742c276d
Showing 8 changed files with 166 additions and 2 deletions Side-by-side Diff
arch/arm/cpu/armv7/ls102xa/psci.S
arch/arm/include/asm/arch-ls102xa/config.h
board/freescale/ls1021aqds/Makefile
board/freescale/ls1021aqds/psci.S
board/freescale/ls1021atwr/Makefile
board/freescale/ls1021atwr/psci.S
include/configs/ls1021aqds.h
include/configs/ls1021atwr.h
@@ -12,19 +12,72 @@
 #include <asm/arch-armv7/generictimer.h>
 #include <asm/psci.h>
  
+#define RCPM_TWAITSR		0x04C
+
 #define SCFG_CORE0_SFT_RST      0x130
 #define SCFG_CORESRENCR         0x204
  
-#define DCFG_CCSR_BRR           0x0E4
-#define DCFG_CCSR_SCRATCHRW1    0x200
+#define DCFG_CCSR_RSTCR			0x0B0
+#define DCFG_CCSR_RSTCR_RESET_REQ	0x2
+#define DCFG_CCSR_BRR			0x0E4
+#define DCFG_CCSR_SCRATCHRW1		0x200
  
+#define PSCI_FN_PSCI_VERSION_FEATURE_MASK	0x0
+#define PSCI_FN_CPU_SUSPEND_FEATURE_MASK	0x0
+#define PSCI_FN_CPU_OFF_FEATURE_MASK		0x0
+#define PSCI_FN_CPU_ON_FEATURE_MASK		0x0
+#define PSCI_FN_AFFINITY_INFO_FEATURE_MASK	0x0
+#define PSCI_FN_SYSTEM_OFF_FEATURE_MASK		0x0
+#define PSCI_FN_SYSTEM_RESET_FEATURE_MASK	0x0
+
 	.pushsection ._secure.text, "ax"
  
 	.arch_extension sec
  
+	.align	5
+
 #define	ONE_MS		(GENERIC_TIMER_CLK / 1000)
 #define	RESET_WAIT	(30 * ONE_MS)
  
+.globl	psci_version
+psci_version:
+	movw	r0, #0
+	movt	r0, #1
+
+	bx	lr
+
+_ls102x_psci_supported_table:
+	.word	ARM_PSCI_0_2_FN_PSCI_VERSION
+	.word	PSCI_FN_PSCI_VERSION_FEATURE_MASK
+	.word	ARM_PSCI_0_2_FN_CPU_SUSPEND
+	.word	PSCI_FN_CPU_SUSPEND_FEATURE_MASK
+	.word	ARM_PSCI_0_2_FN_CPU_OFF
+	.word	PSCI_FN_CPU_OFF_FEATURE_MASK
+	.word	ARM_PSCI_0_2_FN_CPU_ON
+	.word	PSCI_FN_CPU_ON_FEATURE_MASK
+	.word	ARM_PSCI_0_2_FN_AFFINITY_INFO
+	.word	PSCI_FN_AFFINITY_INFO_FEATURE_MASK
+	.word	ARM_PSCI_0_2_FN_SYSTEM_OFF
+	.word	PSCI_FN_SYSTEM_OFF_FEATURE_MASK
+	.word	ARM_PSCI_0_2_FN_SYSTEM_RESET
+	.word	PSCI_FN_SYSTEM_RESET_FEATURE_MASK
+	.word	0
+	.word	ARM_PSCI_RET_NI
+
+.globl	psci_features
+psci_features:
+	adr	r2, _ls102x_psci_supported_table
+1:	ldr	r3, [r2]
+	cmp	r3, #0
+	beq	out_psci_features
+	cmp	r1, r3
+	addne	r2, r2, #8
+	bne	1b
+
+out_psci_features:
+	ldr	r0, [r2, #4]
+	bx	lr
+
 @ r0: return value ARM_PSCI_RET_SUCCESS or ARM_PSCI_RET_INVAL
 @ r1: input target CPU ID in MPIDR format, original value in r1 may be dropped
 @ r4: output validated CPU ID if ARM_PSCI_RET_SUCCESS returns, meaningless for
@@ -140,6 +193,52 @@
 .globl	psci_cpu_off
 psci_cpu_off:
 	bl	psci_cpu_off_common
+
+1:	wfi
+	b	1b
+
+.globl	psci_affinity_info
+psci_affinity_info:
+	push	{lr}
+
+	mov	r0, #ARM_PSCI_RET_INVAL
+
+	@ Verify Affinity level
+	cmp	r2, #0
+	bne	out_affinity_info
+
+	bl	psci_check_target_cpu_id
+	cmp	r0, #ARM_PSCI_RET_INVAL
+	beq	out_affinity_info
+	mov	r1, r4
+
+	@ Get RCPM base address
+	movw	r4, #(CONFIG_SYS_FSL_RCPM_ADDR & 0xffff)
+	movt	r4, #(CONFIG_SYS_FSL_RCPM_ADDR >> 16)
+
+	mov	r0, #PSCI_AFFINITY_LEVEL_ON
+
+	@ Detect target CPU state
+	ldr	r2, [r4, #RCPM_TWAITSR]
+	rev	r2, r2
+	lsr	r2, r2, r1
+	ands	r2, r2, #1
+	beq	out_affinity_info
+
+	mov	r0, #PSCI_AFFINITY_LEVEL_OFF
+
+out_affinity_info:
+	pop	{pc}
+
+.globl	psci_system_reset
+psci_system_reset:
+	@ Get DCFG base address
+	movw	r1, #(CONFIG_SYS_FSL_GUTS_ADDR & 0xffff)
+	movt	r1, #(CONFIG_SYS_FSL_GUTS_ADDR >> 16)
+
+	mov	r2, #DCFG_CCSR_RSTCR_RESET_REQ
+	rev	r2, r2
+	str	r2, [r1, #DCFG_CCSR_RSTCR]
  
 1:	wfi
 	b	1b
@@ -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)
@@ -8,4 +8,5 @@
 obj-y += ddr.o
 obj-y += eth.o
 obj-$(CONFIG_FSL_DCU_FB) += dcu.o
+obj-$(CONFIG_ARMV7_PSCI) += psci.o
+/*
+ * Copyright 2016 NXP Semiconductor.
+ * Author: Wang Dongsheng <dongsheng.wang@freescale.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <config.h>
+#include <linux/linkage.h>
+
+#include <asm/armv7.h>
+#include <asm/psci.h>
+
+	.pushsection ._secure.text, "ax"
+
+	.arch_extension sec
+
+	.align	5
+
+.globl	psci_system_off
+psci_system_off:
+	@ Get QIXIS base address
+	movw	r1, #(QIXIS_BASE & 0xffff)
+	movt	r1, #(QIXIS_BASE >> 16)
+
+	ldrb	r2, [r1, #QIXIS_PWR_CTL]
+	orr	r2, r2, #QIXIS_PWR_CTL_POWEROFF
+	strb	r2, [r1, #QIXIS_PWR_CTL]
+
+1:	wfi
+	b	1b
+
+	.popsection
@@ -6,4 +6,5 @@
  
 obj-y += ls1021atwr.o
 obj-$(CONFIG_FSL_DCU_FB) += dcu.o
+obj-$(CONFIG_ARMV7_PSCI) += psci.o
+/*
+ * Copyright 2016 NXP Semiconductor.
+ * Author: Wang Dongsheng <dongsheng.wang@freescale.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <config.h>
+#include <linux/linkage.h>
+
+#include <asm/armv7.h>
+#include <asm/psci.h>
+
+	.pushsection ._secure.text, "ax"
+
+	.arch_extension sec
+
+	.align	5
+
+.globl	psci_system_off
+psci_system_off:
+1:	wfi
+	b	1b
+
+	.popsection
@@ -10,6 +10,7 @@
 #define CONFIG_LS102XA
  
 #define CONFIG_ARMV7_PSCI
+#define CONFIG_ARMV7_PSCI_1_0
 #define CONFIG_ARMV7_PSCI_NR_CPUS	CONFIG_MAX_CPUS
  
 #define CONFIG_SYS_FSL_CLK
@@ -280,6 +281,8 @@
 #define QIXIS_LBMAP_SHIFT		0
 #define QIXIS_LBMAP_DFLTBANK		0x00
 #define QIXIS_LBMAP_ALTBANK		0x04
+#define QIXIS_PWR_CTL			0x21
+#define QIXIS_PWR_CTL_POWEROFF		0x80
 #define QIXIS_RST_CTL_RESET		0x44
 #define QIXIS_RCFG_CTL_RECONFIG_IDLE	0x20
 #define QIXIS_RCFG_CTL_RECONFIG_START	0x21
@@ -10,6 +10,7 @@
 #define CONFIG_LS102XA
  
 #define CONFIG_ARMV7_PSCI
+#define CONFIG_ARMV7_PSCI_1_0
 #define CONFIG_ARMV7_PSCI_NR_CPUS	CONFIG_MAX_CPUS
  
 #define CONFIG_SYS_FSL_CLK
...	...	@@ -12,19 +12,72 @@
12	12	#include <asm/arch-armv7/generictimer.h>
13	13	#include <asm/psci.h>
14	14
	15	+#define RCPM_TWAITSR 0x04C
	16	+
15	17	#define SCFG_CORE0_SFT_RST 0x130
16	18	#define SCFG_CORESRENCR 0x204
17	19
18		-#define DCFG_CCSR_BRR 0x0E4
19		-#define DCFG_CCSR_SCRATCHRW1 0x200
	20	+#define DCFG_CCSR_RSTCR 0x0B0
	21	+#define DCFG_CCSR_RSTCR_RESET_REQ 0x2
	22	+#define DCFG_CCSR_BRR 0x0E4
	23	+#define DCFG_CCSR_SCRATCHRW1 0x200
20	24
	25	+#define PSCI_FN_PSCI_VERSION_FEATURE_MASK 0x0
	26	+#define PSCI_FN_CPU_SUSPEND_FEATURE_MASK 0x0
	27	+#define PSCI_FN_CPU_OFF_FEATURE_MASK 0x0
	28	+#define PSCI_FN_CPU_ON_FEATURE_MASK 0x0
	29	+#define PSCI_FN_AFFINITY_INFO_FEATURE_MASK 0x0
	30	+#define PSCI_FN_SYSTEM_OFF_FEATURE_MASK 0x0
	31	+#define PSCI_FN_SYSTEM_RESET_FEATURE_MASK 0x0
	32	+
21	33	.pushsection ._secure.text, "ax"
22	34
23	35	.arch_extension sec
24	36
	37	+ .align 5
	38	+
25	39	#define ONE_MS (GENERIC_TIMER_CLK / 1000)
26	40	#define RESET_WAIT (30 * ONE_MS)
27	41
	42	+.globl psci_version
	43	+psci_version:
	44	+ movw r0, #0
	45	+ movt r0, #1
	46	+
	47	+ bx lr
	48	+
	49	+_ls102x_psci_supported_table:
	50	+ .word ARM_PSCI_0_2_FN_PSCI_VERSION
	51	+ .word PSCI_FN_PSCI_VERSION_FEATURE_MASK
	52	+ .word ARM_PSCI_0_2_FN_CPU_SUSPEND
	53	+ .word PSCI_FN_CPU_SUSPEND_FEATURE_MASK
	54	+ .word ARM_PSCI_0_2_FN_CPU_OFF
	55	+ .word PSCI_FN_CPU_OFF_FEATURE_MASK
	56	+ .word ARM_PSCI_0_2_FN_CPU_ON
	57	+ .word PSCI_FN_CPU_ON_FEATURE_MASK
	58	+ .word ARM_PSCI_0_2_FN_AFFINITY_INFO
	59	+ .word PSCI_FN_AFFINITY_INFO_FEATURE_MASK
	60	+ .word ARM_PSCI_0_2_FN_SYSTEM_OFF
	61	+ .word PSCI_FN_SYSTEM_OFF_FEATURE_MASK
	62	+ .word ARM_PSCI_0_2_FN_SYSTEM_RESET
	63	+ .word PSCI_FN_SYSTEM_RESET_FEATURE_MASK
	64	+ .word 0
	65	+ .word ARM_PSCI_RET_NI
	66	+
	67	+.globl psci_features
	68	+psci_features:
	69	+ adr r2, _ls102x_psci_supported_table
	70	+1: ldr r3, [r2]
	71	+ cmp r3, #0
	72	+ beq out_psci_features
	73	+ cmp r1, r3
	74	+ addne r2, r2, #8
	75	+ bne 1b
	76	+
	77	+out_psci_features:
	78	+ ldr r0, [r2, #4]
	79	+ bx lr
	80	+
28	81	@ r0: return value ARM_PSCI_RET_SUCCESS or ARM_PSCI_RET_INVAL
29	82	@ r1: input target CPU ID in MPIDR format, original value in r1 may be dropped
30	83	@ r4: output validated CPU ID if ARM_PSCI_RET_SUCCESS returns, meaningless for
...	...	@@ -140,6 +193,52 @@
140	193	.globl psci_cpu_off
141	194	psci_cpu_off:
142	195	bl psci_cpu_off_common
	196	+
	197	+1: wfi
	198	+ b 1b
	199	+
	200	+.globl psci_affinity_info
	201	+psci_affinity_info:
	202	+ push {lr}
	203	+
	204	+ mov r0, #ARM_PSCI_RET_INVAL
	205	+
	206	+ @ Verify Affinity level
	207	+ cmp r2, #0
	208	+ bne out_affinity_info
	209	+
	210	+ bl psci_check_target_cpu_id
	211	+ cmp r0, #ARM_PSCI_RET_INVAL
	212	+ beq out_affinity_info
	213	+ mov r1, r4
	214	+
	215	+ @ Get RCPM base address
	216	+ movw r4, #(CONFIG_SYS_FSL_RCPM_ADDR & 0xffff)
	217	+ movt r4, #(CONFIG_SYS_FSL_RCPM_ADDR >> 16)
	218	+
	219	+ mov r0, #PSCI_AFFINITY_LEVEL_ON
	220	+
	221	+ @ Detect target CPU state
	222	+ ldr r2, [r4, #RCPM_TWAITSR]
	223	+ rev r2, r2
	224	+ lsr r2, r2, r1
	225	+ ands r2, r2, #1
	226	+ beq out_affinity_info
	227	+
	228	+ mov r0, #PSCI_AFFINITY_LEVEL_OFF
	229	+
	230	+out_affinity_info:
	231	+ pop {pc}
	232	+
	233	+.globl psci_system_reset
	234	+psci_system_reset:
	235	+ @ Get DCFG base address
	236	+ movw r1, #(CONFIG_SYS_FSL_GUTS_ADDR & 0xffff)
	237	+ movt r1, #(CONFIG_SYS_FSL_GUTS_ADDR >> 16)
	238	+
	239	+ mov r2, #DCFG_CCSR_RSTCR_RESET_REQ
	240	+ rev r2, r2
	241	+ str r2, [r1, #DCFG_CCSR_RSTCR]
143	242
144	243	1: wfi
145	244	b 1b
...	...	@@ -32,6 +32,7 @@
32	32	#define CONFIG_SYS_FSL_SERDES_ADDR (CONFIG_SYS_IMMR + 0x00ea0000)
33	33	#define CONFIG_SYS_FSL_GUTS_ADDR (CONFIG_SYS_IMMR + 0x00ee0000)
34	34	#define CONFIG_SYS_FSL_LS1_CLK_ADDR (CONFIG_SYS_IMMR + 0x00ee1000)
	35	+#define CONFIG_SYS_FSL_RCPM_ADDR (CONFIG_SYS_IMMR + 0x00ee2000)
35	36	#define CONFIG_SYS_NS16550_COM1 (CONFIG_SYS_IMMR + 0x011c0500)
36	37	#define CONFIG_SYS_NS16550_COM2 (CONFIG_SYS_IMMR + 0x011d0500)
37	38	#define CONFIG_SYS_DCU_ADDR (CONFIG_SYS_IMMR + 0x01ce0000)
...	...	@@ -8,4 +8,5 @@
8	8	obj-y += ddr.o
9	9	obj-y += eth.o
10	10	obj-$(CONFIG_FSL_DCU_FB) += dcu.o
	11	+obj-$(CONFIG_ARMV7_PSCI) += psci.o
	1	+/*
	2	+ * Copyright 2016 NXP Semiconductor.
	3	+ * Author: Wang Dongsheng <dongsheng.wang@freescale.com>
	4	+ *
	5	+ * SPDX-License-Identifier: GPL-2.0+
	6	+ */
	7	+
	8	+#include <config.h>
	9	+#include <linux/linkage.h>
	10	+
	11	+#include <asm/armv7.h>
	12	+#include <asm/psci.h>
	13	+
	14	+ .pushsection ._secure.text, "ax"
	15	+
	16	+ .arch_extension sec
	17	+
	18	+ .align 5
	19	+
	20	+.globl psci_system_off
	21	+psci_system_off:
	22	+ @ Get QIXIS base address
	23	+ movw r1, #(QIXIS_BASE & 0xffff)
	24	+ movt r1, #(QIXIS_BASE >> 16)
	25	+
	26	+ ldrb r2, [r1, #QIXIS_PWR_CTL]
	27	+ orr r2, r2, #QIXIS_PWR_CTL_POWEROFF
	28	+ strb r2, [r1, #QIXIS_PWR_CTL]
	29	+
	30	+1: wfi
	31	+ b 1b
	32	+
	33	+ .popsection
...	...	@@ -6,4 +6,5 @@
6	6
7	7	obj-y += ls1021atwr.o
8	8	obj-$(CONFIG_FSL_DCU_FB) += dcu.o
	9	+obj-$(CONFIG_ARMV7_PSCI) += psci.o
...	...	@@ -10,6 +10,7 @@
10	10	#define CONFIG_LS102XA
11	11
12	12	#define CONFIG_ARMV7_PSCI
	13	+#define CONFIG_ARMV7_PSCI_1_0
13	14	#define CONFIG_ARMV7_PSCI_NR_CPUS CONFIG_MAX_CPUS
14	15
15	16	#define CONFIG_SYS_FSL_CLK
...	...	@@ -280,6 +281,8 @@
280	281	#define QIXIS_LBMAP_SHIFT 0
281	282	#define QIXIS_LBMAP_DFLTBANK 0x00
282	283	#define QIXIS_LBMAP_ALTBANK 0x04
	284	+#define QIXIS_PWR_CTL 0x21
	285	+#define QIXIS_PWR_CTL_POWEROFF 0x80
283	286	#define QIXIS_RST_CTL_RESET 0x44
284	287	#define QIXIS_RCFG_CTL_RECONFIG_IDLE 0x20
285	288	#define QIXIS_RCFG_CTL_RECONFIG_START 0x21