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