src/smpdetect.c

   1 // CPU count detection
   2 //
   3 // Copyright (C) 2008  Kevin O'Connor <kevin@koconnor.net>
   4 // Copyright (C) 2006 Fabrice Bellard
   5 //
   6 // This file may be distributed under the terms of the GNU GPLv3 license.
   7
   8 #include "util.h" // dprintf
   9 #include "config.h" // CONFIG_*
  10
  11 #define CPUID_APIC (1 << 9)
  12
  13 #define APIC_BASE    ((u8 *)0xfee00000)
  14 #define APIC_ICR_LOW 0x300
  15 #define APIC_SVR     0x0F0
  16 #define APIC_ID      0x020
  17 #define APIC_LVT3    0x370
  18
  19 #define APIC_ENABLED 0x0100
  20
  21 static inline void writel(void *addr, u32 val)
  22 {
  23     *(volatile u32 *)addr = val;
  24 }
  25
  26 static inline void writew(void *addr, u16 val)
  27 {
  28     *(volatile u16 *)addr = val;
  29 }
  30
  31 static inline void writeb(void *addr, u8 val)
  32 {
  33     *(volatile u8 *)addr = val;
  34 }
  35
  36 static inline u32 readl(const void *addr)
  37 {
  38     return *(volatile const u32 *)addr;
  39 }
  40
  41 static inline u16 readw(const void *addr)
  42 {
  43     return *(volatile const u16 *)addr;
  44 }
  45
  46 static inline u8 readb(const void *addr)
  47 {
  48     return *(volatile const u8 *)addr;
  49 }
  50
  51 asm(
  52     ".global smp_ap_boot_code_start\n"
  53     ".global smp_ap_boot_code_end\n"
  54     "  .code16\n"
  55
  56     "smp_ap_boot_code_start:\n"
  57     "  xor %ax, %ax\n"
  58     "  mov %ax, %ds\n"
  59     "  incw " __stringify(BUILD_CPU_COUNT_ADDR) "\n"
  60     "1:\n"
  61     "  hlt\n"
  62     "  jmp 1b\n"
  63     "smp_ap_boot_code_end:\n"
  64
  65     "  .code32\n"
  66     );
  67
  68 extern u8 smp_ap_boot_code_start;
  69 extern u8 smp_ap_boot_code_end;
  70
  71 static int smp_cpus;
  72
  73 /* find the number of CPUs by launching a SIPI to them */
  74 int
  75 smp_probe(void)
  76 {
  77     if (smp_cpus)
  78         return smp_cpus;
  79
  80     smp_cpus = 1;
  81
  82     u32 eax, ebx, ecx, cpuid_features;
  83     cpuid(1, &eax, &ebx, &ecx, &cpuid_features);
  84     if (cpuid_features & CPUID_APIC) {
  85         /* enable local APIC */
  86         u32 val = readl(APIC_BASE + APIC_SVR);
  87         val |= APIC_ENABLED;
  88         writel(APIC_BASE + APIC_SVR, val);
  89
  90         writew((void *)BUILD_CPU_COUNT_ADDR, 1);
  91         /* copy AP boot code */
  92         memcpy((void *)BUILD_AP_BOOT_ADDR, &smp_ap_boot_code_start,
  93                &smp_ap_boot_code_end - &smp_ap_boot_code_start);
  94
  95         /* broadcast SIPI */
  96         writel(APIC_BASE + APIC_ICR_LOW, 0x000C4500);
  97         u32 sipi_vector = BUILD_AP_BOOT_ADDR >> 12;
  98         writel(APIC_BASE + APIC_ICR_LOW, 0x000C4600 | sipi_vector);
  99
 100         usleep(10*1000);
 101
 102         smp_cpus = readw((void *)BUILD_CPU_COUNT_ADDR);
 103     }
 104     dprintf(1, "Found %d cpu(s)\n", smp_cpus);
 105
 106     return smp_cpus;
 107 }