1/* SPDX-License-Identifier: GPL-2.0 */
2/*
3 * Copyright (C) 2018 MediaTek Inc.
4 */
5
6#include <linux/linkage.h>
7#include <asm/proc-armv/ptrace.h>
8
9#define WAIT_CODE_SRAM_BASE	0x0010ff00
10
11#define SLAVE_JUMP_REG		0x10202034
12#define SLAVE1_MAGIC_REG	0x10202038
13#define SLAVE1_MAGIC_NUM	0x534c4131
14
15#define GIC_CPU_BASE		0x10320000
16
17ENTRY(lowlevel_init)
18
19#ifndef CONFIG_SPL_BUILD
20	/* Return to U-Boot via saved link register */
21	mov	pc, lr
22#else
23	/*
24	 * Arch timer :
25	 * set CNTFRQ = 20Mhz, set CNTVOFF = 0
26	 */
27	movw	r0, #0x2d00
28	movt	r0, #0x131
29	mcr	p15, 0, r0, c14, c0, 0
30
31	cps	#MON_MODE
32	mrc	p15, 0, r1, c1, c1, 0	@ Get Secure Config
33	orr	r0, r1, #1
34	mcr	p15, 0, r0, c1, c1, 0	@ Set Non Secure bit
35	isb
36	mov	r0, #0
37	mcrr	p15, 4, r0, r0, c14	@ CNTVOFF = 0
38	isb
39	mcr	p15, 0, r1, c1, c1, 0	@ Set Secure bit
40	isb
41	cps	#SVC_MODE
42
43	/* enable SMP bit */
44	mrc	p15, 0, r0, c1, c0, 1
45	orr	r0, r0, #0x40
46	mcr	p15, 0, r0, c1, c0, 1
47
48	/* if MP core, handle secondary cores */
49	mrc	p15, 0, r0, c0, c0, 5
50	ands	r1, r0, #0x40000000
51	bne	go			@ Go if UP
52	/* read slave CPU number */
53	ands	r0, r0, #0x0f
54	beq	go			@ Go if core0 on primary core tile
55	b	secondary
56
57go:
58	/* master CPU */
59	mov	pc, lr
60
61secondary:
62	/* enable GIC as cores will be waken up by IPI */
63	ldr	r2, =GIC_CPU_BASE
64	mov	r1, #0xf0
65	str	r1, [r2, #4]
66	mov	r1, #1
67	str	r1, [r2, #0]
68
69	ldr	r1, [r2]
70	orr	r1, #1
71	str	r1, [r2]
72
73	/* copy wait code into SRAM */
74	ldr	r0, =slave_cpu_wait
75	ldm	r0, {r1 - r8}		@ slave_cpu_wait has eight insns
76	ldr	r0, =WAIT_CODE_SRAM_BASE
77	stm	r0, {r1 - r8}
78
79	/* pass args to slave_cpu_wait */
80	ldr	r0, =SLAVE1_MAGIC_REG
81	ldr	r1, =SLAVE1_MAGIC_NUM
82
83	/* jump to wait code in SRAM */
84	ldr	pc, =WAIT_CODE_SRAM_BASE
85
86#endif
87ENDPROC(lowlevel_init)
88
89/* This function will be copied into SRAM */
90ENTRY(slave_cpu_wait)
91	wfi
92	ldr	r2, [r0]
93	cmp	r2, r1
94	bne	slave_cpu_wait
95	movw	r0, #:lower16:SLAVE_JUMP_REG
96	movt	r0, #:upper16:SLAVE_JUMP_REG
97	ldr	r1, [r0]
98	mov	pc, r1
99ENDPROC(slave_cpu_wait)
100