1 #include <libcflat.h>
2 #include <migrate.h>
3 #include <errata.h>
4 #include <asm/setup.h>
5 #include <asm/processor.h>
6 #include <asm/delay.h>
7 #include <asm/smp.h>
8 #include <asm/barrier.h>
9 #include <asm/io.h>
10
11 #define MDSCR_KDE (1 << 13)
12 #define MDSCR_MDE (1 << 15)
13 #define MDSCR_SS (1 << 0)
14
15 #define DBGBCR_LEN8 (0xff << 5)
16 #define DBGBCR_EXEC (0x0 << 3)
17 #define DBGBCR_EL1 (0x1 << 1)
18 #define DBGBCR_E (0x1 << 0)
19
20 #define DBGWCR_LEN8 (0xff << 5)
21 #define DBGWCR_RD (0x1 << 3)
22 #define DBGWCR_WR (0x2 << 3)
23 #define DBGWCR_EL1 (0x1 << 1)
24 #define DBGWCR_E (0x1 << 0)
25
26 #define SPSR_D (1 << 9)
27 #define SPSR_SS (1 << 21)
28
29 #define ESR_EC_HW_BP_CURRENT 0x31
30 #define ESR_EC_SSTEP_CURRENT 0x33
31 #define ESR_EC_WP_CURRENT 0x35
32
33 #define ID_AA64DFR0_BRPS_SHIFT 12
34 #define ID_AA64DFR0_BRPS_MASK 0xf
35 #define ID_AA64DFR0_WRPS_SHIFT 20
36 #define ID_AA64DFR0_WRPS_MASK 0xf
37
38 static volatile uint64_t hw_bp_idx, hw_bp_addr[16];
39 static volatile uint64_t wp_idx, wp_data_addr[16];
40 static volatile uint64_t ss_addr[4], ss_idx;
41
hw_bp_handler(struct pt_regs * regs,unsigned int esr)42 static void hw_bp_handler(struct pt_regs *regs, unsigned int esr)
43 {
44 hw_bp_addr[hw_bp_idx++] = regs->pc;
45 regs->pstate |= SPSR_D;
46 }
47
wp_handler(struct pt_regs * regs,unsigned int esr)48 static void wp_handler(struct pt_regs *regs, unsigned int esr)
49 {
50 wp_data_addr[wp_idx++] = read_sysreg(far_el1);
51 regs->pstate |= SPSR_D;
52 }
53
ss_handler(struct pt_regs * regs,unsigned int esr)54 static void ss_handler(struct pt_regs *regs, unsigned int esr)
55 {
56 ss_addr[ss_idx++] = regs->pc;
57 regs->pstate |= SPSR_SS;
58 }
59
get_num_hw_bp(void)60 static int get_num_hw_bp(void)
61 {
62 uint64_t reg = read_sysreg(id_aa64dfr0_el1);
63 /* Number of breakpoints, minus 1 */
64 uint8_t brps = (reg >> ID_AA64DFR0_BRPS_SHIFT) & ID_AA64DFR0_BRPS_MASK;
65
66 return brps + 1;
67 }
68
get_num_wp(void)69 static int get_num_wp(void)
70 {
71 uint64_t reg = read_sysreg(id_aa64dfr0_el1);
72 /* Number of watchpoints, minus 1 */
73 uint8_t wrps = (reg >> ID_AA64DFR0_WRPS_SHIFT) & ID_AA64DFR0_WRPS_MASK;
74
75 return wrps + 1;
76 }
77
write_dbgbcr(int n,uint32_t bcr)78 static void write_dbgbcr(int n, uint32_t bcr)
79 {
80 switch (n) {
81 case 0:
82 write_sysreg(bcr, dbgbcr0_el1); break;
83 case 1:
84 write_sysreg(bcr, dbgbcr1_el1); break;
85 case 2:
86 write_sysreg(bcr, dbgbcr2_el1); break;
87 case 3:
88 write_sysreg(bcr, dbgbcr3_el1); break;
89 case 4:
90 write_sysreg(bcr, dbgbcr4_el1); break;
91 case 5:
92 write_sysreg(bcr, dbgbcr5_el1); break;
93 case 6:
94 write_sysreg(bcr, dbgbcr6_el1); break;
95 case 7:
96 write_sysreg(bcr, dbgbcr7_el1); break;
97 case 8:
98 write_sysreg(bcr, dbgbcr8_el1); break;
99 case 9:
100 write_sysreg(bcr, dbgbcr9_el1); break;
101 case 10:
102 write_sysreg(bcr, dbgbcr10_el1); break;
103 case 11:
104 write_sysreg(bcr, dbgbcr11_el1); break;
105 case 12:
106 write_sysreg(bcr, dbgbcr12_el1); break;
107 case 13:
108 write_sysreg(bcr, dbgbcr13_el1); break;
109 case 14:
110 write_sysreg(bcr, dbgbcr14_el1); break;
111 case 15:
112 write_sysreg(bcr, dbgbcr15_el1); break;
113 default:
114 report_abort("Invalid bcr");
115 }
116 }
117
write_dbgbvr(int n,uint64_t bvr)118 static void write_dbgbvr(int n, uint64_t bvr)
119 {
120 switch (n) {
121 case 0:
122 write_sysreg(bvr, dbgbvr0_el1); break;
123 case 1:
124 write_sysreg(bvr, dbgbvr1_el1); break;
125 case 2:
126 write_sysreg(bvr, dbgbvr2_el1); break;
127 case 3:
128 write_sysreg(bvr, dbgbvr3_el1); break;
129 case 4:
130 write_sysreg(bvr, dbgbvr4_el1); break;
131 case 5:
132 write_sysreg(bvr, dbgbvr5_el1); break;
133 case 6:
134 write_sysreg(bvr, dbgbvr6_el1); break;
135 case 7:
136 write_sysreg(bvr, dbgbvr7_el1); break;
137 case 8:
138 write_sysreg(bvr, dbgbvr8_el1); break;
139 case 9:
140 write_sysreg(bvr, dbgbvr9_el1); break;
141 case 10:
142 write_sysreg(bvr, dbgbvr10_el1); break;
143 case 11:
144 write_sysreg(bvr, dbgbvr11_el1); break;
145 case 12:
146 write_sysreg(bvr, dbgbvr12_el1); break;
147 case 13:
148 write_sysreg(bvr, dbgbvr13_el1); break;
149 case 14:
150 write_sysreg(bvr, dbgbvr14_el1); break;
151 case 15:
152 write_sysreg(bvr, dbgbvr15_el1); break;
153 default:
154 report_abort("invalid bvr");
155 }
156 }
157
write_dbgwcr(int n,uint32_t wcr)158 static void write_dbgwcr(int n, uint32_t wcr)
159 {
160 switch (n) {
161 case 0:
162 write_sysreg(wcr, dbgwcr0_el1); break;
163 case 1:
164 write_sysreg(wcr, dbgwcr1_el1); break;
165 case 2:
166 write_sysreg(wcr, dbgwcr2_el1); break;
167 case 3:
168 write_sysreg(wcr, dbgwcr3_el1); break;
169 case 4:
170 write_sysreg(wcr, dbgwcr4_el1); break;
171 case 5:
172 write_sysreg(wcr, dbgwcr5_el1); break;
173 case 6:
174 write_sysreg(wcr, dbgwcr6_el1); break;
175 case 7:
176 write_sysreg(wcr, dbgwcr7_el1); break;
177 case 8:
178 write_sysreg(wcr, dbgwcr8_el1); break;
179 case 9:
180 write_sysreg(wcr, dbgwcr9_el1); break;
181 case 10:
182 write_sysreg(wcr, dbgwcr10_el1); break;
183 case 11:
184 write_sysreg(wcr, dbgwcr11_el1); break;
185 case 12:
186 write_sysreg(wcr, dbgwcr12_el1); break;
187 case 13:
188 write_sysreg(wcr, dbgwcr13_el1); break;
189 case 14:
190 write_sysreg(wcr, dbgwcr14_el1); break;
191 case 15:
192 write_sysreg(wcr, dbgwcr15_el1); break;
193 default:
194 report_abort("Invalid wcr");
195 }
196 }
197
write_dbgwvr(int n,uint64_t wvr)198 static void write_dbgwvr(int n, uint64_t wvr)
199 {
200 switch (n) {
201 case 0:
202 write_sysreg(wvr, dbgwvr0_el1); break;
203 case 1:
204 write_sysreg(wvr, dbgwvr1_el1); break;
205 case 2:
206 write_sysreg(wvr, dbgwvr2_el1); break;
207 case 3:
208 write_sysreg(wvr, dbgwvr3_el1); break;
209 case 4:
210 write_sysreg(wvr, dbgwvr4_el1); break;
211 case 5:
212 write_sysreg(wvr, dbgwvr5_el1); break;
213 case 6:
214 write_sysreg(wvr, dbgwvr6_el1); break;
215 case 7:
216 write_sysreg(wvr, dbgwvr7_el1); break;
217 case 8:
218 write_sysreg(wvr, dbgwvr8_el1); break;
219 case 9:
220 write_sysreg(wvr, dbgwvr9_el1); break;
221 case 10:
222 write_sysreg(wvr, dbgwvr10_el1); break;
223 case 11:
224 write_sysreg(wvr, dbgwvr11_el1); break;
225 case 12:
226 write_sysreg(wvr, dbgwvr12_el1); break;
227 case 13:
228 write_sysreg(wvr, dbgwvr13_el1); break;
229 case 14:
230 write_sysreg(wvr, dbgwvr14_el1); break;
231 case 15:
232 write_sysreg(wvr, dbgwvr15_el1); break;
233 default:
234 report_abort("invalid wvr");
235 }
236 }
237
reset_debug_state(void)238 static void reset_debug_state(void)
239 {
240 int i, num_bp = get_num_hw_bp();
241 int num_wp = get_num_wp();
242
243 asm volatile("msr daifset, #8");
244
245 write_sysreg(0, osdlr_el1);
246 write_sysreg(0, oslar_el1);
247 isb();
248
249 write_sysreg(0, mdscr_el1);
250 for (i = 0; i < num_bp; i++) {
251 write_dbgbvr(i, 0);
252 write_dbgbcr(i, 0);
253 }
254 for (i = 0; i < num_wp; i++) {
255 write_dbgwvr(i, 0);
256 write_dbgwcr(i, 0);
257 }
258 isb();
259 }
260
test_hw_bp(bool migrate)261 static noinline void test_hw_bp(bool migrate)
262 {
263 extern unsigned char hw_bp0;
264 uint32_t bcr;
265 uint32_t mdscr;
266 uint64_t addr;
267 int num_bp = get_num_hw_bp();
268 int i;
269
270 install_exception_handler(EL1H_SYNC, ESR_EC_HW_BP_CURRENT, hw_bp_handler);
271
272 reset_debug_state();
273
274 bcr = DBGBCR_LEN8 | DBGBCR_EXEC | DBGBCR_EL1 | DBGBCR_E;
275 for (i = 0, addr = (uint64_t)&hw_bp0; i < num_bp; i++, addr += 4) {
276 write_dbgbcr(i, bcr);
277 write_dbgbvr(i, addr);
278 }
279 isb();
280
281 asm volatile("msr daifclr, #8");
282
283 mdscr = read_sysreg(mdscr_el1) | MDSCR_KDE | MDSCR_MDE;
284 write_sysreg(mdscr, mdscr_el1);
285 isb();
286
287 if (migrate) {
288 migrate_once();
289 report(num_bp == get_num_hw_bp(), "brps match after migrate");
290 }
291
292 hw_bp_idx = 0;
293
294 /* Trap on up to 16 debug exception unmask instructions. */
295 asm volatile(
296 ".globl hw_bp0\n"
297 "hw_bp0:\n"
298 "msr daifclr, #8; msr daifclr, #8; msr daifclr, #8; msr daifclr, #8\n"
299 "msr daifclr, #8; msr daifclr, #8; msr daifclr, #8; msr daifclr, #8\n"
300 "msr daifclr, #8; msr daifclr, #8; msr daifclr, #8; msr daifclr, #8\n"
301 "msr daifclr, #8; msr daifclr, #8; msr daifclr, #8; msr daifclr, #8\n"
302 );
303
304 for (i = 0, addr = (uint64_t)&hw_bp0; i < num_bp; i++, addr += 4)
305 report(hw_bp_addr[i] == addr, "hw breakpoint: %d", i);
306 }
307
308 static volatile char write_data[16];
309
test_wp(bool migrate)310 static noinline void test_wp(bool migrate)
311 {
312 uint32_t wcr;
313 uint32_t mdscr;
314 int num_wp = get_num_wp();
315 int i;
316
317 install_exception_handler(EL1H_SYNC, ESR_EC_WP_CURRENT, wp_handler);
318
319 reset_debug_state();
320
321 wcr = DBGWCR_LEN8 | DBGWCR_RD | DBGWCR_WR | DBGWCR_EL1 | DBGWCR_E;
322 for (i = 0; i < num_wp; i++) {
323 write_dbgwcr(i, wcr);
324 write_dbgwvr(i, (uint64_t)&write_data[i]);
325 }
326 isb();
327
328 asm volatile("msr daifclr, #8");
329
330 mdscr = read_sysreg(mdscr_el1) | MDSCR_KDE | MDSCR_MDE;
331 write_sysreg(mdscr, mdscr_el1);
332 isb();
333
334 if (migrate) {
335 migrate_once();
336 report(num_wp == get_num_wp(), "wrps match after migrate");
337 }
338
339 wp_idx = 0;
340
341 for (i = 0; i < num_wp; i++) {
342 write_data[i] = i;
343 asm volatile("msr daifclr, #8");
344 }
345
346 for (i = 0; i < num_wp; i++) {
347 report(wp_data_addr[i] == (uint64_t)&write_data[i],
348 "watchpoint received: %d", i);
349 report(write_data[i] == i, "watchpoint data: %d", i);
350 }
351 }
352
test_ss(bool migrate)353 static noinline void test_ss(bool migrate)
354 {
355 extern unsigned char ss_start;
356 uint32_t mdscr;
357
358 install_exception_handler(EL1H_SYNC, ESR_EC_SSTEP_CURRENT, ss_handler);
359
360 reset_debug_state();
361
362 ss_idx = 0;
363
364 mdscr = read_sysreg(mdscr_el1) | MDSCR_KDE | MDSCR_SS;
365 write_sysreg(mdscr, mdscr_el1);
366 isb();
367
368 if (migrate)
369 migrate_once();
370
371 asm volatile("msr daifclr, #8");
372
373 asm volatile(
374 ".globl ss_start\n"
375 "ss_start:\n"
376 "mrs x0, esr_el1\n"
377 "add x0, x0, #1\n"
378 "msr daifset, #8\n"
379 : : : "x0"
380 );
381
382 report(ss_addr[0] == (uint64_t)&ss_start, "single step");
383 }
384
main(int argc,char ** argv)385 int main(int argc, char **argv)
386 {
387 if (argc < 2)
388 report_abort("no test specified");
389
390 if (strcmp(argv[1], "bp") == 0) {
391 report_prefix_push(argv[1]);
392 test_hw_bp(false);
393 report_prefix_pop();
394 } else if (strcmp(argv[1], "bp-migration") == 0) {
395 report_prefix_push(argv[1]);
396 test_hw_bp(true);
397 report_prefix_pop();
398 } else if (strcmp(argv[1], "wp") == 0) {
399 report_prefix_push(argv[1]);
400 test_wp(false);
401 report_prefix_pop();
402 } else if (strcmp(argv[1], "wp-migration") == 0) {
403 report_prefix_push(argv[1]);
404 test_wp(true);
405 report_prefix_pop();
406 } else if (strcmp(argv[1], "ss") == 0) {
407 report_prefix_push(argv[1]);
408 test_ss(false);
409 report_prefix_pop();
410 } else if (strcmp(argv[1], "ss-migration") == 0) {
411 report_prefix_push(argv[1]);
412 test_ss(true);
413 report_prefix_pop();
414 } else {
415 report_abort("Unknown subtest '%s'", argv[1]);
416 }
417
418 return report_summary();
419 }
420