| // CPU count detection |
| // |
| // Copyright (C) 2008 Kevin O'Connor <kevin@koconnor.net> |
| // Copyright (C) 2006 Fabrice Bellard |
| // |
| // This file may be distributed under the terms of the GNU GPLv3 license. |
| |
| #include "util.h" // dprintf |
| |
| #define CPUID_APIC (1 << 9) |
| |
| #define APIC_BASE ((u8 *)0xfee00000) |
| #define APIC_ICR_LOW 0x300 |
| #define APIC_SVR 0x0F0 |
| #define APIC_ID 0x020 |
| #define APIC_LVT3 0x370 |
| |
| #define APIC_ENABLED 0x0100 |
| |
| static inline void writel(void *addr, u32 val) |
| { |
| *(volatile u32 *)addr = val; |
| } |
| |
| static inline void writew(void *addr, u16 val) |
| { |
| *(volatile u16 *)addr = val; |
| } |
| |
| static inline void writeb(void *addr, u8 val) |
| { |
| *(volatile u8 *)addr = val; |
| } |
| |
| static inline u32 readl(const void *addr) |
| { |
| return *(volatile const u32 *)addr; |
| } |
| |
| static inline u16 readw(const void *addr) |
| { |
| return *(volatile const u16 *)addr; |
| } |
| |
| static inline u8 readb(const void *addr) |
| { |
| return *(volatile const u8 *)addr; |
| } |
| |
| asm( |
| ".globl smp_ap_boot_code_start\n" |
| ".globl smp_ap_boot_code_end\n" |
| " .code16\n" |
| |
| "smp_ap_boot_code_start:\n" |
| " xor %ax, %ax\n" |
| " mov %ax, %ds\n" |
| " incw " __stringify(BUILD_CPU_COUNT_ADDR) "\n" |
| "1:\n" |
| " hlt\n" |
| " jmp 1b\n" |
| "smp_ap_boot_code_end:\n" |
| |
| " .code32\n" |
| ); |
| |
| extern u8 smp_ap_boot_code_start; |
| extern u8 smp_ap_boot_code_end; |
| |
| static int smp_cpus; |
| |
| /* find the number of CPUs by launching a SIPI to them */ |
| int |
| smp_probe(void) |
| { |
| if (smp_cpus) |
| return smp_cpus; |
| |
| smp_cpus = 1; |
| |
| u32 eax, ebx, ecx, cpuid_features; |
| cpuid(1, &eax, &ebx, &ecx, &cpuid_features); |
| if (cpuid_features & CPUID_APIC) { |
| /* enable local APIC */ |
| u32 val = readl(APIC_BASE + APIC_SVR); |
| val |= APIC_ENABLED; |
| writel(APIC_BASE + APIC_SVR, val); |
| |
| writew((void *)BUILD_CPU_COUNT_ADDR, 1); |
| /* copy AP boot code */ |
| memcpy((void *)BUILD_AP_BOOT_ADDR, &smp_ap_boot_code_start, |
| &smp_ap_boot_code_end - &smp_ap_boot_code_start); |
| |
| /* broadcast SIPI */ |
| writel(APIC_BASE + APIC_ICR_LOW, 0x000C4500); |
| u32 sipi_vector = BUILD_AP_BOOT_ADDR >> 12; |
| writel(APIC_BASE + APIC_ICR_LOW, 0x000C4600 | sipi_vector); |
| |
| usleep(10*1000); |
| |
| smp_cpus = readw((void *)BUILD_CPU_COUNT_ADDR); |
| } |
| dprintf(1, "Found %d cpu(s)\n", smp_cpus); |
| |
| return smp_cpus; |
| } |