summaryrefslogtreecommitdiff
path: root/arch/arm/cpu/armv8/fsl-layerscape/spintable.S
blob: a92f930e04b4ca09f5e5c0b5545196d5b0878633 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
/* SPDX-License-Identifier: GPL-2.0+ */
/*
 * (C) Copyright 2014-2015 Freescale Semiconductor
 * Copyright 2019 NXP
 */

#include <config.h>
#include <linux/linkage.h>
#include <asm/macro.h>
#include <asm/system.h>
#include <asm/arch/mp.h>

	.align 3
	.global secondary_boot_addr
secondary_boot_addr:
	.quad secondary_boot_func

	/* Keep literals not used by the secondary boot code outside it */
	.ltorg

	/* Using 64 bit alignment since the spin table is accessed as data */
	.align 3
	.global secondary_boot_code
	/* Secondary Boot Code starts here */
secondary_boot_code:
	.global __spin_table
__spin_table:
	.space CONFIG_MAX_CPUS*SPIN_TABLE_ELEM_SIZE

	.align 2
ENTRY(secondary_boot_func)
	/*
	 * MPIDR_EL1 Fields:
	 * MPIDR[1:0] = AFF0_CPUID <- Core ID (0,1)
	 * MPIDR[7:2] = AFF0_RES
	 * MPIDR[15:8] = AFF1_CLUSTERID <- Cluster ID (0,1,2,3)
	 * MPIDR[23:16] = AFF2_CLUSTERID
	 * MPIDR[24] = MT
	 * MPIDR[29:25] = RES0
	 * MPIDR[30] = U
	 * MPIDR[31] = ME
	 * MPIDR[39:32] = AFF3
	 *
	 * Linear Processor ID (LPID) calculation from MPIDR_EL1:
	 * (We only use AFF0_CPUID and AFF1_CLUSTERID for now
	 * until AFF2_CLUSTERID and AFF3 have non-zero values)
	 *
	 * LPID = MPIDR[15:8] | MPIDR[1:0]
	 */
	mrs	x0, mpidr_el1
	ubfm	x1, x0, #8, #15
	ubfm	x2, x0, #0, #1
	orr	x10, x2, x1, lsl #2	/* x10 has LPID */
	ubfm    x9, x0, #0, #15         /* x9 contains MPIDR[15:0] */
	/*
	 * offset of the spin table element for this core from start of spin
	 * table (each elem is padded to 64 bytes)
	 */
	lsl	x1, x10, #6
	adr	x0, __spin_table
	/* physical address of this cpus spin table element */
	add	x11, x1, x0

	adr	x0, __real_cntfrq
	ldr	x0, [x0]
	msr	cntfrq_el0, x0	/* set with real frequency */
	str	x9, [x11, #16]	/* LPID */
	mov	x4, #1
	str	x4, [x11, #8]	/* STATUS */
	dsb	sy

slave_cpu:
	wfe
	ldr	x0, [x11]
	cbz	x0, slave_cpu
#ifndef CONFIG_ARMV8_SWITCH_TO_EL1
	mrs     x1, sctlr_el2
#else
	mrs     x1, sctlr_el1
#endif
	tbz     x1, #25, cpu_is_le
	rev     x0, x0                  /* BE to LE conversion */
cpu_is_le:
	ldr	x5, [x11, #24]
	cbz	x5, 1f

#ifdef CONFIG_ARMV8_SWITCH_TO_EL1
	adr	x4, secondary_switch_to_el1
	ldr	x5, =ES_TO_AARCH64
#else
	ldr	x4, [x11]
	ldr	x5, =ES_TO_AARCH32
#endif
	bl	secondary_switch_to_el2

1:
#ifdef CONFIG_ARMV8_SWITCH_TO_EL1
	adr	x4, secondary_switch_to_el1
#else
	ldr	x4, [x11]
#endif
	ldr	x5, =ES_TO_AARCH64
	bl	secondary_switch_to_el2

ENDPROC(secondary_boot_func)

ENTRY(secondary_switch_to_el2)
	switch_el x6, 1f, 0f, 0f
0:	ret
1:	armv8_switch_to_el2_m x4, x5, x6
ENDPROC(secondary_switch_to_el2)

ENTRY(secondary_switch_to_el1)
	mrs	x0, mpidr_el1
	ubfm	x1, x0, #8, #15
	ubfm	x2, x0, #0, #1
	orr	x10, x2, x1, lsl #2	/* x10 has LPID */

	lsl	x1, x10, #6
	adr	x0, __spin_table
	/* physical address of this cpus spin table element */
	add	x11, x1, x0

	ldr	x4, [x11]

	ldr	x5, [x11, #24]
	cbz	x5, 2f

	ldr	x5, =ES_TO_AARCH32
	bl	switch_to_el1

2:	ldr	x5, =ES_TO_AARCH64

switch_to_el1:
	switch_el x6, 0f, 1f, 0f
0:	ret
1:	armv8_switch_to_el1_m x4, x5, x6
ENDPROC(secondary_switch_to_el1)

	/* Ensure that the literals used by the secondary boot code are
	 * assembled within it (this is required so that we can protect
	 * this area with a single memreserve region
	 */
	.ltorg

	/* 64 bit alignment for elements accessed as data */
	.align 3
	.global __real_cntfrq
__real_cntfrq:
	.quad COUNTER_FREQUENCY
	.globl __secondary_boot_code_size
	.type __secondary_boot_code_size, %object
	/* Secondary Boot Code ends here */
__secondary_boot_code_size:
	.quad .-secondary_boot_code