1/* SPDX-License-Identifier: GPL-2.0+ */
2/*
3 * (C) Copyright 2014-2015 Freescale Semiconductor
4 * Copyright 2019 NXP
5 */
6
7#include <config.h>
8#include <linux/linkage.h>
9#include <asm/macro.h>
10#include <asm/system.h>
11#include <asm/arch/mp.h>
12
13.align 3
14.global secondary_boot_addr
15secondary_boot_addr:
16	.quad __secondary_boot_func
17
18.global secondary_boot_code_start
19secondary_boot_code_start:
20	.quad __secondary_boot_code_start
21
22.global secondary_boot_code_size
23secondary_boot_code_size:
24	.quad __secondary_boot_code_end - __secondary_boot_code_start
25
26	/* Using 64 bit alignment since the spin table is accessed as data */
27	.align 3
28	/* Secondary Boot Code starts here */
29__secondary_boot_code_start:
30__spin_table:
31	.space CONFIG_MAX_CPUS*SPIN_TABLE_ELEM_SIZE
32
33	.align 2
34__secondary_boot_func:
35	/*
36	 * MPIDR_EL1 Fields:
37	 * MPIDR[1:0] = AFF0_CPUID <- Core ID (0,1)
38	 * MPIDR[7:2] = AFF0_RES
39	 * MPIDR[15:8] = AFF1_CLUSTERID <- Cluster ID (0,1,2,3)
40	 * MPIDR[23:16] = AFF2_CLUSTERID
41	 * MPIDR[24] = MT
42	 * MPIDR[29:25] = RES0
43	 * MPIDR[30] = U
44	 * MPIDR[31] = ME
45	 * MPIDR[39:32] = AFF3
46	 *
47	 * Linear Processor ID (LPID) calculation from MPIDR_EL1:
48	 * (We only use AFF0_CPUID and AFF1_CLUSTERID for now
49	 * until AFF2_CLUSTERID and AFF3 have non-zero values)
50	 *
51	 * LPID = MPIDR[15:8] | MPIDR[1:0]
52	 */
53	mrs	x0, mpidr_el1
54	ubfm	x1, x0, #8, #15
55	ubfm	x2, x0, #0, #1
56	orr	x10, x2, x1, lsl #2	/* x10 has LPID */
57	ubfm    x9, x0, #0, #15         /* x9 contains MPIDR[15:0] */
58	/*
59	 * offset of the spin table element for this core from start of spin
60	 * table (each elem is padded to 64 bytes)
61	 */
62	lsl	x1, x10, #6
63	adr	x0, __spin_table
64	/* physical address of this cpus spin table element */
65	add	x11, x1, x0
66
67	adr	x0, __real_cntfrq
68	ldr	x0, [x0]
69	msr	cntfrq_el0, x0	/* set with real frequency */
70	str	x9, [x11, #16]	/* LPID */
71	mov	x4, #1
72	str	x4, [x11, #8]	/* STATUS */
73	dsb	sy
74
751:
76	wfe
77	ldr	x4, [x11]
78	cbz	x4, 1b
79	mrs     x1, sctlr_el2
80	tbz     x1, #25, 2f
81	rev     x4, x4                  /* BE to LE conversion */
822:
83	ldr	x6, =ES_TO_AARCH64
84#ifdef CONFIG_ARMV8_SWITCH_TO_EL1
85	adr	x5, 3f
86	switch_el x7, 0f, _dead_loop, _dead_loop
870:	armv8_switch_to_el2_m x5, x6, x7
88#endif
893:
90	ldr	x7, [x11, #24]	/* ARCH_COMP */
91	cbz	x7, 4f
92	ldr	x6, =ES_TO_AARCH32
934:
94#ifdef CONFIG_ARMV8_SWITCH_TO_EL1
95	switch_el x7, _dead_loop, 0f, _dead_loop
960:	armv8_switch_to_el1_m x4, x6, x7
97#else
98	switch_el x7, 0f, _dead_loop, _dead_loop
990:	armv8_switch_to_el2_m x4, x6, x7
100#endif
101
102_dead_loop:
103	wfe
104	b _dead_loop
105
106	/* Ensure that the literals used by the secondary boot code are
107	 * assembled within it (this is required so that we can protect
108	 * this area with a single memreserve region
109	 */
110	.ltorg
111
112	/* 64 bit alignment for elements accessed as data */
113	.align 3
114	.global __real_cntfrq
115__real_cntfrq:
116	.quad COUNTER_FREQUENCY
117	/* Secondary Boot Code ends here */
118__secondary_boot_code_end:
119