1/*
2 * Copyright (c) 2013-2015, Google Inc. All rights reserved
3 *
4 * Permission is hereby granted, free of charge, to any person obtaining
5 * a copy of this software and associated documentation files
6 * (the "Software"), to deal in the Software without restriction,
7 * including without limitation the rights to use, copy, modify, merge,
8 * publish, distribute, sublicense, and/or sell copies of the Software,
9 * and to permit persons to whom the Software is furnished to do so,
10 * subject to the following conditions:
11 *
12 * The above copyright notice and this permission notice shall be
13 * included in all copies or substantial portions of the Software.
14 *
15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
16 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
17 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
18 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
19 * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
20 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
21 * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
22 */
23
24#include <asm.h>
25#include <lib/sm/monitor.h>
26#include <lib/sm/smcall.h>
27#include <lib/sm/sm_err.h>
28
29#include <kernel/vm.h>
30
31/*
32 * Monitor stacks:
33 *  secure context
34 *  non-secure context
35 *
36 * Context:
37 *  mon_lr
38 *  usr/sys_sp, usr/sys_lr
39 *  und_spsr, und_sp, und_lr
40 *  abt_spsr, abt_sp, abt_lr
41 *  svc_spsr, svc_sp, svc_lr
42 *  irq_spsr, irq_sp, irq_lr
43 *  fiq_spsr, fiq_r8-fiq_lr
44 *  r4-r11, lr
45 *  r0-r3, ip
46 *  lr, spsr
47 *
48 */
49
50#define CONTEXT_SIZE_RETURN (2 * 4)
51#define CONTEXT_SIZE_SCRATCH_GEN_REGS ((5 + 9) * 4)
52#define CONTEXT_SIZE_MODE_REGS_MON_RETURN ((8 + 3 + 3 + 3 + 3 + 2 + 1) * 4)
53#define CONTEXT_SIZE (CONTEXT_SIZE_RETURN + CONTEXT_SIZE_SCRATCH_GEN_REGS + CONTEXT_SIZE_MODE_REGS_MON_RETURN)
54#define MONITOR_STACK_SIZE (CONTEXT_SIZE * 2)
55
56.macro SAVE_CONTEXT_RETURN
57	srsdb	sp!, #MODE_MON	/* srsfd alias not recognized by current assembler */
58.endm
59
60.macro SAVE_CONTEXT_GEN_REGS
61	push	{r4-r11, lr}
62.endm
63
64.macro SAVE_CONTEXT_SCRATCH_REGS
65	push	{r0-r3, ip}
66.endm
67
68.macro SAVE_CONTEXT
69	SAVE_CONTEXT_RETURN
70	SAVE_CONTEXT_SCRATCH_REGS
71	SAVE_CONTEXT_GEN_REGS
72.endm
73
74.macro SAVE_CONTEXT_MODE_REGS
75	mov	r4, sp
76
77	cps	#MODE_FIQ
78	mrs	r5, spsr
79	stmfd	r4!, {r5, r8-lr}
80
81	cps	#MODE_IRQ
82	mrs	r5, spsr
83	stmfd	r4!, {r5, sp, lr}
84
85	cps	#MODE_SVC
86	mrs	r5, spsr
87	stmfd	r4!, {r5, sp, lr}
88
89	cps	#MODE_ABT
90	mrs	r5, spsr
91	stmfd	r4!, {r5, sp, lr}
92
93	cps	#MODE_UND
94	mrs	r5, spsr
95	stmfd	r4!, {r5, sp, lr}
96
97	cps	#MODE_SYS
98	stmfd	r4!, {sp, lr}
99
100	cps	#MODE_MON
101	mov	sp, r4
102.endm
103
104.macro SAVE_RETURN_ADDR reg
105	push	{\reg}
106.endm
107
108.macro RESTORE_RETURN_ADDR reg
109	pop	{\reg}
110.endm
111
112.macro RESTORE_CONTEXT_MODE_REGS
113	mov	r4, sp
114
115	cps	#MODE_SYS
116	ldmfd	r4!, {sp, lr}
117
118	cps	#MODE_UND
119	ldmfd	r4!, {r5, sp, lr}
120	msr	spsr, r5
121
122	cps	#MODE_ABT
123	ldmfd	r4!, {r5, sp, lr}
124	msr	spsr, r5
125
126	cps	#MODE_SVC
127	ldmfd	r4!, {r5, sp, lr}
128	msr	spsr, r5
129
130	cps	#MODE_IRQ
131	ldmfd	r4!, {r5, sp, lr}
132	msr	spsr, r5
133
134	cps	#MODE_FIQ
135	ldmfd	r4!, {r5, r8-lr}
136	msr	spsr, r5
137
138	cps	#MODE_MON
139	mov	sp, r4
140.endm
141
142.macro RESTORE_CONTEXT_GEN_REGS
143	pop	{r4-r11, lr}
144.endm
145
146.macro RESTORE_CONTEXT_SCRATCH_REGS
147	pop	{r0-r3, ip}
148.endm
149
150.macro RESTORE_CONTEXT_AND_RETURN
151	RESTORE_CONTEXT_GEN_REGS
152	add	sp, sp, #5 * 4
153	rfefd	sp!
154.endm
155
156.p2align 5
157.globl monitor_vector_table
158monitor_vector_table:
159	nop				/* RESET	*/
160	b	.			/* UNDEF	*/
161	b	.Lmon_smcall		/* SWI		*/
162	b	.			/* IABORT	*/
163	b	.			/* DABORT	*/
164	nop				/* reserved	*/
165	b	.			/* IRQ		*/
166	b	.Lmon_fiq_entry		/* FIQ		*/
167
168/*
169 * Called in monitor mode
170 * return address in mon-lr
171 * ns-return address in svc-lr
172 */
173
174FUNCTION(monitor_reset)
175	/* figure out our cpu number */
176	mrc     p15, 0, r11, c0, c0, 5 /* read MPIDR */
177
178	/* mask off the bottom 12 bits to test cluster number:cpu number */
179	ubfx    r11, r11, #0, #12
180
181	/* return if cpu >= SMP_MAX_CPUS */
182	cmp	r11, #SMP_MAX_CPUS
183	bxge	lr
184
185	/* find cpu specifc stack offset */
186	mov	ip, #MONITOR_STACK_SIZE
187	mul	r11, r11,ip
188
189	adr	sp, monitor_reset	/* sp = paddr */
190	ldr	ip, =monitor_reset	/* ip = vaddr */
191	sub	ip, sp, ip		/* ip = phys_offset */
192	ldr	sp, =mon_ns_stack_top	/* sp = ns_stack vaddr */
193	add	sp, sp, ip		/* sp = ns_stack paddr */
194
195	/* apply per-cpu stack offset */
196	sub	sp, sp, r11
197
198	/* Save non-secure return address, mode and registers */
199	cps	#MODE_SVC
200	msr	spsr_cfsx, #MODE_SVC_IRQ_DISABLED
201	SAVE_CONTEXT_RETURN
202	cps	#MODE_MON
203
204	SAVE_CONTEXT_SCRATCH_REGS
205	SAVE_CONTEXT_GEN_REGS
206	SAVE_CONTEXT_MODE_REGS
207	ldr	r4, =platform_mon_initial_ns_return
208	SAVE_RETURN_ADDR r4
209
210	sub	sp, sp, ip	/* sp = sp vaddr */
211
212	/* Initialize NS mode CPU context registers */
213	mrc	p15, 0, r4, c12, c0, 0	/* r4 = VBAR */
214	mrc	p15, 0, r5, c2, c0, 0	/* r5 = TTBR0 */
215	mrc	p15, 0, r6, c2, c0, 1	/* r6 = TTBR1 */
216	mrc	p15, 0, r7, c3, c0, 0	/* r7 = DACR */
217	mrc	p15, 0, r8, c1, c0, 0	/* r8 = SCTLR */
218
219	SWITCH_SCR_TO_NONSECURE r9
220
221	mcr	p15, 0, r4, c12, c0, 0	/* VBAR = r4 */
222	mcr	p15, 0, r5, c2, c0, 0	/* TTBR0 = r5 */
223	mcr	p15, 0, r6, c2, c0, 1	/* TTBR1 = r6 */
224	mcr	p15, 0, r7, c3, c0, 0	/* DACR = r7 */
225	mcr	p15, 0, r8, c1, c0, 0	/* SCTLR = r8 */
226
227	SWITCH_SCR_TO_SECURE r9
228
229	bx	lr
230
231FUNCTION(monitor_init_mmu_on)
232	bx	lr
233
234.Lmon_invalid_ns_return_addr:
235	b	.
236
237FUNCTION(mon_initial_ns_return)
238	cmp	lr, #~0
239	strne	lr, [sp, #CONTEXT_SIZE_SCRATCH_GEN_REGS]
240	b	.Lmon_smcall_initial_ns_return
241
242/* direction in flags */
243.Lmon_smcall_from_secure:
244	adreq	lr, .Lmon_smcall_secure_return
245.Lmon_context_switch:
246	SAVE_CONTEXT_MODE_REGS
247	SAVE_RETURN_ADDR lr
248
249	addeq	sp, sp, #(CONTEXT_SIZE)	/* secure => non-secure */
250	subne	sp, sp, #(CONTEXT_SIZE)	/* non-secure => secure */
251
252	RESTORE_RETURN_ADDR lr
253	RESTORE_CONTEXT_MODE_REGS
254	bx	lr
255
256.Lmon_smcall:
257	SAVE_CONTEXT
258
259	mrc	p15, 0, r4, c1, c1, 0	/* r4 = SCR */
260	tst	r4, #0x1
261	beq	.Lmon_smcall_from_secure
262
263	SWITCH_SCR_TO_SECURE r4
264	bl	.Lmon_context_switch
265.Lmon_smcall_initial_ns_return:
266.weak platform_mon_initial_ns_return
267platform_mon_initial_ns_return:
268	RESTORE_CONTEXT_GEN_REGS
269
270	ldr	lr, [sp]
271
272	/* Handle SMC_FC_FIQ_EXIT from non-secure */
273	cmp	lr, #SMC_FC_FIQ_EXIT
274	beq	.Lmon_fiq_exit_return
275
276	SWITCH_SCR_TO_NONSECURE r3
277	mov	r0, r1	/* secure os passes the return code in r1 */
278
279	add	sp, sp, #5 * 4
280	rfefd	sp!
281
282
283.Lmon_smcall_secure_return:
284	RESTORE_CONTEXT_AND_RETURN
285
286.Lmon_fiq_entry:
287
288	SAVE_CONTEXT
289
290	mrc	p15, 0, r4, c1, c1, 0	/* r4 = SCR */
291	tst	r4, #0x1
292	beq	.	/* fiqs are not trapped in secure mode */
293
294	ldr	r0, =SMC_FC_FIQ_ENTER
295
296	SWITCH_SCR_TO_SECURE r4
297	bl	.Lmon_context_switch
298	SWITCH_SCR_TO_NONSECURE r4
299
300	RESTORE_CONTEXT_GEN_REGS
301	ldr	lr, [sp, #(5 * 4)]
302
303	cmp	r1, #0	/* secure os passes the return code in r1 */
304	subne	lr, lr, #4
305	bne	.Lmon_fiq_return
306
307	ldr	r0, [sp, #(6 * 4)] /* load saved spsr */
308	mov	r1, lr
309
310	mov	r2, #0x91 /* fiq mode with IRQ disabled */
311	str	r2, [sp, #(6 * 4)] /* store spsr */
312
313	mrc	p15, 0, r3, c1, c0, 0	/* r3 = Non-secure SCTLR */
314	tst	r3, #(1 << 13) /* Non-secure SCTLR.V */
315	ldrne	lr, =0xffff0000
316	mrceq	p15, 0, lr, c12, c0, 0 /* lr = VBAR (if not using Hivecs) */
317	add	lr, lr, #0x1c /* fiq vector offset */
318
319	SWITCH_SCR_TO_SECURE r2
320
321	/* Set fiq mode lr and spsr */
322	cps	#MODE_FIQ
323	msr	spsr_cfsx, r0
324	mov	lr, r1
325	cps	#MODE_MON
326
327	SWITCH_SCR_TO_NONSECURE r2
328
329.Lmon_fiq_return:
330	str	lr, [sp, #(5 * 4)]
331	RESTORE_CONTEXT_SCRATCH_REGS
332	rfefd	sp!
333
334.Lmon_fiq_exit_return:
335	/* Retrieve FIQ mode spsr, lr and restore r0 from FIQ mode r12 */
336	cps	#MODE_FIQ
337
338	mov	r0, r12 /* restore r0 used for smc number */
339	mrs	r1, spsr
340	mov	r2, lr
341
342	cps	#MODE_MON
343
344	str	r2, [sp, #(5 * 4)] /* save return addr */
345
346	/* don't allow return to monitor mode */
347	and	r2, r1, #0x1f
348	cmp	r2, #MODE_MON
349	moveq	r1, #MODE_UND
350
351	str	r1, [sp, #(6 * 4)] /* store spsr */
352
353	SWITCH_SCR_TO_NONSECURE r3
354
355	mov	lr, r0
356	RESTORE_CONTEXT_SCRATCH_REGS
357	mov	r0, lr
358	rfefd	sp!
359
360.data
361.align 3
362
363/* Monitor stack for primary cpu */
364LOCAL_DATA(mon_ns_stack)
365	.skip	MONITOR_STACK_SIZE * SMP_MAX_CPUS
366LOCAL_DATA(mon_ns_stack_top)
367