sys/amd64/amd64/mp_machdep.c

   1 /*-
   2  * Copyright (c) 1996, by Steve Passe
   3  * Copyright (c) 2003, by Peter Wemm
   4  * All rights reserved.
   5  *
   6  * Redistribution and use in source and binary forms, with or without
   7  * modification, are permitted provided that the following conditions
   8  * are met:
   9  * 1. Redistributions of source code must retain the above copyright
  10  *    notice, this list of conditions and the following disclaimer.
  11  * 2. The name of the developer may NOT be used to endorse or promote products
  12  *    derived from this software without specific prior written permission.
  13  *
  14  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  15  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  16  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  17  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  18  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  19  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  20  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  21  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  22  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  23  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  24  * SUCH DAMAGE.
  25  */
  26
  27 #include <sys/cdefs.h>
  28 __FBSDID("$FreeBSD$");
  29
  30 #include "opt_cpu.h"
  31 #include "opt_kstack_pages.h"
  32
  33 #include <sys/param.h>
  34 #include <sys/systm.h>
  35 #include <sys/bus.h>
  36 #ifdef GPROF
  37 #include <sys/gmon.h>
  38 #endif
  39 #include <sys/kernel.h>
  40 #include <sys/ktr.h>
  41 #include <sys/lock.h>
  42 #include <sys/malloc.h>
  43 #include <sys/memrange.h>
  44 #include <sys/mutex.h>
  45 #include <sys/pcpu.h>
  46 #include <sys/proc.h>
  47 #include <sys/smp.h>
  48 #include <sys/sysctl.h>
  49
  50 #include <vm/vm.h>
  51 #include <vm/vm_param.h>
  52 #include <vm/pmap.h>
  53 #include <vm/vm_kern.h>
  54 #include <vm/vm_extern.h>
  55
  56 #include <machine/apicreg.h>
  57 #include <machine/clock.h>
  58 #include <machine/md_var.h>
  59 #include <machine/pcb.h>
  60 #include <machine/psl.h>
  61 #include <machine/smp.h>
  62 #include <machine/specialreg.h>
  63 #include <machine/tss.h>
  64
  65 #define WARMBOOT_TARGET         0
  66 #define WARMBOOT_OFF            (KERNBASE + 0x0467)
  67 #define WARMBOOT_SEG            (KERNBASE + 0x0469)
  68
  69 #define CMOS_REG                (0x70)
  70 #define CMOS_DATA               (0x71)
  71 #define BIOS_RESET              (0x0f)
  72 #define BIOS_WARM               (0x0a)
  73
  74 /* lock region used by kernel profiling */
  75 int     mcount_lock;
  76
  77 int     mp_naps;                /* # of Applications processors */
  78 int     boot_cpu_id = -1;       /* designated BSP */
  79 extern  int nkpt;
  80
  81 /*
  82  * CPU topology map datastructures for HTT.
  83  */
  84 static struct cpu_group mp_groups[MAXCPU];
  85 static struct cpu_top mp_top;
  86
  87 /* AP uses this during bootstrap.  Do not staticize.  */
  88 char *bootSTK;
  89 static int bootAP;
  90
  91 /* Free these after use */
  92 void *bootstacks[MAXCPU];
  93
  94 /* Hotwire a 0->4MB V==P mapping */
  95 extern pt_entry_t *KPTphys;
  96
  97 /* SMP page table page */
  98 extern pt_entry_t *SMPpt;
  99
 100 struct pcb stoppcbs[MAXCPU];
 101
 102 /* Variables needed for SMP tlb shootdown. */
 103 vm_offset_t smp_tlb_addr1;
 104 vm_offset_t smp_tlb_addr2;
 105 volatile int smp_tlb_wait;
 106 struct mtx smp_tlb_mtx;
 107
 108 extern inthand_t IDTVEC(fast_syscall), IDTVEC(fast_syscall32);
 109
 110 /*
 111  * Local data and functions.
 112  */
 113
 114 static u_int logical_cpus;
 115 static u_int logical_cpus_mask;
 116
 117 /* used to hold the AP's until we are ready to release them */
 118 static struct mtx ap_boot_mtx;
 119
 120 /* Set to 1 once we're ready to let the APs out of the pen. */
 121 static volatile int aps_ready = 0;
 122
 123 /*
 124  * Store data from cpu_add() until later in the boot when we actually setup
 125  * the APs.
 126  */
 127 struct cpu_info {
 128         int     cpu_present:1;
 129         int     cpu_bsp:1;
 130 } static cpu_info[MAXCPU];
 131 static int cpu_apic_ids[MAXCPU];
 132
 133 static u_int boot_address;
 134
 135 static void     set_logical_apic_ids(void);
 136 static int      start_all_aps(void);
 137 static int      start_ap(int apic_id);
 138 static void     release_aps(void *dummy);
 139
 140 static int      hlt_cpus_mask;
 141 static int      hlt_logical_cpus;
 142 static struct   sysctl_ctx_list logical_cpu_clist;
 143 static u_int    bootMP_size;
 144
 145 static void
 146 mem_range_AP_init(void)
 147 {
 148         if (mem_range_softc.mr_op && mem_range_softc.mr_op->initAP)
 149                 mem_range_softc.mr_op->initAP(&mem_range_softc);
 150 }
 151
 152 void
 153 mp_topology(void)
 154 {
 155         struct cpu_group *group;
 156         int logical_cpus;
 157         int apic_id;
 158         int groups;
 159         int cpu;
 160
 161         /* Build the smp_topology map. */
 162         /* Nothing to do if there is no HTT support. */
 163         if ((cpu_feature & CPUID_HTT) == 0)
 164                 return;
 165         logical_cpus = (cpu_procinfo & CPUID_HTT_CORES) >> 16;
 166         if (logical_cpus <= 1)
 167                 return;
 168         group = &mp_groups[0];
 169         groups = 1;
 170         for (cpu = 0, apic_id = 0; apic_id < MAXCPU; apic_id++) {
 171                 if (!cpu_info[apic_id].cpu_present)
 172                         continue;
 173                 /*
 174                  * If the current group has members and we're not a logical
 175                  * cpu, create a new group.
 176                  */
 177                 if (group->cg_count != 0 && (apic_id % logical_cpus) == 0) {
 178                         group++;
 179                         groups++;
 180                 }
 181                 group->cg_count++;
 182                 group->cg_mask |= 1 << cpu;
 183                 cpu++;
 184         }
 185
 186         mp_top.ct_count = groups;
 187         mp_top.ct_group = mp_groups;
 188         smp_topology = &mp_top;
 189 }
 190
 191
 192 /*
 193  * Calculate usable address in base memory for AP trampoline code.
 194  */
 195 u_int
 196 mp_bootaddress(u_int basemem)
 197 {
 198
 199         bootMP_size = mptramp_end - mptramp_start;
 200         boot_address = trunc_page(basemem * 1024); /* round down to 4k boundary */
 201         if ((basemem - boot_address) < bootMP_size)
 202                 boot_address -= PAGE_SIZE;      /* not enough, lower by 4k */
 203         /* 3 levels of page table pages */
 204         mptramp_pagetables = boot_address - (PAGE_SIZE * 3);
 205
 206         return mptramp_pagetables;
 207 }
 208
 209 void
 210 cpu_add(u_int apic_id, char boot_cpu)
 211 {
 212
 213         if (apic_id >= MAXCPU) {
 214                 printf("SMP: CPU %d exceeds maximum CPU %d, ignoring\n",
 215                     apic_id, MAXCPU - 1);
 216                 return;
 217         }
 218         KASSERT(cpu_info[apic_id].cpu_present == 0, ("CPU %d added twice",
 219             apic_id));
 220         cpu_info[apic_id].cpu_present = 1;
 221         if (boot_cpu) {
 222                 KASSERT(boot_cpu_id == -1,
 223                     ("CPU %d claims to be BSP, but CPU %d already is", apic_id,
 224                     boot_cpu_id));
 225                 boot_cpu_id = apic_id;
 226                 cpu_info[apic_id].cpu_bsp = 1;
 227         }
 228         mp_ncpus++;
 229         if (apic_id > mp_maxid)
 230                 mp_maxid = apic_id;
 231         if (bootverbose)
 232                 printf("SMP: Added CPU %d (%s)\n", apic_id, boot_cpu ? "BSP" :
 233                     "AP");
 234
 235 }
 236
 237 void
 238 cpu_mp_setmaxid(void)
 239 {
 240
 241         /*
 242          * mp_maxid should be already set by calls to cpu_add().
 243          * Just sanity check its value here.
 244          */
 245         if (mp_ncpus == 0)
 246                 KASSERT(mp_maxid == 0,
 247                     ("%s: mp_ncpus is zero, but mp_maxid is not", __func__));
 248         else if (mp_ncpus == 1)
 249                 mp_maxid = 0;
 250         else
 251                 KASSERT(mp_maxid >= mp_ncpus - 1,
 252                     ("%s: counters out of sync: max %d, count %d", __func__,
 253                         mp_maxid, mp_ncpus));
 254
 255 }
 256
 257 int
 258 cpu_mp_probe(void)
 259 {
 260
 261         /*
 262          * Always record BSP in CPU map so that the mbuf init code works
 263          * correctly.
 264          */
 265         all_cpus = 1;
 266         if (mp_ncpus == 0) {
 267                 /*
 268                  * No CPUs were found, so this must be a UP system.  Setup
 269                  * the variables to represent a system with a single CPU
 270                  * with an id of 0.
 271                  */
 272                 mp_ncpus = 1;
 273                 return (0);
 274         }
 275
 276         /* At least one CPU was found. */
 277         if (mp_ncpus == 1) {
 278                 /*
 279                  * One CPU was found, so this must be a UP system with
 280                  * an I/O APIC.
 281                  */
 282                 mp_maxid = 0;
 283                 return (0);
 284         }
 285
 286         /* At least two CPUs were found. */
 287         return (1);
 288 }
 289
 290 /*
 291  * Initialize the IPI handlers and start up the AP's.
 292  */
 293 void
 294 cpu_mp_start(void)
 295 {
 296         int i;
 297
 298         /* Initialize the logical ID to APIC ID table. */
 299         for (i = 0; i < MAXCPU; i++)
 300                 cpu_apic_ids[i] = -1;
 301
 302         /* Install an inter-CPU IPI for TLB invalidation */
 303         setidt(IPI_INVLTLB, IDTVEC(invltlb), SDT_SYSIGT, SEL_KPL, 0);
 304         setidt(IPI_INVLPG, IDTVEC(invlpg), SDT_SYSIGT, SEL_KPL, 0);
 305         setidt(IPI_INVLRNG, IDTVEC(invlrng), SDT_SYSIGT, SEL_KPL, 0);
 306
 307         /* Install an inter-CPU IPI for forwarding hardclock() */
 308         setidt(IPI_HARDCLOCK, IDTVEC(hardclock), SDT_SYSIGT, SEL_KPL, 0);
 309
 310         /* Install an inter-CPU IPI for forwarding statclock() */
 311         setidt(IPI_STATCLOCK, IDTVEC(statclock), SDT_SYSIGT, SEL_KPL, 0);
 312
 313         /* Install an inter-CPU IPI for all-CPU rendezvous */
 314         setidt(IPI_RENDEZVOUS, IDTVEC(rendezvous), SDT_SYSIGT, SEL_KPL, 0);
 315
 316         /* Install an inter-CPU IPI for forcing an additional software trap */
 317         setidt(IPI_AST, IDTVEC(cpuast), SDT_SYSIGT, SEL_KPL, 0);
 318
 319         /* Install an inter-CPU IPI for CPU stop/restart */
 320         setidt(IPI_STOP, IDTVEC(cpustop), SDT_SYSIGT, SEL_KPL, 0);
 321
 322         mtx_init(&smp_tlb_mtx, "tlb", NULL, MTX_SPIN);
 323
 324         /* Set boot_cpu_id if needed. */
 325         if (boot_cpu_id == -1) {
 326                 boot_cpu_id = PCPU_GET(apic_id);
 327                 cpu_info[boot_cpu_id].cpu_bsp = 1;
 328         } else
 329                 KASSERT(boot_cpu_id == PCPU_GET(apic_id),
 330                     ("BSP's APIC ID doesn't match boot_cpu_id"));
 331         cpu_apic_ids[0] = boot_cpu_id;
 332
 333         /* Start each Application Processor */
 334         start_all_aps();
 335
 336         /* Setup the initial logical CPUs info. */
 337         logical_cpus = logical_cpus_mask = 0;
 338         if (cpu_feature & CPUID_HTT)
 339                 logical_cpus = (cpu_procinfo & CPUID_HTT_CORES) >> 16;
 340
 341         set_logical_apic_ids();
 342 }
 343
 344
 345 /*
 346  * Print various information about the SMP system hardware and setup.
 347  */
 348 void
 349 cpu_mp_announce(void)
 350 {
 351         int i, x;
 352
 353         /* List CPUs */
 354         printf(" cpu0 (BSP): APIC ID: %2d\n", boot_cpu_id);
 355         for (i = 1, x = 0; x < MAXCPU; x++) {
 356                 if (cpu_info[x].cpu_present && !cpu_info[x].cpu_bsp) {
 357                         KASSERT(i < mp_ncpus,
 358                             ("mp_ncpus and actual cpus are out of whack"));
 359                         printf(" cpu%d (AP): APIC ID: %2d\n", i++, x);
 360                 }
 361         }
 362 }
 363
 364 /*
 365  * AP CPU's call this to initialize themselves.
 366  */
 367 void
 368 init_secondary(void)
 369 {
 370         struct pcpu *pc;
 371         u_int64_t msr, cr0;
 372         int cpu, gsel_tss;
 373
 374         /* Set by the startup code for us to use */
 375         cpu = bootAP;
 376
 377         /* Init tss */
 378         common_tss[cpu] = common_tss[0];
 379         common_tss[cpu].tss_rsp0 = 0;   /* not used until after switch */
 380
 381         gdt_segs[GPROC0_SEL].ssd_base = (long) &common_tss[cpu];
 382         ssdtosyssd(&gdt_segs[GPROC0_SEL],
 383            (struct system_segment_descriptor *)&gdt[GPROC0_SEL]);
 384
 385         lgdt(&r_gdt);                   /* does magic intra-segment return */
 386
 387         /* Get per-cpu data */
 388         pc = &__pcpu[cpu];
 389
 390         /* prime data page for it to use */
 391         pcpu_init(pc, cpu, sizeof(struct pcpu));
 392         pc->pc_apic_id = cpu_apic_ids[cpu];
 393         pc->pc_prvspace = pc;
 394         pc->pc_curthread = 0;
 395         pc->pc_tssp = &common_tss[cpu];
 396         pc->pc_rsp0 = 0;
 397
 398         wrmsr(MSR_FSBASE, 0);           /* User value */
 399         wrmsr(MSR_GSBASE, (u_int64_t)pc);
 400         wrmsr(MSR_KGSBASE, (u_int64_t)pc);      /* XXX User value while we're in the kernel */
 401
 402         lidt(&r_idt);
 403
 404         gsel_tss = GSEL(GPROC0_SEL, SEL_KPL);
 405         ltr(gsel_tss);
 406
 407         /*
 408          * Set to a known state:
 409          * Set by mpboot.s: CR0_PG, CR0_PE
 410          * Set by cpu_setregs: CR0_NE, CR0_MP, CR0_TS, CR0_WP, CR0_AM
 411          */
 412         cr0 = rcr0();
 413         cr0 &= ~(CR0_CD | CR0_NW | CR0_EM);
 414         load_cr0(cr0);
 415
 416         /* Set up the fast syscall stuff */
 417         msr = rdmsr(MSR_EFER) | EFER_SCE;
 418         wrmsr(MSR_EFER, msr);
 419         wrmsr(MSR_LSTAR, (u_int64_t)IDTVEC(fast_syscall));
 420         wrmsr(MSR_CSTAR, (u_int64_t)IDTVEC(fast_syscall32));
 421         msr = ((u_int64_t)GSEL(GCODE_SEL, SEL_KPL) << 32) |
 422               ((u_int64_t)GSEL(GUCODE32_SEL, SEL_UPL) << 48);
 423         wrmsr(MSR_STAR, msr);
 424         wrmsr(MSR_SF_MASK, PSL_NT|PSL_T|PSL_I|PSL_C|PSL_D);
 425
 426         /* Disable local apic just to be sure. */
 427         lapic_disable();
 428
 429         /* signal our startup to the BSP. */
 430         mp_naps++;
 431
 432         /* Spin until the BSP releases the AP's. */
 433         while (!aps_ready)
 434                 ia32_pause();
 435
 436         /* set up CPU registers and state */
 437         cpu_setregs();
 438
 439         /* set up SSE/NX registers */
 440         initializecpu();
 441
 442         /* set up FPU state on the AP */
 443         fpuinit();
 444
 445         /* A quick check from sanity claus */
 446         if (PCPU_GET(apic_id) != lapic_id()) {
 447                 printf("SMP: cpuid = %d\n", PCPU_GET(cpuid));
 448                 printf("SMP: actual apic_id = %d\n", lapic_id());
 449                 printf("SMP: correct apic_id = %d\n", PCPU_GET(apic_id));
 450                 panic("cpuid mismatch! boom!!");
 451         }
 452
 453         mtx_lock_spin(&ap_boot_mtx);
 454
 455         /* Init local apic for irq's */
 456         lapic_setup();
 457
 458         /* Set memory range attributes for this CPU to match the BSP */
 459         mem_range_AP_init();
 460
 461         smp_cpus++;
 462
 463         CTR1(KTR_SMP, "SMP: AP CPU #%d Launched", PCPU_GET(cpuid));
 464         printf("SMP: AP CPU #%d Launched!\n", PCPU_GET(cpuid));
 465
 466         /* Determine if we are a logical CPU. */
 467         if (logical_cpus > 1 && PCPU_GET(apic_id) % logical_cpus != 0)
 468                 logical_cpus_mask |= PCPU_GET(cpumask);
 469
 470         /* Build our map of 'other' CPUs. */
 471         PCPU_SET(other_cpus, all_cpus & ~PCPU_GET(cpumask));
 472
 473         if (bootverbose)
 474                 lapic_dump("AP");
 475
 476         if (smp_cpus == mp_ncpus) {
 477                 /* enable IPI's, tlb shootdown, freezes etc */
 478                 atomic_store_rel_int(&smp_started, 1);
 479                 smp_active = 1;  /* historic */
 480         }
 481
 482         mtx_unlock_spin(&ap_boot_mtx);
 483
 484         /* wait until all the AP's are up */
 485         while (smp_started == 0)
 486                 ia32_pause();
 487
 488         /* ok, now grab sched_lock and enter the scheduler */
 489         mtx_lock_spin(&sched_lock);
 490
 491         binuptime(PCPU_PTR(switchtime));
 492         PCPU_SET(switchticks, ticks);
 493
 494         cpu_throw(NULL, choosethread());        /* doesn't return */
 495
 496         panic("scheduler returned us to %s", __func__);
 497         /* NOTREACHED */
 498 }
 499
 500 /*******************************************************************
 501  * local functions and data
 502  */
 503
 504 /*
 505  * Set the APIC logical IDs.
 506  *
 507  * We want to cluster logical CPU's within the same APIC ID cluster.
 508  * Since logical CPU's are aligned simply filling in the clusters in
 509  * APIC ID order works fine.  Note that this does not try to balance
 510  * the number of CPU's in each cluster. (XXX?)
 511  */
 512 static void
 513 set_logical_apic_ids(void)
 514 {
 515         u_int apic_id, cluster, cluster_id;
 516
 517         /* Force us to allocate cluster 0 at the start. */
 518         cluster = -1;
 519         cluster_id = APIC_MAX_INTRACLUSTER_ID;
 520         for (apic_id = 0; apic_id < MAXCPU; apic_id++) {
 521                 if (!cpu_info[apic_id].cpu_present)
 522                         continue;
 523                 if (cluster_id == APIC_MAX_INTRACLUSTER_ID) {
 524                         cluster = ioapic_next_logical_cluster();
 525                         cluster_id = 0;
 526                 } else
 527                         cluster_id++;
 528                 if (bootverbose)
 529                         printf("APIC ID: physical %u, logical %u:%u\n",
 530                             apic_id, cluster, cluster_id);
 531                 lapic_set_logical_id(apic_id, cluster, cluster_id);
 532         }
 533 }
 534
 535 /*
 536  * start each AP in our list
 537  */
 538 static int
 539 start_all_aps(void)
 540 {
 541         u_char mpbiosreason;
 542         u_int32_t mpbioswarmvec;
 543         int apic_id, cpu, i;
 544         u_int64_t *pt4, *pt3, *pt2;
 545
 546         mtx_init(&ap_boot_mtx, "ap boot", NULL, MTX_SPIN);
 547
 548         /* install the AP 1st level boot code */
 549         pmap_kenter(boot_address + KERNBASE, boot_address);
 550         bcopy(mptramp_start, (void *)((uintptr_t)boot_address + KERNBASE), bootMP_size);
 551
 552         /* Locate the page tables, they'll be below the trampoline */
 553         pt4 = (u_int64_t *)(uintptr_t)(mptramp_pagetables + KERNBASE);
 554         pt3 = pt4 + (PAGE_SIZE) / sizeof(u_int64_t);
 555         pt2 = pt3 + (PAGE_SIZE) / sizeof(u_int64_t);
 556
 557         /* Create the initial 1GB replicated page tables */
 558         for (i = 0; i < 512; i++) {
 559                 /* Each slot of the level 4 pages points to the same level 3 page */
 560                 pt4[i] = (u_int64_t)(uintptr_t)(mptramp_pagetables + PAGE_SIZE);
 561                 pt4[i] |= PG_V | PG_RW | PG_U;
 562
 563                 /* Each slot of the level 3 pages points to the same level 2 page */
 564                 pt3[i] = (u_int64_t)(uintptr_t)(mptramp_pagetables + (2 * PAGE_SIZE));
 565                 pt3[i] |= PG_V | PG_RW | PG_U;
 566
 567                 /* The level 2 page slots are mapped with 2MB pages for 1GB. */
 568                 pt2[i] = i * (2 * 1024 * 1024);
 569                 pt2[i] |= PG_V | PG_RW | PG_PS | PG_U;
 570         }
 571
 572         /* save the current value of the warm-start vector */
 573         mpbioswarmvec = *((u_int32_t *) WARMBOOT_OFF);
 574         outb(CMOS_REG, BIOS_RESET);
 575         mpbiosreason = inb(CMOS_DATA);
 576
 577         /* setup a vector to our boot code */
 578         *((volatile u_short *) WARMBOOT_OFF) = WARMBOOT_TARGET;
 579         *((volatile u_short *) WARMBOOT_SEG) = (boot_address >> 4);
 580         outb(CMOS_REG, BIOS_RESET);
 581         outb(CMOS_DATA, BIOS_WARM);     /* 'warm-start' */
 582
 583         /* start each AP */
 584         cpu = 0;
 585         for (apic_id = 0; apic_id < MAXCPU; apic_id++) {
 586                 if (!cpu_info[apic_id].cpu_present ||
 587                     cpu_info[apic_id].cpu_bsp)
 588                         continue;
 589                 cpu++;
 590
 591                 /* save APIC ID for this logical ID */
 592                 cpu_apic_ids[cpu] = apic_id;
 593
 594                 /* allocate and set up an idle stack data page */
 595                 bootstacks[cpu] = (char *)kmem_alloc(kernel_map, KSTACK_PAGES * PAGE_SIZE);
 596
 597                 bootSTK = (char *)bootstacks[cpu] + KSTACK_PAGES * PAGE_SIZE - 8;
 598                 bootAP = cpu;
 599
 600                 /* attempt to start the Application Processor */
 601                 if (!start_ap(apic_id)) {
 602                         /* restore the warmstart vector */
 603                         *(u_int32_t *) WARMBOOT_OFF = mpbioswarmvec;
 604                         panic("AP #%d (PHY# %d) failed!", cpu, apic_id);
 605                 }
 606
 607                 all_cpus |= (1 << cpu);         /* record AP in CPU map */
 608         }
 609
 610         /* build our map of 'other' CPUs */
 611         PCPU_SET(other_cpus, all_cpus & ~PCPU_GET(cpumask));
 612
 613         /* restore the warmstart vector */
 614         *(u_int32_t *) WARMBOOT_OFF = mpbioswarmvec;
 615
 616         outb(CMOS_REG, BIOS_RESET);
 617         outb(CMOS_DATA, mpbiosreason);
 618
 619         /* number of APs actually started */
 620         return mp_naps;
 621 }
 622
 623
 624 /*
 625  * This function starts the AP (application processor) identified
 626  * by the APIC ID 'physicalCpu'.  It does quite a "song and dance"
 627  * to accomplish this.  This is necessary because of the nuances
 628  * of the different hardware we might encounter.  It isn't pretty,
 629  * but it seems to work.
 630  */
 631 static int
 632 start_ap(int apic_id)
 633 {
 634         int vector, ms;
 635         int cpus;
 636
 637         /* calculate the vector */
 638         vector = (boot_address >> 12) & 0xff;
 639
 640         /* used as a watchpoint to signal AP startup */
 641         cpus = mp_naps;
 642
 643         /*
 644          * first we do an INIT/RESET IPI this INIT IPI might be run, reseting
 645          * and running the target CPU. OR this INIT IPI might be latched (P5
 646          * bug), CPU waiting for STARTUP IPI. OR this INIT IPI might be
 647          * ignored.
 648          */
 649
 650         /* do an INIT IPI: assert RESET */
 651         lapic_ipi_raw(APIC_DEST_DESTFLD | APIC_TRIGMOD_EDGE |
 652             APIC_LEVEL_ASSERT | APIC_DESTMODE_PHY | APIC_DELMODE_INIT, apic_id);
 653
 654         /* wait for pending status end */
 655         lapic_ipi_wait(-1);
 656
 657         /* do an INIT IPI: deassert RESET */
 658         lapic_ipi_raw(APIC_DEST_ALLESELF | APIC_TRIGMOD_LEVEL |
 659             APIC_LEVEL_DEASSERT | APIC_DESTMODE_PHY | APIC_DELMODE_INIT, 0);
 660
 661         /* wait for pending status end */
 662         DELAY(10000);           /* wait ~10mS */
 663         lapic_ipi_wait(-1);
 664
 665         /*
 666          * next we do a STARTUP IPI: the previous INIT IPI might still be
 667          * latched, (P5 bug) this 1st STARTUP would then terminate
 668          * immediately, and the previously started INIT IPI would continue. OR
 669          * the previous INIT IPI has already run. and this STARTUP IPI will
 670          * run. OR the previous INIT IPI was ignored. and this STARTUP IPI
 671          * will run.
 672          */
 673
 674         /* do a STARTUP IPI */
 675         lapic_ipi_raw(APIC_DEST_DESTFLD | APIC_TRIGMOD_EDGE |
 676             APIC_LEVEL_DEASSERT | APIC_DESTMODE_PHY | APIC_DELMODE_STARTUP |
 677             vector, apic_id);
 678         lapic_ipi_wait(-1);
 679         DELAY(200);             /* wait ~200uS */
 680
 681         /*
 682          * finally we do a 2nd STARTUP IPI: this 2nd STARTUP IPI should run IF
 683          * the previous STARTUP IPI was cancelled by a latched INIT IPI. OR
 684          * this STARTUP IPI will be ignored, as only ONE STARTUP IPI is
 685          * recognized after hardware RESET or INIT IPI.
 686          */
 687
 688         lapic_ipi_raw(APIC_DEST_DESTFLD | APIC_TRIGMOD_EDGE |
 689             APIC_LEVEL_DEASSERT | APIC_DESTMODE_PHY | APIC_DELMODE_STARTUP |
 690             vector, apic_id);
 691         lapic_ipi_wait(-1);
 692         DELAY(200);             /* wait ~200uS */
 693
 694         /* Wait up to 5 seconds for it to start. */
 695         for (ms = 0; ms < 50; ms++) {
 696                 if (mp_naps > cpus)
 697                         return 1;       /* return SUCCESS */
 698                 DELAY(100000);
 699         }
 700         return 0;               /* return FAILURE */
 701 }
 702
 703 /*
 704  * Flush the TLB on all other CPU's
 705  */
 706 static void
 707 smp_tlb_shootdown(u_int vector, vm_offset_t addr1, vm_offset_t addr2)
 708 {
 709         u_int ncpu;
 710
 711         ncpu = mp_ncpus - 1;    /* does not shootdown self */
 712         if (ncpu < 1)
 713                 return;         /* no other cpus */
 714         mtx_assert(&smp_tlb_mtx, MA_OWNED);
 715         smp_tlb_addr1 = addr1;
 716         smp_tlb_addr2 = addr2;
 717         atomic_store_rel_int(&smp_tlb_wait, 0);
 718         ipi_all_but_self(vector);
 719         while (smp_tlb_wait < ncpu)
 720                 ia32_pause();
 721 }
 722
 723 /*
 724  * This is about as magic as it gets.  fortune(1) has got similar code
 725  * for reversing bits in a word.  Who thinks up this stuff??
 726  *
 727  * Yes, it does appear to be consistently faster than:
 728  * while (i = ffs(m)) {
 729  *      m >>= i;
 730  *      bits++;
 731  * }
 732  * and
 733  * while (lsb = (m & -m)) {     // This is magic too
 734  *      m &= ~lsb;              // or: m ^= lsb
 735  *      bits++;
 736  * }
 737  * Both of these latter forms do some very strange things on gcc-3.1 with
 738  * -mcpu=pentiumpro and/or -march=pentiumpro and/or -O or -O2.
 739  * There is probably an SSE or MMX popcnt instruction.
 740  *
 741  * I wonder if this should be in libkern?
 742  *
 743  * XXX Stop the presses!  Another one:
 744  * static __inline u_int32_t
 745  * popcnt1(u_int32_t v)
 746  * {
 747  *      v -= ((v >> 1) & 0x55555555);
 748  *      v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
 749  *      v = (v + (v >> 4)) & 0x0F0F0F0F;
 750  *      return (v * 0x01010101) >> 24;
 751  * }
 752  * The downside is that it has a multiply.  With a pentium3 with
 753  * -mcpu=pentiumpro and -march=pentiumpro then gcc-3.1 will use
 754  * an imull, and in that case it is faster.  In most other cases
 755  * it appears slightly slower.
 756  *
 757  * Another variant (also from fortune):
 758  * #define BITCOUNT(x) (((BX_(x)+(BX_(x)>>4)) & 0x0F0F0F0F) % 255)
 759  * #define  BX_(x)     ((x) - (((x)>>1)&0x77777777)            \
 760  *                          - (((x)>>2)&0x33333333)            \
 761  *                          - (((x)>>3)&0x11111111))
 762  */
 763 static __inline u_int32_t
 764 popcnt(u_int32_t m)
 765 {
 766
 767         m = (m & 0x55555555) + ((m & 0xaaaaaaaa) >> 1);
 768         m = (m & 0x33333333) + ((m & 0xcccccccc) >> 2);
 769         m = (m & 0x0f0f0f0f) + ((m & 0xf0f0f0f0) >> 4);
 770         m = (m & 0x00ff00ff) + ((m & 0xff00ff00) >> 8);
 771         m = (m & 0x0000ffff) + ((m & 0xffff0000) >> 16);
 772         return m;
 773 }
 774
 775 static void
 776 smp_targeted_tlb_shootdown(u_int mask, u_int vector, vm_offset_t addr1, vm_offset_t addr2)
 777 {
 778         int ncpu, othercpus;
 779
 780         othercpus = mp_ncpus - 1;
 781         if (mask == (u_int)-1) {
 782                 ncpu = othercpus;
 783                 if (ncpu < 1)
 784                         return;
 785         } else {
 786                 mask &= ~PCPU_GET(cpumask);
 787                 if (mask == 0)
 788                         return;
 789                 ncpu = popcnt(mask);
 790                 if (ncpu > othercpus) {
 791                         /* XXX this should be a panic offence */
 792                         printf("SMP: tlb shootdown to %d other cpus (only have %d)\n",
 793                             ncpu, othercpus);
 794                         ncpu = othercpus;
 795                 }
 796                 /* XXX should be a panic, implied by mask == 0 above */
 797                 if (ncpu < 1)
 798                         return;
 799         }
 800         mtx_assert(&smp_tlb_mtx, MA_OWNED);
 801         smp_tlb_addr1 = addr1;
 802         smp_tlb_addr2 = addr2;
 803         atomic_store_rel_int(&smp_tlb_wait, 0);
 804         if (mask == (u_int)-1)
 805                 ipi_all_but_self(vector);
 806         else
 807                 ipi_selected(mask, vector);
 808         while (smp_tlb_wait < ncpu)
 809                 ia32_pause();
 810 }
 811
 812 void
 813 smp_invltlb(void)
 814 {
 815
 816         if (smp_started)
 817                 smp_tlb_shootdown(IPI_INVLTLB, 0, 0);
 818 }
 819
 820 void
 821 smp_invlpg(vm_offset_t addr)
 822 {
 823
 824         if (smp_started)
 825                 smp_tlb_shootdown(IPI_INVLPG, addr, 0);
 826 }
 827
 828 void
 829 smp_invlpg_range(vm_offset_t addr1, vm_offset_t addr2)
 830 {
 831
 832         if (smp_started)
 833                 smp_tlb_shootdown(IPI_INVLRNG, addr1, addr2);
 834 }
 835
 836 void
 837 smp_masked_invltlb(u_int mask)
 838 {
 839
 840         if (smp_started)
 841                 smp_targeted_tlb_shootdown(mask, IPI_INVLTLB, 0, 0);
 842 }
 843
 844 void
 845 smp_masked_invlpg(u_int mask, vm_offset_t addr)
 846 {
 847
 848         if (smp_started)
 849                 smp_targeted_tlb_shootdown(mask, IPI_INVLPG, addr, 0);
 850 }
 851
 852 void
 853 smp_masked_invlpg_range(u_int mask, vm_offset_t addr1, vm_offset_t addr2)
 854 {
 855
 856         if (smp_started)
 857                 smp_targeted_tlb_shootdown(mask, IPI_INVLRNG, addr1, addr2);
 858 }
 859
 860
 861 /*
 862  * For statclock, we send an IPI to all CPU's to have them call this
 863  * function.
 864  */
 865 void
 866 forwarded_statclock(struct clockframe frame)
 867 {
 868         struct thread *td;
 869
 870         CTR0(KTR_SMP, "forwarded_statclock");
 871         td = curthread;
 872         td->td_intr_nesting_level++;
 873         if (profprocs != 0)
 874                 profclock(&frame);
 875         if (pscnt == psdiv)
 876                 statclock(&frame);
 877         td->td_intr_nesting_level--;
 878 }
 879
 880 void
 881 forward_statclock(void)
 882 {
 883         int map;
 884
 885         CTR0(KTR_SMP, "forward_statclock");
 886
 887         if (!smp_started || cold || panicstr)
 888                 return;
 889
 890         map = PCPU_GET(other_cpus) & ~(stopped_cpus|hlt_cpus_mask);
 891         if (map != 0)
 892                 ipi_selected(map, IPI_STATCLOCK);
 893 }
 894
 895 /*
 896  * For each hardclock(), we send an IPI to all other CPU's to have them
 897  * execute this function.  It would be nice to reduce contention on
 898  * sched_lock if we could simply peek at the CPU to determine the user/kernel
 899  * state and call hardclock_process() on the CPU receiving the clock interrupt
 900  * and then just use a simple IPI to handle any ast's if needed.
 901  */
 902 void
 903 forwarded_hardclock(struct clockframe frame)
 904 {
 905         struct thread *td;
 906
 907         CTR0(KTR_SMP, "forwarded_hardclock");
 908         td = curthread;
 909         td->td_intr_nesting_level++;
 910         hardclock_process(&frame);
 911         td->td_intr_nesting_level--;
 912 }
 913
 914 void
 915 forward_hardclock(void)
 916 {
 917         u_int map;
 918
 919         CTR0(KTR_SMP, "forward_hardclock");
 920
 921         if (!smp_started || cold || panicstr)
 922                 return;
 923
 924         map = PCPU_GET(other_cpus) & ~(stopped_cpus|hlt_cpus_mask);
 925         if (map != 0)
 926                 ipi_selected(map, IPI_HARDCLOCK);
 927 }
 928
 929 /*
 930  * send an IPI to a set of cpus.
 931  */
 932 void
 933 ipi_selected(u_int32_t cpus, u_int ipi)
 934 {
 935         int cpu;
 936
 937         CTR3(KTR_SMP, "%s: cpus: %x ipi: %x", __func__, cpus, ipi);
 938         while ((cpu = ffs(cpus)) != 0) {
 939                 cpu--;
 940                 KASSERT(cpu_apic_ids[cpu] != -1,
 941                     ("IPI to non-existent CPU %d", cpu));
 942                 lapic_ipi_vectored(ipi, cpu_apic_ids[cpu]);
 943                 cpus &= ~(1 << cpu);
 944         }
 945 }
 946
 947 /*
 948  * send an IPI INTerrupt containing 'vector' to all CPUs, including myself
 949  */
 950 void
 951 ipi_all(u_int ipi)
 952 {
 953
 954         CTR2(KTR_SMP, "%s: ipi: %x", __func__, ipi);
 955         lapic_ipi_vectored(ipi, APIC_IPI_DEST_ALL);
 956 }
 957
 958 /*
 959  * send an IPI to all CPUs EXCEPT myself
 960  */
 961 void
 962 ipi_all_but_self(u_int ipi)
 963 {
 964
 965         CTR2(KTR_SMP, "%s: ipi: %x", __func__, ipi);
 966         lapic_ipi_vectored(ipi, APIC_IPI_DEST_OTHERS);
 967 }
 968
 969 /*
 970  * send an IPI to myself
 971  */
 972 void
 973 ipi_self(u_int ipi)
 974 {
 975
 976         CTR2(KTR_SMP, "%s: ipi: %x", __func__, ipi);
 977         lapic_ipi_vectored(ipi, APIC_IPI_DEST_SELF);
 978 }
 979
 980 /*
 981  * This is called once the rest of the system is up and running and we're
 982  * ready to let the AP's out of the pen.
 983  */
 984 static void
 985 release_aps(void *dummy __unused)
 986 {
 987
 988         if (mp_ncpus == 1)
 989                 return;
 990         mtx_lock_spin(&sched_lock);
 991         atomic_store_rel_int(&aps_ready, 1);
 992         while (smp_started == 0)
 993                 ia32_pause();
 994         mtx_unlock_spin(&sched_lock);
 995 }
 996 SYSINIT(start_aps, SI_SUB_SMP, SI_ORDER_FIRST, release_aps, NULL);
 997
 998 static int
 999 sysctl_hlt_cpus(SYSCTL_HANDLER_ARGS)
1000 {
1001         u_int mask;
1002         int error;
1003
1004         mask = hlt_cpus_mask;
1005         error = sysctl_handle_int(oidp, &mask, 0, req);
1006         if (error || !req->newptr)
1007                 return (error);
1008
1009         if (logical_cpus_mask != 0 &&
1010             (mask & logical_cpus_mask) == logical_cpus_mask)
1011                 hlt_logical_cpus = 1;
1012         else
1013                 hlt_logical_cpus = 0;
1014
1015         if ((mask & all_cpus) == all_cpus)
1016                 mask &= ~(1<<0);
1017         hlt_cpus_mask = mask;
1018         return (error);
1019 }
1020 SYSCTL_PROC(_machdep, OID_AUTO, hlt_cpus, CTLTYPE_INT|CTLFLAG_RW,
1021     0, 0, sysctl_hlt_cpus, "IU",
1022     "Bitmap of CPUs to halt.  101 (binary) will halt CPUs 0 and 2.");
1023
1024 static int
1025 sysctl_hlt_logical_cpus(SYSCTL_HANDLER_ARGS)
1026 {
1027         int disable, error;
1028
1029         disable = hlt_logical_cpus;
1030         error = sysctl_handle_int(oidp, &disable, 0, req);
1031         if (error || !req->newptr)
1032                 return (error);
1033
1034         if (disable)
1035                 hlt_cpus_mask |= logical_cpus_mask;
1036         else
1037                 hlt_cpus_mask &= ~logical_cpus_mask;
1038
1039         if ((hlt_cpus_mask & all_cpus) == all_cpus)
1040                 hlt_cpus_mask &= ~(1<<0);
1041
1042         hlt_logical_cpus = disable;
1043         return (error);
1044 }
1045
1046 static void
1047 cpu_hlt_setup(void *dummy __unused)
1048 {
1049
1050         if (logical_cpus_mask != 0) {
1051                 TUNABLE_INT_FETCH("machdep.hlt_logical_cpus",
1052                     &hlt_logical_cpus);
1053                 sysctl_ctx_init(&logical_cpu_clist);
1054                 SYSCTL_ADD_PROC(&logical_cpu_clist,
1055                     SYSCTL_STATIC_CHILDREN(_machdep), OID_AUTO,
1056                     "hlt_logical_cpus", CTLTYPE_INT|CTLFLAG_RW, 0, 0,
1057                     sysctl_hlt_logical_cpus, "IU", "");
1058                 SYSCTL_ADD_UINT(&logical_cpu_clist,
1059                     SYSCTL_STATIC_CHILDREN(_machdep), OID_AUTO,
1060                     "logical_cpus_mask", CTLTYPE_INT|CTLFLAG_RD,
1061                     &logical_cpus_mask, 0, "");
1062
1063                 if (hlt_logical_cpus)
1064                         hlt_cpus_mask |= logical_cpus_mask;
1065         }
1066 }
1067 SYSINIT(cpu_hlt, SI_SUB_SMP, SI_ORDER_ANY, cpu_hlt_setup, NULL);
1068
1069 int
1070 mp_grab_cpu_hlt(void)
1071 {
1072         u_int mask = PCPU_GET(cpumask);
1073         int retval;
1074
1075         retval = mask & hlt_cpus_mask;
1076         while (mask & hlt_cpus_mask)
1077                 __asm __volatile("sti; hlt" : : : "memory");
1078         return (retval);
1079 }