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 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 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 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 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 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 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 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 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 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 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 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("hw_bp0:\n" 296 "msr daifclr, #8; msr daifclr, #8; msr daifclr, #8; msr daifclr, #8\n" 297 "msr daifclr, #8; msr daifclr, #8; msr daifclr, #8; msr daifclr, #8\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 301 for (i = 0, addr = (uint64_t)&hw_bp0; i < num_bp; i++, addr += 4) 302 report(hw_bp_addr[i] == addr, "hw breakpoint: %d", i); 303 } 304 305 static volatile char write_data[16]; 306 307 static noinline void test_wp(bool migrate) 308 { 309 uint32_t wcr; 310 uint32_t mdscr; 311 int num_wp = get_num_wp(); 312 int i; 313 314 install_exception_handler(EL1H_SYNC, ESR_EC_WP_CURRENT, wp_handler); 315 316 reset_debug_state(); 317 318 wcr = DBGWCR_LEN8 | DBGWCR_RD | DBGWCR_WR | DBGWCR_EL1 | DBGWCR_E; 319 for (i = 0; i < num_wp; i++) { 320 write_dbgwcr(i, wcr); 321 write_dbgwvr(i, (uint64_t)&write_data[i]); 322 } 323 isb(); 324 325 asm volatile("msr daifclr, #8"); 326 327 mdscr = read_sysreg(mdscr_el1) | MDSCR_KDE | MDSCR_MDE; 328 write_sysreg(mdscr, mdscr_el1); 329 isb(); 330 331 if (migrate) { 332 migrate_once(); 333 report(num_wp == get_num_wp(), "wrps match after migrate"); 334 } 335 336 wp_idx = 0; 337 338 for (i = 0; i < num_wp; i++) { 339 write_data[i] = i; 340 asm volatile("msr daifclr, #8"); 341 } 342 343 for (i = 0; i < num_wp; i++) { 344 report(wp_data_addr[i] == (uint64_t)&write_data[i], 345 "watchpoint received: %d", i); 346 report(write_data[i] == i, "watchpoint data: %d", i); 347 } 348 } 349 350 static noinline void test_ss(bool migrate) 351 { 352 extern unsigned char ss_start; 353 uint32_t mdscr; 354 355 install_exception_handler(EL1H_SYNC, ESR_EC_SSTEP_CURRENT, ss_handler); 356 357 reset_debug_state(); 358 359 ss_idx = 0; 360 361 mdscr = read_sysreg(mdscr_el1) | MDSCR_KDE | MDSCR_SS; 362 write_sysreg(mdscr, mdscr_el1); 363 isb(); 364 365 if (migrate) 366 migrate_once(); 367 368 asm volatile("msr daifclr, #8"); 369 370 asm volatile("ss_start:\n" 371 "mrs x0, esr_el1\n" 372 "add x0, x0, #1\n" 373 "msr daifset, #8\n" 374 : : : "x0"); 375 376 report(ss_addr[0] == (uint64_t)&ss_start, "single step"); 377 } 378 379 int main(int argc, char **argv) 380 { 381 if (argc < 2) 382 report_abort("no test specified"); 383 384 if (strcmp(argv[1], "bp") == 0) { 385 report_prefix_push(argv[1]); 386 test_hw_bp(false); 387 report_prefix_pop(); 388 } else if (strcmp(argv[1], "bp-migration") == 0) { 389 report_prefix_push(argv[1]); 390 test_hw_bp(true); 391 report_prefix_pop(); 392 } else if (strcmp(argv[1], "wp") == 0) { 393 report_prefix_push(argv[1]); 394 test_wp(false); 395 report_prefix_pop(); 396 } else if (strcmp(argv[1], "wp-migration") == 0) { 397 report_prefix_push(argv[1]); 398 test_wp(true); 399 report_prefix_pop(); 400 } else if (strcmp(argv[1], "ss") == 0) { 401 report_prefix_push(argv[1]); 402 test_ss(false); 403 report_prefix_pop(); 404 } else if (strcmp(argv[1], "ss-migration") == 0) { 405 report_prefix_push(argv[1]); 406 test_ss(true); 407 report_prefix_pop(); 408 } else { 409 report_abort("Unknown subtest '%s'", argv[1]); 410 } 411 412 return report_summary(); 413 } 414