1 /* 2 * Test the framework itself. These tests confirm that setup works. 3 * 4 * Copyright (C) 2014, Red Hat Inc, Andrew Jones <drjones@redhat.com> 5 * 6 * This work is licensed under the terms of the GNU LGPL, version 2. 7 */ 8 #include <libcflat.h> 9 #include <util.h> 10 #include <devicetree.h> 11 #include <asm/setup.h> 12 #include <asm/ptrace.h> 13 #include <asm/asm-offsets.h> 14 #include <asm/processor.h> 15 #include <asm/thread_info.h> 16 #include <asm/psci.h> 17 #include <asm/smp.h> 18 #include <asm/barrier.h> 19 20 static cpumask_t ready, valid; 21 22 static void __user_psci_system_off(void) 23 { 24 psci_system_off(); 25 halt(); 26 __builtin_unreachable(); 27 } 28 29 static void check_setup(int argc, char **argv) 30 { 31 int nr_tests = 0, len, i; 32 long val; 33 34 for (i = 0; i < argc; ++i) { 35 36 len = parse_keyval(argv[i], &val); 37 if (len == -1) 38 continue; 39 40 argv[i][len] = '\0'; 41 report_prefix_push(argv[i]); 42 43 if (strcmp(argv[i], "mem") == 0) { 44 45 phys_addr_t memsize = PHYS_END - PHYS_OFFSET; 46 phys_addr_t expected = ((phys_addr_t)val)*1024*1024; 47 48 report("memory size matches expectation", 49 memsize == expected); 50 report_info("found %" PRIu64 " MB", memsize/1024/1024); 51 ++nr_tests; 52 53 } else if (strcmp(argv[i], "smp") == 0) { 54 55 report("number of CPUs matches expectation", 56 nr_cpus == (int)val); 57 report_info("found %d CPUs", nr_cpus); 58 ++nr_tests; 59 } 60 61 report_prefix_pop(); 62 } 63 64 if (nr_tests < 2) 65 report_abort("missing input"); 66 } 67 68 static struct pt_regs expected_regs; 69 static bool und_works; 70 static bool svc_works; 71 #if defined(__arm__) 72 /* 73 * Capture the current register state and execute an instruction 74 * that causes an exception. The test handler will check that its 75 * capture of the current register state matches the capture done 76 * here. 77 * 78 * NOTE: update clobber list if passed insns needs more than r0,r1 79 */ 80 #define test_exception(pre_insns, excptn_insn, post_insns) \ 81 asm volatile( \ 82 pre_insns "\n" \ 83 "mov r0, %0\n" \ 84 "stmia r0, { r0-lr }\n" \ 85 "mrs r1, cpsr\n" \ 86 "str r1, [r0, #" xstr(S_PSR) "]\n" \ 87 "mov r1, #-1\n" \ 88 "str r1, [r0, #" xstr(S_OLD_R0) "]\n" \ 89 "add r1, pc, #8\n" \ 90 "str r1, [r0, #" xstr(S_R1) "]\n" \ 91 "str r1, [r0, #" xstr(S_PC) "]\n" \ 92 excptn_insn "\n" \ 93 post_insns "\n" \ 94 :: "r" (&expected_regs) : "r0", "r1") 95 96 static bool check_regs(struct pt_regs *regs) 97 { 98 unsigned i; 99 100 /* exception handlers should always run in svc mode */ 101 if (current_mode() != SVC_MODE) 102 return false; 103 104 for (i = 0; i < ARRAY_SIZE(regs->uregs); ++i) { 105 if (regs->uregs[i] != expected_regs.uregs[i]) 106 return false; 107 } 108 109 return true; 110 } 111 112 static void und_handler(struct pt_regs *regs) 113 { 114 und_works = check_regs(regs); 115 } 116 117 static bool check_und(void) 118 { 119 install_exception_handler(EXCPTN_UND, und_handler); 120 121 /* issue an instruction to a coprocessor we don't have */ 122 test_exception("", "mcr p2, 0, r0, c0, c0", ""); 123 124 install_exception_handler(EXCPTN_UND, NULL); 125 126 return und_works; 127 } 128 129 static void svc_handler(struct pt_regs *regs) 130 { 131 u32 svc = *(u32 *)(regs->ARM_pc - 4) & 0xffffff; 132 133 if (processor_mode(regs) == SVC_MODE) { 134 /* 135 * When issuing an svc from supervisor mode lr_svc will 136 * get corrupted. So before issuing the svc, callers must 137 * always push it on the stack. We pushed it to offset 4. 138 */ 139 regs->ARM_lr = *(unsigned long *)(regs->ARM_sp + 4); 140 } 141 142 svc_works = check_regs(regs) && svc == 123; 143 } 144 145 static bool check_svc(void) 146 { 147 install_exception_handler(EXCPTN_SVC, svc_handler); 148 149 if (current_mode() == SVC_MODE) { 150 /* 151 * An svc from supervisor mode will corrupt lr_svc and 152 * spsr_svc. We need to save/restore them separately. 153 */ 154 test_exception( 155 "mrs r0, spsr\n" 156 "push { r0,lr }\n", 157 "svc #123\n", 158 "pop { r0,lr }\n" 159 "msr spsr_cxsf, r0\n" 160 ); 161 } else { 162 test_exception("", "svc #123", ""); 163 } 164 165 install_exception_handler(EXCPTN_SVC, NULL); 166 167 return svc_works; 168 } 169 170 static void user_psci_system_off(struct pt_regs *regs) 171 { 172 __user_psci_system_off(); 173 } 174 #elif defined(__aarch64__) 175 176 /* 177 * Capture the current register state and execute an instruction 178 * that causes an exception. The test handler will check that its 179 * capture of the current register state matches the capture done 180 * here. 181 * 182 * NOTE: update clobber list if passed insns needs more than x0,x1 183 */ 184 #define test_exception(pre_insns, excptn_insn, post_insns) \ 185 asm volatile( \ 186 pre_insns "\n" \ 187 "mov x1, %0\n" \ 188 "ldr x0, [x1, #" xstr(S_PSTATE) "]\n" \ 189 "mrs x1, nzcv\n" \ 190 "orr w0, w0, w1\n" \ 191 "mov x1, %0\n" \ 192 "str w0, [x1, #" xstr(S_PSTATE) "]\n" \ 193 "mov x0, sp\n" \ 194 "str x0, [x1, #" xstr(S_SP) "]\n" \ 195 "adr x0, 1f\n" \ 196 "str x0, [x1, #" xstr(S_PC) "]\n" \ 197 "stp x2, x3, [x1, #16]\n" \ 198 "stp x4, x5, [x1, #32]\n" \ 199 "stp x6, x7, [x1, #48]\n" \ 200 "stp x8, x9, [x1, #64]\n" \ 201 "stp x10, x11, [x1, #80]\n" \ 202 "stp x12, x13, [x1, #96]\n" \ 203 "stp x14, x15, [x1, #112]\n" \ 204 "stp x16, x17, [x1, #128]\n" \ 205 "stp x18, x19, [x1, #144]\n" \ 206 "stp x20, x21, [x1, #160]\n" \ 207 "stp x22, x23, [x1, #176]\n" \ 208 "stp x24, x25, [x1, #192]\n" \ 209 "stp x26, x27, [x1, #208]\n" \ 210 "stp x28, x29, [x1, #224]\n" \ 211 "str x30, [x1, #" xstr(S_LR) "]\n" \ 212 "stp x0, x1, [x1]\n" \ 213 "1:" excptn_insn "\n" \ 214 post_insns "\n" \ 215 :: "r" (&expected_regs) : "x0", "x1") 216 217 static bool check_regs(struct pt_regs *regs) 218 { 219 unsigned i; 220 221 /* exception handlers should always run in EL1 */ 222 if (current_level() != CurrentEL_EL1) 223 return false; 224 225 for (i = 0; i < ARRAY_SIZE(regs->regs); ++i) { 226 if (regs->regs[i] != expected_regs.regs[i]) 227 return false; 228 } 229 230 regs->pstate &= 0xf0000000 /* NZCV */ | 0x3c0 /* DAIF */ 231 | PSR_MODE_MASK; 232 233 return regs->sp == expected_regs.sp 234 && regs->pc == expected_regs.pc 235 && regs->pstate == expected_regs.pstate; 236 } 237 238 static enum vector check_vector_prep(void) 239 { 240 unsigned long daif; 241 242 if (is_user()) 243 return EL0_SYNC_64; 244 245 asm volatile("mrs %0, daif" : "=r" (daif) ::); 246 expected_regs.pstate = daif | PSR_MODE_EL1h; 247 return EL1H_SYNC; 248 } 249 250 static void unknown_handler(struct pt_regs *regs, unsigned int esr __unused) 251 { 252 und_works = check_regs(regs); 253 regs->pc += 4; 254 } 255 256 static bool check_und(void) 257 { 258 enum vector v = check_vector_prep(); 259 260 install_exception_handler(v, ESR_EL1_EC_UNKNOWN, unknown_handler); 261 262 /* try to read an el2 sysreg from el0/1 */ 263 test_exception("", "mrs x0, sctlr_el2", ""); 264 265 install_exception_handler(v, ESR_EL1_EC_UNKNOWN, NULL); 266 267 return und_works; 268 } 269 270 static void svc_handler(struct pt_regs *regs, unsigned int esr) 271 { 272 u16 svc = esr & 0xffff; 273 274 expected_regs.pc += 4; 275 svc_works = check_regs(regs) && svc == 123; 276 } 277 278 static bool check_svc(void) 279 { 280 enum vector v = check_vector_prep(); 281 282 install_exception_handler(v, ESR_EL1_EC_SVC64, svc_handler); 283 284 test_exception("", "svc #123", ""); 285 286 install_exception_handler(v, ESR_EL1_EC_SVC64, NULL); 287 288 return svc_works; 289 } 290 291 static void user_psci_system_off(struct pt_regs *regs, unsigned int esr) 292 { 293 __user_psci_system_off(); 294 } 295 #endif 296 297 static void check_vectors(void *arg __unused) 298 { 299 report("und", check_und()); 300 report("svc", check_svc()); 301 if (is_user()) { 302 #ifdef __arm__ 303 install_exception_handler(EXCPTN_UND, user_psci_system_off); 304 #else 305 install_exception_handler(EL0_SYNC_64, ESR_EL1_EC_UNKNOWN, 306 user_psci_system_off); 307 #endif 308 } 309 exit(report_summary()); 310 } 311 312 static bool psci_check(void) 313 { 314 const struct fdt_property *method; 315 int node, len, ver; 316 317 node = fdt_node_offset_by_compatible(dt_fdt(), -1, "arm,psci-0.2"); 318 if (node < 0) { 319 printf("PSCI v0.2 compatibility required\n"); 320 return false; 321 } 322 323 method = fdt_get_property(dt_fdt(), node, "method", &len); 324 if (method == NULL) { 325 printf("bad psci device tree node\n"); 326 return false; 327 } 328 329 if (len < 4 || strcmp(method->data, "hvc") != 0) { 330 printf("psci method must be hvc\n"); 331 return false; 332 } 333 334 ver = psci_invoke(PSCI_0_2_FN_PSCI_VERSION, 0, 0, 0); 335 printf("PSCI version %d.%d\n", PSCI_VERSION_MAJOR(ver), 336 PSCI_VERSION_MINOR(ver)); 337 338 return true; 339 } 340 341 static void cpu_report(void *data __unused) 342 { 343 uint64_t mpidr = get_mpidr(); 344 int cpu = smp_processor_id(); 345 346 if (mpidr_to_cpu(mpidr) == cpu) 347 cpumask_set_cpu(smp_processor_id(), &valid); 348 smp_wmb(); /* Paired with rmb in main(). */ 349 cpumask_set_cpu(smp_processor_id(), &ready); 350 report_info("CPU%3d: MPIDR=%010" PRIx64, cpu, mpidr); 351 } 352 353 int main(int argc, char **argv) 354 { 355 report_prefix_push("selftest"); 356 357 if (argc < 2) 358 report_abort("no test specified"); 359 360 report_prefix_push(argv[1]); 361 362 if (strcmp(argv[1], "setup") == 0) { 363 364 check_setup(argc-2, &argv[2]); 365 366 } else if (strcmp(argv[1], "vectors-kernel") == 0) { 367 368 check_vectors(NULL); 369 370 } else if (strcmp(argv[1], "vectors-user") == 0) { 371 372 start_usr(check_vectors, NULL, 373 (unsigned long)thread_stack_alloc()); 374 375 } else if (strcmp(argv[1], "smp") == 0) { 376 377 report("PSCI version", psci_check()); 378 on_cpus(cpu_report, NULL); 379 while (!cpumask_full(&ready)) 380 cpu_relax(); 381 smp_rmb(); /* Paired with wmb in cpu_report(). */ 382 report("MPIDR test on all CPUs", cpumask_full(&valid)); 383 report_info("%d CPUs reported back", nr_cpus); 384 385 } else { 386 printf("Unknown subtest\n"); 387 abort(); 388 } 389 390 return report_summary(); 391 } 392