tsp_entrypoint.S 11.2 KB
Newer Older
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
/*
 * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * Neither the name of ARM nor the names of its contributors may be used
 * to endorse or promote products derived from this software without specific
 * prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <arch.h>
32
#include <asm_macros.S>
33
#include <tsp.h>
34
#include <xlat_tables.h>
35
#include "../tsp_private.h"
36
37
38


	.globl	tsp_entrypoint
39
	.globl  tsp_vector_table
40

41
42


43
44
45
46
47
48
49
50
51
52
53
54
55
	/* ---------------------------------------------
	 * Populate the params in x0-x7 from the pointer
	 * to the smc args structure in x0.
	 * ---------------------------------------------
	 */
	.macro restore_args_call_smc
	ldp	x6, x7, [x0, #TSP_ARG6]
	ldp	x4, x5, [x0, #TSP_ARG4]
	ldp	x2, x3, [x0, #TSP_ARG2]
	ldp	x0, x1, [x0, #TSP_ARG0]
	smc	#0
	.endm

56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
	.macro	save_eret_context reg1 reg2
	mrs	\reg1, elr_el1
	mrs	\reg2, spsr_el1
	stp	\reg1, \reg2, [sp, #-0x10]!
	stp	x30, x18, [sp, #-0x10]!
	.endm

	.macro restore_eret_context reg1 reg2
	ldp	x30, x18, [sp], #0x10
	ldp	\reg1, \reg2, [sp], #0x10
	msr	elr_el1, \reg1
	msr	spsr_el1, \reg2
	.endm

	.section	.text, "ax"
	.align 3
72

73
func tsp_entrypoint
74
75
76
77
78

	/* ---------------------------------------------
	 * Set the exception vector to something sane.
	 * ---------------------------------------------
	 */
79
	adr	x0, tsp_exceptions
80
81
82
	msr	vbar_el1, x0

	/* ---------------------------------------------
83
84
	 * Enable the instruction cache, stack pointer
	 * and data access alignment checks
85
86
	 * ---------------------------------------------
	 */
87
	mov	x1, #(SCTLR_I_BIT | SCTLR_A_BIT | SCTLR_SA_BIT)
88
	mrs	x0, sctlr_el1
89
	orr	x0, x0, x1
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
	msr	sctlr_el1, x0
	isb

	/* ---------------------------------------------
	 * Zero out NOBITS sections. There are 2 of them:
	 *   - the .bss section;
	 *   - the coherent memory section.
	 * ---------------------------------------------
	 */
	ldr	x0, =__BSS_START__
	ldr	x1, =__BSS_SIZE__
	bl	zeromem16

	ldr	x0, =__COHERENT_RAM_START__
	ldr	x1, =__COHERENT_RAM_UNALIGNED_SIZE__
	bl	zeromem16

	/* --------------------------------------------
108
109
110
111
112
	 * Allocate a stack whose memory will be marked
	 * as Normal-IS-WBWA when the MMU is enabled.
	 * There is no risk of reading stale stack
	 * memory after enabling the MMU as only the
	 * primary cpu is running at the moment.
113
114
115
	 * --------------------------------------------
	 */
	mrs	x0, mpidr_el1
116
	bl	platform_set_stack
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

	/* ---------------------------------------------
	 * Perform early platform setup & platform
	 * specific early arch. setup e.g. mmu setup
	 * ---------------------------------------------
	 */
	bl	bl32_early_platform_setup
	bl	bl32_plat_arch_setup

	/* ---------------------------------------------
	 * Jump to main function.
	 * ---------------------------------------------
	 */
	bl	tsp_main

	/* ---------------------------------------------
	 * Tell TSPD that we are done initialising
	 * ---------------------------------------------
	 */
	mov	x1, x0
	mov	x0, #TSP_ENTRY_DONE
	smc	#0

tsp_entrypoint_panic:
	b	tsp_entrypoint_panic

143
144
145
146
147
148
149
150
151
152
153
154
155
156
157

	/* -------------------------------------------
	 * Table of entrypoint vectors provided to the
	 * TSPD for the various entrypoints
	 * -------------------------------------------
	 */
func tsp_vector_table
	b	tsp_std_smc_entry
	b	tsp_fast_smc_entry
	b	tsp_cpu_on_entry
	b	tsp_cpu_off_entry
	b	tsp_cpu_resume_entry
	b	tsp_cpu_suspend_entry
	b	tsp_fiq_entry

158
159
160
161
162
163
164
165
166
167
	/*---------------------------------------------
	 * This entrypoint is used by the TSPD when this
	 * cpu is to be turned off through a CPU_OFF
	 * psci call to ask the TSP to perform any
	 * bookeeping necessary. In the current
	 * implementation, the TSPD expects the TSP to
	 * re-initialise its state so nothing is done
	 * here except for acknowledging the request.
	 * ---------------------------------------------
	 */
168
func tsp_cpu_off_entry
169
170
171
172
173
174
175
176
177
178
179
180
181
182
	bl	tsp_cpu_off_main
	restore_args_call_smc

	/*---------------------------------------------
	 * This entrypoint is used by the TSPD when this
	 * cpu is turned on using a CPU_ON psci call to
	 * ask the TSP to initialise itself i.e. setup
	 * the mmu, stacks etc. Minimal architectural
	 * state will be initialised by the TSPD when
	 * this function is entered i.e. Caches and MMU
	 * will be turned off, the execution state
	 * will be aarch64 and exceptions masked.
	 * ---------------------------------------------
	 */
183
func tsp_cpu_on_entry
184
185
186
187
	/* ---------------------------------------------
	 * Set the exception vector to something sane.
	 * ---------------------------------------------
	 */
188
	adr	x0, tsp_exceptions
189
190
191
	msr	vbar_el1, x0

	/* ---------------------------------------------
192
193
	 * Enable the instruction cache, stack pointer
	 * and data access alignment checks
194
195
	 * ---------------------------------------------
	 */
196
	mov	x1, #(SCTLR_I_BIT | SCTLR_A_BIT | SCTLR_SA_BIT)
197
	mrs	x0, sctlr_el1
198
	orr	x0, x0, x1
199
200
201
202
	msr	sctlr_el1, x0
	isb

	/* --------------------------------------------
203
204
205
	 * Give ourselves a stack whose memory will be
	 * marked as Normal-IS-WBWA when the MMU is
	 * enabled.
206
207
208
	 * --------------------------------------------
	 */
	mrs	x0, mpidr_el1
209
	bl	platform_set_stack
210

211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
	/* --------------------------------------------
	 * Enable the MMU with the DCache disabled. It
	 * is safe to use stacks allocated in normal
	 * memory as a result. All memory accesses are
	 * marked nGnRnE when the MMU is disabled. So
	 * all the stack writes will make it to memory.
	 * All memory accesses are marked Non-cacheable
	 * when the MMU is enabled but D$ is disabled.
	 * So used stack memory is guaranteed to be
	 * visible immediately after the MMU is enabled
	 * Enabling the DCache at the same time as the
	 * MMU can lead to speculatively fetched and
	 * possibly stale stack memory being read from
	 * other caches. This can lead to coherency
	 * issues.
	 * --------------------------------------------
227
	 */
228
	mov	x0, #DISABLE_DCACHE
229
	bl	bl32_plat_enable_mmu
230
231

	/* ---------------------------------------------
232
233
234
235
236
237
238
	 * Enable the Data cache now that the MMU has
	 * been enabled. The stack has been unwound. It
	 * will be written first before being read. This
	 * will invalidate any stale cache lines resi-
	 * -dent in other caches. We assume that
	 * interconnect coherency has been enabled for
	 * this cluster by EL3 firmware.
239
240
	 * ---------------------------------------------
	 */
241
242
243
244
	mrs	x0, sctlr_el1
	orr	x0, x0, #SCTLR_C_BIT
	msr	sctlr_el1, x0
	isb
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266

	/* ---------------------------------------------
	 * Enter C runtime to perform any remaining
	 * book keeping
	 * ---------------------------------------------
	 */
	bl	tsp_cpu_on_main
	restore_args_call_smc

	/* Should never reach here */
tsp_cpu_on_entry_panic:
	b	tsp_cpu_on_entry_panic

	/*---------------------------------------------
	 * This entrypoint is used by the TSPD when this
	 * cpu is to be suspended through a CPU_SUSPEND
	 * psci call to ask the TSP to perform any
	 * bookeeping necessary. In the current
	 * implementation, the TSPD saves and restores
	 * the EL1 state.
	 * ---------------------------------------------
	 */
267
func tsp_cpu_suspend_entry
268
269
270
	bl	tsp_cpu_suspend_main
	restore_args_call_smc

271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
	/*---------------------------------------------
	 * This entrypoint is used by the TSPD to pass
	 * control for handling a pending S-EL1 FIQ.
	 * 'x0' contains a magic number which indicates
	 * this. TSPD expects control to be handed back
	 * at the end of FIQ processing. This is done
	 * through an SMC. The handover agreement is:
	 *
	 * 1. PSTATE.DAIF are set upon entry. 'x1' has
	 *    the ELR_EL3 from the non-secure state.
	 * 2. TSP has to preserve the callee saved
	 *    general purpose registers, SP_EL1/EL0 and
	 *    LR.
	 * 3. TSP has to preserve the system and vfp
	 *    registers (if applicable).
	 * 4. TSP can use 'x0-x18' to enable its C
	 *    runtime.
	 * 5. TSP returns to TSPD using an SMC with
	 *    'x0' = TSP_HANDLED_S_EL1_FIQ
	 * ---------------------------------------------
	 */
func	tsp_fiq_entry
#if DEBUG
	mov	x2, #(TSP_HANDLE_FIQ_AND_RETURN & ~0xffff)
	movk	x2, #(TSP_HANDLE_FIQ_AND_RETURN &  0xffff)
	cmp	x0, x2
	b.ne	tsp_fiq_entry_panic
#endif
	/*---------------------------------------------
	 * Save any previous context needed to perform
	 * an exception return from S-EL1 e.g. context
	 * from a previous IRQ. Update statistics and
	 * handle the FIQ before returning to the TSPD.
	 * IRQ/FIQs are not enabled since that will
	 * complicate the implementation. Execution
	 * will be transferred back to the normal world
	 * in any case. A non-zero return value from the
	 * fiq handler is an error.
	 * ---------------------------------------------
	 */
	save_eret_context x2 x3
	bl	tsp_update_sync_fiq_stats
	bl	tsp_fiq_handler
	cbnz	x0, tsp_fiq_entry_panic
	restore_eret_context x2 x3
	mov	x0, #(TSP_HANDLED_S_EL1_FIQ & ~0xffff)
	movk	x0, #(TSP_HANDLED_S_EL1_FIQ &  0xffff)
	smc	#0

tsp_fiq_entry_panic:
	b	tsp_fiq_entry_panic

323
324
325
326
327
328
329
330
331
332
	/*---------------------------------------------
	 * This entrypoint is used by the TSPD when this
	 * cpu resumes execution after an earlier
	 * CPU_SUSPEND psci call to ask the TSP to
	 * restore its saved context. In the current
	 * implementation, the TSPD saves and restores
	 * EL1 state so nothing is done here apart from
	 * acknowledging the request.
	 * ---------------------------------------------
	 */
333
func tsp_cpu_resume_entry
334
335
336
337
338
339
340
341
342
343
	bl	tsp_cpu_resume_main
	restore_args_call_smc
tsp_cpu_resume_panic:
	b	tsp_cpu_resume_panic

	/*---------------------------------------------
	 * This entrypoint is used by the TSPD to ask
	 * the TSP to service a fast smc request.
	 * ---------------------------------------------
	 */
344
func tsp_fast_smc_entry
345
	bl	tsp_smc_handler
346
347
348
349
	restore_args_call_smc
tsp_fast_smc_entry_panic:
	b	tsp_fast_smc_entry_panic

350
351
352
353
354
355
356
357
358
359
360
361
362
363
	/*---------------------------------------------
	 * This entrypoint is used by the TSPD to ask
	 * the TSP to service a std smc request.
	 * We will enable preemption during execution
	 * of tsp_smc_handler.
	 * ---------------------------------------------
	 */
func tsp_std_smc_entry
	msr	daifclr, #DAIF_FIQ_BIT | DAIF_IRQ_BIT
	bl	tsp_smc_handler
	msr	daifset, #DAIF_FIQ_BIT | DAIF_IRQ_BIT
	restore_args_call_smc
tsp_std_smc_entry_panic:
	b	tsp_std_smc_entry_panic