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
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
|
/*
* Copyright (c) 2016, Linaro Limited
* Copyright (c) 2014, STMicroelectronics International N.V.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. 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.
*
* 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 <asm.S>
#include <arm.h>
#include <arm32_macros.S>
#include <sm/optee_smc.h>
#include <sm/teesmc_opteed_macros.h>
#include <sm/teesmc_opteed.h>
#include <kernel/abort.h>
#include <kernel/thread_defs.h>
#include <kernel/unwind.h>
.section .text.thread_asm
LOCAL_FUNC vector_std_smc_entry , :
UNWIND( .fnstart)
UNWIND( .cantunwind)
push {r0-r7}
mov r0, sp
bl thread_handle_std_smc
/*
* Normally thread_handle_std_smc() should return via
* thread_exit(), thread_rpc(), but if thread_handle_std_smc()
* hasn't switched stack (error detected) it will do a normal "C"
* return.
*/
pop {r1-r8}
ldr r0, =TEESMC_OPTEED_RETURN_CALL_DONE
smc #0
b . /* SMC should not return */
UNWIND( .fnend)
END_FUNC vector_std_smc_entry
LOCAL_FUNC vector_fast_smc_entry , :
UNWIND( .fnstart)
UNWIND( .cantunwind)
push {r0-r7}
mov r0, sp
bl thread_handle_fast_smc
pop {r1-r8}
ldr r0, =TEESMC_OPTEED_RETURN_CALL_DONE
smc #0
b . /* SMC should not return */
UNWIND( .fnend)
END_FUNC vector_fast_smc_entry
LOCAL_FUNC vector_fiq_entry , :
UNWIND( .fnstart)
UNWIND( .cantunwind)
/* Secure Monitor received a FIQ and passed control to us. */
bl thread_check_canaries
ldr lr, =thread_nintr_handler_ptr
ldr lr, [lr]
blx lr
mov r1, r0
ldr r0, =TEESMC_OPTEED_RETURN_FIQ_DONE
smc #0
b . /* SMC should not return */
UNWIND( .fnend)
END_FUNC vector_fiq_entry
LOCAL_FUNC vector_cpu_on_entry , :
UNWIND( .fnstart)
UNWIND( .cantunwind)
ldr lr, =thread_cpu_on_handler_ptr
ldr lr, [lr]
blx lr
mov r1, r0
ldr r0, =TEESMC_OPTEED_RETURN_ON_DONE
smc #0
b . /* SMC should not return */
UNWIND( .fnend)
END_FUNC vector_cpu_on_entry
LOCAL_FUNC vector_cpu_off_entry , :
UNWIND( .fnstart)
UNWIND( .cantunwind)
ldr lr, =thread_cpu_off_handler_ptr
ldr lr, [lr]
blx lr
mov r1, r0
ldr r0, =TEESMC_OPTEED_RETURN_OFF_DONE
smc #0
b . /* SMC should not return */
UNWIND( .fnend)
END_FUNC vector_cpu_off_entry
LOCAL_FUNC vector_cpu_suspend_entry , :
UNWIND( .fnstart)
UNWIND( .cantunwind)
ldr lr, =thread_cpu_suspend_handler_ptr
ldr lr, [lr]
blx lr
mov r1, r0
ldr r0, =TEESMC_OPTEED_RETURN_SUSPEND_DONE
smc #0
b . /* SMC should not return */
UNWIND( .fnend)
END_FUNC vector_cpu_suspend_entry
LOCAL_FUNC vector_cpu_resume_entry , :
UNWIND( .fnstart)
UNWIND( .cantunwind)
ldr lr, =thread_cpu_resume_handler_ptr
ldr lr, [lr]
blx lr
mov r1, r0
ldr r0, =TEESMC_OPTEED_RETURN_RESUME_DONE
smc #0
b . /* SMC should not return */
UNWIND( .fnend)
END_FUNC vector_cpu_resume_entry
LOCAL_FUNC vector_system_off_entry , :
UNWIND( .fnstart)
UNWIND( .cantunwind)
ldr lr, =thread_system_off_handler_ptr
ldr lr, [lr]
blx lr
mov r1, r0
ldr r0, =TEESMC_OPTEED_RETURN_SYSTEM_OFF_DONE
smc #0
b . /* SMC should not return */
UNWIND( .fnend)
END_FUNC vector_system_off_entry
LOCAL_FUNC vector_system_reset_entry , :
UNWIND( .fnstart)
UNWIND( .cantunwind)
ldr lr, =thread_system_reset_handler_ptr
ldr lr, [lr]
blx lr
mov r1, r0
ldr r0, =TEESMC_OPTEED_RETURN_SYSTEM_RESET_DONE
smc #0
b . /* SMC should not return */
UNWIND( .fnend)
END_FUNC vector_system_reset_entry
/*
* Vector table supplied to ARM Trusted Firmware (ARM-TF) at
* initialization. Also used when compiled with the internal monitor, but
* the cpu_*_entry and system_*_entry are not used then.
*
* Note that ARM-TF depends on the layout of this vector table, any change
* in layout has to be synced with ARM-TF.
*/
FUNC thread_vector_table , :
UNWIND( .fnstart)
UNWIND( .cantunwind)
b vector_std_smc_entry
b vector_fast_smc_entry
b vector_cpu_on_entry
b vector_cpu_off_entry
b vector_cpu_resume_entry
b vector_cpu_suspend_entry
b vector_fiq_entry
b vector_system_off_entry
b vector_system_reset_entry
UNWIND( .fnend)
END_FUNC thread_vector_table
FUNC thread_set_abt_sp , :
UNWIND( .fnstart)
UNWIND( .cantunwind)
mrs r1, cpsr
cps #CPSR_MODE_ABT
mov sp, r0
msr cpsr, r1
bx lr
UNWIND( .fnend)
END_FUNC thread_set_abt_sp
FUNC thread_set_irq_sp , :
UNWIND( .fnstart)
UNWIND( .cantunwind)
mrs r1, cpsr
cps #CPSR_MODE_IRQ
mov sp, r0
msr cpsr, r1
bx lr
UNWIND( .fnend)
END_FUNC thread_set_irq_sp
FUNC thread_set_fiq_sp , :
UNWIND( .fnstart)
UNWIND( .cantunwind)
mrs r1, cpsr
cps #CPSR_MODE_FIQ
mov sp, r0
msr cpsr, r1
bx lr
UNWIND( .fnend)
END_FUNC thread_set_fiq_sp
/* void thread_resume(struct thread_ctx_regs *regs) */
FUNC thread_resume , :
UNWIND( .fnstart)
UNWIND( .cantunwind)
add r12, r0, #(13 * 4) /* Restore registers r0-r12 later */
cps #CPSR_MODE_SYS
ldm r12!, {sp, lr}
cps #CPSR_MODE_SVC
ldm r12!, {r1, sp, lr}
msr spsr_fsxc, r1
cps #CPSR_MODE_SVC
ldm r12, {r1, r2}
push {r1, r2}
ldm r0, {r0-r12}
/* Restore CPSR and jump to the instruction to resume at */
rfefd sp!
UNWIND( .fnend)
END_FUNC thread_resume
/*
* Disables IRQ and FIQ and saves state of thread, returns original
* CPSR.
*/
LOCAL_FUNC thread_save_state , :
UNWIND( .fnstart)
UNWIND( .cantunwind)
push {r12, lr}
/*
* Uses stack for temporary storage, while storing needed
* context in the thread context struct.
*/
mrs r12, cpsr
cpsid aif /* Disable Async abort, IRQ and FIQ */
push {r4-r7}
push {r0-r3}
mov r5, r12 /* Save CPSR in a preserved register */
mrs r6, cpsr /* Save current CPSR */
bl thread_get_ctx_regs
pop {r1-r4} /* r0-r3 pushed above */
stm r0!, {r1-r4}
pop {r1-r4} /* r4-r7 pushed above */
stm r0!, {r1-r4}
stm r0!, {r8-r11}
pop {r12, lr}
stm r0!, {r12}
cps #CPSR_MODE_SYS
stm r0!, {sp, lr}
cps #CPSR_MODE_SVC
mrs r1, spsr
stm r0!, {r1, sp, lr}
orr r6, r6, #ARM32_CPSR_FIA /* Disable Async abort, IRQ and FIQ */
msr cpsr, r6 /* Restore mode */
mov r0, r5 /* Return original CPSR */
bx lr
UNWIND( .fnend)
END_FUNC thread_save_state
FUNC thread_std_smc_entry , :
UNWIND( .fnstart)
UNWIND( .cantunwind)
/* Pass r0-r7 in a struct thread_smc_args */
push {r0-r7}
mov r0, sp
bl __thread_std_smc_entry
/*
* Load the returned r0-r3 into preserved registers and skip the
* "returned" r4-r7 since they will not be returned to normal
* world.
*/
pop {r4-r7}
add sp, #(4 * 4)
/* Disable interrupts before switching to temporary stack */
cpsid aif
bl thread_get_tmp_sp
mov sp, r0
bl thread_state_free
ldr r0, =TEESMC_OPTEED_RETURN_CALL_DONE
mov r1, r4
mov r2, r5
mov r3, r6
mov r4, r7
smc #0
b . /* SMC should not return */
UNWIND( .fnend)
END_FUNC thread_std_smc_entry
/* void thread_rpc(uint32_t rv[THREAD_RPC_NUM_ARGS]) */
FUNC thread_rpc , :
/*
* r0-r2 are used to pass parameters to normal world
* r0-r5 are used to pass return vaule back from normal world
*
* note that r3 is used to pass "resume information", that is, which
* thread it is that should resume.
*
* Since the this function is following AAPCS we need to preserve r4-r5
* which are otherwise modified when returning back from normal world.
*/
UNWIND( .fnstart)
push {r4-r5, lr}
UNWIND( .save {r4-r5, lr})
push {r0}
UNWIND( .save {r0})
bl thread_save_state
mov r4, r0 /* Save original CPSR */
/*
* Switch to temporary stack and SVC mode. Save CPSR to resume into.
*/
bl thread_get_tmp_sp
ldr r5, [sp] /* Get pointer to rv[] */
cps #CPSR_MODE_SVC /* Change to SVC mode */
mov sp, r0 /* Switch to tmp stack */
mov r0, #THREAD_FLAGS_COPY_ARGS_ON_RETURN
mov r1, r4 /* CPSR to restore */
ldr r2, =.thread_rpc_return
bl thread_state_suspend
mov r4, r0 /* Supply thread index */
ldr r0, =TEESMC_OPTEED_RETURN_CALL_DONE
ldm r5, {r1-r3} /* Load rv[] into r0-r2 */
smc #0
b . /* SMC should not return */
.thread_rpc_return:
/*
* At this point has the stack pointer been restored to the value
* it had when thread_save_state() was called 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.
*/
pop {r12} /* Get pointer to rv[] */
stm r12, {r0-r5} /* Store r0-r5 into rv[] */
pop {r4-r5, pc}
UNWIND( .fnend)
END_FUNC thread_rpc
LOCAL_FUNC thread_fiq_handler , :
UNWIND( .fnstart)
UNWIND( .cantunwind)
/* FIQ has a +4 offset for lr compared to preferred return address */
sub lr, lr, #4
/*
* We're saving {r0-r3} and the banked fiq registers {r8-r12}. The
* banked fiq registers need to be saved because the secure monitor
* doesn't save those. The treatment of the banked fiq registers is
* somewhat analogous to the lazy save of VFP registers.
*/
push {r0-r3, r8-r12, lr}
bl thread_check_canaries
ldr lr, =thread_nintr_handler_ptr
ldr lr, [lr]
blx lr
pop {r0-r3, r8-r12, lr}
movs pc, lr
UNWIND( .fnend)
END_FUNC thread_fiq_handler
LOCAL_FUNC thread_irq_handler , :
UNWIND( .fnstart)
UNWIND( .cantunwind)
/*
* IRQ mode is set up to use tmp stack so FIQ has to be
* disabled before touching the stack. We can also assign
* SVC sp from IRQ sp to get SVC mode into the state we
* need when doing the SMC below.
*/
cpsid f /* Disable FIQ also */
sub lr, lr, #4
push {lr}
push {r12}
bl thread_save_state
mov r0, #THREAD_FLAGS_EXIT_ON_FOREIGN_INTR
mrs r1, spsr
pop {r12}
pop {r2}
blx thread_state_suspend
mov r4, r0 /* Supply thread index */
/*
* Switch to SVC mode and copy current stack pointer as it already
* is the tmp stack.
*/
mov r0, sp
cps #CPSR_MODE_SVC
mov sp, r0
ldr r0, =TEESMC_OPTEED_RETURN_CALL_DONE
ldr r1, =OPTEE_SMC_RETURN_RPC_FOREIGN_INTR
mov r2, #0
mov r3, #0
/* r4 is already filled in above */
smc #0
b . /* SMC should not return */
UNWIND( .fnend)
END_FUNC thread_irq_handler
FUNC thread_init_vbar , :
UNWIND( .fnstart)
/* Set vector (VBAR) */
ldr r0, =thread_vect_table
write_vbar r0
bx lr
UNWIND( .fnend)
END_FUNC thread_init_vbar
/*
* Below are low level routines handling entry and return from user mode.
*
* thread_enter_user_mode() saves all that registers user mode can change
* so kernel mode can restore needed registers when resuming execution
* after the call to thread_enter_user_mode() has returned.
* thread_enter_user_mode() doesn't return directly since it enters user
* mode instead, it's thread_unwind_user_mode() that does the
* returning by restoring the registers saved by thread_enter_user_mode().
*
* There's three ways for thread_enter_user_mode() to return to caller,
* user TA calls utee_return, user TA calls utee_panic or through an abort.
*
* Calls to utee_return or utee_panic are handled as:
* thread_svc_handler() -> tee_svc_handler() -> tee_svc_do_call() which
* calls syscall_return() or syscall_panic().
*
* These function calls returns normally except thread_svc_handler() which
* which is an exception handling routine so it reads return address and
* SPSR to restore from the stack. syscall_return() and syscall_panic()
* changes return address and SPSR used by thread_svc_handler() to instead of
* returning into user mode as with other syscalls it returns into
* thread_unwind_user_mode() in kernel mode instead. When
* thread_svc_handler() returns the stack pointer at the point where
* thread_enter_user_mode() left it so this is where
* thread_unwind_user_mode() can operate.
*
* Aborts are handled in a similar way but by thread_abort_handler()
* instead, when the pager sees that it's an abort from user mode that
* can't be handled it updates SPSR and return address used by
* thread_abort_handler() to return into thread_unwind_user_mode()
* instead.
*/
/*
* uint32_t __thread_enter_user_mode(unsigned long a0, unsigned long a1,
* unsigned long a2, unsigned long a3, unsigned long user_sp,
* unsigned long user_func, unsigned long spsr,
* uint32_t *exit_status0, uint32_t *exit_status1)
*
*/
FUNC __thread_enter_user_mode , :
UNWIND( .fnstart)
UNWIND( .cantunwind)
/*
* Save all registers to allow syscall_return() to resume execution
* as if this function would have returned. This is also used in
* syscall_panic().
*
* If stack usage of this function is changed
* thread_unwind_user_mode() has to be updated.
*/
push {r4-r12,lr}
ldr r4, [sp, #(10 * 0x4)] /* user stack pointer */
ldr r5, [sp, #(11 * 0x4)] /* user function */
ldr r6, [sp, #(12 * 0x4)] /* spsr */
/*
* Set the saved Processors Status Register to user mode to allow
* entry of user mode through movs below.
*/
msr spsr_cxsf, r6
/*
* Save old user sp and set new user sp.
*/
cps #CPSR_MODE_SYS
mov r6, sp
mov sp, r4
cps #CPSR_MODE_SVC
push {r6,r7}
/*
* Don't allow return from this function, return is done through
* thread_unwind_user_mode() below.
*/
mov lr, #0
/* Call the user function with its arguments */
movs pc, r5
UNWIND( .fnend)
END_FUNC __thread_enter_user_mode
/*
* void thread_unwind_user_mode(uint32_t ret, uint32_t exit_status0,
* uint32_t exit_status1);
* See description in thread.h
*/
FUNC thread_unwind_user_mode , :
UNWIND( .fnstart)
UNWIND( .cantunwind)
ldr ip, [sp, #(15 * 0x4)] /* &ctx->panicked */
str r1, [ip]
ldr ip, [sp, #(16 * 0x4)] /* &ctx->panic_code */
str r2, [ip]
/* Restore old user sp */
pop {r4,r7}
cps #CPSR_MODE_SYS
mov sp, r4
cps #CPSR_MODE_SVC
pop {r4-r12,pc} /* Match the push in thread_enter_user_mode()*/
UNWIND( .fnend)
END_FUNC thread_unwind_user_mode
LOCAL_FUNC thread_abort_handler , :
thread_abort_handler:
thread_und_handler:
UNWIND( .fnstart)
UNWIND( .cantunwind)
/*
* Switch to abort mode to use that stack instead.
*/
cps #CPSR_MODE_ABT
push {r0-r11, ip}
cps #CPSR_MODE_UND
mrs r0, spsr
tst r0, #CPSR_T
subne r1, lr, #2
subeq r1, lr, #4
cps #CPSR_MODE_ABT
push {r0, r1}
msr spsr_fsxc, r0 /* In case some code reads spsr directly */
mov r0, #ABORT_TYPE_UNDEF
b .thread_abort_generic
thread_dabort_handler:
push {r0-r11, ip}
sub r1, lr, #8
mrs r0, spsr
push {r0, r1}
mov r0, #ABORT_TYPE_DATA
b .thread_abort_generic
thread_pabort_handler:
push {r0-r11, ip}
sub r1, lr, #4
mrs r0, spsr
push {r0, r1}
mov r0, #ABORT_TYPE_PREFETCH
b .thread_abort_generic
.thread_abort_generic:
cps #CPSR_MODE_SYS
mov r1, sp
mov r2, lr
cps #CPSR_MODE_ABT
push {r1-r3}
mov r1, sp
bl abort_handler
pop {r1-r3}
cps #CPSR_MODE_SYS
mov sp, r1
mov lr, r2
cps #CPSR_MODE_ABT
pop {r0, r1}
mov lr, r1
msr spsr_fsxc, r0
pop {r0-r11, ip}
movs pc, lr
UNWIND( .fnend)
END_FUNC thread_abort_handler
LOCAL_FUNC thread_svc_handler , :
UNWIND( .fnstart)
UNWIND( .cantunwind)
push {r0-r7, lr}
mrs r0, spsr
push {r0}
mov r0, sp
bl tee_svc_handler
pop {r0}
msr spsr_fsxc, r0
pop {r0-r7, lr}
movs pc, lr
UNWIND( .fnend)
END_FUNC thread_svc_handler
.align 5
LOCAL_FUNC thread_vect_table , :
UNWIND( .fnstart)
UNWIND( .cantunwind)
b . /* Reset */
b thread_und_handler /* Undefined instruction */
b thread_svc_handler /* System call */
b thread_pabort_handler /* Prefetch abort */
b thread_dabort_handler /* Data abort */
b . /* Reserved */
b thread_irq_handler /* IRQ */
b thread_fiq_handler /* FIQ */
UNWIND( .fnend)
END_FUNC thread_vect_table
|