boot/x86_64/smp_boot.c

   1 /*
   2 ** Copyright 2001, Travis Geiselbrecht. All rights reserved.
   3 ** Distributed under the terms of the NewOS License.
   4 */
   5 #include <boot/stage2.h>
   6 #include "stage2_priv.h"
   7
   8 #include <string.h>
   9
  10 #define CHATTY_SMP 0
  11
  12 static unsigned int mp_mem_phys = 0;
  13 static unsigned int mp_mem_virt = 0;
  14 static struct mp_flt_struct *mp_flt_ptr = NULL;
  15 static kernel_args *saved_ka = NULL;
  16 static unsigned int kernel_entry_point = 0;
  17
  18 static int smp_get_current_cpu(kernel_args *ka);
  19
  20 static unsigned int map_page(kernel_args *ka, unsigned int paddr, unsigned int vaddr)
  21 {
  22         unsigned int *pentry;
  23         unsigned int *pgdir = (unsigned int *)(ka->arch_args.page_hole + (4*1024*1024-PAGE_SIZE));
  24
  25         // check to see if a page table exists for this range
  26         if(pgdir[vaddr / PAGE_SIZE / 1024] == 0) {
  27                 unsigned int pgtable;
  28                 // we need to allocate a pgtable
  29                 pgtable = ka->phys_alloc_range[0].start + ka->phys_alloc_range[0].size;
  30                 ka->phys_alloc_range[0].size += PAGE_SIZE;
  31                 ka->arch_args.pgtables[ka->arch_args.num_pgtables++] = pgtable;
  32
  33                 // put it in the pgdir
  34                 pgdir[vaddr / PAGE_SIZE / 1024] = (pgtable & ADDR_MASK) | DEFAULT_PAGE_FLAGS;
  35
  36                 // zero it out in it's new mapping
  37                 memset((unsigned int *)((unsigned int *)ka->arch_args.page_hole + (vaddr / PAGE_SIZE / 1024) * PAGE_SIZE), 0, PAGE_SIZE);
  38         }
  39         // now, fill in the pentry
  40         pentry = (unsigned int *)((unsigned int *)ka->arch_args.page_hole + vaddr / PAGE_SIZE);
  41
  42         *pentry = (paddr & ADDR_MASK) | DEFAULT_PAGE_FLAGS;
  43
  44         asm volatile("invlpg (%0)" : : "r" (vaddr));
  45
  46         return 0;
  47 }
  48
  49 static unsigned int apic_read(unsigned int *addr)
  50 {
  51         return *addr;
  52 }
  53
  54 static void apic_write(unsigned int *addr, unsigned int data)
  55 {
  56         *addr = data;
  57 }
  58
  59 /*
  60 static void *mp_virt_to_phys(void *ptr)
  61 {
  62         return ((void *)(((unsigned int)ptr - mp_mem_virt) + mp_mem_phys));
  63 }
  64 */
  65 static void *mp_phys_to_virt(void *ptr)
  66 {
  67         return ((void *)(((unsigned int)ptr - mp_mem_phys) + mp_mem_virt));
  68 }
  69
  70 static unsigned int *smp_probe(unsigned int base, unsigned int limit)
  71 {
  72         unsigned int *ptr;
  73
  74 //      dprintf("smp_probe: entry base 0x%x, limit 0x%x\n", base, limit);
  75
  76         for (ptr = (unsigned int *) base; (unsigned int) ptr < limit; ptr++) {
  77                 if (*ptr == MP_FLT_SIGNATURE) {
  78 //                      dprintf("smp_probe: found floating pointer structure at 0x%x\n", ptr);
  79                         return ptr;
  80                 }
  81         }
  82         return NULL;
  83 }
  84
  85 static void smp_do_config(kernel_args *ka)
  86 {
  87         char *ptr;
  88         int i;
  89         struct mp_config_table *mpc;
  90         struct mp_ext_pe *pe;
  91         struct mp_ext_ioapic *io;
  92         struct mp_ext_bus *bus;
  93 #if CHATTY_SMP
  94         const char *cpu_family[] = { "", "", "", "", "Intel 486",
  95                 "Intel Pentium", "Intel Pentium Pro", "Intel Pentium II" };
  96 #endif
  97
  98         /*
  99          * we are not running in standard configuration, so we have to look through
 100          * all of the mp configuration table crap to figure out how many processors
 101          * we have, where our apics are, etc.
 102          */
 103         ka->num_cpus = 0;
 104
 105         mpc = mp_phys_to_virt(mp_flt_ptr->mpc);
 106
 107         /* print out our new found configuration. */
 108         ptr = (char *) &(mpc->oem[0]);
 109 #if CHATTY_SMP
 110         dprintf ("smp: oem id: %c%c%c%c%c%c%c%c product id: "
 111                 "%c%c%c%c%c%c%c%c%c%c%c%c\n", ptr[0], ptr[1], ptr[2], ptr[3], ptr[4],
 112                 ptr[5], ptr[6], ptr[7], ptr[8], ptr[9], ptr[10], ptr[11], ptr[12],
 113                 ptr[13], ptr[14], ptr[15], ptr[16], ptr[17], ptr[18], ptr[19],
 114                 ptr[20]);
 115         dprintf("smp: base table has %d entries, extended section %d bytes\n",
 116                 mpc->num_entries, mpc->ext_len);
 117 #endif
 118         ka->arch_args.apic_phys = (unsigned int)mpc->apic;
 119
 120         ptr = (char *) ((unsigned int) mpc + sizeof (struct mp_config_table));
 121         for (i = 0; i < mpc->num_entries; i++) {
 122                 switch (*ptr) {
 123                         case MP_EXT_PE:
 124                                 pe = (struct mp_ext_pe *) ptr;
 125                                 ka->arch_args.cpu_apic_id[ka->num_cpus] = pe->apic_id;
 126                                 ka->arch_args.cpu_os_id[pe->apic_id] = ka->num_cpus;
 127                                 ka->arch_args.cpu_apic_version[ka->num_cpus] = pe->apic_version;
 128 #if CHATTY_SMP
 129                                 dprintf ("smp: cpu#%d: %s, apic id %d, version %d%s\n",
 130                                         ka->num_cpus, cpu_family[(pe->signature & 0xf00) >> 8],
 131                                         pe->apic_id, pe->apic_version, (pe->cpu_flags & 0x2) ?
 132                                         ", BSP" : "");
 133 #endif
 134                                 ptr += 20;
 135                                 ka->num_cpus++;
 136                                 break;
 137                         case MP_EXT_BUS:
 138                                 bus = (struct mp_ext_bus *)ptr;
 139 #if CHATTY_SMP
 140                                 dprintf("smp: bus%d: %c%c%c%c%c%c\n", bus->bus_id,
 141                                         bus->name[0], bus->name[1], bus->name[2], bus->name[3],
 142                                         bus->name[4], bus->name[5]);
 143 #endif
 144                                 ptr += 8;
 145                                 break;
 146                         case MP_EXT_IO_APIC:
 147                                 io = (struct mp_ext_ioapic *) ptr;
 148                                 ka->arch_args.ioapic_phys = (unsigned int)io->addr;
 149 #if CHATTY_SMP
 150                                 dprintf("smp: found io apic with apic id %d, version %d\n",
 151                                         io->ioapic_id, io->ioapic_version);
 152 #endif
 153                                 ptr += 8;
 154                                 break;
 155                         case MP_EXT_IO_INT:
 156                                 ptr += 8;
 157                                 break;
 158                         case MP_EXT_LOCAL_INT:
 159                                 ptr += 8;
 160                                 break;
 161                 }
 162         }
 163         dprintf("smp: apic @ 0x%x, i/o apic @ 0x%x, total %d processors detected\n",
 164                 (unsigned int)ka->arch_args.apic_phys, (unsigned int)ka->arch_args.ioapic_phys, ka->num_cpus);
 165
 166         // this BIOS looks broken, because it didn't report any cpus (VMWare)
 167         if(ka->num_cpus == 0) {
 168                 ka->num_cpus = 1;
 169         }
 170 }
 171
 172 struct smp_scan_spots_struct {
 173         unsigned int start;
 174         unsigned int stop;
 175         unsigned int len;
 176 };
 177
 178 static struct smp_scan_spots_struct smp_scan_spots[] = {
 179         { 0x9fc00, 0xa0000, 0xa0000 - 0x9fc00 },
 180         { 0xf0000, 0x100000, 0x100000 - 0xf0000 },
 181         { 0, 0, 0 }
 182 };
 183
 184 static int smp_find_mp_config(kernel_args *ka)
 185 {
 186         int i;
 187
 188         // XXX for now, assume the memory is identity mapped by the 1st stage
 189         for(i=0; smp_scan_spots[i].len > 0; i++) {
 190                 mp_flt_ptr = (struct mp_flt_struct *)smp_probe(smp_scan_spots[i].start,
 191                         smp_scan_spots[i].stop);
 192                 if(mp_flt_ptr != NULL)
 193                         break;
 194         }
 195 #if !_WITH_SMP
 196         if(0) {
 197 #else
 198         if(mp_flt_ptr != NULL) {
 199 #endif
 200                 mp_mem_phys = smp_scan_spots[i].start;
 201                 mp_mem_virt = smp_scan_spots[i].start;
 202
 203 #if CHATTY_SMP
 204                 dprintf ("smp_boot: intel mp version %s, %s", (mp_flt_ptr->mp_rev == 1) ? "1.1" :
 205                         "1.4", (mp_flt_ptr->mp_feature_2 & 0x80) ?
 206                         "imcr and pic compatibility mode.\n" : "virtual wire compatibility mode.\n");
 207 #endif
 208                 if (mp_flt_ptr->mpc == 0) {
 209                         // XXX need to implement
 210 #if 1
 211                         ka->num_cpus = 1;
 212                         return 1;
 213 #else
 214                         /* this system conforms to one of the default configurations */
 215 //                      mp_num_def_config = mp_flt_ptr->mp_feature_1;
 216                         dprintf ("smp: standard configuration %d\n", mp_flt_ptr->mp_feature_1);
 217 /*                      num_cpus = 2;
 218                         ka->cpu_apic_id[0] = 0;
 219                         ka->cpu_apic_id[1] = 1;
 220                         apic_phys = (unsigned int *) 0xfee00000;
 221                         ioapic_phys = (unsigned int *) 0xfec00000;
 222                         kprintf ("smp: WARNING: standard configuration code is untested");
 223 */
 224 #endif
 225                 } else {
 226                         smp_do_config(ka);
 227                 }
 228                 return ka->num_cpus;
 229         } else {
 230                 ka->num_cpus = 1;
 231                 return 1;
 232         }
 233 }
 234
 235 // target function of the trampoline code
 236 // The trampoline code should have the pgdir and a gdt set up for us,
 237 // along with us being on the final stack for this processor. We need
 238 // to set up the local APIC and load the global idt and gdt. When we're
 239 // done, we'll jump into the kernel with the cpu number as an argument.
 240 static int smp_cpu_ready(void)
 241 {
 242         kernel_args *ka = saved_ka;
 243         unsigned int curr_cpu = smp_get_current_cpu(ka);
 244         struct gdt_idt_descr idt_descr;
 245         struct gdt_idt_descr gdt_descr;
 246
 247 //      dprintf("smp_cpu_ready: entry cpu %d\n", curr_cpu);
 248
 249         // Important.  Make sure supervisor threads can fault on read only pages...
 250         asm("movl %%eax, %%cr0" : : "a" ((1 << 31) | (1 << 16) | (1 << 5) | 1));
 251         asm("cld");
 252         asm("fninit");
 253
 254         // Set up the final idt
 255         idt_descr.a = IDT_LIMIT - 1;
 256         idt_descr.b = (unsigned int *)ka->arch_args.vir_idt;
 257
 258         asm("lidt       %0;"
 259                 : : "m" (idt_descr));
 260
 261         // Set up the final gdt
 262         gdt_descr.a = GDT_LIMIT - 1;
 263         gdt_descr.b = (unsigned int *)ka->arch_args.vir_gdt;
 264
 265         asm("lgdt       %0;"
 266                 : : "m" (gdt_descr));
 267
 268         asm("pushl  %0; "                                       // push the cpu number
 269                 "pushl  %1;     "                                       // kernel args
 270                 "pushl  $0x0;"                                  // dummy retval for call to main
 271                 "pushl  %2;     "               // this is the start address
 272                 "ret;           "                                       // jump.
 273                 : : "r" (curr_cpu), "m" (ka), "g" (kernel_entry_point));
 274
 275         // no where to return to
 276         return 0;
 277 }
 278
 279 static int smp_boot_all_cpus(kernel_args *ka)
 280 {
 281         unsigned int trampoline_code;
 282         unsigned int trampoline_stack;
 283         unsigned int i;
 284
 285         // XXX assume low 1 meg is identity mapped by the 1st stage bootloader
 286         // and nothing important is in 0x9e000 & 0x9f000
 287
 288         // allocate a stack and a code area for the smp trampoline
 289         // (these have to be < 1M physical)
 290         trampoline_code = 0x9f000;      // 640kB - 4096 == 0x9f000
 291         trampoline_stack = 0x9e000; // 640kB - 8192 == 0x9e000
 292         map_page(ka, 0x9f000, 0x9f000);
 293         map_page(ka, 0x9e000, 0x9e000);
 294
 295         // copy the trampoline code over
 296         memcpy((char *)trampoline_code, &smp_trampoline,
 297                 (unsigned int)&smp_trampoline_end - (unsigned int)&smp_trampoline);
 298
 299         // boot the cpus
 300         for(i = 1; i < ka->num_cpus; i++) {
 301                 unsigned int *final_stack;
 302                 unsigned int *final_stack_ptr;
 303                 unsigned int *tramp_stack_ptr;
 304                 unsigned int config;
 305                 unsigned int num_startups;
 306                 unsigned int j;
 307
 308                 // create a final stack the trampoline code will put the ap processor on
 309                 ka->cpu_kstack[i].start = ka->virt_alloc_range[0].start + ka->virt_alloc_range[0].size;
 310                 ka->cpu_kstack[i].size = STACK_SIZE * PAGE_SIZE;
 311                 for(j=0; j<ka->cpu_kstack[i].size/PAGE_SIZE; j++) {
 312                         // map the pages in
 313                         map_page(ka, ka->phys_alloc_range[0].start + ka->phys_alloc_range[0].size,
 314                                 ka->virt_alloc_range[0].start + ka->virt_alloc_range[0].size);
 315                         ka->phys_alloc_range[0].size += PAGE_SIZE;
 316                         ka->virt_alloc_range[0].size += PAGE_SIZE;
 317                 }
 318
 319                 // set this stack up
 320                 final_stack = (unsigned int *)ka->cpu_kstack[i].start;
 321                 memset(final_stack, 0, STACK_SIZE * PAGE_SIZE);
 322                 final_stack_ptr = (final_stack + (STACK_SIZE * PAGE_SIZE) / sizeof(unsigned int)) - 1;
 323                 *final_stack_ptr = (unsigned int)&smp_cpu_ready;
 324                 final_stack_ptr--;
 325
 326                 // set the trampoline stack up
 327                 tramp_stack_ptr = (unsigned int *)(trampoline_stack + PAGE_SIZE - 4);
 328                 // final location of the stack
 329                 *tramp_stack_ptr = ((unsigned int)final_stack) + STACK_SIZE * PAGE_SIZE - sizeof(unsigned int);
 330                 tramp_stack_ptr--;
 331                 // page dir
 332                 *tramp_stack_ptr = ka->arch_args.phys_pgdir;
 333                 tramp_stack_ptr--;
 334
 335                 // put a gdt descriptor at the bottom of the stack
 336                 *((unsigned short *)trampoline_stack) = 0x18-1; // LIMIT
 337                 *((unsigned int *)(trampoline_stack + 2)) = trampoline_stack + 8;
 338                 // put the gdt at the bottom
 339                 memcpy(&((unsigned int *)trampoline_stack)[2], (void *)ka->arch_args.vir_gdt, 6*4);
 340
 341                 /* clear apic errors */
 342                 if(ka->arch_args.cpu_apic_version[i] & 0xf0) {
 343                         apic_write(APIC_ESR, 0);
 344                         apic_read(APIC_ESR);
 345                 }
 346
 347                 /* send (aka assert) INIT IPI */
 348                 config = (apic_read(APIC_ICR2) & 0x00ffffff) | (ka->arch_args.cpu_apic_id[i] << 24);
 349                 apic_write(APIC_ICR2, config); /* set target pe */
 350                 config = (apic_read(APIC_ICR1) & 0xfff00000) | 0x0000c500;
 351                 apic_write(APIC_ICR1, config);
 352
 353                 // wait for pending to end
 354                 while((apic_read(APIC_ICR1) & 0x00001000) == 0x00001000);
 355
 356                 /* deassert INIT */
 357                 config = (apic_read(APIC_ICR2) & 0x00ffffff) | (ka->arch_args.cpu_apic_id[i] << 24);
 358                 apic_write(APIC_ICR2, config);
 359                 config = (apic_read(APIC_ICR1) & 0xfff00000) | 0x00008500;
 360
 361                 // wait for pending to end
 362                 while((apic_read(APIC_ICR1) & 0x00001000) == 0x00001000);
 363 //                      dprintf("0x%x\n", apic_read(APIC_ICR1));
 364
 365                 /* wait 10ms */
 366                 sleep(10000);
 367
 368                 /* is this a local apic or an 82489dx ? */
 369                 num_startups = (ka->arch_args.cpu_apic_version[i] & 0xf0) ? 2 : 0;
 370                 for (j = 0; j < num_startups; j++) {
 371                         /* it's a local apic, so send STARTUP IPIs */
 372                         apic_write(APIC_ESR, 0);
 373
 374                         /* set target pe */
 375                         config = (apic_read(APIC_ICR2) & 0xf0ffffff) | (ka->arch_args.cpu_apic_id[i] << 24);
 376                         apic_write(APIC_ICR2, config);
 377
 378                         /* send the IPI */
 379                         config = (apic_read(APIC_ICR1) & 0xfff0f800) | APIC_DM_STARTUP |
 380                                 (0x9f000 >> 12);
 381                         apic_write(APIC_ICR1, config);
 382
 383                         /* wait */
 384                         sleep(200);
 385
 386                         while((apic_read(APIC_ICR1)& 0x00001000) == 0x00001000);
 387                 }
 388         }
 389
 390         return 0;
 391 }
 392
 393 // XXX fails on cpus with no RDTSC
 394 static void calculate_apic_timer_conversion_factor(kernel_args *ka)
 395 {
 396         long long t1, t2;
 397         unsigned int config;
 398         unsigned int count;
 399
 400         // setup the timer
 401         config = apic_read(APIC_LVTT);
 402         config = (config & ~APIC_LVTT_MASK) + APIC_LVTT_M; // timer masked, vector 0
 403         apic_write(APIC_LVTT, config);
 404
 405         config = (apic_read(APIC_TDCR) & ~0x0000000f) + 0xb; // divide clock by one
 406         apic_write(APIC_TDCR, config);
 407
 408         t1 = system_time();
 409         apic_write(APIC_ICRT, 0xffffffff); // start the counter
 410
 411         execute_n_instructions(128*20000);
 412
 413         count = apic_read(APIC_CCRT);
 414         t2 = system_time();
 415
 416         count = 0xffffffff - count;
 417
 418         ka->arch_args.apic_time_cv_factor = (unsigned int)((1000000.0/(t2 - t1)) * count);
 419
 420         dprintf("APIC ticks/sec = %d\n", ka->arch_args.apic_time_cv_factor);
 421 }
 422
 423 int smp_boot(kernel_args *ka, unsigned int kernel_entry)
 424 {
 425 //      dprintf("smp_boot: entry\n");
 426
 427         kernel_entry_point = kernel_entry;
 428         saved_ka = ka;
 429
 430         if(smp_find_mp_config(ka) > 1) {
 431 //              dprintf("smp_boot: had found > 1 cpus\n");
 432 //              dprintf("post config:\n");
 433 //              dprintf("num_cpus = 0x%p\n", ka->num_cpus);
 434 //              dprintf("apic_phys = 0x%p\n", ka->arch_args.apic_phys);
 435 //              dprintf("ioapic_phys = 0x%p\n", ka->arch_args.ioapic_phys);
 436
 437                 // map in the apic & ioapic
 438                 map_page(ka, ka->arch_args.apic_phys, ka->virt_alloc_range[0].start + ka->virt_alloc_range[0].size);
 439                 ka->arch_args.apic = (unsigned int *)(ka->virt_alloc_range[0].start + ka->virt_alloc_range[0].size);
 440                 ka->virt_alloc_range[0].size += PAGE_SIZE;
 441
 442                 map_page(ka, ka->arch_args.ioapic_phys, ka->virt_alloc_range[0].start + ka->virt_alloc_range[0].size);
 443                 ka->arch_args.ioapic = (unsigned int *)(ka->virt_alloc_range[0].start + ka->virt_alloc_range[0].size);
 444                 ka->virt_alloc_range[0].size += PAGE_SIZE;
 445
 446 //              dprintf("apic = 0x%p\n", ka->arch_args.apic);
 447 //              dprintf("ioapic = 0x%p\n", ka->arch_args.ioapic);
 448
 449                 // calculate how fast the apic timer is
 450                 calculate_apic_timer_conversion_factor(ka);
 451
 452 //              dprintf("trampolining other cpus\n");
 453                 smp_boot_all_cpus(ka);
 454 //              dprintf("done trampolining\n");
 455         }
 456
 457 //      dprintf("smp_boot: exit\n");
 458
 459         return 0;
 460 }
 461
 462 static int smp_get_current_cpu(kernel_args *ka)
 463 {
 464         if(ka->arch_args.apic == NULL)
 465                 return 0;
 466         else
 467                 return ka->arch_args.cpu_os_id[(apic_read(APIC_ID) & 0xffffffff) >> 24];
 468 }