3 files changed, 60 insertions, 5 deletions

diff --git a/src/cpu/intel/model_206ax/Kconfig b/src/cpu/intel/model_206ax/Kconfig
index a8419d53ec..9cc6edd94e 100644
--- a/src/cpu/intel/model_206ax/Kconfig
+++ b/src/cpu/intel/model_206ax/Kconfig
@@ -13,6 +13,7 @@ config CPU_SPECIFIC_OPTIONS
 	select UDELAY_LAPIC
 	select SMM_TSEG
 	select MICROCODE_IN_CBFS
+	#select AP_IN_SIPI_WAIT
 
 config BOOTBLOCK_CPU_INIT
 	string
diff --git a/src/cpu/intel/model_206ax/acpi.c b/src/cpu/intel/model_206ax/acpi.c
index f66df51a2a..c8c30a4986 100644
--- a/src/cpu/intel/model_206ax/acpi.c
+++ b/src/cpu/intel/model_206ax/acpi.c
@@ -26,7 +26,6 @@
 #include <arch/acpigen.h>
 #include <arch/cpu.h>
 #include <cpu/x86/msr.h>
-#include <cpu/x86/lapic.h>
 #include <cpu/intel/acpi.h>
 #include <cpu/intel/speedstep.h>
 #include <cpu/intel/turbo.h>
@@ -89,8 +88,8 @@ static int generate_cstate_entries(acpi_cstate_t *cstates,
 
 static int generate_C_state_entries(void)
 {
+	struct cpu_info *info;
 	struct cpu_driver *cpu;
-	struct device *cpu_dev;
 	int len, lenif;
 	device_t lapic;
 	struct cpu_intel_model_206ax_config *conf = NULL;
@@ -104,10 +103,10 @@ static int generate_C_state_entries(void)
 		return 0;
 
 	/* Find CPU map of supported C-states */
-	cpu_dev = dev_find_lapic(lapicid());
-	if (!cpu_dev)
+	info = cpu_info();
+	if (!info)
 		return 0;
-	cpu = find_cpu_driver(cpu_dev);
+	cpu = find_cpu_driver(info->cpu);
 	if (!cpu || !cpu->cstates)
 		return 0;
 
diff --git a/src/cpu/intel/model_206ax/model_206ax_init.c b/src/cpu/intel/model_206ax/model_206ax_init.c
index 2ad8012edf..9676ad175b 100644
--- a/src/cpu/intel/model_206ax/model_206ax_init.c
+++ b/src/cpu/intel/model_206ax/model_206ax_init.c
@@ -430,6 +430,58 @@ static void configure_mca(void)
 static unsigned ehci_debug_addr;
 #endif
 
+/*
+ * Initialize any extra cores/threads in this package.
+ */
+static void intel_cores_init(device_t cpu)
+{
+	struct cpuid_result result;
+	unsigned cores, threads, i;
+
+	result = cpuid_ext(0xb, 0); /* Threads per core */
+	threads = result.ebx & 0xff;
+
+	result = cpuid_ext(0xb, 1); /* Cores per package */
+	cores = result.ebx & 0xff;
+
+	/* Only initialize extra cores from BSP */
+	if (cpu->path.apic.apic_id)
+		return;
+
+	printk(BIOS_DEBUG, "CPU: %u has %u cores %u threads\n",
+	       cpu->path.apic.apic_id, cores, threads);
+
+	for (i = 1; i < cores; ++i) {
+		struct device_path cpu_path;
+		device_t new;
+
+		/* Build the cpu device path */
+		cpu_path.type = DEVICE_PATH_APIC;
+		cpu_path.apic.apic_id =
+			cpu->path.apic.apic_id + i;
+
+		/* Update APIC ID if no hyperthreading */
+		if (threads == 1)
+			cpu_path.apic.apic_id <<= 1;
+
+		/* Allocate the new cpu device structure */
+		new = alloc_dev(cpu->bus, &cpu_path);
+		if (!new)
+			continue;
+
+		printk(BIOS_DEBUG, "CPU: %u has core %u\n",
+		       cpu->path.apic.apic_id,
+		       new->path.apic.apic_id);
+
+		/* Start the new cpu */
+		if (!start_cpu(new)) {
+			/* Record the error in cpu? */
+			printk(BIOS_ERR, "CPU %u would not start!\n",
+			       new->path.apic.apic_id);
+		}
+	}
+}
+
 static void model_206ax_init(device_t cpu)
 {
 	char processor_name[49];
@@ -491,6 +543,9 @@ static void model_206ax_init(device_t cpu)
 
 	/* Enable Turbo */
 	enable_turbo();
+
+	/* Start up extra cores */
+	intel_cores_init(cpu);
 }
 
 static struct device_operations cpu_dev_ops = {