1
2
3
4
5
6
7
8#include <config.h>
9#include <linux/linkage.h>
10#include <asm/gic.h>
11#include <asm/armv7.h>
12#include <asm/proc-armv/ptrace.h>
13
14.arch_extension sec
15.arch_extension virt
16
17 .pushsection ._secure.text, "ax"
18
19 .align 5
20
21_monitor_vectors:
22 .word 0
23 .word 0
24 adr pc, _secure_monitor
25 .word 0
26 .word 0
27 .word 0
28 .word 0
29 .word 0
30
31.macro is_cpu_virt_capable tmp
32 mrc p15, 0, \tmp, c0, c1, 1 @ read ID_PFR1
33 and \tmp, \tmp,
34 cmp \tmp,
35.endm
36
37
38
39
40
41
42
43
44
45_secure_monitor:
46#ifdef CONFIG_ARMV7_PSCI
47 ldr r5, =_psci_vectors @ Switch to the next monitor
48 mcr p15, 0, r5, c12, c0, 1
49 isb
50
51 @ Obtain a secure stack
52 bl psci_stack_setup
53
54 @ Configure the PSCI backend
55 push {r0, r1, r2, ip}
56 bl psci_arch_init
57 pop {r0, r1, r2, ip}
58#endif
59
60#ifdef CONFIG_ARM_ERRATA_773022
61 mrc p15, 0, r5, c1, c0, 1
62 orr r5, r5,
63 mcr p15, 0, r5, c1, c0, 1
64 isb
65#endif
66
67#ifdef CONFIG_ARM_ERRATA_774769
68 mrc p15, 0, r5, c1, c0, 1
69 orr r5, r5,
70 mcr p15, 0, r5, c1, c0, 1
71 isb
72#endif
73
74 mrc p15, 0, r5, c1, c1, 0 @ read SCR
75 bic r5, r5,
76 orr r5, r5,
77 @ FIQ preserved for secure mode
78 mov r6,
79 is_cpu_virt_capable r4
80#ifdef CONFIG_ARMV7_VIRT
81 orreq r5, r5,
82 moveq r6,
83 mrseq r3, sp_svc
84 msreq sp_hyp, r3 @ migrate SP
85#endif
86
87 mcr p15, 0, r5, c1, c1, 0 @ write SCR (with NS bit set)
88 isb
89
90 bne 1f
91
92 @ Reset CNTVOFF to 0 before leaving monitor mode
93 mrc p15, 0, r4, c0, c1, 1 @ read ID_PFR1
94 ands r4, r4,
95 movne r4,
96 mcrrne p15, 4, r4, r4, c14 @ Reset CNTVOFF to zero
971:
98 mov lr, ip
99 mov ip,
100 tst lr,
101 orrne ip, ip,
102 orr ip, ip, r6 @ Slot target mode in
103 msr spsr_cxfs, ip @ Set full SPSR
104 movs pc, lr @ ERET to non-secure
105
106ENTRY(_do_nonsec_entry)
107 mov ip, r0
108 mov r0, r1
109 mov r1, r2
110 mov r2, r3
111 smc
112ENDPROC(_do_nonsec_entry)
113
114.macro get_cbar_addr addr
115#ifdef CONFIG_ARM_GIC_BASE_ADDRESS
116 ldr \addr, =CONFIG_ARM_GIC_BASE_ADDRESS
117#else
118 mrc p15, 4, \addr, c15, c0, 0 @ read CBAR
119 bfc \addr,
120#endif
121.endm
122
123.macro get_gicd_addr addr
124 get_cbar_addr \addr
125 add \addr, \addr,
126.endm
127
128.macro get_gicc_addr addr, tmp
129 get_cbar_addr \addr
130 is_cpu_virt_capable \tmp
131 movne \tmp,
132 moveq \tmp,
133 add \addr, \addr, \tmp
134.endm
135
136#ifndef CONFIG_ARMV7_PSCI
137
138
139
140
141
142
143ENTRY(_smp_pen)
144 cpsid i
145 cpsid f
146
147 bl _nonsec_init
148
149 adr r0, _smp_pen @ do not use this address again
150 b smp_waitloop @ wait for IPIs, board specific
151ENDPROC(_smp_pen)
152#endif
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167ENTRY(_nonsec_init)
168 get_gicd_addr r3
169
170 mvn r1,
171 str r1, [r3,
172
173 get_gicc_addr r3, r1
174
175 mov r1,
176 str r1, [r3,
177 mov r1,
178 str r1, [r3,
179
180 mrc p15, 0, r0, c1, c1, 2
181 movw r1,
182 movt r1,
183 orr r0, r0, r1
184 mcr p15, 0, r0, c1, c1, 2 @ NSACR = all copros to non-sec
185
186
187
188
189
190
191
192#ifdef COUNTER_FREQUENCY
193 mrc p15, 0, r0, c0, c1, 1 @ read ID_PFR1
194 and r0, r0,
195 cmp r0,
196 ldreq r1, =COUNTER_FREQUENCY
197 mcreq p15, 0, r1, c14, c0, 0 @ write CNTFRQ
198#endif
199
200 adr r1, _monitor_vectors
201 mcr p15, 0, r1, c12, c0, 1 @ set MVBAR to secure vectors
202 isb
203
204 mov r0, r3 @ return GICC address
205 bx lr
206ENDPROC(_nonsec_init)
207
208#ifdef CONFIG_SMP_PEN_ADDR
209
210ENTRY(smp_waitloop)
211 wfi
212 ldr r1, =CONFIG_SMP_PEN_ADDR @ load start address
213 ldr r1, [r1]
214#ifdef CONFIG_PEN_ADDR_BIG_ENDIAN
215 rev r1, r1
216#endif
217 cmp r0, r1 @ make sure we dont execute this code
218 beq smp_waitloop @ again (due to a spurious wakeup)
219 mov r0, r1
220 b _do_nonsec_entry
221ENDPROC(smp_waitloop)
222.weak smp_waitloop
223#endif
224
225 .popsection
226