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
|
/*
* Copyright (c) 2021-2022, Arm Limited. All rights reserved.
*
* SPDX-License-Identifier: BSD-3-Clause
*/
#include <asm_macros.S>
#include <services/rmmd_svc.h>
#include <platform_def.h>
#include "trp_private.h"
.global trp_head
.global trp_smc
.section ".head.text", "ax"
/* ---------------------------------------------
* Populate the params in x0-x7 from the pointer
* to the smc args structure in x0.
* ---------------------------------------------
*/
.macro restore_args_call_smc
ldp x6, x7, [x0, #TRP_ARG6]
ldp x4, x5, [x0, #TRP_ARG4]
ldp x2, x3, [x0, #TRP_ARG2]
ldp x0, x1, [x0, #TRP_ARG0]
smc #0
.endm
/* ---------------------------------------------
* Entry point for TRP
* ---------------------------------------------
*/
trp_head:
/*
* Stash arguments from previous boot stage
*/
mov x20, x0
mov x21, x1
mov x22, x2
mov x23, x3
/*
* Validate CPUId before allocating a stack.
*/
cmp x20, #PLATFORM_CORE_COUNT
b.lo 1f
mov_imm x0, RMM_BOOT_COMPLETE
mov_imm x1, E_RMM_BOOT_CPU_ID_OUT_OF_RANGE
smc #0
/* EL3 should never return back here, so panic if it does */
b trp_panic
1:
bl plat_set_my_stack
/*
* Find out whether this is a cold or warm boot
*/
ldr x1, cold_boot_flag
cbz x1, warm_boot
/*
* Update cold boot flag to indicate cold boot is done
*/
adr x2, cold_boot_flag
str xzr, [x2]
/* ---------------------------------------------
* Zero out BSS section
* ---------------------------------------------
*/
ldr x0, =__BSS_START__
ldr x1, =__BSS_SIZE__
bl zeromem
mov x0, x20
mov x1, x21
mov x2, x22
mov x3, x23
bl trp_setup
bl trp_main
b 1f
warm_boot:
mov x0, x20
mov x1, x21
mov x2, x22
mov x3, x23
bl trp_validate_warmboot_args
cbnz x0, trp_panic /* Failed to validate warmboot args */
1:
mov_imm x0, RMM_BOOT_COMPLETE
mov x1, xzr /* RMM_BOOT_SUCCESS */
smc #0
b trp_handler
trp_panic:
no_ret plat_panic_handler
/*
* Flag to mark if it is a cold boot.
* 1: cold boot, 0: warmboot.
*/
.align 3
cold_boot_flag:
.dword 1
/* ---------------------------------------------
* Direct SMC call to BL31 service provided by
* RMM Dispatcher
* ---------------------------------------------
*/
func trp_smc
restore_args_call_smc
ret
endfunc trp_smc
/* ---------------------------------------------
* RMI call handler
* ---------------------------------------------
*/
func trp_handler
/*
* Save Link Register and X4, as per SMCCC v1.2 its value
* must be preserved unless it contains result, as specified
* in the function definition.
*/
stp x4, lr, [sp, #-16]!
/*
* Zero the space for X0-X3 in trp_smc_result structure
* and pass its address as the last argument.
*/
stp xzr, xzr, [sp, #-16]!
stp xzr, xzr, [sp, #-16]!
mov x7, sp
bl trp_rmi_handler
ldp x1, x2, [sp], #16
ldp x3, x4, [sp], #16
ldp x5, lr, [sp], #16
ldr x0, =RMM_RMI_REQ_COMPLETE
smc #0
b trp_handler
endfunc trp_handler
|
