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
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
|
#include <common/debug.h>
#include <smccc_helpers.h>
#include <services/ffa_svc.h>
#include <services/logical_sp.h>
#include <arch_helpers.h>
#include <assert.h>
#include <errno.h>
#include <libfdt.h>
#include <bl31/bl31.h>
#include <bl31/ehf.h>
#include <common/debug.h>
#include <common/fdt_wrappers.h>
#include <common/runtime_svc.h>
#include <dt-bindings/memory/memory.h>
#include <lib/el3_runtime/context_mgmt.h>
#include <lib/smccc.h>
#include <lib/utils.h>
#include <plat/common/platform.h>
#include <services/ffa_svc.h>
#include <services/spmc_svc.h>
#include <services/spmd_svc.h>
#include <services/logical_sp.h>
#include <plat/arm/common/plat_arm.h>
#include <platform_def.h>
#define LP_PARTITION_ID 0xC001
#define LP_UUID {0x0, 0x0, 0x0, 0x2}
static int64_t sp_init(void) {
// TODO: Do some initialisation.
INFO("LSP: Init function called.\n");
return 0;
}
static int get_sp_index(uint16_t sp_id, unsigned int *sp_index)
{
int i;
for (i = 0; i < SECURE_PARTITION_COUNT; i++) {
if (sp_id == spmc_sp_ctx[i].sp_id) {
*sp_index = i;
return i;
}
}
return -EINVAL;
}
/*******************************************************************************
* Function to perform a call to a Secure Partition.
* TODO: Update to unwind approach if required.
******************************************************************************/
static uint64_t spmc_sp_call(unsigned int sp_index, uint32_t smc_fid,
uint64_t comm_buffer_address,
uint64_t comm_size, uint64_t core_pos)
{
uint64_t rc;
sp_context_t *sp_ptr = &(spmc_sp_ctx[sp_index].sp_ctx);
/* Set values for registers on SP entry */
cpu_context_t *cpu_ctx = &(sp_ptr->cpu_ctx);
write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X0, smc_fid);
write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X1, 0);
write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X2, 0);
write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X3, comm_buffer_address);
write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X4, comm_size);
write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X5, 0);
write_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X6, core_pos);
/* Jump to the Secure Partition. */
rc = spm_sp_synchronous_entry(sp_ptr);
return rc;
}
/*******************************************************************************
* Return FFA_ERROR with specified error code
******************************************************************************/
static uint64_t spmc_ffa_error_return(void *handle, int error_code)
{
SMC_RET8(handle, FFA_ERROR,
FFA_TARGET_INFO_MBZ, error_code,
FFA_PARAM_MBZ, FFA_PARAM_MBZ, FFA_PARAM_MBZ,
FFA_PARAM_MBZ, FFA_PARAM_MBZ);
}
/*******************************************************************************
* MM_INTERFACE handler
******************************************************************************/
static uint64_t spmc_mm_interface_handler(unsigned int sp_index,
uint32_t smc_fid,
uint64_t mm_cookie,
uint64_t comm_buffer_address,
uint64_t comm_size_address,
void *handle)
{
uint64_t rc;
/*
* The current secure partition design mandates
* - at any point, only a single core can be
* executing in the secure partition.
* - a core cannot be preempted by an interrupt
* while executing in secure partition.
* Raise the running priority of the core to the
* interrupt level configured for secure partition
* so as to block any interrupt from preempting this
* core.
*/
ehf_activate_priority(PLAT_SP_PRI);
/* Save the Normal world context */
cm_el1_sysregs_context_save(NON_SECURE);
rc = spmc_sp_call(sp_index, smc_fid, comm_buffer_address,
comm_size_address, plat_my_core_pos());
/* Restore non-secure state */
cm_el1_sysregs_context_restore(NON_SECURE);
cm_set_next_eret_context(NON_SECURE);
/*
* Exited from secure partition. This core can take
* interrupts now.
*/
ehf_deactivate_priority(PLAT_SP_PRI);
if (rc == 0)
SMC_RET2(handle, FFA_SUCCESS_SMC64, 0x7);
return spmc_ffa_error_return(handle,
FFA_ERROR_NOT_SUPPORTED);
}
static uint64_t direct_req_secure_smc_handler(uint64_t x1, uint64_t x2,
uint64_t x3, uint64_t x4,
void *cookie, void *handle,
uint64_t flags)
{
uint64_t rc;
uint16_t sp_id;
unsigned int sp_index;
sp_context_t *sp_ctx;
/* Make next ERET jump to S-EL0 instead of S-EL1. */
cm_set_elr_spsr_el3(SECURE, read_elr_el1(), read_spsr_el1());
switch (x3) {
case SP_MEMORY_ATTRIBUTES_GET_AARCH64:
INFO("Received SP_MEMORY_ATTRIBUTES_GET_AARCH64 request\n");
sp_id = STMM_SP_ID;
if (get_sp_index(sp_id, &sp_index) < 0) {
WARN("Not found the StMM Secure Partition.\n");
break;
}
sp_ctx = &(spmc_sp_ctx[sp_index].sp_ctx);
if (sp_ctx->state != SP_STATE_RESET) {
WARN("SP_MEMORY_ATTRIBUTES_GET_AARCH64 is available at boot time\n");
return spmc_ffa_error_return(handle,
FFA_ERROR_NOT_SUPPORTED);
}
rc = spm_memory_attributes_get_smc_handler(sp_ctx, x4);
if (rc < 0)
return spmc_ffa_error_return(handle,
FFA_ERROR_INVALID_PARAMETER);
SMC_RET4(handle, FFA_MSG_SEND_DIRECT_RESP_SMC64, 0x0, 0x0, rc);
case SP_MEMORY_ATTRIBUTES_SET_AARCH64:
INFO("Received SP_MEMORY_ATTRIBUTES_SET_AARCH64 request\n");
sp_id = STMM_SP_ID;
if (get_sp_index(sp_id, &sp_index) < 0) {
WARN("Not found the StMM Secure Partition.\n");
break;
}
sp_ctx = &(spmc_sp_ctx[sp_index].sp_ctx);
if (sp_ctx->state != SP_STATE_RESET) {
WARN("SP_MEMORY_ATTRIBUTES_SET_AARCH64 is available at boot time\n");
return spmc_ffa_error_return(handle,
FFA_ERROR_NOT_SUPPORTED);
}
rc = spm_memory_attributes_set_smc_handler(sp_ctx, x4,
SMC_GET_GP(handle, CTX_GPREG_X5),
SMC_GET_GP(handle, CTX_GPREG_X6));
if (rc < 0)
return spmc_ffa_error_return(handle,
FFA_ERROR_INVALID_PARAMETER);
SMC_RET4(handle, FFA_MSG_SEND_DIRECT_RESP_SMC64, 0x0, 0x0, rc);
default:
WARN("Not supported direct request handling for ID=0x%llx\n", x3);
break;
}
return spmc_ffa_error_return(handle,
FFA_ERROR_NOT_SUPPORTED);
}
static uint64_t direct_req_non_secure_smc_handler(uint64_t x1,
uint64_t x2,
uint64_t x3,
uint64_t x4,
void *cookie,
void *handle,
uint64_t flags)
{
uint16_t dst_id;
unsigned int sp_index;
switch (x3) {
case MM_INTERFACE_ID_AARCH32:
case MM_INTERFACE_ID_AARCH64:
INFO("MM interface id\n");
dst_id = STMM_SP_ID;
if (get_sp_index(dst_id, &sp_index) < 0) {
WARN("Not found the StMM Secure Partition.\n");
break;
}
return spmc_mm_interface_handler(sp_index, x3, x4,
SMC_GET_GP(handle, CTX_GPREG_X5),
SMC_GET_GP(handle, CTX_GPREG_X6), handle);
default:
WARN("Not supported direct request handling for ID=0x%llx\n", x3);
break;
}
return spmc_ffa_error_return(handle,
FFA_ERROR_NOT_SUPPORTED);
}
static uint64_t handle_ffa_direct_request(uint32_t smc_fid, bool secure_origin, uint64_t x1, uint64_t x2,
uint64_t x3, uint64_t x4, void *cookie, void *handle, uint64_t flags) {
uint64_t ret;
if (secure_origin) {
assert(handle == cm_get_context(SECURE));
return direct_req_secure_smc_handler(x1, x2, x3, x4, cookie,
handle, flags);
} else {
assert(handle == cm_get_context(NON_SECURE));
return direct_req_non_secure_smc_handler(x1, x2, x3, x4, cookie,
handle, flags);
}
}
/* Register logical partition */
DECLARE_LOGICAL_PARTITION(
stmm_lsp,
sp_init, // Init Function
LP_PARTITION_ID, // FFA Partition ID
LP_UUID, // UUID
0x1, // Partition Properties. Can only receive direct messages.
handle_ffa_direct_request // Callback for direct requests.
);
|
