1 /* 2 * Copyright (C) 2015 Google, Inc 3 * 4 * SPDX-License-Identifier: GPL-2.0+ 5 * 6 * Based on code from the coreboot file of the same name 7 */ 8 9 #include <common.h> 10 #include <cpu.h> 11 #include <dm.h> 12 #include <errno.h> 13 #include <malloc.h> 14 #include <asm/atomic.h> 15 #include <asm/cpu.h> 16 #include <asm/interrupt.h> 17 #include <asm/lapic.h> 18 #include <asm/mp.h> 19 #include <asm/msr.h> 20 #include <asm/mtrr.h> 21 #include <asm/processor.h> 22 #include <asm/sipi.h> 23 #include <asm/fw_cfg.h> 24 #include <dm/device-internal.h> 25 #include <dm/uclass-internal.h> 26 #include <dm/lists.h> 27 #include <dm/root.h> 28 #include <linux/linkage.h> 29 30 DECLARE_GLOBAL_DATA_PTR; 31 32 /* Total CPUs include BSP */ 33 static int num_cpus; 34 35 /* This also needs to match the sipi.S assembly code for saved MSR encoding */ 36 struct saved_msr { 37 uint32_t index; 38 uint32_t lo; 39 uint32_t hi; 40 } __packed; 41 42 43 struct mp_flight_plan { 44 int num_records; 45 struct mp_flight_record *records; 46 }; 47 48 static struct mp_flight_plan mp_info; 49 50 struct cpu_map { 51 struct udevice *dev; 52 int apic_id; 53 int err_code; 54 }; 55 56 static inline void barrier_wait(atomic_t *b) 57 { 58 while (atomic_read(b) == 0) 59 asm("pause"); 60 mfence(); 61 } 62 63 static inline void release_barrier(atomic_t *b) 64 { 65 mfence(); 66 atomic_set(b, 1); 67 } 68 69 static inline void stop_this_cpu(void) 70 { 71 /* Called by an AP when it is ready to halt and wait for a new task */ 72 for (;;) 73 cpu_hlt(); 74 } 75 76 /* Returns 1 if timeout waiting for APs. 0 if target APs found */ 77 static int wait_for_aps(atomic_t *val, int target, int total_delay, 78 int delay_step) 79 { 80 int timeout = 0; 81 int delayed = 0; 82 83 while (atomic_read(val) != target) { 84 udelay(delay_step); 85 delayed += delay_step; 86 if (delayed >= total_delay) { 87 timeout = 1; 88 break; 89 } 90 } 91 92 return timeout; 93 } 94 95 static void ap_do_flight_plan(struct udevice *cpu) 96 { 97 int i; 98 99 for (i = 0; i < mp_info.num_records; i++) { 100 struct mp_flight_record *rec = &mp_info.records[i]; 101 102 atomic_inc(&rec->cpus_entered); 103 barrier_wait(&rec->barrier); 104 105 if (rec->ap_call != NULL) 106 rec->ap_call(cpu, rec->ap_arg); 107 } 108 } 109 110 static int find_cpu_by_apic_id(int apic_id, struct udevice **devp) 111 { 112 struct udevice *dev; 113 114 *devp = NULL; 115 for (uclass_find_first_device(UCLASS_CPU, &dev); 116 dev; 117 uclass_find_next_device(&dev)) { 118 struct cpu_platdata *plat = dev_get_parent_platdata(dev); 119 120 if (plat->cpu_id == apic_id) { 121 *devp = dev; 122 return 0; 123 } 124 } 125 126 return -ENOENT; 127 } 128 129 /* 130 * By the time APs call ap_init() caching has been setup, and microcode has 131 * been loaded 132 */ 133 static void ap_init(unsigned int cpu_index) 134 { 135 struct udevice *dev; 136 int apic_id; 137 int ret; 138 139 /* Ensure the local apic is enabled */ 140 enable_lapic(); 141 142 apic_id = lapicid(); 143 ret = find_cpu_by_apic_id(apic_id, &dev); 144 if (ret) { 145 debug("Unknown CPU apic_id %x\n", apic_id); 146 goto done; 147 } 148 149 debug("AP: slot %d apic_id %x, dev %s\n", cpu_index, apic_id, 150 dev ? dev->name : "(apic_id not found)"); 151 152 /* Walk the flight plan */ 153 ap_do_flight_plan(dev); 154 155 /* Park the AP */ 156 debug("parking\n"); 157 done: 158 stop_this_cpu(); 159 } 160 161 static const unsigned int fixed_mtrrs[NUM_FIXED_MTRRS] = { 162 MTRR_FIX_64K_00000_MSR, MTRR_FIX_16K_80000_MSR, MTRR_FIX_16K_A0000_MSR, 163 MTRR_FIX_4K_C0000_MSR, MTRR_FIX_4K_C8000_MSR, MTRR_FIX_4K_D0000_MSR, 164 MTRR_FIX_4K_D8000_MSR, MTRR_FIX_4K_E0000_MSR, MTRR_FIX_4K_E8000_MSR, 165 MTRR_FIX_4K_F0000_MSR, MTRR_FIX_4K_F8000_MSR, 166 }; 167 168 static inline struct saved_msr *save_msr(int index, struct saved_msr *entry) 169 { 170 msr_t msr; 171 172 msr = msr_read(index); 173 entry->index = index; 174 entry->lo = msr.lo; 175 entry->hi = msr.hi; 176 177 /* Return the next entry */ 178 entry++; 179 return entry; 180 } 181 182 static int save_bsp_msrs(char *start, int size) 183 { 184 int msr_count; 185 int num_var_mtrrs; 186 struct saved_msr *msr_entry; 187 int i; 188 msr_t msr; 189 190 /* Determine number of MTRRs need to be saved */ 191 msr = msr_read(MTRR_CAP_MSR); 192 num_var_mtrrs = msr.lo & 0xff; 193 194 /* 2 * num_var_mtrrs for base and mask. +1 for IA32_MTRR_DEF_TYPE */ 195 msr_count = 2 * num_var_mtrrs + NUM_FIXED_MTRRS + 1; 196 197 if ((msr_count * sizeof(struct saved_msr)) > size) { 198 printf("Cannot mirror all %d msrs\n", msr_count); 199 return -ENOSPC; 200 } 201 202 msr_entry = (void *)start; 203 for (i = 0; i < NUM_FIXED_MTRRS; i++) 204 msr_entry = save_msr(fixed_mtrrs[i], msr_entry); 205 206 for (i = 0; i < num_var_mtrrs; i++) { 207 msr_entry = save_msr(MTRR_PHYS_BASE_MSR(i), msr_entry); 208 msr_entry = save_msr(MTRR_PHYS_MASK_MSR(i), msr_entry); 209 } 210 211 msr_entry = save_msr(MTRR_DEF_TYPE_MSR, msr_entry); 212 213 return msr_count; 214 } 215 216 static int load_sipi_vector(atomic_t **ap_countp, int num_cpus) 217 { 218 struct sipi_params_16bit *params16; 219 struct sipi_params *params; 220 static char msr_save[512]; 221 char *stack; 222 ulong addr; 223 int code_len; 224 int size; 225 int ret; 226 227 /* Copy in the code */ 228 code_len = ap_start16_code_end - ap_start16; 229 debug("Copying SIPI code to %x: %d bytes\n", AP_DEFAULT_BASE, 230 code_len); 231 memcpy((void *)AP_DEFAULT_BASE, ap_start16, code_len); 232 233 addr = AP_DEFAULT_BASE + (ulong)sipi_params_16bit - (ulong)ap_start16; 234 params16 = (struct sipi_params_16bit *)addr; 235 params16->ap_start = (uint32_t)ap_start; 236 params16->gdt = (uint32_t)gd->arch.gdt; 237 params16->gdt_limit = X86_GDT_SIZE - 1; 238 debug("gdt = %x, gdt_limit = %x\n", params16->gdt, params16->gdt_limit); 239 240 params = (struct sipi_params *)sipi_params; 241 debug("SIPI 32-bit params at %p\n", params); 242 params->idt_ptr = (uint32_t)x86_get_idt(); 243 244 params->stack_size = CONFIG_AP_STACK_SIZE; 245 size = params->stack_size * num_cpus; 246 stack = memalign(4096, size); 247 if (!stack) 248 return -ENOMEM; 249 params->stack_top = (u32)(stack + size); 250 251 params->microcode_ptr = 0; 252 params->msr_table_ptr = (u32)msr_save; 253 ret = save_bsp_msrs(msr_save, sizeof(msr_save)); 254 if (ret < 0) 255 return ret; 256 params->msr_count = ret; 257 258 params->c_handler = (uint32_t)&ap_init; 259 260 *ap_countp = ¶ms->ap_count; 261 atomic_set(*ap_countp, 0); 262 debug("SIPI vector is ready\n"); 263 264 return 0; 265 } 266 267 static int check_cpu_devices(int expected_cpus) 268 { 269 int i; 270 271 for (i = 0; i < expected_cpus; i++) { 272 struct udevice *dev; 273 int ret; 274 275 ret = uclass_find_device(UCLASS_CPU, i, &dev); 276 if (ret) { 277 debug("Cannot find CPU %d in device tree\n", i); 278 return ret; 279 } 280 } 281 282 return 0; 283 } 284 285 /* Returns 1 for timeout. 0 on success */ 286 static int apic_wait_timeout(int total_delay, const char *msg) 287 { 288 int total = 0; 289 290 if (!(lapic_read(LAPIC_ICR) & LAPIC_ICR_BUSY)) 291 return 0; 292 293 debug("Waiting for %s...", msg); 294 while (lapic_read(LAPIC_ICR) & LAPIC_ICR_BUSY) { 295 udelay(50); 296 total += 50; 297 if (total >= total_delay) { 298 debug("timed out: aborting\n"); 299 return -ETIMEDOUT; 300 } 301 } 302 debug("done\n"); 303 304 return 0; 305 } 306 307 static int start_aps(int ap_count, atomic_t *num_aps) 308 { 309 int sipi_vector; 310 /* Max location is 4KiB below 1MiB */ 311 const int max_vector_loc = ((1 << 20) - (1 << 12)) >> 12; 312 313 if (ap_count == 0) 314 return 0; 315 316 /* The vector is sent as a 4k aligned address in one byte */ 317 sipi_vector = AP_DEFAULT_BASE >> 12; 318 319 if (sipi_vector > max_vector_loc) { 320 printf("SIPI vector too large! 0x%08x\n", 321 sipi_vector); 322 return -1; 323 } 324 325 debug("Attempting to start %d APs\n", ap_count); 326 327 if (apic_wait_timeout(1000, "ICR not to be busy")) 328 return -ETIMEDOUT; 329 330 /* Send INIT IPI to all but self */ 331 lapic_write(LAPIC_ICR2, SET_LAPIC_DEST_FIELD(0)); 332 lapic_write(LAPIC_ICR, LAPIC_DEST_ALLBUT | LAPIC_INT_ASSERT | 333 LAPIC_DM_INIT); 334 debug("Waiting for 10ms after sending INIT\n"); 335 mdelay(10); 336 337 /* Send 1st SIPI */ 338 if (apic_wait_timeout(1000, "ICR not to be busy")) 339 return -ETIMEDOUT; 340 341 lapic_write(LAPIC_ICR2, SET_LAPIC_DEST_FIELD(0)); 342 lapic_write(LAPIC_ICR, LAPIC_DEST_ALLBUT | LAPIC_INT_ASSERT | 343 LAPIC_DM_STARTUP | sipi_vector); 344 if (apic_wait_timeout(10000, "first SIPI to complete")) 345 return -ETIMEDOUT; 346 347 /* Wait for CPUs to check in up to 200 us */ 348 wait_for_aps(num_aps, ap_count, 200, 15); 349 350 /* Send 2nd SIPI */ 351 if (apic_wait_timeout(1000, "ICR not to be busy")) 352 return -ETIMEDOUT; 353 354 lapic_write(LAPIC_ICR2, SET_LAPIC_DEST_FIELD(0)); 355 lapic_write(LAPIC_ICR, LAPIC_DEST_ALLBUT | LAPIC_INT_ASSERT | 356 LAPIC_DM_STARTUP | sipi_vector); 357 if (apic_wait_timeout(10000, "second SIPI to complete")) 358 return -ETIMEDOUT; 359 360 /* Wait for CPUs to check in */ 361 if (wait_for_aps(num_aps, ap_count, 10000, 50)) { 362 debug("Not all APs checked in: %d/%d\n", 363 atomic_read(num_aps), ap_count); 364 return -1; 365 } 366 367 return 0; 368 } 369 370 static int bsp_do_flight_plan(struct udevice *cpu, struct mp_params *mp_params) 371 { 372 int i; 373 int ret = 0; 374 const int timeout_us = 100000; 375 const int step_us = 100; 376 int num_aps = num_cpus - 1; 377 378 for (i = 0; i < mp_params->num_records; i++) { 379 struct mp_flight_record *rec = &mp_params->flight_plan[i]; 380 381 /* Wait for APs if the record is not released */ 382 if (atomic_read(&rec->barrier) == 0) { 383 /* Wait for the APs to check in */ 384 if (wait_for_aps(&rec->cpus_entered, num_aps, 385 timeout_us, step_us)) { 386 debug("MP record %d timeout\n", i); 387 ret = -1; 388 } 389 } 390 391 if (rec->bsp_call != NULL) 392 rec->bsp_call(cpu, rec->bsp_arg); 393 394 release_barrier(&rec->barrier); 395 } 396 return ret; 397 } 398 399 static int init_bsp(struct udevice **devp) 400 { 401 char processor_name[CPU_MAX_NAME_LEN]; 402 int apic_id; 403 int ret; 404 405 cpu_get_name(processor_name); 406 debug("CPU: %s\n", processor_name); 407 408 lapic_setup(); 409 410 apic_id = lapicid(); 411 ret = find_cpu_by_apic_id(apic_id, devp); 412 if (ret) { 413 printf("Cannot find boot CPU, APIC ID %d\n", apic_id); 414 return ret; 415 } 416 417 return 0; 418 } 419 420 #ifdef CONFIG_QEMU 421 static int qemu_cpu_fixup(void) 422 { 423 int ret; 424 int cpu_num; 425 int cpu_online; 426 struct udevice *dev, *pdev; 427 struct cpu_platdata *plat; 428 char *cpu; 429 430 /* first we need to find '/cpus' */ 431 for (device_find_first_child(dm_root(), &pdev); 432 pdev; 433 device_find_next_child(&pdev)) { 434 if (!strcmp(pdev->name, "cpus")) 435 break; 436 } 437 if (!pdev) { 438 printf("unable to find cpus device\n"); 439 return -ENODEV; 440 } 441 442 /* calculate cpus that are already bound */ 443 cpu_num = 0; 444 for (uclass_find_first_device(UCLASS_CPU, &dev); 445 dev; 446 uclass_find_next_device(&dev)) { 447 cpu_num++; 448 } 449 450 /* get actual cpu number */ 451 cpu_online = qemu_fwcfg_online_cpus(); 452 if (cpu_online < 0) { 453 printf("unable to get online cpu number: %d\n", cpu_online); 454 return cpu_online; 455 } 456 457 /* bind addtional cpus */ 458 dev = NULL; 459 for (; cpu_num < cpu_online; cpu_num++) { 460 /* 461 * allocate device name here as device_bind_driver() does 462 * not copy device name, 8 bytes are enough for 463 * sizeof("cpu@") + 3 digits cpu number + '\0' 464 */ 465 cpu = malloc(8); 466 if (!cpu) { 467 printf("unable to allocate device name\n"); 468 return -ENOMEM; 469 } 470 sprintf(cpu, "cpu@%d", cpu_num); 471 ret = device_bind_driver(pdev, "cpu_qemu", cpu, &dev); 472 if (ret) { 473 printf("binding cpu@%d failed: %d\n", cpu_num, ret); 474 return ret; 475 } 476 plat = dev_get_parent_platdata(dev); 477 plat->cpu_id = cpu_num; 478 } 479 return 0; 480 } 481 #endif 482 483 int mp_init(struct mp_params *p) 484 { 485 int num_aps; 486 atomic_t *ap_count; 487 struct udevice *cpu; 488 int ret; 489 490 /* This will cause the CPUs devices to be bound */ 491 struct uclass *uc; 492 ret = uclass_get(UCLASS_CPU, &uc); 493 if (ret) 494 return ret; 495 496 #ifdef CONFIG_QEMU 497 ret = qemu_cpu_fixup(); 498 if (ret) 499 return ret; 500 #endif 501 502 ret = init_bsp(&cpu); 503 if (ret) { 504 debug("Cannot init boot CPU: err=%d\n", ret); 505 return ret; 506 } 507 508 if (p == NULL || p->flight_plan == NULL || p->num_records < 1) { 509 printf("Invalid MP parameters\n"); 510 return -1; 511 } 512 513 num_cpus = cpu_get_count(cpu); 514 if (num_cpus < 0) { 515 debug("Cannot get number of CPUs: err=%d\n", num_cpus); 516 return num_cpus; 517 } 518 519 if (num_cpus < 2) 520 debug("Warning: Only 1 CPU is detected\n"); 521 522 ret = check_cpu_devices(num_cpus); 523 if (ret) 524 debug("Warning: Device tree does not describe all CPUs. Extra ones will not be started correctly\n"); 525 526 /* Copy needed parameters so that APs have a reference to the plan */ 527 mp_info.num_records = p->num_records; 528 mp_info.records = p->flight_plan; 529 530 /* Load the SIPI vector */ 531 ret = load_sipi_vector(&ap_count, num_cpus); 532 if (ap_count == NULL) 533 return -1; 534 535 /* 536 * Make sure SIPI data hits RAM so the APs that come up will see 537 * the startup code even if the caches are disabled 538 */ 539 wbinvd(); 540 541 /* Start the APs providing number of APs and the cpus_entered field */ 542 num_aps = num_cpus - 1; 543 ret = start_aps(num_aps, ap_count); 544 if (ret) { 545 mdelay(1000); 546 debug("%d/%d eventually checked in?\n", atomic_read(ap_count), 547 num_aps); 548 return ret; 549 } 550 551 /* Walk the flight plan for the BSP */ 552 ret = bsp_do_flight_plan(cpu, p); 553 if (ret) { 554 debug("CPU init failed: err=%d\n", ret); 555 return ret; 556 } 557 558 return 0; 559 } 560 561 int mp_init_cpu(struct udevice *cpu, void *unused) 562 { 563 /* 564 * Multiple APs are brought up simultaneously and they may get the same 565 * seq num in the uclass_resolve_seq() during device_probe(). To avoid 566 * this, set req_seq to the reg number in the device tree in advance. 567 */ 568 cpu->req_seq = fdtdec_get_int(gd->fdt_blob, cpu->of_offset, "reg", -1); 569 570 return device_probe(cpu); 571 } 572