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
156
157
158
159
160
161
|
/*
* linux/arch/arm/mach-exynos4/headsmp.S
*
* Cloned from linux/arch/arm/mach-realview/headsmp.S
*
* Copyright (c) 2003 ARM Limited
* All Rights Reserved
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*/
#include <linux/linkage.h>
#include <linux/init.h>
#include <asm/assembler.h>
__CPUINIT
/*
* exynos4 specific entry point for secondary CPUs. This provides
* a "holding pen" into which all secondary cores are held until we're
* ready for them to initialise.
*/
.arch_extension sec
.arch_extension virt
.align 5
/* We use the same vector table for Hyp and Monitor mode, since
* we will only use each once and they don't overlap.
*/
mon_vectors:
W(b) . /* reset */
W(b) . /* undef */
W(b) 2f /* smc */
W(b) . /* pabt */
W(b) . /* dabt */
W(b) 1f /* hyp */
W(b) . /* irq */
W(b) . /* fiq */
/* Return directly back to the caller without leaving Hyp mode: */
1: mrs lr, elr_hyp
mov pc, lr
/* In monitor mode, set up HVBAR and SCR then return to caller in NS-SVC. */
2:
mrc p15, 0, r1, c1, c1, 0 @ SCR
/*
* Set SCR.NS=1(needed for setting HVBAR and also returning to NS state)
* .IRQ,FIQ,EA=0 (don't take aborts/exceptions to Monitor mode)
* .FW,AW=1 (CPSR.A,F modifiable in NS state)
* .nET=0 (early termination OK)
* .SCD=0 (SMC in NS mode OK, so we can call secure firmware)
* .HCE=1 (HVC does Hyp call)
*/
bic r1, r1, #0x07f
ldr r2, =0x131
orr r1, r1, r2
mcr p15, 0, r2, c1, c1, 0 @ SCR
isb
ldr r2, =mon_vectors
adr r4, 1f
ldmia r4, {r5}
sub r4, r4, r5
add r2, r2, r4
mcr p15, 4, r2, c12, c0, 0 @ set HVBAR
THUMB( mrc p15, 4, r2, c1, c0, 0 ) @ ctrl register
THUMB( orr r2, r2, #1 << 30 ) @ HSCTLR.TE (Thumb exceptions)
THUMB( mcr p15, 4, r2, c1, c0, 0 )
THUMB( isb)
/* ...and return to calling code in NS state */
movs pc, lr
.globl monitor_init
monitor_init:
ldr ip, =mon_vectors
adr r4, 1f
ldmia r4, {r5}
sub r4, r4, r5
add ip, ip, r4
mcr p15, 0, ip, c12, c0, 1
THUMB( mrc p15, 0, r1, c1, c0, 0 ) @ ctrl register
THUMB( orr r1, r1, #1 << 30 ) @ SCTLR.TE (Thumb exceptions)
THUMB( mcr p15, 0, r1, c1, c0, 0 )
THUMB( isb )
mov pc, lr
/* Try to go into NS-SVC: void enter_ns(void); */
.globl enter_ns
enter_ns:
smc #0
mov pc, lr
/* void enter_hyp(void); */
.globl enter_hyp
enter_hyp:
/* Now we're in NS-SVC, make a Hyp call to get into Hyp mode */
mov r0, lr
mov r1, sp
hvc #0
/* We will end up here in NS-Hyp. */
mov sp, r1
mov pc, r0
ENTRY(exynos4_secondary_startup)
/*
* ROM code operates in little endian mode, when we get control we
* need to switch it to big endian mode.
*/
ARM_BE8(setend be)
mrc p15, 0, r0, c0, c0, 5
and r0, r0, #15
adr r4, 1f
ldmia r4, {r5, r6}
sub r4, r4, r5
add r6, r6, r4
pen: ldr r7, [r6]
cmp r7, r0
bne pen
ldr r1, =__boot_cpu_mode
add r1, r1, r4
ldr r2, [r1]
mrs r0, cpsr
ands r0, r0, #MODE_MASK
subs r1, r0, r2
beq 3f
subs r2, r2, #HYP_MODE
bne 3f
/* Setting NSACR to allow coprocessor access from non-secure mode */
mrc p15, 0, r0, c1, c1, 2
movw r1, #0x3fff
orr r0, r0, r1
mcr p15, 0, r0, c1, c1, 2
5:
bl monitor_init
bl enter_ns
bl enter_hyp
3:
/*
* we've been released from the holding pen: secondary_stack
* should now contain the SVC stack for this core
*/
b secondary_startup
ENDPROC(exynos4_secondary_startup)
.align 2
1: .long .
.long pen_release
|
