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 <inject_exp.h>
10 #include <rec.h>
11
12 /*
13 * Calculate the address of the vector entry when an exception is inserted
14 * into the Realm.
15 *
16 * @vbar The base address of the vector table in the Realm.
17 * @spsr The Saved Program Status Register at EL2.
18 */
calc_vector_entry(unsigned long vbar,unsigned long spsr)19 static unsigned long calc_vector_entry(unsigned long vbar, unsigned long spsr)
20 {
21 unsigned long offset;
22
23 if ((spsr & MASK(SPSR_EL2_MODE)) == SPSR_EL2_MODE_EL1h) {
24 offset = VBAR_CEL_SP_ELx_OFFSET;
25 } else if ((spsr & MASK(SPSR_EL2_MODE)) == SPSR_EL2_MODE_EL1t) {
26 offset = VBAR_CEL_SP_EL0_OFFSET;
27 } else if ((spsr & MASK(SPSR_EL2_MODE)) == SPSR_EL2_MODE_EL0t) {
28 if ((spsr & MASK(SPSR_EL2_nRW)) == SPSR_EL2_nRW_AARCH64) {
29 offset = VBAR_LEL_AA64_OFFSET;
30 } else {
31 offset = VBAR_LEL_AA32_OFFSET;
32 }
33 } else {
34 assert(false);
35 offset = 0UL;
36 }
37
38 return vbar + offset;
39 }
40
41 /*
42 * Calculate the value of the pstate when an exception
43 * is inserted into the Realm.
44 */
calc_pstate(void)45 static unsigned long calc_pstate(void)
46 {
47 /*
48 * The pstate is EL1, AArch64, SPSel = SP_ELx and:
49 * DAIF = '1111b'
50 * NZCV = '0000b'
51 * TODO: setup TCO, DIT, UAO, PAN, SSBS, BTYPE
52 */
53 unsigned long pstate = SPSR_EL2_MODE_EL1h |
54 SPSR_EL2_nRW_AARCH64 |
55 SPSR_EL2_F_BIT |
56 SPSR_EL2_I_BIT |
57 SPSR_EL2_A_BIT |
58 SPSR_EL2_D_BIT;
59 return pstate;
60 }
61
62 /*
63 * Calculate the content of the Realm's esr_el1 register when
64 * the Synchronous Instruction or Data Abort is injected into
65 * the Realm (EL1).
66 *
67 * The value is constructed from the @esr_el2 & @spsr_el2 that
68 * are captured when the exception from the Realm was taken to EL2.
69 *
70 * The fault status code (ESR_EL1.I/DFSC) is set to @fsc
71 */
calc_esr_idabort(unsigned long esr_el2,unsigned long spsr_el2,unsigned long fsc)72 static unsigned long calc_esr_idabort(unsigned long esr_el2,
73 unsigned long spsr_el2,
74 unsigned long fsc)
75 {
76 /*
77 * Copy esr_el2 into esr_el1 apart from the following fields:
78 * - The exception class (EC). Its value depends on whether the
79 * exception to EL2 was from either EL1 or EL0.
80 * - I/DFSC. It will be set to @fsc.
81 * - FnV. It will set to zero.
82 * - S1PTW. It will be set to zero.
83 */
84 unsigned long esr_el1 = esr_el2 & ~(ESR_EL2_EC_MASK |
85 ESR_EL2_ABORT_FSC_MASK |
86 ESR_EL2_ABORT_FNV_BIT |
87 ESR_EL2_ABORT_S1PTW_BIT);
88
89 unsigned long ec = esr_el2 & ESR_EL2_EC_MASK;
90
91 assert((ec == ESR_EL2_EC_INST_ABORT) || (ec == ESR_EL2_EC_DATA_ABORT));
92 if ((spsr_el2 & MASK(SPSR_EL2_MODE)) != SPSR_EL2_MODE_EL0t) {
93 ec += 1UL << ESR_EL2_EC_SHIFT;
94 }
95 esr_el1 |= ec;
96
97 /*
98 * Set the I/DFSC.
99 */
100 assert((fsc & ~ESR_EL2_ABORT_FSC_MASK) == 0UL);
101 esr_el1 |= fsc;
102
103 /*
104 * Set the EA.
105 */
106 esr_el1 |= ESR_EL2_ABORT_EA_BIT;
107
108 return esr_el1;
109 }
110
111 /*
112 * Inject the Synchronous Instruction or Data Abort into the current REC.
113 * The I/DFSC field in the ESR_EL1 is set to @fsc
114 */
inject_sync_idabort(unsigned long fsc)115 void inject_sync_idabort(unsigned long fsc)
116 {
117 unsigned long esr_el2 = read_esr_el2();
118 unsigned long far_el2 = read_far_el2();
119 unsigned long elr_el2 = read_elr_el2();
120 unsigned long spsr_el2 = read_spsr_el2();
121 unsigned long vbar_el2 = read_vbar_el12();
122
123 unsigned long esr_el1 = calc_esr_idabort(esr_el2, spsr_el2, fsc);
124 unsigned long pc = calc_vector_entry(vbar_el2, spsr_el2);
125 unsigned long pstate = calc_pstate();
126
127 write_far_el12(far_el2);
128 write_elr_el12(elr_el2);
129 write_spsr_el12(spsr_el2);
130 write_esr_el12(esr_el1);
131 write_elr_el2(pc);
132 write_spsr_el2(pstate);
133 }
134
135 /*
136 * Inject the Synchronous Instruction or Data Abort into @rec.
137 * The I/DFSC field in the ESR_EL1 is set to @fsc
138 */
inject_sync_idabort_rec(struct rec * rec,unsigned long fsc)139 void inject_sync_idabort_rec(struct rec *rec, unsigned long fsc)
140 {
141 rec->sysregs.far_el1 = rec->last_run_info.far;
142 rec->sysregs.elr_el1 = rec->pc;
143 rec->sysregs.spsr_el1 = rec->pstate;
144 rec->sysregs.esr_el1 = calc_esr_idabort(rec->last_run_info.esr,
145 rec->pstate, fsc);
146 rec->pc = calc_vector_entry(rec->sysregs.vbar_el1, rec->pstate);
147 rec->pstate = calc_pstate();
148 }
149
150 /*
151 * Inject the Undefined Synchronous Exception into the current REC.
152 */
realm_inject_undef_abort(void)153 void realm_inject_undef_abort(void)
154 {
155 unsigned long esr = ESR_EL2_IL_MASK | ESR_EL2_EC_UNKNOWN;
156 unsigned long elr = read_elr_el2();
157 unsigned long spsr = read_spsr_el2();
158 unsigned long vbar = read_vbar_el12();
159
160 unsigned long pc = calc_vector_entry(vbar, spsr);
161 unsigned long pstate = calc_pstate();
162
163 write_elr_el12(elr);
164 write_spsr_el12(spsr);
165 write_esr_el12(esr);
166
167 write_elr_el2(pc);
168 write_spsr_el2(pstate);
169 }
170