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
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
|
/*
* Copyright (c) 2018, Arm Limited. All rights reserved.
*
* SPDX-License-Identifier: BSD-3-Clause
*/
#include <asm_macros_common.S>
#define COOKIE_SIZE 20
#ifdef AARCH64
/* int do_state_switch(void *) */
.globl do_state_switch
func do_state_switch
/* Temporarily save beginning of stack */
mov x7, sp
/*
* When asking to switch execution state, we can't expect general
* purpose registers hold their values. EL3 might clear them all; even
* if EL3 were to preserve them, the register width shrinks and then
* expands, leaving the upper part unknown. So save them before and
* restore after call to switch.
*/
stp x8, x9, [sp, #-16]!
stp x10, x11, [sp, #-16]!
stp x12, x13, [sp, #-16]!
stp x14, x15, [sp, #-16]!
stp x16, x17, [sp, #-16]!
stp x18, x19, [sp, #-16]!
stp x20, x21, [sp, #-16]!
stp x22, x23, [sp, #-16]!
stp x24, x25, [sp, #-16]!
stp x26, x27, [sp, #-16]!
stp x28, x29, [sp, #-16]!
/*
* State switch effectively means a soft reset; so SCTLR will lose its
* value too.
*/
mrs x1, CurrentEL
cmp x1, #(2 << 2)
b.ne 1f
mrs x1, sctlr_el2
b 2f
1:
mrs x1, sctlr_el1
2:
stp x30, x1, [sp, #-16]!
/* Store the PC in the cookie when switching back to AArch64 */
ldr x4, =state_switch_cookie
adr x2, do_switch_back
mov w1, w2
lsr x1, x1, #32
str w1, [x4, #0] /* PC hi */
str w2, [x4, #4] /* PC lo */
/* Store valid stack pointer in cookie */
mov x8, sp
str x8, [x4, #8]
/* Stash stack and LR before calling functions */
mov x28, x7
mov x29, x30
mov x10, x0
/*
* Clean and invalidate cookie memory as it's going to be accessed with
* MMU off in the new state.
*/
mov x0, x4
ldr x1, =COOKIE_SIZE
bl flush_dcache_range
/*
* Flush stack context saved on stack as it'll be accessed immediately
* after switching back, with MMU off.
*/
mov x0, x8
sub x1, x28, x8
bl flush_dcache_range
/* Prepare arguments for state switch SMC */
ldr x0, [x10], #8
ldr x1, [x10], #8
ldr x2, [x10], #8
ldr x3, [x10], #8
ldr x4, [x10], #8
/* Switch state */
smc #0
/*
* We reach here only if the SMC failed. If so, restore previously
* modified callee-saved registers, rewind stack, and return to caller
* with the error code from SMC.
*/
mov x1, x28
mov x2, x29
ldp x28, x29, [sp, #16]
mov sp, x1
ret x2
restore_context:
/* Restore context */
ldp x30, x1, [sp], #16
ldp x28, x29, [sp], #16
ldp x26, x27, [sp], #16
ldp x24, x25, [sp], #16
ldp x22, x23, [sp], #16
ldp x20, x21, [sp], #16
ldp x18, x19, [sp], #16
ldp x16, x17, [sp], #16
ldp x14, x15, [sp], #16
ldp x12, x13, [sp], #16
ldp x10, x11, [sp], #16
ldp x8, x9, [sp], #16
dsb sy
mrs x0, CurrentEL
cmp x0, #(2 << 2)
b.ne 1f
msr sctlr_el2, x1
b 2f
1:
msr sctlr_el1, x1
2:
isb
mov x0, #0
ret
endfunc do_state_switch
/* AArch64 entry point when switching back from AArch32 */
do_switch_back:
/* w0 and w1 have the cookie */
lsl x0, x0, #32
orr x0, x1, x0
ldr x1, [x0, #8]
mov sp, x1
b restore_context
.section .data, "aw"
/* AArch32 instructions to switch state back to AArch64, stored as data */
.align 2
.globl state_switch_a32_entry
state_switch_a32_entry:
/* Use the same context when switching back */
.word 0xe1a03000 /* mov r3, r0 */
.word 0xe1a04001 /* mov r4, r1 */
/* Set success flag in cookie */
.word 0xe3a00001 /* mov r0, #1 */
.word 0xe5810010 /* str r0, [r1, #16] */
/* Setup arguments for SMC */
.word 0xe3a00020 /* mov r0, #0x0020 */
.word 0xe3480200 /* movt r0, #0x8200 */
.word 0xe5912004 /* ldr r2, [r1, #4] */
.word 0xe5911000 /* ldr r1, [r1, #0] */
.word 0xe1600070 /* smc #0x0 */
.word 0xeafffffe /* b . */
#else /* !AARCH64 */
/* Not supported on AArch32 yet */
func do_state_switch
mov r0, #-1
bx lr
endfunc do_state_switch
#endif /* AARCH64 */
|