blob: 9da2f9af991afecad2433205e316b79a72700d7a [file] [log] [blame]
Achin Gupta7c88f3f2014-02-18 18:09:12 +00001/*
Paul Beesley1fbc97b2019-01-11 18:26:51 +00002 * Copyright (c) 2013-2019, ARM Limited and Contributors. All rights reserved.
Achin Gupta7c88f3f2014-02-18 18:09:12 +00003 *
dp-armfa3cf0b2017-05-03 09:38:09 +01004 * SPDX-License-Identifier: BSD-3-Clause
Achin Gupta7c88f3f2014-02-18 18:09:12 +00005 */
6
Alexei Fedorovf41355c2019-09-13 14:11:59 +01007#include <assert.h>
Antonio Nino Diaze0f90632018-12-14 00:18:21 +00008
Alexei Fedorovf41355c2019-09-13 14:11:59 +01009#include <arch_features.h>
Antonio Nino Diaze0f90632018-12-14 00:18:21 +000010#include <arch_helpers.h>
11#include <bl32/tsp/tsp.h>
12#include <common/bl_common.h>
13#include <common/debug.h>
14#include <lib/spinlock.h>
15#include <plat/common/platform.h>
Alexei Fedorovf41355c2019-09-13 14:11:59 +010016#include <platform_def.h>
Dan Handley4fd2f5c2014-08-04 11:41:20 +010017#include <platform_tsp.h>
Antonio Nino Diaze0f90632018-12-14 00:18:21 +000018
Dan Handleye2c27f52014-08-01 17:58:27 +010019#include "tsp_private.h"
Achin Gupta7c88f3f2014-02-18 18:09:12 +000020
Vikram Kanigirid8c9d262014-05-16 18:48:12 +010021
22/*******************************************************************************
Achin Gupta7c88f3f2014-02-18 18:09:12 +000023 * Lock to control access to the console
24 ******************************************************************************/
25spinlock_t console_lock;
26
27/*******************************************************************************
28 * Per cpu data structure to populate parameters for an SMC in C code and use
29 * a pointer to this structure in assembler code to populate x0-x7
30 ******************************************************************************/
Dan Handleye2712bc2014-04-10 15:37:22 +010031static tsp_args_t tsp_smc_args[PLATFORM_CORE_COUNT];
Achin Gupta7c88f3f2014-02-18 18:09:12 +000032
33/*******************************************************************************
34 * Per cpu data structure to keep track of TSP activity
35 ******************************************************************************/
Achin Gupta76717892014-05-09 11:42:56 +010036work_statistics_t tsp_stats[PLATFORM_CORE_COUNT];
Achin Gupta7c88f3f2014-02-18 18:09:12 +000037
38/*******************************************************************************
Sandrine Bailleuxbdba5e52016-06-16 14:24:26 +010039 * The TSP memory footprint starts at address BL32_BASE and ends with the
40 * linker symbol __BL32_END__. Use these addresses to compute the TSP image
41 * size.
Vikram Kanigirid8c9d262014-05-16 18:48:12 +010042 ******************************************************************************/
Antonio Nino Diazde97ff32019-01-25 13:28:38 +000043#define BL32_TOTAL_LIMIT BL32_END
Sandrine Bailleuxbdba5e52016-06-16 14:24:26 +010044#define BL32_TOTAL_SIZE (BL32_TOTAL_LIMIT - (unsigned long) BL32_BASE)
Vikram Kanigirid8c9d262014-05-16 18:48:12 +010045
Dan Handleye2712bc2014-04-10 15:37:22 +010046static tsp_args_t *set_smc_args(uint64_t arg0,
Achin Gupta7c88f3f2014-02-18 18:09:12 +000047 uint64_t arg1,
48 uint64_t arg2,
49 uint64_t arg3,
50 uint64_t arg4,
51 uint64_t arg5,
52 uint64_t arg6,
53 uint64_t arg7)
54{
Achin Gupta7c88f3f2014-02-18 18:09:12 +000055 uint32_t linear_id;
Dan Handleye2712bc2014-04-10 15:37:22 +010056 tsp_args_t *pcpu_smc_args;
Achin Gupta7c88f3f2014-02-18 18:09:12 +000057
58 /*
59 * Return to Secure Monitor by raising an SMC. The results of the
60 * service are passed as an arguments to the SMC
61 */
Soby Mathewda43b662015-07-08 21:45:46 +010062 linear_id = plat_my_core_pos();
Achin Gupta7c88f3f2014-02-18 18:09:12 +000063 pcpu_smc_args = &tsp_smc_args[linear_id];
64 write_sp_arg(pcpu_smc_args, TSP_ARG0, arg0);
65 write_sp_arg(pcpu_smc_args, TSP_ARG1, arg1);
66 write_sp_arg(pcpu_smc_args, TSP_ARG2, arg2);
67 write_sp_arg(pcpu_smc_args, TSP_ARG3, arg3);
68 write_sp_arg(pcpu_smc_args, TSP_ARG4, arg4);
69 write_sp_arg(pcpu_smc_args, TSP_ARG5, arg5);
70 write_sp_arg(pcpu_smc_args, TSP_ARG6, arg6);
71 write_sp_arg(pcpu_smc_args, TSP_ARG7, arg7);
72
73 return pcpu_smc_args;
74}
75
76/*******************************************************************************
Antonio Nino Diaze61ece02019-02-26 11:41:03 +000077 * Setup function for TSP.
78 ******************************************************************************/
79void tsp_setup(void)
80{
81 /* Perform early platform-specific setup */
82 tsp_early_platform_setup();
83
Antonio Nino Diaze61ece02019-02-26 11:41:03 +000084 /* Perform late platform-specific setup */
85 tsp_plat_arch_setup();
Alexei Fedorovf41355c2019-09-13 14:11:59 +010086
87#if ENABLE_PAUTH
88 /*
89 * Assert that the ARMv8.3-PAuth registers are present or an access
90 * fault will be triggered when they are being saved or restored.
91 */
92 assert(is_armv8_3_pauth_present());
93#endif /* ENABLE_PAUTH */
Antonio Nino Diaze61ece02019-02-26 11:41:03 +000094}
95
96/*******************************************************************************
Achin Gupta7c88f3f2014-02-18 18:09:12 +000097 * TSP main entry point where it gets the opportunity to initialize its secure
98 * state/applications. Once the state is initialized, it must return to the
Andrew Thoelke891c4ca2014-05-20 21:43:27 +010099 * SPD with a pointer to the 'tsp_vector_table' jump table.
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000100 ******************************************************************************/
101uint64_t tsp_main(void)
102{
Dan Handley91b624e2014-07-29 17:14:00 +0100103 NOTICE("TSP: %s\n", version_string);
104 NOTICE("TSP: %s\n", build_message);
Sandrine Bailleuxbdba5e52016-06-16 14:24:26 +0100105 INFO("TSP: Total memory base : 0x%lx\n", (unsigned long) BL32_BASE);
106 INFO("TSP: Total memory size : 0x%lx bytes\n", BL32_TOTAL_SIZE);
Dan Handley91b624e2014-07-29 17:14:00 +0100107
Soby Mathewda43b662015-07-08 21:45:46 +0100108 uint32_t linear_id = plat_my_core_pos();
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000109
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000110 /* Initialize the platform */
Dan Handley4fd2f5c2014-08-04 11:41:20 +0100111 tsp_platform_setup();
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000112
113 /* Initialize secure/applications state here */
Achin Guptabbc33f22014-05-09 13:33:42 +0100114 tsp_generic_timer_start();
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000115
116 /* Update this cpu's statistics */
117 tsp_stats[linear_id].smc_count++;
118 tsp_stats[linear_id].eret_count++;
119 tsp_stats[linear_id].cpu_on_count++;
120
Dan Handley91b624e2014-07-29 17:14:00 +0100121#if LOG_LEVEL >= LOG_LEVEL_INFO
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000122 spin_lock(&console_lock);
Soby Mathewda43b662015-07-08 21:45:46 +0100123 INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu on requests\n",
124 read_mpidr(),
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000125 tsp_stats[linear_id].smc_count,
126 tsp_stats[linear_id].eret_count,
127 tsp_stats[linear_id].cpu_on_count);
128 spin_unlock(&console_lock);
Dan Handley91b624e2014-07-29 17:14:00 +0100129#endif
Andrew Thoelke891c4ca2014-05-20 21:43:27 +0100130 return (uint64_t) &tsp_vector_table;
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000131}
132
133/*******************************************************************************
134 * This function performs any remaining book keeping in the test secure payload
135 * after this cpu's architectural state has been setup in response to an earlier
136 * psci cpu_on request.
137 ******************************************************************************/
Dan Handleye2712bc2014-04-10 15:37:22 +0100138tsp_args_t *tsp_cpu_on_main(void)
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000139{
Soby Mathewda43b662015-07-08 21:45:46 +0100140 uint32_t linear_id = plat_my_core_pos();
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000141
Achin Guptabbc33f22014-05-09 13:33:42 +0100142 /* Initialize secure/applications state here */
143 tsp_generic_timer_start();
144
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000145 /* Update this cpu's statistics */
146 tsp_stats[linear_id].smc_count++;
147 tsp_stats[linear_id].eret_count++;
148 tsp_stats[linear_id].cpu_on_count++;
149
Dan Handley91b624e2014-07-29 17:14:00 +0100150#if LOG_LEVEL >= LOG_LEVEL_INFO
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000151 spin_lock(&console_lock);
Soby Mathewda43b662015-07-08 21:45:46 +0100152 INFO("TSP: cpu 0x%lx turned on\n", read_mpidr());
153 INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu on requests\n",
154 read_mpidr(),
Dan Handley91b624e2014-07-29 17:14:00 +0100155 tsp_stats[linear_id].smc_count,
156 tsp_stats[linear_id].eret_count,
157 tsp_stats[linear_id].cpu_on_count);
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000158 spin_unlock(&console_lock);
Dan Handley91b624e2014-07-29 17:14:00 +0100159#endif
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000160 /* Indicate to the SPD that we have completed turned ourselves on */
161 return set_smc_args(TSP_ON_DONE, 0, 0, 0, 0, 0, 0, 0);
162}
163
164/*******************************************************************************
165 * This function performs any remaining book keeping in the test secure payload
166 * before this cpu is turned off in response to a psci cpu_off request.
167 ******************************************************************************/
Dan Handleye2712bc2014-04-10 15:37:22 +0100168tsp_args_t *tsp_cpu_off_main(uint64_t arg0,
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000169 uint64_t arg1,
170 uint64_t arg2,
171 uint64_t arg3,
172 uint64_t arg4,
173 uint64_t arg5,
174 uint64_t arg6,
175 uint64_t arg7)
176{
Soby Mathewda43b662015-07-08 21:45:46 +0100177 uint32_t linear_id = plat_my_core_pos();
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000178
Achin Guptabbc33f22014-05-09 13:33:42 +0100179 /*
180 * This cpu is being turned off, so disable the timer to prevent the
181 * secure timer interrupt from interfering with power down. A pending
182 * interrupt will be lost but we do not care as we are turning off.
183 */
184 tsp_generic_timer_stop();
185
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000186 /* Update this cpu's statistics */
187 tsp_stats[linear_id].smc_count++;
188 tsp_stats[linear_id].eret_count++;
189 tsp_stats[linear_id].cpu_off_count++;
190
Dan Handley91b624e2014-07-29 17:14:00 +0100191#if LOG_LEVEL >= LOG_LEVEL_INFO
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000192 spin_lock(&console_lock);
Soby Mathewda43b662015-07-08 21:45:46 +0100193 INFO("TSP: cpu 0x%lx off request\n", read_mpidr());
194 INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu off requests\n",
195 read_mpidr(),
Dan Handley91b624e2014-07-29 17:14:00 +0100196 tsp_stats[linear_id].smc_count,
197 tsp_stats[linear_id].eret_count,
198 tsp_stats[linear_id].cpu_off_count);
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000199 spin_unlock(&console_lock);
Dan Handley91b624e2014-07-29 17:14:00 +0100200#endif
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000201
Achin Gupta607084e2014-02-09 18:24:19 +0000202 /* Indicate to the SPD that we have completed this request */
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000203 return set_smc_args(TSP_OFF_DONE, 0, 0, 0, 0, 0, 0, 0);
204}
205
206/*******************************************************************************
207 * This function performs any book keeping in the test secure payload before
208 * this cpu's architectural state is saved in response to an earlier psci
209 * cpu_suspend request.
210 ******************************************************************************/
Soby Mathewf5121572014-09-30 11:19:51 +0100211tsp_args_t *tsp_cpu_suspend_main(uint64_t arg0,
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000212 uint64_t arg1,
213 uint64_t arg2,
214 uint64_t arg3,
215 uint64_t arg4,
216 uint64_t arg5,
217 uint64_t arg6,
218 uint64_t arg7)
219{
Soby Mathewda43b662015-07-08 21:45:46 +0100220 uint32_t linear_id = plat_my_core_pos();
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000221
Achin Guptabbc33f22014-05-09 13:33:42 +0100222 /*
223 * Save the time context and disable it to prevent the secure timer
224 * interrupt from interfering with wakeup from the suspend state.
225 */
226 tsp_generic_timer_save();
227 tsp_generic_timer_stop();
228
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000229 /* Update this cpu's statistics */
230 tsp_stats[linear_id].smc_count++;
231 tsp_stats[linear_id].eret_count++;
232 tsp_stats[linear_id].cpu_suspend_count++;
233
Dan Handley91b624e2014-07-29 17:14:00 +0100234#if LOG_LEVEL >= LOG_LEVEL_INFO
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000235 spin_lock(&console_lock);
Sandrine Bailleux8723adf2015-02-05 15:42:31 +0000236 INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu suspend requests\n",
Soby Mathewda43b662015-07-08 21:45:46 +0100237 read_mpidr(),
Dan Handley91b624e2014-07-29 17:14:00 +0100238 tsp_stats[linear_id].smc_count,
239 tsp_stats[linear_id].eret_count,
240 tsp_stats[linear_id].cpu_suspend_count);
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000241 spin_unlock(&console_lock);
Dan Handley91b624e2014-07-29 17:14:00 +0100242#endif
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000243
Achin Gupta607084e2014-02-09 18:24:19 +0000244 /* Indicate to the SPD that we have completed this request */
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000245 return set_smc_args(TSP_SUSPEND_DONE, 0, 0, 0, 0, 0, 0, 0);
246}
247
248/*******************************************************************************
249 * This function performs any book keeping in the test secure payload after this
250 * cpu's architectural state has been restored after wakeup from an earlier psci
251 * cpu_suspend request.
252 ******************************************************************************/
Achin Gupta9a0ff9b2015-09-07 20:43:27 +0100253tsp_args_t *tsp_cpu_resume_main(uint64_t max_off_pwrlvl,
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000254 uint64_t arg1,
255 uint64_t arg2,
256 uint64_t arg3,
257 uint64_t arg4,
258 uint64_t arg5,
259 uint64_t arg6,
260 uint64_t arg7)
261{
Soby Mathewda43b662015-07-08 21:45:46 +0100262 uint32_t linear_id = plat_my_core_pos();
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000263
Achin Guptabbc33f22014-05-09 13:33:42 +0100264 /* Restore the generic timer context */
265 tsp_generic_timer_restore();
266
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000267 /* Update this cpu's statistics */
268 tsp_stats[linear_id].smc_count++;
269 tsp_stats[linear_id].eret_count++;
270 tsp_stats[linear_id].cpu_resume_count++;
271
Dan Handley91b624e2014-07-29 17:14:00 +0100272#if LOG_LEVEL >= LOG_LEVEL_INFO
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000273 spin_lock(&console_lock);
Masahiro Yamadae93a0f42018-02-02 15:09:36 +0900274 INFO("TSP: cpu 0x%lx resumed. maximum off power level %lld\n",
Achin Gupta9a0ff9b2015-09-07 20:43:27 +0100275 read_mpidr(), max_off_pwrlvl);
Manish Pandeyc4b47a22020-03-06 14:36:25 +0000276 INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu resume requests\n",
Soby Mathewda43b662015-07-08 21:45:46 +0100277 read_mpidr(),
Dan Handley91b624e2014-07-29 17:14:00 +0100278 tsp_stats[linear_id].smc_count,
279 tsp_stats[linear_id].eret_count,
Manish Pandeyc4b47a22020-03-06 14:36:25 +0000280 tsp_stats[linear_id].cpu_resume_count);
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000281 spin_unlock(&console_lock);
Dan Handley91b624e2014-07-29 17:14:00 +0100282#endif
Achin Gupta607084e2014-02-09 18:24:19 +0000283 /* Indicate to the SPD that we have completed this request */
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000284 return set_smc_args(TSP_RESUME_DONE, 0, 0, 0, 0, 0, 0, 0);
285}
286
287/*******************************************************************************
Juan Castillo4dc4a472014-08-12 11:17:06 +0100288 * This function performs any remaining bookkeeping in the test secure payload
289 * before the system is switched off (in response to a psci SYSTEM_OFF request)
290 ******************************************************************************/
291tsp_args_t *tsp_system_off_main(uint64_t arg0,
292 uint64_t arg1,
293 uint64_t arg2,
294 uint64_t arg3,
295 uint64_t arg4,
296 uint64_t arg5,
297 uint64_t arg6,
298 uint64_t arg7)
299{
Soby Mathewda43b662015-07-08 21:45:46 +0100300 uint32_t linear_id = plat_my_core_pos();
Juan Castillo4dc4a472014-08-12 11:17:06 +0100301
302 /* Update this cpu's statistics */
303 tsp_stats[linear_id].smc_count++;
304 tsp_stats[linear_id].eret_count++;
305
306#if LOG_LEVEL >= LOG_LEVEL_INFO
307 spin_lock(&console_lock);
Soby Mathewda43b662015-07-08 21:45:46 +0100308 INFO("TSP: cpu 0x%lx SYSTEM_OFF request\n", read_mpidr());
309 INFO("TSP: cpu 0x%lx: %d smcs, %d erets requests\n", read_mpidr(),
Juan Castillo4dc4a472014-08-12 11:17:06 +0100310 tsp_stats[linear_id].smc_count,
311 tsp_stats[linear_id].eret_count);
312 spin_unlock(&console_lock);
313#endif
314
315 /* Indicate to the SPD that we have completed this request */
316 return set_smc_args(TSP_SYSTEM_OFF_DONE, 0, 0, 0, 0, 0, 0, 0);
317}
318
319/*******************************************************************************
320 * This function performs any remaining bookkeeping in the test secure payload
321 * before the system is reset (in response to a psci SYSTEM_RESET request)
322 ******************************************************************************/
323tsp_args_t *tsp_system_reset_main(uint64_t arg0,
324 uint64_t arg1,
325 uint64_t arg2,
326 uint64_t arg3,
327 uint64_t arg4,
328 uint64_t arg5,
329 uint64_t arg6,
330 uint64_t arg7)
331{
Soby Mathewda43b662015-07-08 21:45:46 +0100332 uint32_t linear_id = plat_my_core_pos();
Juan Castillo4dc4a472014-08-12 11:17:06 +0100333
334 /* Update this cpu's statistics */
335 tsp_stats[linear_id].smc_count++;
336 tsp_stats[linear_id].eret_count++;
337
338#if LOG_LEVEL >= LOG_LEVEL_INFO
339 spin_lock(&console_lock);
Soby Mathewda43b662015-07-08 21:45:46 +0100340 INFO("TSP: cpu 0x%lx SYSTEM_RESET request\n", read_mpidr());
341 INFO("TSP: cpu 0x%lx: %d smcs, %d erets requests\n", read_mpidr(),
Juan Castillo4dc4a472014-08-12 11:17:06 +0100342 tsp_stats[linear_id].smc_count,
343 tsp_stats[linear_id].eret_count);
344 spin_unlock(&console_lock);
345#endif
346
347 /* Indicate to the SPD that we have completed this request */
348 return set_smc_args(TSP_SYSTEM_RESET_DONE, 0, 0, 0, 0, 0, 0, 0);
349}
350
351/*******************************************************************************
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000352 * TSP fast smc handler. The secure monitor jumps to this function by
353 * doing the ERET after populating X0-X7 registers. The arguments are received
354 * in the function arguments in order. Once the service is rendered, this
Soby Mathew9f71f702014-05-09 20:49:17 +0100355 * function returns to Secure Monitor by raising SMC.
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000356 ******************************************************************************/
Soby Mathew9f71f702014-05-09 20:49:17 +0100357tsp_args_t *tsp_smc_handler(uint64_t func,
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000358 uint64_t arg1,
359 uint64_t arg2,
360 uint64_t arg3,
361 uint64_t arg4,
362 uint64_t arg5,
363 uint64_t arg6,
364 uint64_t arg7)
365{
Achin Gupta916a2c12014-02-09 23:11:46 +0000366 uint64_t results[2];
367 uint64_t service_args[2];
Soby Mathewda43b662015-07-08 21:45:46 +0100368 uint32_t linear_id = plat_my_core_pos();
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000369
Achin Gupta916a2c12014-02-09 23:11:46 +0000370 /* Update this cpu's statistics */
371 tsp_stats[linear_id].smc_count++;
372 tsp_stats[linear_id].eret_count++;
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000373
Masahiro Yamadae93a0f42018-02-02 15:09:36 +0900374 INFO("TSP: cpu 0x%lx received %s smc 0x%llx\n", read_mpidr(),
David Cunado28f69ab2017-04-05 11:34:03 +0100375 ((func >> 31) & 1) == 1 ? "fast" : "yielding",
Dan Handley91b624e2014-07-29 17:14:00 +0100376 func);
Soby Mathewda43b662015-07-08 21:45:46 +0100377 INFO("TSP: cpu 0x%lx: %d smcs, %d erets\n", read_mpidr(),
Dan Handley91b624e2014-07-29 17:14:00 +0100378 tsp_stats[linear_id].smc_count,
379 tsp_stats[linear_id].eret_count);
Achin Gupta916a2c12014-02-09 23:11:46 +0000380
381 /* Render secure services and obtain results here */
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000382 results[0] = arg1;
383 results[1] = arg2;
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000384
385 /*
386 * Request a service back from dispatcher/secure monitor. This call
Paul Beesley1fbc97b2019-01-11 18:26:51 +0000387 * return and thereafter resume execution
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000388 */
389 tsp_get_magic(service_args);
390
Justin Chadwell1c7c13a2019-07-18 14:25:33 +0100391#if CTX_INCLUDE_MTE_REGS
392 /*
393 * Write a dummy value to an MTE register, to simulate usage in the
394 * secure world
395 */
396 write_gcr_el1(0x99);
397#endif
398
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000399 /* Determine the function to perform based on the function ID */
Soby Mathew9f71f702014-05-09 20:49:17 +0100400 switch (TSP_BARE_FID(func)) {
401 case TSP_ADD:
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000402 results[0] += service_args[0];
403 results[1] += service_args[1];
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000404 break;
Soby Mathew9f71f702014-05-09 20:49:17 +0100405 case TSP_SUB:
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000406 results[0] -= service_args[0];
407 results[1] -= service_args[1];
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000408 break;
Soby Mathew9f71f702014-05-09 20:49:17 +0100409 case TSP_MUL:
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000410 results[0] *= service_args[0];
411 results[1] *= service_args[1];
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000412 break;
Soby Mathew9f71f702014-05-09 20:49:17 +0100413 case TSP_DIV:
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000414 results[0] /= service_args[0] ? service_args[0] : 1;
415 results[1] /= service_args[1] ? service_args[1] : 1;
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000416 break;
417 default:
418 break;
419 }
420
Soby Mathew9f71f702014-05-09 20:49:17 +0100421 return set_smc_args(func, 0,
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000422 results[0],
423 results[1],
Soby Mathew9f71f702014-05-09 20:49:17 +0100424 0, 0, 0, 0);
Achin Gupta7c88f3f2014-02-18 18:09:12 +0000425}
426
Douglas Raillardf2129652016-11-24 15:43:19 +0000427/*******************************************************************************
Paul Beesley1fbc97b2019-01-11 18:26:51 +0000428 * TSP smc abort handler. This function is called when aborting a preempted
David Cunado28f69ab2017-04-05 11:34:03 +0100429 * yielding SMC request. It should cleanup all resources owned by the SMC
Douglas Raillardf2129652016-11-24 15:43:19 +0000430 * handler such as locks or dynamically allocated memory so following SMC
431 * request are executed in a clean environment.
432 ******************************************************************************/
433tsp_args_t *tsp_abort_smc_handler(uint64_t func,
434 uint64_t arg1,
435 uint64_t arg2,
436 uint64_t arg3,
437 uint64_t arg4,
438 uint64_t arg5,
439 uint64_t arg6,
440 uint64_t arg7)
441{
442 return set_smc_args(TSP_ABORT_DONE, 0, 0, 0, 0, 0, 0, 0);
443}