18 files changed, 448 insertions, 236 deletions

diff --git a/arch/x86/cpu/Makefile b/arch/x86/cpu/Makefile
index 7ff05e6628..8a8e63e1d3 100644
--- a/arch/x86/cpu/Makefile
+++ b/arch/x86/cpu/Makefile
@@ -10,7 +10,7 @@
 
 extra-y	= start.o
 obj-$(CONFIG_X86_RESET_VECTOR) += resetvec.o start16.o
-obj-y	+= interrupts.o cpu.o call64.o
+obj-y	+= interrupts.o cpu.o cpu_x86.o call64.o
 
 obj-$(CONFIG_INTEL_BAYTRAIL) += baytrail/
 obj-$(CONFIG_SYS_COREBOOT) += coreboot/
@@ -19,7 +19,7 @@ obj-$(CONFIG_NORTHBRIDGE_INTEL_SANDYBRIDGE) += ivybridge/
 obj-$(CONFIG_NORTHBRIDGE_INTEL_IVYBRIDGE) += ivybridge/
 obj-$(CONFIG_INTEL_QUARK) += quark/
 obj-$(CONFIG_INTEL_QUEENSBAY) += queensbay/
-obj-y += irq.o lapic.o
+obj-y += irq.o lapic.o ioapic.o
 obj-$(CONFIG_SMP) += mp_init.o
 obj-y += mtrr.o
 obj-$(CONFIG_PCI) += pci.o
diff --git a/arch/x86/cpu/baytrail/Makefile b/arch/x86/cpu/baytrail/Makefile
index c78b644eb7..5be5491643 100644
--- a/arch/x86/cpu/baytrail/Makefile
+++ b/arch/x86/cpu/baytrail/Makefile
@@ -7,5 +7,4 @@
 obj-y += cpu.o
 obj-y += early_uart.o
 obj-y += fsp_configs.o
-obj-y += pci.o
 obj-y += valleyview.o
diff --git a/arch/x86/cpu/baytrail/cpu.c b/arch/x86/cpu/baytrail/cpu.c
index 1d482064b2..a0117308ae 100644
--- a/arch/x86/cpu/baytrail/cpu.c
+++ b/arch/x86/cpu/baytrail/cpu.c
@@ -10,79 +10,11 @@
 #include <cpu.h>
 #include <dm.h>
 #include <asm/cpu.h>
+#include <asm/cpu_x86.h>
 #include <asm/lapic.h>
-#include <asm/mp.h>
 #include <asm/msr.h>
 #include <asm/turbo.h>
 
-#ifdef CONFIG_SMP
-static int enable_smis(struct udevice *cpu, void *unused)
-{
-	return 0;
-}
-
-static struct mp_flight_record mp_steps[] = {
-	MP_FR_BLOCK_APS(mp_init_cpu, NULL, mp_init_cpu, NULL),
-	/* Wait for APs to finish initialization before proceeding. */
-	MP_FR_BLOCK_APS(NULL, NULL, enable_smis, NULL),
-};
-
-static int detect_num_cpus(void)
-{
-	int ecx = 0;
-
-	/*
-	 * Use the algorithm described in Intel 64 and IA-32 Architectures
-	 * Software Developer's Manual Volume 3 (3A, 3B & 3C): System
-	 * Programming Guide, Jan-2015. Section 8.9.2: Hierarchical Mapping
-	 * of CPUID Extended Topology Leaf.
-	 */
-	while (1) {
-		struct cpuid_result leaf_b;
-
-		leaf_b = cpuid_ext(0xb, ecx);
-
-		/*
-		 * Bay Trail doesn't have hyperthreading so just determine the
-		 * number of cores by from level type (ecx[15:8] == * 2)
-		 */
-		if ((leaf_b.ecx & 0xff00) == 0x0200)
-			return leaf_b.ebx & 0xffff;
-		ecx++;
-	}
-}
-
-static int baytrail_init_cpus(void)
-{
-	struct mp_params mp_params;
-
-	lapic_setup();
-
-	mp_params.num_cpus = detect_num_cpus();
-	mp_params.parallel_microcode_load = 0,
-	mp_params.flight_plan = &mp_steps[0];
-	mp_params.num_records = ARRAY_SIZE(mp_steps);
-	mp_params.microcode_pointer = 0;
-
-	if (mp_init(&mp_params)) {
-		printf("Warning: MP init failure\n");
-		return -EIO;
-	}
-
-	return 0;
-}
-#endif
-
-int x86_init_cpus(void)
-{
-#ifdef CONFIG_SMP
-	debug("Init additional CPUs\n");
-	baytrail_init_cpus();
-#endif
-
-	return 0;
-}
-
 static void set_max_freq(void)
 {
 	msr_t perf_ctl;
@@ -175,19 +107,38 @@ static int baytrail_get_info(struct udevice *dev, struct cpu_info *info)
 	return 0;
 }
 
-static int cpu_x86_baytrail_bind(struct udevice *dev)
+static int baytrail_get_count(struct udevice *dev)
 {
-	struct cpu_platdata *plat = dev_get_parent_platdata(dev);
+	int ecx = 0;
+
+	/*
+	 * Use the algorithm described in Intel 64 and IA-32 Architectures
+	 * Software Developer's Manual Volume 3 (3A, 3B & 3C): System
+	 * Programming Guide, Jan-2015. Section 8.9.2: Hierarchical Mapping
+	 * of CPUID Extended Topology Leaf.
+	 */
+	while (1) {
+		struct cpuid_result leaf_b;
+
+		leaf_b = cpuid_ext(0xb, ecx);
+
+		/*
+		 * Bay Trail doesn't have hyperthreading so just determine the
+		 * number of cores by from level type (ecx[15:8] == * 2)
+		 */
+		if ((leaf_b.ecx & 0xff00) == 0x0200)
+			return leaf_b.ebx & 0xffff;
 
-	plat->cpu_id = fdtdec_get_int(gd->fdt_blob, dev->of_offset,
-				      "intel,apic-id", -1);
+		ecx++;
+	}
 
 	return 0;
 }
 
 static const struct cpu_ops cpu_x86_baytrail_ops = {
-	.get_desc	= x86_cpu_get_desc,
+	.get_desc	= cpu_x86_get_desc,
 	.get_info	= baytrail_get_info,
+	.get_count	= baytrail_get_count,
 };
 
 static const struct udevice_id cpu_x86_baytrail_ids[] = {
@@ -199,7 +150,7 @@ U_BOOT_DRIVER(cpu_x86_baytrail_drv) = {
 	.name		= "cpu_x86_baytrail",
 	.id		= UCLASS_CPU,
 	.of_match	= cpu_x86_baytrail_ids,
-	.bind		= cpu_x86_baytrail_bind,
+	.bind		= cpu_x86_bind,
 	.probe		= cpu_x86_baytrail_probe,
 	.ops		= &cpu_x86_baytrail_ops,
 };
diff --git a/arch/x86/cpu/baytrail/pci.c b/arch/x86/cpu/baytrail/pci.c
deleted file mode 100644
index 48409de5c4..0000000000
--- a/arch/x86/cpu/baytrail/pci.c
+++ /dev/null
@@ -1,46 +0,0 @@
-/*
- * Copyright (C) 2014, Bin Meng <bmeng.cn@gmail.com>
- *
- * SPDX-License-Identifier:	GPL-2.0+
- */
-
-#include <common.h>
-#include <pci.h>
-#include <asm/pci.h>
-#include <asm/fsp/fsp_support.h>
-
-DECLARE_GLOBAL_DATA_PTR;
-
-void board_pci_setup_hose(struct pci_controller *hose)
-{
-	hose->first_busno = 0;
-	hose->last_busno = 0;
-
-	/* PCI memory space */
-	pci_set_region(hose->regions + 0,
-		       CONFIG_PCI_MEM_BUS,
-		       CONFIG_PCI_MEM_PHYS,
-		       CONFIG_PCI_MEM_SIZE,
-		       PCI_REGION_MEM);
-
-	/* PCI IO space */
-	pci_set_region(hose->regions + 1,
-		       CONFIG_PCI_IO_BUS,
-		       CONFIG_PCI_IO_PHYS,
-		       CONFIG_PCI_IO_SIZE,
-		       PCI_REGION_IO);
-
-	pci_set_region(hose->regions + 2,
-		       CONFIG_PCI_PREF_BUS,
-		       CONFIG_PCI_PREF_PHYS,
-		       CONFIG_PCI_PREF_SIZE,
-		       PCI_REGION_PREFETCH);
-
-	pci_set_region(hose->regions + 3,
-		       0,
-		       0,
-		       gd->ram_size < 0x80000000 ? gd->ram_size : 0x80000000,
-		       PCI_REGION_MEM | PCI_REGION_SYS_MEMORY);
-
-	hose->region_count = 4;
-}
diff --git a/arch/x86/cpu/config.mk b/arch/x86/cpu/config.mk
index 4c4d0c7cd2..1263221e69 100644
--- a/arch/x86/cpu/config.mk
+++ b/arch/x86/cpu/config.mk
@@ -10,8 +10,8 @@ CROSS_COMPILE ?= i386-linux-
 PLATFORM_CPPFLAGS += -D__I386__
 
 # DO NOT MODIFY THE FOLLOWING UNLESS YOU REALLY KNOW WHAT YOU ARE DOING!
-LDPPFLAGS += -DRESET_SEG_START=0xffff0000
-LDPPFLAGS += -DRESET_SEG_SIZE=0x10000
-LDPPFLAGS += -DRESET_VEC_LOC=0xfffffff0
+LDPPFLAGS += -DRESET_SEG_START=$(CONFIG_RESET_SEG_START)
+LDPPFLAGS += -DRESET_SEG_SIZE=$(CONFIG_RESET_SEG_SIZE)
+LDPPFLAGS += -DRESET_VEC_LOC=$(CONFIG_RESET_VEC_LOC)
 LDPPFLAGS += -DSTART_16=$(CONFIG_SYS_X86_START16)
 LDPPFLAGS += -DRESET_BASE="CONFIG_SYS_TEXT_BASE + (CONFIG_SYS_MONITOR_LEN - RESET_SEG_SIZE)"
diff --git a/arch/x86/cpu/coreboot/pci.c b/arch/x86/cpu/coreboot/pci.c
index 67eb14ce99..41e29a6086 100644
--- a/arch/x86/cpu/coreboot/pci.c
+++ b/arch/x86/cpu/coreboot/pci.c
@@ -11,29 +11,7 @@
 
 #include <common.h>
 #include <dm.h>
-#include <errno.h>
 #include <pci.h>
-#include <asm/io.h>
-#include <asm/pci.h>
-
-DECLARE_GLOBAL_DATA_PTR;
-
-static const struct dm_pci_ops pci_x86_ops = {
-	.read_config	= pci_x86_read_config,
-	.write_config	= pci_x86_write_config,
-};
-
-static const struct udevice_id pci_x86_ids[] = {
-	{ .compatible = "pci-x86" },
-	{ }
-};
-
-U_BOOT_DRIVER(pci_x86_drv) = {
-	.name		= "pci_x86",
-	.id		= UCLASS_PCI,
-	.of_match	= pci_x86_ids,
-	.ops		= &pci_x86_ops,
-};
 
 static const struct udevice_id generic_pch_ids[] = {
 	{ .compatible = "intel,pch" },
diff --git a/arch/x86/cpu/cpu.c b/arch/x86/cpu/cpu.c
index bb4a110c00..af927b94e0 100644
--- a/arch/x86/cpu/cpu.c
+++ b/arch/x86/cpu/cpu.c
@@ -21,12 +21,15 @@
 
 #include <common.h>
 #include <command.h>
-#include <cpu.h>
 #include <dm.h>
 #include <errno.h>
 #include <malloc.h>
 #include <asm/control_regs.h>
 #include <asm/cpu.h>
+#include <asm/lapic.h>
+#include <asm/mp.h>
+#include <asm/msr.h>
+#include <asm/mtrr.h>
 #include <asm/post.h>
 #include <asm/processor.h>
 #include <asm/processor-flags.h>
@@ -164,6 +167,26 @@ void setup_gdt(gd_t *id, u64 *gdt_addr)
 	load_fs(X86_GDT_ENTRY_32BIT_FS);
 }
 
+#ifdef CONFIG_HAVE_FSP
+/*
+ * Setup FSP execution environment GDT
+ *
+ * Per Intel FSP external architecture specification, before calling any FSP
+ * APIs, we need make sure the system is in flat 32-bit mode and both the code
+ * and data selectors should have full 4GB access range. Here we reuse the one
+ * we used in arch/x86/cpu/start16.S, and reload the segement registers.
+ */
+void setup_fsp_gdt(void)
+{
+	load_gdt((const u64 *)(gdt_rom + CONFIG_RESET_SEG_START), 4);
+	load_ds(X86_GDT_ENTRY_32BIT_DS);
+	load_ss(X86_GDT_ENTRY_32BIT_DS);
+	load_es(X86_GDT_ENTRY_32BIT_DS);
+	load_fs(X86_GDT_ENTRY_32BIT_DS);
+	load_gs(X86_GDT_ENTRY_32BIT_DS);
+}
+#endif
+
 int __weak x86_cleanup_before_linux(void)
 {
 #ifdef CONFIG_BOOTSTAGE_STASH
@@ -330,6 +353,28 @@ int x86_cpu_init_f(void)
 
 		gd->arch.has_mtrr = has_mtrr();
 	}
+	/* Don't allow PCI region 3 to use memory in the 2-4GB memory hole */
+	gd->pci_ram_top = 0x80000000U;
+
+	/* Configure fixed range MTRRs for some legacy regions */
+	if (gd->arch.has_mtrr) {
+		u64 mtrr_cap;
+
+		mtrr_cap = native_read_msr(MTRR_CAP_MSR);
+		if (mtrr_cap & MTRR_CAP_FIX) {
+			/* Mark the VGA RAM area as uncacheable */
+			native_write_msr(MTRR_FIX_16K_A0000_MSR, 0, 0);
+
+			/* Mark the PCI ROM area as uncacheable */
+			native_write_msr(MTRR_FIX_4K_C0000_MSR, 0, 0);
+			native_write_msr(MTRR_FIX_4K_C8000_MSR, 0, 0);
+			native_write_msr(MTRR_FIX_4K_D0000_MSR, 0, 0);
+			native_write_msr(MTRR_FIX_4K_D8000_MSR, 0, 0);
+
+			/* Enable the fixed range MTRRs */
+			msr_setbits_64(MTRR_DEF_TYPE_MSR, MTRR_DEF_TYPE_FIX_EN);
+		}
+	}
 
 	return 0;
 }
@@ -520,16 +565,6 @@ char *cpu_get_name(char *name)
 	return ptr;
 }
 
-int x86_cpu_get_desc(struct udevice *dev, char *buf, int size)
-{
-	if (size < CPU_MAX_NAME_LEN)
-		return -ENOSPC;
-
-	cpu_get_name(buf);
-
-	return 0;
-}
-
 int default_print_cpuinfo(void)
 {
 	printf("CPU: %s, vendor %s, device %xh\n",
@@ -613,28 +648,47 @@ int last_stage_init(void)
 }
 #endif
 
-__weak int x86_init_cpus(void)
+#ifdef CONFIG_SMP
+static int enable_smis(struct udevice *cpu, void *unused)
 {
 	return 0;
 }
 
-int cpu_init_r(void)
+static struct mp_flight_record mp_steps[] = {
+	MP_FR_BLOCK_APS(mp_init_cpu, NULL, mp_init_cpu, NULL),
+	/* Wait for APs to finish initialization before proceeding */
+	MP_FR_BLOCK_APS(NULL, NULL, enable_smis, NULL),
+};
+
+static int x86_mp_init(void)
 {
-	return x86_init_cpus();
+	struct mp_params mp_params;
+
+	mp_params.parallel_microcode_load = 0,
+	mp_params.flight_plan = &mp_steps[0];
+	mp_params.num_records = ARRAY_SIZE(mp_steps);
+	mp_params.microcode_pointer = 0;
+
+	if (mp_init(&mp_params)) {
+		printf("Warning: MP init failure\n");
+		return -EIO;
+	}
+
+	return 0;
 }
+#endif
 
-static const struct cpu_ops cpu_x86_ops = {
-	.get_desc	= x86_cpu_get_desc,
-};
+__weak int x86_init_cpus(void)
+{
+#ifdef CONFIG_SMP
+	debug("Init additional CPUs\n");
+	x86_mp_init();
+#endif
 
-static const struct udevice_id cpu_x86_ids[] = {
-	{ .compatible = "cpu-x86" },
-	{ }
-};
+	return 0;
+}
 
-U_BOOT_DRIVER(cpu_x86_drv) = {
-	.name		= "cpu_x86",
-	.id		= UCLASS_CPU,
-	.of_match	= cpu_x86_ids,
-	.ops		= &cpu_x86_ops,
-};
+int cpu_init_r(void)
+{
+	return x86_init_cpus();
+}
diff --git a/arch/x86/cpu/cpu_x86.c b/arch/x86/cpu/cpu_x86.c
new file mode 100644
index 0000000000..09410416a1
--- /dev/null
+++ b/arch/x86/cpu/cpu_x86.c
@@ -0,0 +1,76 @@
+/*
+ * Copyright (C) 2015, Bin Meng <bmeng.cn@gmail.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <cpu.h>
+#include <dm.h>
+#include <errno.h>
+#include <asm/cpu.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+int cpu_x86_bind(struct udevice *dev)
+{
+	struct cpu_platdata *plat = dev_get_parent_platdata(dev);
+
+	plat->cpu_id = fdtdec_get_int(gd->fdt_blob, dev->of_offset,
+				      "intel,apic-id", -1);
+
+	return 0;
+}
+
+int cpu_x86_get_desc(struct udevice *dev, char *buf, int size)
+{
+	if (size < CPU_MAX_NAME_LEN)
+		return -ENOSPC;
+
+	cpu_get_name(buf);
+
+	return 0;
+}
+
+static int cpu_x86_get_count(struct udevice *dev)
+{
+	int node, cpu;
+	int num = 0;
+
+	node = fdt_path_offset(gd->fdt_blob, "/cpus");
+	if (node < 0)
+		return -ENOENT;
+
+	for (cpu = fdt_first_subnode(gd->fdt_blob, node);
+	     cpu >= 0;
+	     cpu = fdt_next_subnode(gd->fdt_blob, cpu)) {
+		const char *device_type;
+
+		device_type = fdt_getprop(gd->fdt_blob, cpu,
+					  "device_type", NULL);
+		if (!device_type)
+			continue;
+		if (strcmp(device_type, "cpu") == 0)
+			num++;
+	}
+
+	return num;
+}
+
+static const struct cpu_ops cpu_x86_ops = {
+	.get_desc	= cpu_x86_get_desc,
+	.get_count	= cpu_x86_get_count,
+};
+
+static const struct udevice_id cpu_x86_ids[] = {
+	{ .compatible = "cpu-x86" },
+	{ }
+};
+
+U_BOOT_DRIVER(cpu_x86_drv) = {
+	.name		= "cpu_x86",
+	.id		= UCLASS_CPU,
+	.of_match	= cpu_x86_ids,
+	.bind		= cpu_x86_bind,
+	.ops		= &cpu_x86_ops,
+};
diff --git a/arch/x86/cpu/ioapic.c b/arch/x86/cpu/ioapic.c
new file mode 100644
index 0000000000..112a9c63b4
--- /dev/null
+++ b/arch/x86/cpu/ioapic.c
@@ -0,0 +1,21 @@
+/*
+ * Copyright (C) 2015, Bin Meng <bmeng.cn@gmail.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/ioapic.h>
+
+u32 io_apic_read(u32 reg)
+{
+	writel(reg, IO_APIC_INDEX);
+	return readl(IO_APIC_DATA);
+}
+
+void io_apic_write(u32 reg, u32 val)
+{
+	writel(reg, IO_APIC_INDEX);
+	writel(val, IO_APIC_DATA);
+}
diff --git a/arch/x86/cpu/irq.c b/arch/x86/cpu/irq.c
index 74b89ad2ff..97dd000039 100644
--- a/arch/x86/cpu/irq.c
+++ b/arch/x86/cpu/irq.c
@@ -58,17 +58,28 @@ void pirq_assign_irq(int link, u8 irq)
 		writeb(irq, irq_router.ibase + LINK_N2V(link, base));
 }
 
-static inline void fill_irq_info(struct irq_info **slotp, int *entries, u8 bus,
-				 u8 device, u8 func, u8 pin, u8 pirq)
+static struct irq_info *check_dup_entry(struct irq_info *slot_base,
+					int entry_num, int bus, int device)
 {
-	struct irq_info *slot = *slotp;
+	struct irq_info *slot = slot_base;
+	int i;
+
+	for (i = 0; i < entry_num; i++) {
+		if (slot->bus == bus && slot->devfn == (device << 3))
+			break;
+		slot++;
+	}
 
+	return (i == entry_num) ? NULL : slot;
+}
+
+static inline void fill_irq_info(struct irq_info *slot, int bus, int device,
+				 int pin, int pirq)
+{
 	slot->bus = bus;
-	slot->devfn = (device << 3) | func;
+	slot->devfn = (device << 3) | 0;
 	slot->irq[pin - 1].link = LINK_N2V(pirq, irq_router.link_base);
 	slot->irq[pin - 1].bitmap = irq_router.irq_mask;
-	(*entries)++;
-	(*slotp)++;
 }
 
 __weak void cpu_irq_init(void)
@@ -84,7 +95,7 @@ static int create_pirq_routing_table(void)
 	int len, count;
 	const u32 *cell;
 	struct irq_routing_table *rt;
-	struct irq_info *slot;
+	struct irq_info *slot, *slot_base;
 	int irq_entries = 0;
 	int i;
 	int ret;
@@ -161,13 +172,13 @@ static int create_pirq_routing_table(void)
 	/* Populate the PIRQ table fields */
 	rt->signature = PIRQ_SIGNATURE;
 	rt->version = PIRQ_VERSION;
-	rt->rtr_bus = 0;
+	rt->rtr_bus = PCI_BUS(irq_router.bdf);
 	rt->rtr_devfn = (PCI_DEV(irq_router.bdf) << 3) |
 			PCI_FUNC(irq_router.bdf);
 	rt->rtr_vendor = PCI_VENDOR_ID_INTEL;
 	rt->rtr_device = PCI_DEVICE_ID_INTEL_ICH7_31;
 
-	slot = rt->slots;
+	slot_base = rt->slots;
 
 	/* Now fill in the irq_info entries in the PIRQ table */
 	for (i = 0; i < count; i++) {
@@ -181,9 +192,44 @@ static int create_pirq_routing_table(void)
 		      i, PCI_BUS(pr.bdf), PCI_DEV(pr.bdf),
 		      PCI_FUNC(pr.bdf), 'A' + pr.pin - 1,
 		      'A' + pr.pirq);
-		fill_irq_info(&slot, &irq_entries, PCI_BUS(pr.bdf),
-			      PCI_DEV(pr.bdf), PCI_FUNC(pr.bdf),
+
+		slot = check_dup_entry(slot_base, irq_entries,
+				       PCI_BUS(pr.bdf), PCI_DEV(pr.bdf));
+		if (slot) {
+			debug("found entry for bus %d device %d, ",
+			      PCI_BUS(pr.bdf), PCI_DEV(pr.bdf));
+
+			if (slot->irq[pr.pin - 1].link) {
+				debug("skipping\n");
+
+				/*
+				 * Sanity test on the routed PIRQ pin
+				 *
+				 * If they don't match, show a warning to tell
+				 * there might be something wrong with the PIRQ
+				 * routing information in the device tree.
+				 */
+				if (slot->irq[pr.pin - 1].link !=
+					LINK_N2V(pr.pirq, irq_router.link_base))
+					debug("WARNING: Inconsistent PIRQ routing information\n");
+
+				cell += sizeof(struct pirq_routing) /
+					sizeof(u32);
+				continue;
+			} else {
+				debug("writing INT%c\n", 'A' + pr.pin - 1);
+				fill_irq_info(slot, PCI_BUS(pr.bdf),
+					      PCI_DEV(pr.bdf), pr.pin, pr.pirq);
+				cell += sizeof(struct pirq_routing) /
+					sizeof(u32);
+				continue;
+			}
+		}
+
+		slot = slot_base + irq_entries;
+		fill_irq_info(slot, PCI_BUS(pr.bdf), PCI_DEV(pr.bdf),
 			      pr.pin, pr.pirq);
+		irq_entries++;
 		cell += sizeof(struct pirq_routing) / sizeof(u32);
 	}
 
diff --git a/arch/x86/cpu/ivybridge/Kconfig b/arch/x86/cpu/ivybridge/Kconfig
index e4595be3ae..0e249a40a6 100644
--- a/arch/x86/cpu/ivybridge/Kconfig
+++ b/arch/x86/cpu/ivybridge/Kconfig
@@ -95,7 +95,6 @@ config CPU_SPECIFIC_OPTIONS
 	select ARCH_BOOTBLOCK_X86_32
 	select ARCH_ROMSTAGE_X86_32
 	select ARCH_RAMSTAGE_X86_32
-	select SMP
 	select SSE2
 	select UDELAY_LAPIC
 	select CPU_MICROCODE_IN_CBFS
diff --git a/arch/x86/cpu/ivybridge/model_206ax.c b/arch/x86/cpu/ivybridge/model_206ax.c
index 8b08c40bcb..fd7db97cbd 100644
--- a/arch/x86/cpu/ivybridge/model_206ax.c
+++ b/arch/x86/cpu/ivybridge/model_206ax.c
@@ -13,12 +13,12 @@
 #include <asm/acpi.h>
 #include <asm/cpu.h>
 #include <asm/lapic.h>
-#include <asm/lapic_def.h>
 #include <asm/msr.h>
 #include <asm/mtrr.h>
 #include <asm/processor.h>
 #include <asm/speedstep.h>
 #include <asm/turbo.h>
+#include <asm/arch/bd82x6x.h>
 #include <asm/arch/model_206ax.h>
 
 static void enable_vmx(void)
diff --git a/arch/x86/cpu/lapic.c b/arch/x86/cpu/lapic.c
index 4690603c75..30d23130eb 100644
--- a/arch/x86/cpu/lapic.c
+++ b/arch/x86/cpu/lapic.c
@@ -8,50 +8,153 @@
  */
 
 #include <common.h>
-#include <asm/msr.h>
 #include <asm/io.h>
 #include <asm/lapic.h>
+#include <asm/msr.h>
+#include <asm/msr-index.h>
 #include <asm/post.h>
 
+unsigned long lapic_read(unsigned long reg)
+{
+	return readl(LAPIC_DEFAULT_BASE + reg);
+}
+
+#define xchg(ptr, v)	((__typeof__(*(ptr)))__xchg((unsigned long)(v), (ptr), \
+						    sizeof(*(ptr))))
+
+struct __xchg_dummy	{ unsigned long a[100]; };
+#define __xg(x)		((struct __xchg_dummy *)(x))
+
+/*
+ * Note: no "lock" prefix even on SMP. xchg always implies lock anyway.
+ *
+ * Note 2: xchg has side effect, so that attribute volatile is necessary,
+ *         but generally the primitive is invalid, *ptr is output argument.
+ */
+static inline unsigned long __xchg(unsigned long x, volatile void *ptr,
+				   int size)
+{
+	switch (size) {
+	case 1:
+		__asm__ __volatile__("xchgb %b0,%1"
+			: "=q" (x)
+			: "m" (*__xg(ptr)), "0" (x)
+			: "memory");
+		break;
+	case 2:
+		__asm__ __volatile__("xchgw %w0,%1"
+			: "=r" (x)
+			: "m" (*__xg(ptr)), "0" (x)
+			: "memory");
+		break;
+	case 4:
+		__asm__ __volatile__("xchgl %0,%1"
+			: "=r" (x)
+			: "m" (*__xg(ptr)), "0" (x)
+			: "memory");
+		break;
+	}
+
+	return x;
+}
+
+void lapic_write(unsigned long reg, unsigned long v)
+{
+	(void)xchg((volatile unsigned long *)(LAPIC_DEFAULT_BASE + reg), v);
+}
+
+void enable_lapic(void)
+{
+	msr_t msr;
+
+	msr = msr_read(MSR_IA32_APICBASE);
+	msr.hi &= 0xffffff00;
+	msr.lo |= MSR_IA32_APICBASE_ENABLE;
+	msr.lo &= ~MSR_IA32_APICBASE_BASE;
+	msr.lo |= LAPIC_DEFAULT_BASE;
+	msr_write(MSR_IA32_APICBASE, msr);
+}
+
+void disable_lapic(void)
+{
+	msr_t msr;
+
+	msr = msr_read(MSR_IA32_APICBASE);
+	msr.lo &= ~MSR_IA32_APICBASE_ENABLE;
+	msr_write(MSR_IA32_APICBASE, msr);
+}
+
+unsigned long lapicid(void)
+{
+	return lapic_read(LAPIC_ID) >> 24;
+}
+
+static void lapic_wait_icr_idle(void)
+{
+	do { } while (lapic_read(LAPIC_ICR) & LAPIC_ICR_BUSY);
+}
+
+int lapic_remote_read(int apicid, int reg, unsigned long *pvalue)
+{
+	int timeout;
+	unsigned long status;
+	int result;
+
+	lapic_wait_icr_idle();
+	lapic_write(LAPIC_ICR2, SET_LAPIC_DEST_FIELD(apicid));
+	lapic_write(LAPIC_ICR, LAPIC_DM_REMRD | (reg >> 4));
+
+	timeout = 0;
+	do {
+		status = lapic_read(LAPIC_ICR) & LAPIC_ICR_RR_MASK;
+	} while (status == LAPIC_ICR_RR_INPROG && timeout++ < 1000);
+
+	result = -1;
+	if (status == LAPIC_ICR_RR_VALID) {
+		*pvalue = lapic_read(LAPIC_RRR);
+		result = 0;
+	}
+
+	return result;
+}
+
 void lapic_setup(void)
 {
-#if NEED_LAPIC == 1
+#ifdef CONFIG_SMP
 	/* Only Pentium Pro and later have those MSR stuff */
 	debug("Setting up local apic: ");
 
 	/* Enable the local apic */
 	enable_lapic();
 
-	/*
-	 * Set Task Priority to 'accept all'.
-	 */
-	lapic_write_around(LAPIC_TASKPRI,
-			   lapic_read_around(LAPIC_TASKPRI) & ~LAPIC_TPRI_MASK);
+	/* Set Task Priority to 'accept all' */
+	lapic_write(LAPIC_TASKPRI,
+		    lapic_read(LAPIC_TASKPRI) & ~LAPIC_TPRI_MASK);
 
 	/* Put the local apic in virtual wire mode */
-	lapic_write_around(LAPIC_SPIV, (lapic_read_around(LAPIC_SPIV) &
-				~(LAPIC_VECTOR_MASK)) | LAPIC_SPIV_ENABLE);
-	lapic_write_around(LAPIC_LVT0, (lapic_read_around(LAPIC_LVT0) &
-			~(LAPIC_LVT_MASKED | LAPIC_LVT_LEVEL_TRIGGER |
-			  LAPIC_LVT_REMOTE_IRR | LAPIC_INPUT_POLARITY |
-			  LAPIC_SEND_PENDING | LAPIC_LVT_RESERVED_1 |
-			  LAPIC_DELIVERY_MODE_MASK)) |
-			(LAPIC_LVT_REMOTE_IRR | LAPIC_SEND_PENDING |
-			 LAPIC_DELIVERY_MODE_EXTINT));
-	lapic_write_around(LAPIC_LVT1, (lapic_read_around(LAPIC_LVT1) &
-			~(LAPIC_LVT_MASKED | LAPIC_LVT_LEVEL_TRIGGER |
-			  LAPIC_LVT_REMOTE_IRR | LAPIC_INPUT_POLARITY |
-			  LAPIC_SEND_PENDING | LAPIC_LVT_RESERVED_1 |
-			  LAPIC_DELIVERY_MODE_MASK)) |
-		(LAPIC_LVT_REMOTE_IRR | LAPIC_SEND_PENDING |
-			LAPIC_DELIVERY_MODE_NMI));
+	lapic_write(LAPIC_SPIV, (lapic_read(LAPIC_SPIV) &
+		    ~(LAPIC_VECTOR_MASK)) | LAPIC_SPIV_ENABLE);
+	lapic_write(LAPIC_LVT0, (lapic_read(LAPIC_LVT0) &
+		    ~(LAPIC_LVT_MASKED | LAPIC_LVT_LEVEL_TRIGGER |
+		    LAPIC_LVT_REMOTE_IRR | LAPIC_INPUT_POLARITY |
+		    LAPIC_SEND_PENDING | LAPIC_LVT_RESERVED_1 |
+		    LAPIC_DELIVERY_MODE_MASK)) |
+		    (LAPIC_LVT_REMOTE_IRR | LAPIC_SEND_PENDING |
+		    LAPIC_DELIVERY_MODE_EXTINT));
+	lapic_write(LAPIC_LVT1, (lapic_read(LAPIC_LVT1) &
+		    ~(LAPIC_LVT_MASKED | LAPIC_LVT_LEVEL_TRIGGER |
+		    LAPIC_LVT_REMOTE_IRR | LAPIC_INPUT_POLARITY |
+		    LAPIC_SEND_PENDING | LAPIC_LVT_RESERVED_1 |
+		    LAPIC_DELIVERY_MODE_MASK)) |
+		    (LAPIC_LVT_REMOTE_IRR | LAPIC_SEND_PENDING |
+		    LAPIC_DELIVERY_MODE_NMI));
 
 	debug("apic_id: 0x%02lx, ", lapicid());
-#else /* !NEED_LLAPIC */
+#else /* !CONFIG_SMP */
 	/* Only Pentium Pro and later have those MSR stuff */
 	debug("Disabling local apic: ");
 	disable_lapic();
-#endif /* !NEED_LAPIC */
+#endif /* CONFIG_SMP */
 	debug("done.\n");
 	post_code(POST_LAPIC);
 }
diff --git a/arch/x86/cpu/mp_init.c b/arch/x86/cpu/mp_init.c
index ac5753a1fd..e686b28c9c 100644
--- a/arch/x86/cpu/mp_init.c
+++ b/arch/x86/cpu/mp_init.c
@@ -16,12 +16,17 @@
 #include <asm/interrupt.h>
 #include <asm/lapic.h>
 #include <asm/mp.h>
+#include <asm/msr.h>
 #include <asm/mtrr.h>
+#include <asm/processor.h>
 #include <asm/sipi.h>
 #include <dm/device-internal.h>
 #include <dm/uclass-internal.h>
 #include <linux/linkage.h>
 
+/* Total CPUs include BSP */
+static int num_cpus;
+
 /* This also needs to match the sipi.S assembly code for saved MSR encoding */
 struct saved_msr {
 	uint32_t index;
@@ -56,6 +61,13 @@ static inline void release_barrier(atomic_t *b)
 	atomic_set(b, 1);
 }
 
+static inline void stop_this_cpu(void)
+{
+	/* Called by an AP when it is ready to halt and wait for a new task */
+	for (;;)
+		cpu_hlt();
+}
+
 /* Returns 1 if timeout waiting for APs. 0 if target APs found */
 static int wait_for_aps(atomic_t *val, int target, int total_delay,
 			int delay_step)
@@ -314,9 +326,9 @@ static int start_aps(int ap_count, atomic_t *num_aps)
 	}
 
 	/* Send INIT IPI to all but self */
-	lapic_write_around(LAPIC_ICR2, SET_LAPIC_DEST_FIELD(0));
-	lapic_write_around(LAPIC_ICR, LAPIC_DEST_ALLBUT | LAPIC_INT_ASSERT |
-			   LAPIC_DM_INIT);
+	lapic_write(LAPIC_ICR2, SET_LAPIC_DEST_FIELD(0));
+	lapic_write(LAPIC_ICR, LAPIC_DEST_ALLBUT | LAPIC_INT_ASSERT |
+		    LAPIC_DM_INIT);
 	debug("Waiting for 10ms after sending INIT.\n");
 	mdelay(10);
 
@@ -331,9 +343,9 @@ static int start_aps(int ap_count, atomic_t *num_aps)
 		}
 	}
 
-	lapic_write_around(LAPIC_ICR2, SET_LAPIC_DEST_FIELD(0));
-	lapic_write_around(LAPIC_ICR, LAPIC_DEST_ALLBUT | LAPIC_INT_ASSERT |
-			   LAPIC_DM_STARTUP | sipi_vector);
+	lapic_write(LAPIC_ICR2, SET_LAPIC_DEST_FIELD(0));
+	lapic_write(LAPIC_ICR, LAPIC_DEST_ALLBUT | LAPIC_INT_ASSERT |
+		    LAPIC_DM_STARTUP | sipi_vector);
 	debug("Waiting for 1st SIPI to complete...");
 	if (apic_wait_timeout(10000, 50)) {
 		debug("timed out.\n");
@@ -356,9 +368,9 @@ static int start_aps(int ap_count, atomic_t *num_aps)
 		}
 	}
 
-	lapic_write_around(LAPIC_ICR2, SET_LAPIC_DEST_FIELD(0));
-	lapic_write_around(LAPIC_ICR, LAPIC_DEST_ALLBUT | LAPIC_INT_ASSERT |
-			   LAPIC_DM_STARTUP | sipi_vector);
+	lapic_write(LAPIC_ICR2, SET_LAPIC_DEST_FIELD(0));
+	lapic_write(LAPIC_ICR, LAPIC_DEST_ALLBUT | LAPIC_INT_ASSERT |
+		    LAPIC_DM_STARTUP | sipi_vector);
 	debug("Waiting for 2nd SIPI to complete...");
 	if (apic_wait_timeout(10000, 50)) {
 		debug("timed out.\n");
@@ -383,7 +395,7 @@ static int bsp_do_flight_plan(struct udevice *cpu, struct mp_params *mp_params)
 	int ret = 0;
 	const int timeout_us = 100000;
 	const int step_us = 100;
-	int num_aps = mp_params->num_cpus - 1;
+	int num_aps = num_cpus - 1;
 
 	for (i = 0; i < mp_params->num_records; i++) {
 		struct mp_flight_record *rec = &mp_params->flight_plan[i];
@@ -415,7 +427,7 @@ static int init_bsp(struct udevice **devp)
 	cpu_get_name(processor_name);
 	debug("CPU: %s.\n", processor_name);
 
-	enable_lapic();
+	lapic_setup();
 
 	apic_id = lapicid();
 	ret = find_cpu_by_apid_id(apic_id, devp);
@@ -451,7 +463,16 @@ int mp_init(struct mp_params *p)
 		return -1;
 	}
 
-	ret = check_cpu_devices(p->num_cpus);
+	num_cpus = cpu_get_count(cpu);
+	if (num_cpus < 0) {
+		debug("Cannot get number of CPUs: err=%d\n", num_cpus);
+		return num_cpus;
+	}
+
+	if (num_cpus < 2)
+		debug("Warning: Only 1 CPU is detected\n");
+
+	ret = check_cpu_devices(num_cpus);
 	if (ret)
 		debug("Warning: Device tree does not describe all CPUs. Extra ones will not be started correctly\n");
 
@@ -471,7 +492,7 @@ int mp_init(struct mp_params *p)
 	wbinvd();
 
 	/* Start the APs providing number of APs and the cpus_entered field */
-	num_aps = p->num_cpus - 1;
+	num_aps = num_cpus - 1;
 	ret = start_aps(num_aps, ap_count);
 	if (ret) {
 		mdelay(1000);
diff --git a/arch/x86/cpu/queensbay/Kconfig b/arch/x86/cpu/queensbay/Kconfig
index 397e599f93..fbf85f233f 100644
--- a/arch/x86/cpu/queensbay/Kconfig
+++ b/arch/x86/cpu/queensbay/Kconfig
@@ -38,4 +38,8 @@ config CMC_ADDR
 	  The default base address of 0xfffb0000 indicates that the binary must
 	  be located at offset 0xb0000 from the beginning of a 1MB flash device.
 
+config CPU_ADDR_BITS
+	int
+	default 32
+
 endif
diff --git a/arch/x86/cpu/queensbay/tnc.c b/arch/x86/cpu/queensbay/tnc.c
index 873de7be9d..d27b2d9ec6 100644
--- a/arch/x86/cpu/queensbay/tnc.c
+++ b/arch/x86/cpu/queensbay/tnc.c
@@ -69,17 +69,18 @@ void cpu_irq_init(void)
 	 * Route TunnelCreek PCI device interrupt pin to PIRQ
 	 *
 	 * Since PCIe downstream ports received INTx are routed to PIRQ
-	 * A/B/C/D directly and not configurable, we route internal PCI
-	 * device's INTx to PIRQ E/F/G/H.
+	 * A/B/C/D directly and not configurable, we have to route PCIe
+	 * root ports' INTx to PIRQ A/B/C/D as well. For other devices
+	 * on TunneCreek, route them to PIRQ E/F/G/H.
 	 */
 	writew(PIRQE, &rcba->d02ir);
 	writew(PIRQF, &rcba->d03ir);
 	writew(PIRQG, &rcba->d27ir);
 	writew(PIRQH, &rcba->d31ir);
-	writew(PIRQE, &rcba->d23ir);
-	writew(PIRQF, &rcba->d24ir);
-	writew(PIRQG, &rcba->d25ir);
-	writew(PIRQH, &rcba->d26ir);
+	writew(PIRQA, &rcba->d23ir);
+	writew(PIRQB, &rcba->d24ir);
+	writew(PIRQC, &rcba->d25ir);
+	writew(PIRQD, &rcba->d26ir);
 }
 
 int arch_misc_init(void)
diff --git a/arch/x86/cpu/start.S b/arch/x86/cpu/start.S
index 2e5f9da756..00e585e19b 100644
--- a/arch/x86/cpu/start.S
+++ b/arch/x86/cpu/start.S
@@ -116,12 +116,16 @@ car_init_ret:
 	rep	stosb
 
 #ifdef CONFIG_HAVE_FSP
+	test	%esi, %esi
+	jz	skip_hob
+
 	/* Store HOB list */
 	movl	%esp, %edx
 	addl	$GD_HOB_LIST, %edx
 	movl	%esi, (%edx)
-#endif
 
+skip_hob:
+#endif
 	/* Setup first parameter to setup_gdt, pointer to global_data */
 	movl	%esp, %eax
 
diff --git a/arch/x86/cpu/start16.S b/arch/x86/cpu/start16.S
index 826e2b4361..5eb17f15c9 100644
--- a/arch/x86/cpu/start16.S
+++ b/arch/x86/cpu/start16.S
@@ -71,11 +71,12 @@ idt_ptr:
 	 */
 gdt_ptr:
 	.word	0x1f		/* limit (31 bytes = 4 GDT entries - 1) */
-	.long	BOOT_SEG + gdt	/* base */
+	.long	BOOT_SEG + gdt_rom	/* base */
 
 	/* Some CPUs are picky about GDT alignment... */
 	.align	16
-gdt:
+.globl gdt_rom
+gdt_rom:
 	/*
 	 * The GDT table ...
 	 *