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
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
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
/*
* 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_helpers.h>
#include <bl_common.h>
#include <bl32.h>
#include <debug.h>
#include <platform.h>
#include <spinlock.h>
#include <stdio.h>
#include <tsp.h>
/*******************************************************************************
* Lock to control access to the console
******************************************************************************/
spinlock_t console_lock;
/*******************************************************************************
* Per cpu data structure to populate parameters for an SMC in C code and use
* a pointer to this structure in assembler code to populate x0-x7
******************************************************************************/
static tsp_args_t tsp_smc_args[PLATFORM_CORE_COUNT];
/*******************************************************************************
* Per cpu data structure to keep track of TSP activity
******************************************************************************/
work_statistics_t tsp_stats[PLATFORM_CORE_COUNT];
/*******************************************************************************
* Single reference to the various entry points exported by the test secure
* payload. A single copy should suffice for all cpus as they are not expected
* to change.
******************************************************************************/
static const entry_info_t tsp_entry_info = {
tsp_std_smc_entry,
tsp_fast_smc_entry,
tsp_cpu_on_entry,
tsp_cpu_off_entry,
tsp_cpu_resume_entry,
tsp_cpu_suspend_entry,
tsp_fiq_entry,
};
static tsp_args_t *set_smc_args(uint64_t arg0,
uint64_t arg1,
uint64_t arg2,
uint64_t arg3,
uint64_t arg4,
uint64_t arg5,
uint64_t arg6,
uint64_t arg7)
{
uint64_t mpidr = read_mpidr();
uint32_t linear_id;
tsp_args_t *pcpu_smc_args;
/*
* Return to Secure Monitor by raising an SMC. The results of the
* service are passed as an arguments to the SMC
*/
linear_id = platform_get_core_pos(mpidr);
pcpu_smc_args = &tsp_smc_args[linear_id];
write_sp_arg(pcpu_smc_args, TSP_ARG0, arg0);
write_sp_arg(pcpu_smc_args, TSP_ARG1, arg1);
write_sp_arg(pcpu_smc_args, TSP_ARG2, arg2);
write_sp_arg(pcpu_smc_args, TSP_ARG3, arg3);
write_sp_arg(pcpu_smc_args, TSP_ARG4, arg4);
write_sp_arg(pcpu_smc_args, TSP_ARG5, arg5);
write_sp_arg(pcpu_smc_args, TSP_ARG6, arg6);
write_sp_arg(pcpu_smc_args, TSP_ARG7, arg7);
return pcpu_smc_args;
}
/*******************************************************************************
* TSP main entry point where it gets the opportunity to initialize its secure
* state/applications. Once the state is initialized, it must return to the
* SPD with a pointer to the 'tsp_entry_info' structure.
******************************************************************************/
uint64_t tsp_main(void)
{
uint64_t mpidr = read_mpidr();
uint32_t linear_id = platform_get_core_pos(mpidr);
#if DEBUG
meminfo_t *mem_layout = bl32_plat_sec_mem_layout();
#endif
/* Initialize the platform */
bl32_platform_setup();
/* Initialize secure/applications state here */
tsp_generic_timer_start();
/* Update this cpu's statistics */
tsp_stats[linear_id].smc_count++;
tsp_stats[linear_id].eret_count++;
tsp_stats[linear_id].cpu_on_count++;
spin_lock(&console_lock);
printf("TSP %s\n\r", build_message);
INFO("Total memory base : 0x%x\n", mem_layout->total_base);
INFO("Total memory size : 0x%x bytes\n", mem_layout->total_size);
INFO("Free memory base : 0x%x\n", mem_layout->free_base);
INFO("Free memory size : 0x%x bytes\n", mem_layout->free_size);
INFO("cpu 0x%x: %d smcs, %d erets %d cpu on requests\n", mpidr,
tsp_stats[linear_id].smc_count,
tsp_stats[linear_id].eret_count,
tsp_stats[linear_id].cpu_on_count);
spin_unlock(&console_lock);
/*
* TODO: There is a massive assumption that the SPD and SP can see each
* other's memory without issues so it is safe to pass pointers to
* internal memory. Replace this with a shared communication buffer.
*/
return (uint64_t) &tsp_entry_info;
}
/*******************************************************************************
* This function performs any remaining book keeping in the test secure payload
* after this cpu's architectural state has been setup in response to an earlier
* psci cpu_on request.
******************************************************************************/
tsp_args_t *tsp_cpu_on_main(void)
{
uint64_t mpidr = read_mpidr();
uint32_t linear_id = platform_get_core_pos(mpidr);
/* Initialize secure/applications state here */
tsp_generic_timer_start();
/* Update this cpu's statistics */
tsp_stats[linear_id].smc_count++;
tsp_stats[linear_id].eret_count++;
tsp_stats[linear_id].cpu_on_count++;
spin_lock(&console_lock);
printf("SP: cpu 0x%x turned on\n\r", mpidr);
INFO("cpu 0x%x: %d smcs, %d erets %d cpu on requests\n", mpidr,
tsp_stats[linear_id].smc_count,
tsp_stats[linear_id].eret_count,
tsp_stats[linear_id].cpu_on_count);
spin_unlock(&console_lock);
/* Indicate to the SPD that we have completed turned ourselves on */
return set_smc_args(TSP_ON_DONE, 0, 0, 0, 0, 0, 0, 0);
}
/*******************************************************************************
* This function performs any remaining book keeping in the test secure payload
* before this cpu is turned off in response to a psci cpu_off request.
******************************************************************************/
tsp_args_t *tsp_cpu_off_main(uint64_t arg0,
uint64_t arg1,
uint64_t arg2,
uint64_t arg3,
uint64_t arg4,
uint64_t arg5,
uint64_t arg6,
uint64_t arg7)
{
uint64_t mpidr = read_mpidr();
uint32_t linear_id = platform_get_core_pos(mpidr);
/*
* This cpu is being turned off, so disable the timer to prevent the
* secure timer interrupt from interfering with power down. A pending
* interrupt will be lost but we do not care as we are turning off.
*/
tsp_generic_timer_stop();
/* Update this cpu's statistics */
tsp_stats[linear_id].smc_count++;
tsp_stats[linear_id].eret_count++;
tsp_stats[linear_id].cpu_off_count++;
spin_lock(&console_lock);
printf("SP: cpu 0x%x off request\n\r", mpidr);
INFO("cpu 0x%x: %d smcs, %d erets %d cpu off requests\n", mpidr,
tsp_stats[linear_id].smc_count,
tsp_stats[linear_id].eret_count,
tsp_stats[linear_id].cpu_off_count);
spin_unlock(&console_lock);
/* Indicate to the SPD that we have completed this request */
return set_smc_args(TSP_OFF_DONE, 0, 0, 0, 0, 0, 0, 0);
}
/*******************************************************************************
* This function performs any book keeping in the test secure payload before
* this cpu's architectural state is saved in response to an earlier psci
* cpu_suspend request.
******************************************************************************/
tsp_args_t *tsp_cpu_suspend_main(uint64_t power_state,
uint64_t arg1,
uint64_t arg2,
uint64_t arg3,
uint64_t arg4,
uint64_t arg5,
uint64_t arg6,
uint64_t arg7)
{
uint64_t mpidr = read_mpidr();
uint32_t linear_id = platform_get_core_pos(mpidr);
/*
* Save the time context and disable it to prevent the secure timer
* interrupt from interfering with wakeup from the suspend state.
*/
tsp_generic_timer_save();
tsp_generic_timer_stop();
/* Update this cpu's statistics */
tsp_stats[linear_id].smc_count++;
tsp_stats[linear_id].eret_count++;
tsp_stats[linear_id].cpu_suspend_count++;
spin_lock(&console_lock);
printf("SP: cpu 0x%x suspend request. power state: 0x%x\n\r",
mpidr, power_state);
INFO("cpu 0x%x: %d smcs, %d erets %d cpu suspend requests\n", mpidr,
tsp_stats[linear_id].smc_count,
tsp_stats[linear_id].eret_count,
tsp_stats[linear_id].cpu_suspend_count);
spin_unlock(&console_lock);
/* Indicate to the SPD that we have completed this request */
return set_smc_args(TSP_SUSPEND_DONE, 0, 0, 0, 0, 0, 0, 0);
}
/*******************************************************************************
* This function performs any book keeping in the test secure payload after this
* cpu's architectural state has been restored after wakeup from an earlier psci
* cpu_suspend request.
******************************************************************************/
tsp_args_t *tsp_cpu_resume_main(uint64_t suspend_level,
uint64_t arg1,
uint64_t arg2,
uint64_t arg3,
uint64_t arg4,
uint64_t arg5,
uint64_t arg6,
uint64_t arg7)
{
uint64_t mpidr = read_mpidr();
uint32_t linear_id = platform_get_core_pos(mpidr);
/* Restore the generic timer context */
tsp_generic_timer_restore();
/* Update this cpu's statistics */
tsp_stats[linear_id].smc_count++;
tsp_stats[linear_id].eret_count++;
tsp_stats[linear_id].cpu_resume_count++;
spin_lock(&console_lock);
printf("SP: cpu 0x%x resumed. suspend level %d \n\r",
mpidr, suspend_level);
INFO("cpu 0x%x: %d smcs, %d erets %d cpu suspend requests\n", mpidr,
tsp_stats[linear_id].smc_count,
tsp_stats[linear_id].eret_count,
tsp_stats[linear_id].cpu_suspend_count);
spin_unlock(&console_lock);
/* Indicate to the SPD that we have completed this request */
return set_smc_args(TSP_RESUME_DONE, 0, 0, 0, 0, 0, 0, 0);
}
/*******************************************************************************
* TSP fast smc handler. The secure monitor jumps to this function by
* doing the ERET after populating X0-X7 registers. The arguments are received
* in the function arguments in order. Once the service is rendered, this
* function returns to Secure Monitor by raising SMC.
******************************************************************************/
tsp_args_t *tsp_smc_handler(uint64_t func,
uint64_t arg1,
uint64_t arg2,
uint64_t arg3,
uint64_t arg4,
uint64_t arg5,
uint64_t arg6,
uint64_t arg7)
{
uint64_t results[2];
uint64_t service_args[2];
uint64_t mpidr = read_mpidr();
uint32_t linear_id = platform_get_core_pos(mpidr);
const char *smc_type;
/* Update this cpu's statistics */
tsp_stats[linear_id].smc_count++;
tsp_stats[linear_id].eret_count++;
smc_type = ((func >> 31) & 1) == 1 ? "fast" : "standard";
printf("SP: cpu 0x%x received %s smc 0x%x\n", read_mpidr(), smc_type, func);
INFO("cpu 0x%x: %d smcs, %d erets\n", mpidr,
tsp_stats[linear_id].smc_count,
tsp_stats[linear_id].eret_count);
/* Render secure services and obtain results here */
results[0] = arg1;
results[1] = arg2;
/*
* Request a service back from dispatcher/secure monitor. This call
* return and thereafter resume exectuion
*/
tsp_get_magic(service_args);
/* Determine the function to perform based on the function ID */
switch (TSP_BARE_FID(func)) {
case TSP_ADD:
results[0] += service_args[0];
results[1] += service_args[1];
break;
case TSP_SUB:
results[0] -= service_args[0];
results[1] -= service_args[1];
break;
case TSP_MUL:
results[0] *= service_args[0];
results[1] *= service_args[1];
break;
case TSP_DIV:
results[0] /= service_args[0] ? service_args[0] : 1;
results[1] /= service_args[1] ? service_args[1] : 1;
break;
default:
break;
}
return set_smc_args(func, 0,
results[0],
results[1],
0, 0, 0, 0);
}