1/* SPDX-License-Identifier: GPL-2.0+ */
2/*
3 * Copyright 2015 Freescale Semiconductor, Inc.
4 * Author: Wang Dongsheng <dongsheng.wang@freescale.com>
5 */
6
7#include <config.h>
8#include <linux/linkage.h>
9
10#include <asm/armv7.h>
11#include <asm/arch-armv7/generictimer.h>
12#include <asm/psci.h>
13
14#define RCPM_TWAITSR		0x04C
15
16#define SCFG_CORE0_SFT_RST      0x130
17#define SCFG_CORESRENCR         0x204
18
19#define DCFG_CCSR_RSTCR			0x0B0
20#define DCFG_CCSR_RSTCR_RESET_REQ	0x2
21#define DCFG_CCSR_BRR			0x0E4
22#define DCFG_CCSR_SCRATCHRW1		0x200
23
24#define PSCI_FN_PSCI_VERSION_FEATURE_MASK	0x0
25#define PSCI_FN_CPU_SUSPEND_FEATURE_MASK	0x0
26#define PSCI_FN_CPU_OFF_FEATURE_MASK		0x0
27#define PSCI_FN_CPU_ON_FEATURE_MASK		0x0
28#define PSCI_FN_AFFINITY_INFO_FEATURE_MASK	0x0
29#define PSCI_FN_SYSTEM_OFF_FEATURE_MASK		0x0
30#define PSCI_FN_SYSTEM_RESET_FEATURE_MASK	0x0
31#define PSCI_FN_SYSTEM_SUSPEND_FEATURE_MASK	0x0
32
33	.pushsection ._secure.text, "ax"
34
35	.arch_extension sec
36
37	.align	5
38
39#define	ONE_MS		(CONFIG_COUNTER_FREQUENCY / 1000)
40#define	RESET_WAIT	(30 * ONE_MS)
41
42.globl	psci_version
43psci_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	ARM_PSCI_1_0_FN_SYSTEM_SUSPEND
65	.word	PSCI_FN_SYSTEM_SUSPEND_FEATURE_MASK
66	.word	0
67	.word	ARM_PSCI_RET_NI
68
69.globl	psci_features
70psci_features:
71	adr	r2, _ls102x_psci_supported_table
721:	ldr	r3, [r2]
73	cmp	r3, #0
74	beq	out_psci_features
75	cmp	r1, r3
76	addne	r2, r2, #8
77	bne	1b
78
79out_psci_features:
80	ldr	r0, [r2, #4]
81	bx	lr
82
83@ r0: return value ARM_PSCI_RET_SUCCESS or ARM_PSCI_RET_INVAL
84@ r1: input target CPU ID in MPIDR format, original value in r1 may be dropped
85@ r4: output validated CPU ID if ARM_PSCI_RET_SUCCESS returns, meaningless for
86@ ARM_PSCI_RET_INVAL,suppose caller saves r4 before calling
87LENTRY(psci_check_target_cpu_id)
88	@ Get the real CPU number
89	and	r4, r1, #0xff
90	mov	r0, #ARM_PSCI_RET_INVAL
91
92	@ Bit[31:24], bits must be zero.
93	tst	r1, #0xff000000
94	bxne	lr
95
96	@ Affinity level 2 - Cluster: only one cluster in LS1021xa.
97	tst	r1, #0xff0000
98	bxne	lr
99
100	@ Affinity level 1 - Processors: should be in 0xf00 format.
101	lsr	r1, r1, #8
102	teq	r1, #0xf
103	bxne	lr
104
105	@ Affinity level 0 - CPU: only 0, 1 are valid in LS1021xa.
106	cmp	r4, #2
107	bxge	lr
108
109	mov	r0, #ARM_PSCI_RET_SUCCESS
110	bx	lr
111ENDPROC(psci_check_target_cpu_id)
112
113	@ r1 = target CPU
114	@ r2 = target PC
115.globl	psci_cpu_on
116psci_cpu_on:
117	push	{r4, r5, r6, lr}
118
119	@ Clear and Get the correct CPU number
120	@ r1 = 0xf01
121	bl	psci_check_target_cpu_id
122	cmp	r0, #ARM_PSCI_RET_INVAL
123	beq	out_psci_cpu_on
124
125	mov	r0, r4
126	mov	r1, r2
127	mov	r2, r3
128	bl	psci_save
129	mov	r1, r4
130
131	@ Get DCFG base address
132	movw	r4, #(CFG_SYS_FSL_GUTS_ADDR & 0xffff)
133	movt	r4, #(CFG_SYS_FSL_GUTS_ADDR >> 16)
134
135	@ Detect target CPU state
136	ldr	r2, [r4, #DCFG_CCSR_BRR]
137	rev	r2, r2
138	lsr	r2, r2, r1
139	ands	r2, r2, #1
140	beq	holdoff_release
141
142	@ Reset target CPU
143	@ Get SCFG base address
144	movw	r0, #(CFG_SYS_FSL_SCFG_ADDR & 0xffff)
145	movt	r0, #(CFG_SYS_FSL_SCFG_ADDR >> 16)
146
147	@ Enable CORE Soft Reset
148	movw	r5, #0
149	movt	r5, #(1 << 15)
150	rev	r5, r5
151	str	r5, [r0, #SCFG_CORESRENCR]
152
153	@ Get CPUx offset register
154	mov	r6, #0x4
155	mul	r6, r6, r1
156	add	r2, r0, r6
157
158	@ Do reset on target CPU
159	movw	r5, #0
160	movt	r5, #(1 << 15)
161	rev	r5, r5
162	str	r5, [r2, #SCFG_CORE0_SFT_RST]
163
164	@ Wait target CPU up
165	timer_wait	r2, RESET_WAIT
166
167	@ Disable CORE soft reset
168	mov	r5, #0
169	str	r5, [r0, #SCFG_CORESRENCR]
170
171holdoff_release:
172	@ Release on target CPU
173	ldr	r2, [r4, #DCFG_CCSR_BRR]
174	mov	r6, #1
175	lsl	r6, r6, r1	@ 32 bytes per CPU
176
177	rev	r6, r6
178	orr	r2, r2, r6
179	str	r2, [r4, #DCFG_CCSR_BRR]
180
181	@ Set secondary boot entry
182	ldr	r6, =psci_cpu_entry
183	rev	r6, r6
184	str	r6, [r4, #DCFG_CCSR_SCRATCHRW1]
185
186	isb
187	dsb
188
189	@ Return
190	mov	r0, #ARM_PSCI_RET_SUCCESS
191
192out_psci_cpu_on:
193	pop	{r4, r5, r6, lr}
194	bx	lr
195
196.globl	psci_cpu_off
197psci_cpu_off:
198	bl	psci_cpu_off_common
199
2001:	wfi
201	b	1b
202
203.globl	psci_affinity_info
204psci_affinity_info:
205	push	{lr}
206
207	mov	r0, #ARM_PSCI_RET_INVAL
208
209	@ Verify Affinity level
210	cmp	r2, #0
211	bne	out_affinity_info
212
213	bl	psci_check_target_cpu_id
214	cmp	r0, #ARM_PSCI_RET_INVAL
215	beq	out_affinity_info
216	mov	r1, r4
217
218	@ Get RCPM base address
219	movw	r4, #(CFG_SYS_FSL_RCPM_ADDR & 0xffff)
220	movt	r4, #(CFG_SYS_FSL_RCPM_ADDR >> 16)
221
222	mov	r0, #PSCI_AFFINITY_LEVEL_ON
223
224	@ Detect target CPU state
225	ldr	r2, [r4, #RCPM_TWAITSR]
226	rev	r2, r2
227	lsr	r2, r2, r1
228	ands	r2, r2, #1
229	beq	out_affinity_info
230
231	mov	r0, #PSCI_AFFINITY_LEVEL_OFF
232
233out_affinity_info:
234	pop	{pc}
235
236.globl	psci_system_reset
237psci_system_reset:
238	@ Get DCFG base address
239	movw	r1, #(CFG_SYS_FSL_GUTS_ADDR & 0xffff)
240	movt	r1, #(CFG_SYS_FSL_GUTS_ADDR >> 16)
241
242	mov	r2, #DCFG_CCSR_RSTCR_RESET_REQ
243	rev	r2, r2
244	str	r2, [r1, #DCFG_CCSR_RSTCR]
245
2461:	wfi
247	b	1b
248
249.globl	psci_system_suspend
250psci_system_suspend:
251	push	{lr}
252
253	bl	ls1_system_suspend
254
255	pop	{pc}
256
257	.popsection
258