1 /*
2 * SPDX-License-Identifier: BSD-3-Clause
3 * SPDX-FileCopyrightText: Copyright TF-RMM Contributors.
4 */
5
6 #include <arch.h>
7 #include <arch_helpers.h>
8 #include <assert.h>
9 #include <debug.h>
10 #include <inject_exp.h>
11 #include <rec.h>
12
13 /*
14 * Calculate the address of the vector entry when an exception is inserted
15 * into the Realm.
16 *
17 * @vbar The base address of the vector table in the Realm.
18 * @spsr The Saved Program Status Register at EL2.
19 * @apply_sea_ease_offset Flag indicating whether we need to apply an offset
20 * to the vector entry to execute SERROR vector.
21 */
calc_vector_entry(unsigned long vbar,unsigned long spsr,bool apply_sea_ease_offset)22 static unsigned long calc_vector_entry(unsigned long vbar, unsigned long spsr,
23 bool apply_sea_ease_offset)
24 {
25 unsigned long offset;
26
27 if ((spsr & MASK(SPSR_EL2_MODE)) == SPSR_EL2_MODE_EL1h) {
28 offset = VBAR_CEL_SP_ELx_OFFSET;
29 } else if ((spsr & MASK(SPSR_EL2_MODE)) == SPSR_EL2_MODE_EL1t) {
30 offset = VBAR_CEL_SP_EL0_OFFSET;
31 } else if ((spsr & MASK(SPSR_EL2_MODE)) == SPSR_EL2_MODE_EL0t) {
32 if ((spsr & MASK(SPSR_EL2_nRW)) == SPSR_EL2_nRW_AARCH64) {
33 offset = VBAR_LEL_AA64_OFFSET;
34 } else {
35 offset = VBAR_LEL_AA32_OFFSET;
36 return vbar + offset;
37 }
38 } else {
39 assert(false);
40 offset = 0UL;
41 }
42
43 if (apply_sea_ease_offset) {
44 offset += VBAR_SERROR_OFFSET;
45 }
46
47 return vbar + offset;
48 }
49
50 /*
51 * Explicitly create all bits of SPSR to get PSTATE when an exception is
52 * injected into the Realm.
53 *
54 * The code is based on "Aarch64.exceptions.takeexception" described in
55 * DDI0602 revision 2023-06.
56 * "https://developer.arm.com/documentation/ddi0602/2023-06/Shared-Pseudocode/aarch64-exceptions-takeexception?lang=en"
57 *
58 * NOTE: This piece of code must be reviewed every release to ensure that
59 * we keep up with new ARCH features which introduces a new SPSR bit.
60 */
calc_spsr(unsigned long old_spsr,unsigned long old_sctlr)61 static unsigned long calc_spsr(unsigned long old_spsr,
62 unsigned long old_sctlr)
63 {
64 /* Set the mode */
65 unsigned long spsr = SPSR_EL2_MODE_EL1h;
66
67 /* Mask all exceptions, update DAIF bits */
68 spsr |= MASK(SPSR_EL2_DAIF);
69
70 /*
71 * BTYPE bits are cleared as FEAT_BTI is mandatory since
72 * v9.2 of the architecture.
73 */
74 spsr &= ~MASK(SPSR_EL2_BTYPE);
75
76 /*
77 * If SSBS is implemented, take the value from SCTLR.DSSBS.
78 * Otherwise, SPSR.SSBS = RES0.
79 */
80 if (is_feat_ssbs_present()) {
81 if ((old_sctlr & SCTLR_ELx_DSSBS_BIT) != 0UL) {
82 spsr |= SPSR_EL2_SSBS_BIT;
83 } else {
84 spsr &= ~SPSR_EL2_SSBS_BIT;
85 }
86 }
87
88 /*
89 * If FEAT_NMI is implemented, ALLINT = !(SCTLR.SPINTMASK).
90 * Otherwise, SPSR.ALLINT = RES0.
91 */
92 if (is_feat_nmi_present()) {
93 if ((old_sctlr & SCTLR_ELx_SPINTMASK_BIT) == 0UL) {
94 spsr |= SPSR_EL2_ALLINT_BIT;
95 } else {
96 spsr &= ~SPSR_EL2_ALLINT_BIT;
97 }
98 }
99
100 /* Clear PSTATE.IL bit explicitly */
101 spsr &= ~SPSR_EL2_IL_BIT;
102
103 /* Clear PSTATE.SS bit explicitly */
104 spsr &= ~SPSR_EL2_SS_BIT;
105
106 /*
107 * Update PSTATE.PAN bit.
108 * FEAT_PAN is mandatory from v8.1.
109 */
110 if ((old_sctlr & SCTLR_ELx_SPAN_BIT) == 0UL) {
111 spsr |= SPSR_EL2_PAN_BIT;
112 }
113
114 /*
115 * UAO bit is cleared as FEAT_UAO is mandatory since
116 * v9.2 of the architecture.
117 */
118 spsr &= ~SPSR_EL2_UAO_BIT;
119
120 /* DIT bit is unchanged */
121 spsr |= (old_spsr & SPSR_EL2_DIT_BIT);
122
123 /*
124 * If FEAT_MTE2 is implemented, mask tag faults by setting TCO bit.
125 * Otherwise, SPSR.TCO = RES0.
126 */
127 if (is_feat_mte2_present()) {
128 spsr |= SPSR_EL2_TCO_BIT;
129 }
130
131 /* NZCV bits are unchanged */
132 spsr |= (old_spsr & MASK(SPSR_EL2_NZCV_BITS));
133
134 /*
135 * If FEAT_EBEP is present, set PM bit.
136 * Otherwise, SPSR.PM = RES0.
137 */
138 if (is_feat_ebep_present()) {
139 spsr |= SPSR_EL2_PM_BIT;
140 }
141
142 /*
143 * If FEAT_SEBEP is present, explicitly clear PPEND bit.
144 * Otherwise, SPSR.PPEND = RES0.
145 */
146 if (is_feat_sebep_present()) {
147 spsr &= ~SPSR_EL2_PPEND_BIT;
148 }
149
150 /*
151 * If FEAT_GCS is present, update EXLOCK bit.
152 * Otherwise, SPSR.EXLOCK = RES0.
153 */
154 if (is_feat_gcs_present()) {
155 if ((read_gcscr_el12() & GCSCR_EXLOCK_EN_BIT) != 0UL) {
156 spsr |= SPSR_EL2_EXLOCK_BIT;
157 } else {
158 spsr &= ~SPSR_EL2_EXLOCK_BIT;
159 }
160 }
161
162 return spsr;
163 }
164
165 /*
166 * Calculate the content of the Realm's esr_el1 register when
167 * the Synchronous Instruction or Data Abort is injected into
168 * the Realm (EL1).
169 *
170 * The value is constructed from the @esr_el2 & @spsr_el2 that
171 * are captured when the exception from the Realm was taken to EL2.
172 *
173 * The fault status code (ESR_EL1.I/DFSC) is set to @fsc
174 */
calc_esr_idabort(unsigned long esr_el2,unsigned long spsr_el2,unsigned long fsc)175 static unsigned long calc_esr_idabort(unsigned long esr_el2,
176 unsigned long spsr_el2,
177 unsigned long fsc)
178 {
179 /*
180 * Copy esr_el2 into esr_el1 apart from the following fields:
181 * - The exception class (EC). Its value depends on whether the
182 * exception to EL2 was from either EL1 or EL0.
183 * - I/DFSC. It will be set to @fsc.
184 * - FnV. It will set to zero.
185 * - S1PTW. It will be set to zero.
186 */
187 unsigned long esr_el1 = esr_el2 & ~(MASK(ESR_EL2_EC) |
188 MASK(ESR_EL2_ABORT_FSC) |
189 ESR_EL2_ABORT_FNV_BIT |
190 ESR_EL2_ABORT_S1PTW_BIT);
191
192 unsigned long ec = esr_el2 & MASK(ESR_EL2_EC);
193
194 assert((ec == ESR_EL2_EC_INST_ABORT) || (ec == ESR_EL2_EC_DATA_ABORT));
195 if ((spsr_el2 & MASK(SPSR_EL2_MODE)) != SPSR_EL2_MODE_EL0t) {
196 ec += 1UL << ESR_EL2_EC_SHIFT;
197 }
198 esr_el1 |= ec;
199
200 /*
201 * Set the I/DFSC.
202 */
203 assert((fsc & ~MASK(ESR_EL2_ABORT_FSC)) == 0UL);
204 esr_el1 |= fsc;
205
206 /*
207 * Set the EA.
208 */
209 esr_el1 |= ESR_EL2_ABORT_EA_BIT;
210
211 return esr_el1;
212 }
213
214 /*
215 * Inject the Synchronous Instruction or Data Abort into the current REC.
216 * The I/DFSC field in the ESR_EL1 is set to @fsc
217 */
inject_sync_idabort(unsigned long fsc)218 void inject_sync_idabort(unsigned long fsc)
219 {
220 unsigned long esr_el2 = read_esr_el2();
221 unsigned long far_el2 = read_far_el2();
222 unsigned long elr_el2 = read_elr_el2();
223 unsigned long spsr_el2 = read_spsr_el2();
224 unsigned long vbar_el1 = read_vbar_el12();
225 unsigned long sctlr_el1 = read_sctlr_el12();
226
227 unsigned long esr_el1 = calc_esr_idabort(esr_el2, spsr_el2, fsc);
228 bool sctlr2_ease_bit = ((read_sctlr2_el12_if_present() &
229 SCTLR2_ELx_EASE_BIT) != 0UL);
230 unsigned long pc = calc_vector_entry(vbar_el1, spsr_el2,
231 sctlr2_ease_bit);
232 unsigned long spsr = calc_spsr(spsr_el2, sctlr_el1);
233
234 write_far_el12(far_el2);
235 write_elr_el12(elr_el2);
236 write_spsr_el12(spsr_el2);
237 write_esr_el12(esr_el1);
238 write_elr_el2(pc);
239 write_spsr_el2(spsr);
240
241 INFO("Inject Sync EA into current REC. FAR_EL2: 0x%lx, ELR_EL2: 0x%lx\n",
242 far_el2, elr_el2);
243 }
244
245 /*
246 * Inject the Synchronous Instruction or Data Abort into @rec.
247 * The I/DFSC field in the ESR_EL1 is set to @fsc
248 */
inject_sync_idabort_rec(struct rec * rec,unsigned long fsc)249 void inject_sync_idabort_rec(struct rec *rec, unsigned long fsc)
250 {
251 bool sctlr2_ease_bit = ((rec->sysregs.sctlr2_el1 &
252 SCTLR2_ELx_EASE_BIT) != 0UL);
253
254 rec->sysregs.far_el1 = rec->last_run_info.far;
255 rec->sysregs.elr_el1 = rec->pc;
256 rec->sysregs.spsr_el1 = rec->pstate;
257 rec->sysregs.esr_el1 = calc_esr_idabort(rec->last_run_info.esr,
258 rec->pstate, fsc);
259 rec->pc = calc_vector_entry(rec->sysregs.vbar_el1, rec->pstate,
260 sctlr2_ease_bit);
261 rec->pstate = calc_spsr(rec->pstate, rec->sysregs.sctlr_el1);
262
263 INFO("Inject Sync EA into REC 0x%lx FAR_EL1: 0x%lx, ELR_EL1: 0x%lx\n",
264 (unsigned long)rec, rec->sysregs.far_el1, rec->sysregs.elr_el1);
265 }
266
267 /*
268 * Inject the Undefined Synchronous Exception into the current REC.
269 */
realm_inject_undef_abort(void)270 void realm_inject_undef_abort(void)
271 {
272 unsigned long esr = MASK(ESR_EL2_IL) | ESR_EL2_EC_UNKNOWN;
273 unsigned long elr = read_elr_el2();
274 unsigned long spsr = read_spsr_el2();
275 unsigned long vbar = read_vbar_el12();
276 unsigned long sctlr = read_sctlr_el12();
277
278 unsigned long pc = calc_vector_entry(vbar, spsr, false);
279 unsigned long new_spsr = calc_spsr(spsr, sctlr);
280
281 write_elr_el12(elr);
282 write_spsr_el12(spsr);
283 write_esr_el12(esr);
284
285 write_elr_el2(pc);
286 write_spsr_el2(new_spsr);
287 }
288