1 /*
2  * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions are met:
6  *
7  * Redistributions of source code must retain the above copyright notice, this
8  * list of conditions and the following disclaimer.
9  *
10  * Redistributions in binary form must reproduce the above copyright notice,
11  * this list of conditions and the following disclaimer in the documentation
12  * and/or other materials provided with the distribution.
13  *
14  * Neither the name of ARM nor the names of its contributors may be used
15  * to endorse or promote products derived from this software without specific
16  * prior written permission.
17  *
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
22  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28  * POSSIBILITY OF SUCH DAMAGE.
29  */
30 
31 
32 /*******************************************************************************
33  * This is the Secure Payload Dispatcher (SPD). The dispatcher is meant to be a
34  * plug-in component to the Secure Monitor, registered as a runtime service. The
35  * SPD is expected to be a functional extension of the Secure Payload (SP) that
36  * executes in Secure EL1. The Secure Monitor will delegate all SMCs targeting
37  * the Trusted OS/Applications range to the dispatcher. The SPD will either
38  * handle the request locally or delegate it to the Secure Payload. It is also
39  * responsible for initialising and maintaining communication with the SP.
40  ******************************************************************************/
41 #include <arch_helpers.h>
42 #include <assert.h>
43 #include <bl_common.h>
44 #include <bl31.h>
45 #include <context_mgmt.h>
46 #include <debug.h>
47 #include <errno.h>
48 #include <platform.h>
49 #include <runtime_svc.h>
50 #include <stddef.h>
51 #include <string.h>
52 #include <tsp.h>
53 #include <uuid.h>
54 #include "tspd_private.h"
55 
56 /*******************************************************************************
57  * Address of the entrypoint vector table in the Secure Payload. It is
58  * initialised once on the primary core after a cold boot.
59  ******************************************************************************/
60 tsp_vectors_t *tsp_vectors;
61 
62 /*******************************************************************************
63  * Array to keep track of per-cpu Secure Payload state
64  ******************************************************************************/
65 tsp_context_t tspd_sp_context[TSPD_CORE_COUNT];
66 
67 
68 /* TSP UID */
69 DEFINE_SVC_UUID(tsp_uuid,
70 		0x5b3056a0, 0x3291, 0x427b, 0x98, 0x11,
71 		0x71, 0x68, 0xca, 0x50, 0xf3, 0xfa);
72 
73 int32_t tspd_init(void);
74 
tspd_handle_sp_preemption(void * handle)75 uint64_t tspd_handle_sp_preemption(void *handle)
76 {
77 	cpu_context_t *ns_cpu_context;
78 	assert(handle == cm_get_context(SECURE));
79 	cm_el1_sysregs_context_save(SECURE);
80 	/* Get a reference to the non-secure context */
81 	ns_cpu_context = cm_get_context(NON_SECURE);
82 	assert(ns_cpu_context);
83 
84 	/*
85 	 * Restore non-secure state. The secure system
86 	 * register context will be saved when required.
87 	 */
88 	cm_el1_sysregs_context_restore(NON_SECURE);
89 	cm_set_next_eret_context(NON_SECURE);
90 
91 	SMC_RET1(ns_cpu_context, SMC_PREEMPTED);
92 }
93 /*******************************************************************************
94  * This function is the handler registered for S-EL1 interrupts by the TSPD. It
95  * validates the interrupt and upon success arranges entry into the TSP at
96  * 'tsp_fiq_entry()' for handling the interrupt.
97  ******************************************************************************/
tspd_sel1_interrupt_handler(uint32_t id,uint32_t flags,void * handle,void * cookie)98 static uint64_t tspd_sel1_interrupt_handler(uint32_t id,
99 					    uint32_t flags,
100 					    void *handle,
101 					    void *cookie)
102 {
103 	uint32_t linear_id;
104 	uint64_t mpidr;
105 	tsp_context_t *tsp_ctx;
106 
107 	/* Check the security state when the exception was generated */
108 	assert(get_interrupt_src_ss(flags) == NON_SECURE);
109 
110 #if IMF_READ_INTERRUPT_ID
111 	/* Check the security status of the interrupt */
112 	assert(plat_ic_get_interrupt_type(id) == INTR_TYPE_S_EL1);
113 #endif
114 
115 	/* Sanity check the pointer to this cpu's context */
116 	mpidr = read_mpidr();
117 	assert(handle == cm_get_context(NON_SECURE));
118 
119 	/* Save the non-secure context before entering the TSP */
120 	cm_el1_sysregs_context_save(NON_SECURE);
121 
122 	/* Get a reference to this cpu's TSP context */
123 	linear_id = platform_get_core_pos(mpidr);
124 	tsp_ctx = &tspd_sp_context[linear_id];
125 	assert(&tsp_ctx->cpu_ctx == cm_get_context(SECURE));
126 
127 	/*
128 	 * Determine if the TSP was previously preempted. Its last known
129 	 * context has to be preserved in this case.
130 	 * The TSP should return control to the TSPD after handling this
131 	 * FIQ. Preserve essential EL3 context to allow entry into the
132 	 * TSP at the FIQ entry point using the 'cpu_context' structure.
133 	 * There is no need to save the secure system register context
134 	 * since the TSP is supposed to preserve it during S-EL1 interrupt
135 	 * handling.
136 	 */
137 	if (get_std_smc_active_flag(tsp_ctx->state)) {
138 		tsp_ctx->saved_spsr_el3 = SMC_GET_EL3(&tsp_ctx->cpu_ctx,
139 						      CTX_SPSR_EL3);
140 		tsp_ctx->saved_elr_el3 = SMC_GET_EL3(&tsp_ctx->cpu_ctx,
141 						     CTX_ELR_EL3);
142 #if TSPD_ROUTE_IRQ_TO_EL3
143 		/*Need to save the previously interrupted secure context */
144 		memcpy(&tsp_ctx->sp_ctx, &tsp_ctx->cpu_ctx, TSPD_SP_CTX_SIZE);
145 #endif
146 	}
147 
148 	cm_el1_sysregs_context_restore(SECURE);
149 	cm_set_elr_spsr_el3(SECURE, (uint64_t) &tsp_vectors->fiq_entry,
150 		    SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS));
151 
152 	cm_set_next_eret_context(SECURE);
153 
154 	/*
155 	 * Tell the TSP that it has to handle an FIQ synchronously. Also the
156 	 * instruction in normal world where the interrupt was generated is
157 	 * passed for debugging purposes. It is safe to retrieve this address
158 	 * from ELR_EL3 as the secure context will not take effect until
159 	 * el3_exit().
160 	 */
161 	SMC_RET2(&tsp_ctx->cpu_ctx, TSP_HANDLE_FIQ_AND_RETURN, read_elr_el3());
162 }
163 
164 #if TSPD_ROUTE_IRQ_TO_EL3
165 /*******************************************************************************
166  * This function is the handler registered for S-EL1 interrupts by the TSPD. It
167  * validates the interrupt and upon success arranges entry into the TSP at
168  * 'tsp_fiq_entry()' for handling the interrupt.
169  ******************************************************************************/
tspd_ns_interrupt_handler(uint32_t id,uint32_t flags,void * handle,void * cookie)170 static uint64_t tspd_ns_interrupt_handler(uint32_t id,
171 					    uint32_t flags,
172 					    void *handle,
173 					    void *cookie)
174 {
175 	/* Check the security state when the exception was generated */
176 	assert(get_interrupt_src_ss(flags) == SECURE);
177 
178 #if IMF_READ_INTERRUPT_ID
179 	/* Check the security status of the interrupt */
180 	assert(plat_ic_get_interrupt_type(id) == INTR_TYPE_NS);
181 #endif
182 	/*
183 	 * Disable the routing of NS interrupts from secure world to EL3 while
184 	 * interrupted on this core.
185 	 */
186 	disable_intr_rm_local(INTR_TYPE_NS, SECURE);
187 
188 	return tspd_handle_sp_preemption(handle);
189 }
190 #endif
191 
192 /*******************************************************************************
193  * Secure Payload Dispatcher setup. The SPD finds out the SP entrypoint and type
194  * (aarch32/aarch64) if not already known and initialises the context for entry
195  * into the SP for its initialisation.
196  ******************************************************************************/
tspd_setup(void)197 int32_t tspd_setup(void)
198 {
199 	entry_point_info_t *tsp_ep_info;
200 	uint64_t mpidr = read_mpidr();
201 	uint32_t linear_id;
202 
203 	linear_id = platform_get_core_pos(mpidr);
204 
205 	/*
206 	 * Get information about the Secure Payload (BL32) image. Its
207 	 * absence is a critical failure.  TODO: Add support to
208 	 * conditionally include the SPD service
209 	 */
210 	tsp_ep_info = bl31_plat_get_next_image_ep_info(SECURE);
211 	if (!tsp_ep_info) {
212 		WARN("No TSP provided by BL2 boot loader, Booting device"
213 			" without TSP initialization. SMC`s destined for TSP"
214 			" will return SMC_UNK\n");
215 		return 1;
216 	}
217 
218 	/*
219 	 * If there's no valid entry point for SP, we return a non-zero value
220 	 * signalling failure initializing the service. We bail out without
221 	 * registering any handlers
222 	 */
223 	if (!tsp_ep_info->pc)
224 		return 1;
225 
226 	/*
227 	 * We could inspect the SP image and determine it's execution
228 	 * state i.e whether AArch32 or AArch64. Assuming it's AArch64
229 	 * for the time being.
230 	 */
231 	tspd_init_tsp_ep_state(tsp_ep_info,
232 				TSP_AARCH64,
233 				tsp_ep_info->pc,
234 				&tspd_sp_context[linear_id]);
235 
236 #if TSP_INIT_ASYNC
237 	bl31_set_next_image_type(SECURE);
238 #else
239 	/*
240 	 * All TSPD initialization done. Now register our init function with
241 	 * BL31 for deferred invocation
242 	 */
243 	bl31_register_bl32_init(&tspd_init);
244 #endif
245 	return 0;
246 }
247 
248 /*******************************************************************************
249  * This function passes control to the Secure Payload image (BL32) for the first
250  * time on the primary cpu after a cold boot. It assumes that a valid secure
251  * context has already been created by tspd_setup() which can be directly used.
252  * It also assumes that a valid non-secure context has been initialised by PSCI
253  * so it does not need to save and restore any non-secure state. This function
254  * performs a synchronous entry into the Secure payload. The SP passes control
255  * back to this routine through a SMC.
256  ******************************************************************************/
tspd_init(void)257 int32_t tspd_init(void)
258 {
259 	uint64_t mpidr = read_mpidr();
260 	uint32_t linear_id = platform_get_core_pos(mpidr);
261 	tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
262 	entry_point_info_t *tsp_entry_point;
263 	uint64_t rc;
264 
265 	/*
266 	 * Get information about the Secure Payload (BL32) image. Its
267 	 * absence is a critical failure.
268 	 */
269 	tsp_entry_point = bl31_plat_get_next_image_ep_info(SECURE);
270 	assert(tsp_entry_point);
271 
272 	cm_init_context(mpidr, tsp_entry_point);
273 
274 	/*
275 	 * Arrange for an entry into the test secure payload. It will be
276 	 * returned via TSP_ENTRY_DONE case
277 	 */
278 	rc = tspd_synchronous_sp_entry(tsp_ctx);
279 	assert(rc != 0);
280 
281 	return rc;
282 }
283 
284 
285 /*******************************************************************************
286  * This function is responsible for handling all SMCs in the Trusted OS/App
287  * range from the non-secure state as defined in the SMC Calling Convention
288  * Document. It is also responsible for communicating with the Secure payload
289  * to delegate work and return results back to the non-secure state. Lastly it
290  * will also return any information that the secure payload needs to do the
291  * work assigned to it.
292  ******************************************************************************/
tspd_smc_handler(uint32_t smc_fid,uint64_t x1,uint64_t x2,uint64_t x3,uint64_t x4,void * cookie,void * handle,uint64_t flags)293 uint64_t tspd_smc_handler(uint32_t smc_fid,
294 			 uint64_t x1,
295 			 uint64_t x2,
296 			 uint64_t x3,
297 			 uint64_t x4,
298 			 void *cookie,
299 			 void *handle,
300 			 uint64_t flags)
301 {
302 	cpu_context_t *ns_cpu_context;
303 	unsigned long mpidr = read_mpidr();
304 	uint32_t linear_id = platform_get_core_pos(mpidr), ns;
305 	tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
306 	uint64_t rc;
307 #if TSP_INIT_ASYNC
308 	entry_point_info_t *next_image_info;
309 #endif
310 
311 	/* Determine which security state this SMC originated from */
312 	ns = is_caller_non_secure(flags);
313 
314 	switch (smc_fid) {
315 
316 	/*
317 	 * This function ID is used by TSP to indicate that it was
318 	 * preempted by a normal world IRQ.
319 	 *
320 	 */
321 	case TSP_PREEMPTED:
322 		if (ns)
323 			SMC_RET1(handle, SMC_UNK);
324 
325 		return tspd_handle_sp_preemption(handle);
326 
327 	/*
328 	 * This function ID is used only by the TSP to indicate that it has
329 	 * finished handling a S-EL1 FIQ interrupt. Execution should resume
330 	 * in the normal world.
331 	 */
332 	case TSP_HANDLED_S_EL1_FIQ:
333 		if (ns)
334 			SMC_RET1(handle, SMC_UNK);
335 
336 		assert(handle == cm_get_context(SECURE));
337 
338 		/*
339 		 * Restore the relevant EL3 state which saved to service
340 		 * this SMC.
341 		 */
342 		if (get_std_smc_active_flag(tsp_ctx->state)) {
343 			SMC_SET_EL3(&tsp_ctx->cpu_ctx,
344 				    CTX_SPSR_EL3,
345 				    tsp_ctx->saved_spsr_el3);
346 			SMC_SET_EL3(&tsp_ctx->cpu_ctx,
347 				    CTX_ELR_EL3,
348 				    tsp_ctx->saved_elr_el3);
349 #if TSPD_ROUTE_IRQ_TO_EL3
350 			/*
351 			 * Need to restore the previously interrupted
352 			 * secure context.
353 			 */
354 			memcpy(&tsp_ctx->cpu_ctx, &tsp_ctx->sp_ctx,
355 				TSPD_SP_CTX_SIZE);
356 #endif
357 		}
358 
359 		/* Get a reference to the non-secure context */
360 		ns_cpu_context = cm_get_context(NON_SECURE);
361 		assert(ns_cpu_context);
362 
363 		/*
364 		 * Restore non-secure state. There is no need to save the
365 		 * secure system register context since the TSP was supposed
366 		 * to preserve it during S-EL1 interrupt handling.
367 		 */
368 		cm_el1_sysregs_context_restore(NON_SECURE);
369 		cm_set_next_eret_context(NON_SECURE);
370 
371 		SMC_RET0((uint64_t) ns_cpu_context);
372 
373 
374 	/*
375 	 * This function ID is used only by the TSP to indicate that it was
376 	 * interrupted due to a EL3 FIQ interrupt. Execution should resume
377 	 * in the normal world.
378 	 */
379 	case TSP_EL3_FIQ:
380 		if (ns)
381 			SMC_RET1(handle, SMC_UNK);
382 
383 		assert(handle == cm_get_context(SECURE));
384 
385 		/* Assert that standard SMC execution has been preempted */
386 		assert(get_std_smc_active_flag(tsp_ctx->state));
387 
388 		/* Save the secure system register state */
389 		cm_el1_sysregs_context_save(SECURE);
390 
391 		/* Get a reference to the non-secure context */
392 		ns_cpu_context = cm_get_context(NON_SECURE);
393 		assert(ns_cpu_context);
394 
395 		/* Restore non-secure state */
396 		cm_el1_sysregs_context_restore(NON_SECURE);
397 		cm_set_next_eret_context(NON_SECURE);
398 
399 		SMC_RET1(ns_cpu_context, TSP_EL3_FIQ);
400 
401 
402 	/*
403 	 * This function ID is used only by the SP to indicate it has
404 	 * finished initialising itself after a cold boot
405 	 */
406 	case TSP_ENTRY_DONE:
407 		if (ns)
408 			SMC_RET1(handle, SMC_UNK);
409 
410 		/*
411 		 * Stash the SP entry points information. This is done
412 		 * only once on the primary cpu
413 		 */
414 		assert(tsp_vectors == NULL);
415 		tsp_vectors = (tsp_vectors_t *) x1;
416 
417 		if (tsp_vectors) {
418 			set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_ON);
419 
420 			/*
421 			 * TSP has been successfully initialized. Register power
422 			 * managemnt hooks with PSCI
423 			 */
424 			psci_register_spd_pm_hook(&tspd_pm);
425 
426 			/*
427 			 * Register an interrupt handler for S-EL1 interrupts
428 			 * when generated during code executing in the
429 			 * non-secure state.
430 			 */
431 			flags = 0;
432 			set_interrupt_rm_flag(flags, NON_SECURE);
433 			rc = register_interrupt_type_handler(INTR_TYPE_S_EL1,
434 						tspd_sel1_interrupt_handler,
435 						flags);
436 			if (rc)
437 				panic();
438 
439 #if TSPD_ROUTE_IRQ_TO_EL3
440 			/*
441 			 * Register an interrupt handler for NS interrupts when
442 			 * generated during code executing in secure state are
443 			 * routed to EL3.
444 			 */
445 			flags = 0;
446 			set_interrupt_rm_flag(flags, SECURE);
447 
448 			rc = register_interrupt_type_handler(INTR_TYPE_NS,
449 						tspd_ns_interrupt_handler,
450 						flags);
451 			if (rc)
452 				panic();
453 
454 			/*
455 			 * Disable the interrupt NS locally since it will be enabled globally
456 			 * within cm_init_context.
457 			 */
458 			disable_intr_rm_local(INTR_TYPE_NS, SECURE);
459 #endif
460 		}
461 
462 
463 #if TSP_INIT_ASYNC
464 		/* Save the Secure EL1 system register context */
465 		assert(cm_get_context(SECURE) == &tsp_ctx->cpu_ctx);
466 		cm_el1_sysregs_context_save(SECURE);
467 
468 		/* Program EL3 registers to enable entry into the next EL */
469 		next_image_info = bl31_plat_get_next_image_ep_info(NON_SECURE);
470 		assert(next_image_info);
471 		assert(NON_SECURE ==
472 				GET_SECURITY_STATE(next_image_info->h.attr));
473 
474 		cm_init_context(read_mpidr_el1(), next_image_info);
475 		cm_prepare_el3_exit(NON_SECURE);
476 		SMC_RET0(cm_get_context(NON_SECURE));
477 #else
478 		/*
479 		 * SP reports completion. The SPD must have initiated
480 		 * the original request through a synchronous entry
481 		 * into the SP. Jump back to the original C runtime
482 		 * context.
483 		 */
484 		tspd_synchronous_sp_exit(tsp_ctx, x1);
485 #endif
486 
487 	/*
488 	 * These function IDs is used only by the SP to indicate it has
489 	 * finished:
490 	 * 1. turning itself on in response to an earlier psci
491 	 *    cpu_on request
492 	 * 2. resuming itself after an earlier psci cpu_suspend
493 	 *    request.
494 	 */
495 	case TSP_ON_DONE:
496 	case TSP_RESUME_DONE:
497 
498 	/*
499 	 * These function IDs is used only by the SP to indicate it has
500 	 * finished:
501 	 * 1. suspending itself after an earlier psci cpu_suspend
502 	 *    request.
503 	 * 2. turning itself off in response to an earlier psci
504 	 *    cpu_off request.
505 	 */
506 	case TSP_OFF_DONE:
507 	case TSP_SUSPEND_DONE:
508 	case TSP_SYSTEM_OFF_DONE:
509 	case TSP_SYSTEM_RESET_DONE:
510 		if (ns)
511 			SMC_RET1(handle, SMC_UNK);
512 
513 		/*
514 		 * SP reports completion. The SPD must have initiated the
515 		 * original request through a synchronous entry into the SP.
516 		 * Jump back to the original C runtime context, and pass x1 as
517 		 * return value to the caller
518 		 */
519 		tspd_synchronous_sp_exit(tsp_ctx, x1);
520 
521 		/*
522 		 * Request from non-secure client to perform an
523 		 * arithmetic operation or response from secure
524 		 * payload to an earlier request.
525 		 */
526 	case TSP_FAST_FID(TSP_ADD):
527 	case TSP_FAST_FID(TSP_SUB):
528 	case TSP_FAST_FID(TSP_MUL):
529 	case TSP_FAST_FID(TSP_DIV):
530 
531 	case TSP_STD_FID(TSP_ADD):
532 	case TSP_STD_FID(TSP_SUB):
533 	case TSP_STD_FID(TSP_MUL):
534 	case TSP_STD_FID(TSP_DIV):
535 		if (ns) {
536 			/*
537 			 * This is a fresh request from the non-secure client.
538 			 * The parameters are in x1 and x2. Figure out which
539 			 * registers need to be preserved, save the non-secure
540 			 * state and send the request to the secure payload.
541 			 */
542 			assert(handle == cm_get_context(NON_SECURE));
543 
544 			/* Check if we are already preempted */
545 			if (get_std_smc_active_flag(tsp_ctx->state))
546 				SMC_RET1(handle, SMC_UNK);
547 
548 			cm_el1_sysregs_context_save(NON_SECURE);
549 
550 			/* Save x1 and x2 for use by TSP_GET_ARGS call below */
551 			store_tsp_args(tsp_ctx, x1, x2);
552 
553 			/*
554 			 * We are done stashing the non-secure context. Ask the
555 			 * secure payload to do the work now.
556 			 */
557 
558 			/*
559 			 * Verify if there is a valid context to use, copy the
560 			 * operation type and parameters to the secure context
561 			 * and jump to the fast smc entry point in the secure
562 			 * payload. Entry into S-EL1 will take place upon exit
563 			 * from this function.
564 			 */
565 			assert(&tsp_ctx->cpu_ctx == cm_get_context(SECURE));
566 
567 			/* Set appropriate entry for SMC.
568 			 * We expect the TSP to manage the PSTATE.I and PSTATE.F
569 			 * flags as appropriate.
570 			 */
571 			if (GET_SMC_TYPE(smc_fid) == SMC_TYPE_FAST) {
572 				cm_set_elr_el3(SECURE, (uint64_t)
573 						&tsp_vectors->fast_smc_entry);
574 			} else {
575 				set_std_smc_active_flag(tsp_ctx->state);
576 				cm_set_elr_el3(SECURE, (uint64_t)
577 						&tsp_vectors->std_smc_entry);
578 #if TSPD_ROUTE_IRQ_TO_EL3
579 				/*
580 				 * Enable the routing of NS interrupts to EL3
581 				 * during STD SMC processing on this core.
582 				 */
583 				enable_intr_rm_local(INTR_TYPE_NS, SECURE);
584 #endif
585 			}
586 
587 			cm_el1_sysregs_context_restore(SECURE);
588 			cm_set_next_eret_context(SECURE);
589 			SMC_RET3(&tsp_ctx->cpu_ctx, smc_fid, x1, x2);
590 		} else {
591 			/*
592 			 * This is the result from the secure client of an
593 			 * earlier request. The results are in x1-x3. Copy it
594 			 * into the non-secure context, save the secure state
595 			 * and return to the non-secure state.
596 			 */
597 			assert(handle == cm_get_context(SECURE));
598 			cm_el1_sysregs_context_save(SECURE);
599 
600 			/* Get a reference to the non-secure context */
601 			ns_cpu_context = cm_get_context(NON_SECURE);
602 			assert(ns_cpu_context);
603 
604 			/* Restore non-secure state */
605 			cm_el1_sysregs_context_restore(NON_SECURE);
606 			cm_set_next_eret_context(NON_SECURE);
607 			if (GET_SMC_TYPE(smc_fid) == SMC_TYPE_STD) {
608 				clr_std_smc_active_flag(tsp_ctx->state);
609 #if TSPD_ROUTE_IRQ_TO_EL3
610 				/*
611 				 * Disable the routing of NS interrupts to EL3
612 				 * after STD SMC processing is finished on this
613 				 * core.
614 				 */
615 				disable_intr_rm_local(INTR_TYPE_NS, SECURE);
616 #endif
617 			}
618 
619 			SMC_RET3(ns_cpu_context, x1, x2, x3);
620 		}
621 
622 		break;
623 
624 		/*
625 		 * Request from non secure world to resume the preempted
626 		 * Standard SMC call.
627 		 */
628 	case TSP_FID_RESUME:
629 		/* RESUME should be invoked only by normal world */
630 		if (!ns) {
631 			assert(0);
632 			break;
633 		}
634 
635 		/*
636 		 * This is a resume request from the non-secure client.
637 		 * save the non-secure state and send the request to
638 		 * the secure payload.
639 		 */
640 		assert(handle == cm_get_context(NON_SECURE));
641 
642 		/* Check if we are already preempted before resume */
643 		if (!get_std_smc_active_flag(tsp_ctx->state))
644 			SMC_RET1(handle, SMC_UNK);
645 
646 		cm_el1_sysregs_context_save(NON_SECURE);
647 
648 		/*
649 		 * We are done stashing the non-secure context. Ask the
650 		 * secure payload to do the work now.
651 		 */
652 #if TSPD_ROUTE_IRQ_TO_EL3
653 		/*
654 		 * Enable the routing of NS interrupts to EL3 during resumption
655 		 * of STD SMC call on this core.
656 		 */
657 		enable_intr_rm_local(INTR_TYPE_NS, SECURE);
658 #endif
659 
660 
661 
662 		/* We just need to return to the preempted point in
663 		 * TSP and the execution will resume as normal.
664 		 */
665 		cm_el1_sysregs_context_restore(SECURE);
666 		cm_set_next_eret_context(SECURE);
667 		SMC_RET0(&tsp_ctx->cpu_ctx);
668 
669 		/*
670 		 * This is a request from the secure payload for more arguments
671 		 * for an ongoing arithmetic operation requested by the
672 		 * non-secure world. Simply return the arguments from the non-
673 		 * secure client in the original call.
674 		 */
675 	case TSP_GET_ARGS:
676 		if (ns)
677 			SMC_RET1(handle, SMC_UNK);
678 
679 		get_tsp_args(tsp_ctx, x1, x2);
680 		SMC_RET2(handle, x1, x2);
681 
682 	case TOS_CALL_COUNT:
683 		/*
684 		 * Return the number of service function IDs implemented to
685 		 * provide service to non-secure
686 		 */
687 		SMC_RET1(handle, TSP_NUM_FID);
688 
689 	case TOS_UID:
690 		/* Return TSP UID to the caller */
691 		SMC_UUID_RET(handle, tsp_uuid);
692 
693 	case TOS_CALL_VERSION:
694 		/* Return the version of current implementation */
695 		SMC_RET2(handle, TSP_VERSION_MAJOR, TSP_VERSION_MINOR);
696 
697 	default:
698 		break;
699 	}
700 
701 	SMC_RET1(handle, SMC_UNK);
702 }
703 
704 /* Define a SPD runtime service descriptor for fast SMC calls */
705 DECLARE_RT_SVC(
706 	tspd_fast,
707 
708 	OEN_TOS_START,
709 	OEN_TOS_END,
710 	SMC_TYPE_FAST,
711 	tspd_setup,
712 	tspd_smc_handler
713 );
714 
715 /* Define a SPD runtime service descriptor for standard SMC calls */
716 DECLARE_RT_SVC(
717 	tspd_std,
718 
719 	OEN_TOS_START,
720 	OEN_TOS_END,
721 	SMC_TYPE_STD,
722 	NULL,
723 	tspd_smc_handler
724 );
725