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