1// SPDX-License-Identifier: GPL-2.0+
2/*
3 * (C) Copyright 2014 - 2015 Xilinx, Inc.
4 * Michal Simek <michal.simek@amd.com>
5 */
6
7#include <common.h>
8#include <cpu_func.h>
9#include <log.h>
10#include <zynqmp_firmware.h>
11#include <asm/arch/hardware.h>
12#include <asm/arch/sys_proto.h>
13#include <asm/io.h>
14#include <linux/delay.h>
15
16#define LOCK		0
17#define SPLIT		1
18
19#define HALT		0
20#define RELEASE		1
21
22#define ZYNQMP_BOOTADDR_HIGH_MASK		0xFFFFFFFF
23#define ZYNQMP_R5_HIVEC_ADDR			0xFFFF0000
24#define ZYNQMP_R5_LOVEC_ADDR			0x0
25#define ZYNQMP_RPU_CFG_CPU_HALT_MASK		0x01
26#define ZYNQMP_RPU_CFG_HIVEC_MASK		0x04
27#define ZYNQMP_RPU_GLBL_CTRL_SPLIT_LOCK_MASK	0x08
28#define ZYNQMP_RPU_GLBL_CTRL_TCM_COMB_MASK	0x40
29#define ZYNQMP_RPU_GLBL_CTRL_SLCLAMP_MASK	0x10
30
31#define ZYNQMP_CRLAPB_RST_LPD_AMBA_RST_MASK	0x04
32#define ZYNQMP_CRLAPB_RST_LPD_R50_RST_MASK	0x01
33#define ZYNQMP_CRLAPB_RST_LPD_R51_RST_MASK	0x02
34#define ZYNQMP_CRLAPB_CPU_R5_CTRL_CLKACT_MASK	0x1000000
35
36#define ZYNQMP_R5_0_TCM_START_ADDR		0xFFE00000
37#define ZYNQMP_R5_1_TCM_START_ADDR		0xFFE90000
38#define ZYNQMP_TCM_BOTH_SIZE			0x40000
39
40#define ZYNQMP_CORE_APU0	0
41#define ZYNQMP_CORE_APU3	3
42#define ZYNQMP_CORE_RPU0	4
43#define ZYNQMP_CORE_RPU1	5
44
45#define ZYNQMP_MAX_CORES	6
46
47#define ZYNQMP_RPU0_USE_MASK BIT(1)
48#define ZYNQMP_RPU1_USE_MASK BIT(2)
49
50int is_core_valid(unsigned int core)
51{
52	if (core < ZYNQMP_MAX_CORES)
53		return 1;
54
55	return 0;
56}
57
58int cpu_reset(u32 nr)
59{
60	puts("Feature is not implemented.\n");
61	return 0;
62}
63
64static void set_r5_halt_mode(u32 nr, u8 halt, u8 mode)
65{
66	u32 tmp;
67
68	if (mode == LOCK || nr == ZYNQMP_CORE_RPU0) {
69		tmp = readl(&rpu_base->rpu0_cfg);
70		if (halt == HALT)
71			tmp &= ~ZYNQMP_RPU_CFG_CPU_HALT_MASK;
72		else
73			tmp |= ZYNQMP_RPU_CFG_CPU_HALT_MASK;
74		writel(tmp, &rpu_base->rpu0_cfg);
75	}
76
77	if (mode == LOCK || nr == ZYNQMP_CORE_RPU1) {
78		tmp = readl(&rpu_base->rpu1_cfg);
79		if (halt == HALT)
80			tmp &= ~ZYNQMP_RPU_CFG_CPU_HALT_MASK;
81		else
82			tmp |= ZYNQMP_RPU_CFG_CPU_HALT_MASK;
83		writel(tmp, &rpu_base->rpu1_cfg);
84	}
85}
86
87static void set_r5_tcm_mode(u8 mode)
88{
89	u32 tmp;
90
91	tmp = readl(&rpu_base->rpu_glbl_ctrl);
92	if (mode == LOCK) {
93		tmp &= ~ZYNQMP_RPU_GLBL_CTRL_SPLIT_LOCK_MASK;
94		tmp |= ZYNQMP_RPU_GLBL_CTRL_TCM_COMB_MASK |
95		       ZYNQMP_RPU_GLBL_CTRL_SLCLAMP_MASK;
96	} else {
97		tmp |= ZYNQMP_RPU_GLBL_CTRL_SPLIT_LOCK_MASK;
98		tmp &= ~(ZYNQMP_RPU_GLBL_CTRL_TCM_COMB_MASK |
99		       ZYNQMP_RPU_GLBL_CTRL_SLCLAMP_MASK);
100	}
101
102	writel(tmp, &rpu_base->rpu_glbl_ctrl);
103}
104
105static void set_r5_reset(u32 nr, u8 mode)
106{
107	u32 tmp;
108
109	tmp = readl(&crlapb_base->rst_lpd_top);
110	if (mode == LOCK) {
111		tmp |= (ZYNQMP_CRLAPB_RST_LPD_AMBA_RST_MASK |
112			ZYNQMP_CRLAPB_RST_LPD_R50_RST_MASK |
113			ZYNQMP_CRLAPB_RST_LPD_R51_RST_MASK);
114	} else {
115		if (nr == ZYNQMP_CORE_RPU0) {
116			tmp |= ZYNQMP_CRLAPB_RST_LPD_R50_RST_MASK;
117			if (tmp & ZYNQMP_CRLAPB_RST_LPD_R51_RST_MASK)
118				tmp |= ZYNQMP_CRLAPB_RST_LPD_AMBA_RST_MASK;
119		} else {
120			tmp |= ZYNQMP_CRLAPB_RST_LPD_R51_RST_MASK;
121			if (tmp & ZYNQMP_CRLAPB_RST_LPD_R50_RST_MASK)
122				tmp |= ZYNQMP_CRLAPB_RST_LPD_AMBA_RST_MASK;
123		}
124	}
125
126	writel(tmp, &crlapb_base->rst_lpd_top);
127}
128
129static void release_r5_reset(u32 nr, u8 mode)
130{
131	u32 tmp;
132
133	tmp = readl(&crlapb_base->rst_lpd_top);
134	if (mode == LOCK || nr == ZYNQMP_CORE_RPU0)
135		tmp &= ~(ZYNQMP_CRLAPB_RST_LPD_AMBA_RST_MASK |
136			 ZYNQMP_CRLAPB_RST_LPD_R50_RST_MASK);
137
138	if (mode == LOCK || nr == ZYNQMP_CORE_RPU1)
139		tmp &= ~(ZYNQMP_CRLAPB_RST_LPD_AMBA_RST_MASK |
140			 ZYNQMP_CRLAPB_RST_LPD_R51_RST_MASK);
141
142	writel(tmp, &crlapb_base->rst_lpd_top);
143}
144
145static void enable_clock_r5(void)
146{
147	u32 tmp;
148
149	tmp = readl(&crlapb_base->cpu_r5_ctrl);
150	tmp |= ZYNQMP_CRLAPB_CPU_R5_CTRL_CLKACT_MASK;
151	writel(tmp, &crlapb_base->cpu_r5_ctrl);
152
153	/* Give some delay for clock
154	 * to propagate */
155	udelay(0x500);
156}
157
158static int check_r5_mode(void)
159{
160	u32 tmp;
161
162	tmp = readl(&rpu_base->rpu_glbl_ctrl);
163	if (tmp & ZYNQMP_RPU_GLBL_CTRL_SPLIT_LOCK_MASK)
164		return SPLIT;
165
166	return LOCK;
167}
168
169int cpu_disable(u32 nr)
170{
171	if (nr <= ZYNQMP_CORE_APU3) {
172		u32 val = readl(&crfapb_base->rst_fpd_apu);
173		val |= 1 << nr;
174		writel(val, &crfapb_base->rst_fpd_apu);
175	} else {
176		set_r5_reset(nr, check_r5_mode());
177	}
178
179	return 0;
180}
181
182int cpu_status(u32 nr)
183{
184	if (nr <= ZYNQMP_CORE_APU3) {
185		u32 addr_low = readl(((u8 *)&apu_base->rvbar_addr0_l) + nr * 8);
186		u32 addr_high = readl(((u8 *)&apu_base->rvbar_addr0_h) +
187				      nr * 8);
188		u32 val = readl(&crfapb_base->rst_fpd_apu);
189		val &= 1 << nr;
190		printf("APU CPU%d %s - starting address HI: %x, LOW: %x\n",
191		       nr, val ? "OFF" : "ON" , addr_high, addr_low);
192	} else {
193		u32 val = readl(&crlapb_base->rst_lpd_top);
194		val &= 1 << (nr - 4);
195		printf("RPU CPU%d %s\n", nr - 4, val ? "OFF" : "ON");
196	}
197
198	return 0;
199}
200
201static void set_r5_start(u8 high)
202{
203	u32 tmp;
204
205	tmp = readl(&rpu_base->rpu0_cfg);
206	if (high)
207		tmp |= ZYNQMP_RPU_CFG_HIVEC_MASK;
208	else
209		tmp &= ~ZYNQMP_RPU_CFG_HIVEC_MASK;
210	writel(tmp, &rpu_base->rpu0_cfg);
211
212	tmp = readl(&rpu_base->rpu1_cfg);
213	if (high)
214		tmp |= ZYNQMP_RPU_CFG_HIVEC_MASK;
215	else
216		tmp &= ~ZYNQMP_RPU_CFG_HIVEC_MASK;
217	writel(tmp, &rpu_base->rpu1_cfg);
218}
219
220static void write_tcm_boot_trampoline(u32 nr, u32 boot_addr)
221{
222	if (boot_addr) {
223		u64 tcm_start_addr = ZYNQMP_R5_0_TCM_START_ADDR;
224
225		if (nr == ZYNQMP_CORE_RPU1)
226			tcm_start_addr = ZYNQMP_R5_1_TCM_START_ADDR;
227
228		/*
229		 * Boot trampoline is simple ASM code below.
230		 *
231		 *		b over;
232		 *	label:
233		 *	.word	0
234		 *	over:	ldr	r0, =label
235		 *		ldr	r1, [r0]
236		 *		bx	r1
237		 */
238		debug("Write boot trampoline for %x\n", boot_addr);
239		writel(0xea000000, tcm_start_addr);
240		writel(boot_addr, tcm_start_addr + 0x4);
241		writel(0xe59f0004, tcm_start_addr + 0x8);
242		writel(0xe5901000, tcm_start_addr + 0xc);
243		writel(0xe12fff11, tcm_start_addr + 0x10);
244		writel(0x00000004, tcm_start_addr + 0x14);
245	}
246}
247
248void initialize_tcm(bool mode)
249{
250	if (!mode) {
251		set_r5_tcm_mode(LOCK);
252		set_r5_halt_mode(ZYNQMP_CORE_RPU0, HALT, LOCK);
253		enable_clock_r5();
254		release_r5_reset(ZYNQMP_CORE_RPU0, LOCK);
255	} else {
256		set_r5_tcm_mode(SPLIT);
257		set_r5_halt_mode(ZYNQMP_CORE_RPU0, HALT, SPLIT);
258		set_r5_halt_mode(ZYNQMP_CORE_RPU1, HALT, SPLIT);
259		enable_clock_r5();
260		release_r5_reset(ZYNQMP_CORE_RPU0, SPLIT);
261		release_r5_reset(ZYNQMP_CORE_RPU1, SPLIT);
262	}
263}
264
265static void mark_r5_used(u32 nr, u8 mode)
266{
267	u32 mask = 0;
268
269	if (mode == LOCK) {
270		mask = ZYNQMP_RPU0_USE_MASK | ZYNQMP_RPU1_USE_MASK;
271	} else {
272		switch (nr) {
273		case ZYNQMP_CORE_RPU0:
274			mask = ZYNQMP_RPU0_USE_MASK;
275			break;
276		case ZYNQMP_CORE_RPU1:
277			mask = ZYNQMP_RPU1_USE_MASK;
278			break;
279		default:
280			return;
281		}
282	}
283	zynqmp_mmio_write((ulong)&pmu_base->gen_storage4, mask, mask);
284}
285
286int cpu_release(u32 nr, int argc, char *const argv[])
287{
288	if (nr <= ZYNQMP_CORE_APU3) {
289		u64 boot_addr = simple_strtoull(argv[0], NULL, 16);
290		/* HIGH */
291		writel((u32)(boot_addr >> 32),
292		       ((u8 *)&apu_base->rvbar_addr0_h) + nr * 8);
293		/* LOW */
294		writel((u32)(boot_addr & ZYNQMP_BOOTADDR_HIGH_MASK),
295		       ((u8 *)&apu_base->rvbar_addr0_l) + nr * 8);
296
297		u32 val = readl(&crfapb_base->rst_fpd_apu);
298		val &= ~(1 << nr);
299		writel(val, &crfapb_base->rst_fpd_apu);
300	} else {
301		if (argc != 2) {
302			printf("Invalid number of arguments to release.\n");
303			printf("<addr> <mode>-Start addr lockstep or split\n");
304			return 1;
305		}
306
307		u32 boot_addr = hextoul(argv[0], NULL);
308		u32 boot_addr_uniq = 0;
309		if (!(boot_addr == ZYNQMP_R5_LOVEC_ADDR ||
310		      boot_addr == ZYNQMP_R5_HIVEC_ADDR)) {
311			printf("Using TCM jump trampoline for address 0x%x\n",
312			       boot_addr);
313			/* Save boot address for later usage */
314			boot_addr_uniq = boot_addr;
315			/*
316			 * R5 needs to start from LOVEC at TCM
317			 * OCM will be probably occupied by ATF
318			 */
319			boot_addr = ZYNQMP_R5_LOVEC_ADDR;
320		}
321
322		/*
323		 * Since we don't know where the user may have loaded the image
324		 * for an R5 we have to flush all the data cache to ensure
325		 * the R5 sees it.
326		 */
327		flush_dcache_all();
328
329		if (!strncmp(argv[1], "lockstep", 8)) {
330			if (nr != ZYNQMP_CORE_RPU0) {
331				printf("Lockstep mode should run on ZYNQMP_CORE_RPU0\n");
332				return 1;
333			}
334			printf("R5 lockstep mode\n");
335			set_r5_reset(nr, LOCK);
336			set_r5_tcm_mode(LOCK);
337			set_r5_halt_mode(nr, HALT, LOCK);
338			set_r5_start(boot_addr);
339			enable_clock_r5();
340			release_r5_reset(nr, LOCK);
341			dcache_disable();
342			write_tcm_boot_trampoline(nr, boot_addr_uniq);
343			dcache_enable();
344			set_r5_halt_mode(nr, RELEASE, LOCK);
345			mark_r5_used(nr, LOCK);
346		} else if (!strncmp(argv[1], "split", 5)) {
347			printf("R5 split mode\n");
348			set_r5_reset(nr, SPLIT);
349			set_r5_tcm_mode(SPLIT);
350			set_r5_halt_mode(nr, HALT, SPLIT);
351			set_r5_start(boot_addr);
352			enable_clock_r5();
353			release_r5_reset(nr, SPLIT);
354			dcache_disable();
355			write_tcm_boot_trampoline(nr, boot_addr_uniq);
356			dcache_enable();
357			set_r5_halt_mode(nr, RELEASE, SPLIT);
358			mark_r5_used(nr, SPLIT);
359		} else {
360			printf("Unsupported mode\n");
361			return 1;
362		}
363	}
364
365	return 0;
366}
367