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
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
|
/* SPDX-License-Identifier: BSD-2-Clause */
/*
* Copyright (c) 2020, Linaro Limited
* Copyright (c) 2019-2020, Arm Limited
*/
#include <platform_config.h>
#include <arm64_macros.S>
#include <arm.h>
#include <asm.S>
#include <ffa.h>
#include <generated/asm-defines.h>
#include <keep.h>
#include <kernel/thread_defs.h>
#include <optee_ffa.h>
FUNC thread_ffa_msg_wait , :
mov_imm x0, FFA_MSG_WAIT /* FID */
mov x1, #FFA_TARGET_INFO_MBZ /* Target info MBZ */
mov x2, #FFA_PARAM_MBZ /* Param MBZ */
mov x3, #FFA_PARAM_MBZ /* Param MBZ */
mov x4, #FFA_PARAM_MBZ /* Param MBZ */
mov x5, #FFA_PARAM_MBZ /* Param MBZ */
mov x6, #FFA_PARAM_MBZ /* Param MBZ */
mov x7, #FFA_PARAM_MBZ /* Param MBZ */
b .ffa_msg_loop
END_FUNC thread_ffa_msg_wait
/* Caller provides x1, x3-x7 params */
LOCAL_FUNC ffa_msg_send_direct_resp , :
mov_imm x0, FFA_MSG_SEND_DIRECT_RESP_32 /* FID */
mov x2, #FFA_PARAM_MBZ /* RES MBZ */
.ffa_msg_loop:
/* Invoke SMC with caller provided parameters */
smc #0
/* Store the parameters as struct thread_smc_args on stack */
sub sp, sp, #THREAD_SMC_ARGS_SIZE
store_xregs sp, THREAD_SMC_ARGS_X0, 0, 7
mov x0, sp
mov x1, #0 /* Set the caller to 0 = normal world */
/* parse and handle message */
bl thread_spmc_msg_recv
/* Load struct thread_smc_args into registers */
load_xregs sp, THREAD_SMC_ARGS_X0, 0, 7
add sp, sp, #THREAD_SMC_ARGS_SIZE
b .ffa_msg_loop
END_FUNC ffa_msg_send_direct_resp
FUNC thread_std_smc_entry , :
/*
* A thread will be created during the init process (if CFG_WITH_SP).
* Afterward it needs to run ffa_msg_wait_invoke to finish init.
* The thread will be started with x1=OPTEE_FFA_SP_INIT.
* The value is pushed on the stack and popped into x19.
* Whenever x19 is OPTEE_FFA_SP_INIT, we call ffa_msg_wait_invoke
*/
push x0,x1
ror w19, w0, #16 /* Save target info with src and dst swapped */
bl __thread_std_smc_entry
pop x20,x19
mov w20, w0 /* Save return value */
/* Mask all maskable exceptions before switching to temporary stack */
msr daifset, #DAIFBIT_ALL
bl thread_get_tmp_sp
mov sp, x0
bl thread_state_free
mov w1, w19 /* Target info */
mov w3, w20 /* Return value */
mov x4, #FFA_PARAM_MBZ /* Unused parameter */
mov x5, #FFA_PARAM_MBZ /* Unused parameter */
mov x6, #FFA_PARAM_MBZ /* Unused parameter */
mov x7, #FFA_PARAM_MBZ /* Unused parameter */
cmp x19, #OPTEE_FFA_SP_INIT
beq thread_ffa_msg_wait
b ffa_msg_send_direct_resp
END_FUNC thread_std_smc_entry
/* void thread_sp_smc_entry(args, uint32_t src_id) */
FUNC thread_sp_smc_entry , :
/* Store the parameters as struct thread_smc_args on stack */
sub sp, sp, #THREAD_SMC_ARGS_SIZE
store_xregs sp, THREAD_SMC_ARGS_X0, 0, 7
mov x0, sp
mov x1, #0 /* Set the caller to 0 = normal world */
bl thread_spmc_msg_recv
load_xregs sp, THREAD_SMC_ARGS_X0, 20, 27
/* Mask all maskable exceptions before switching to temporary stack */
msr daifset, #DAIFBIT_ALL
bl thread_get_tmp_sp
mov sp, x0
bl thread_state_free
/* Restore the FF-A arguments before the SMC instruction. */
mov w0, w20
mov w1, w21
mov w2, w22
mov w3, w23
mov w4, w24
mov w5, w25
mov w6, w26
mov w7, w27
b .ffa_msg_loop
END_FUNC thread_sp_smc_entry
/* void thread_rpc(struct thread_rpc_arg *rpc_arg) */
FUNC thread_rpc , :
/* Read daif and create an SPSR */
mrs x1, daif
orr x1, x1, #(SPSR_64_MODE_EL1 << SPSR_64_MODE_EL_SHIFT)
/* Mask all maskable exceptions before switching to temporary stack */
msr daifset, #DAIFBIT_ALL
push x0, xzr
push x1, x30
bl thread_get_ctx_regs
ldr x30, [sp, #8]
store_xregs x0, THREAD_CTX_REGS_X19, 19, 30
mov x19, x0
bl thread_get_tmp_sp
pop x1, xzr /* Match "push x1, x30" above */
mov x2, sp
str x2, [x19, #THREAD_CTX_REGS_SP]
ldr x20, [sp] /* Get pointer to rpc_arg[] */
mov sp, x0 /* Switch to tmp stack */
/*
* We need to read rpc_arg[] early, because thread_state_suspend
* can invoke virt_unset_guest() which will unmap pages,
* where rpc_arg[] resides
*/
load_wregs x20, 0, 21, 24 /* Load rpc_arg[] into w21-w24 */
adr x2, .thread_rpc_return
mov w0, #THREAD_FLAGS_COPY_ARGS_ON_RETURN
bl thread_state_suspend
mov w7, w0 /* Supply thread index */
ldr w0, =FFA_MSG_SEND_DIRECT_RESP_32
mov w1, w21
mov w2, #FFA_PARAM_MBZ
mov w3, #0 /* Error code = 0 */
mov w4, w22
mov w5, w23
mov w6, w24
b ffa_msg_send_direct_resp
.thread_rpc_return:
/*
* At this point has the stack pointer been restored to the value
* stored in THREAD_CTX above.
*
* Jumps here from thread_resume above when RPC has returned. The
* IRQ and FIQ bits are restored to what they where when this
* function was originally entered. w0-w3 holds the values supplied
* to thread_resume_from_rpc() in a0-a3.
*/
pop x16, xzr /* Get pointer to rv[] */
store_wregs x16, 0, 0, 3 /* Store w0-w3 into rv[] */
ret
END_FUNC thread_rpc
DECLARE_KEEP_PAGER thread_rpc
/*
* void thread_foreign_intr_exit(uint32_t thread_index)
*
* This function is jumped to at the end of macro foreign_intr_handler().
* The current thread as indicated by @thread_index has just been
* suspended. The job here is just to inform normal world the thread id to
* resume when returning.
*/
FUNC thread_foreign_intr_exit , :
/* load threads[w0].tsd.rpc_target_info into w1 */
mov x1, #THREAD_CTX_SIZE
adr_l x2, threads
madd x1, x1, x0, x2
ldr w1, [x1, #THREAD_CTX_TSD_RPC_TARGET_INFO]
mov x2, #FFA_PARAM_MBZ
mov w3, #FFA_PARAM_MBZ
mov w4, #OPTEE_FFA_YIELDING_CALL_RETURN_INTERRUPT
mov x5, #FFA_PARAM_MBZ
mov w6, #FFA_PARAM_MBZ
mov w7, w0
b ffa_msg_send_direct_resp
END_FUNC thread_foreign_intr_exit
/*
* void spmc_send_spmd_msg(uint64_t msg, uint64_t a1, uint64_t a2, uint64_t a3,
* uint64_t a4, uint32_t *ffa_res, int32_t *spmd_res)
*/
FUNC spmc_send_spmd_msg , :
/* Store addresses of return values */
push x5, x6
/* Move function arguments to correct registers */
mov x3, x0
mov x4, x1
mov x5, x2
mov x6, x3
mov x7, x4
/* Set FFA parameters */
mov_imm x0, FFA_MSG_SEND_DIRECT_REQ_32
movz x1, #0x8000, LSL #16 /* SPMC ID */
orr x1, x1, #0xffff /* SPMD ID */
mov x2, #FFA_PARAM_MBZ
smc #0
/* Store return values */
pop x5, x6
str w0, [x5]
str w2, [x6]
ret
END_FUNC spmc_send_spmd_msg
|
