cpu/amd/family16: Use parallel mp init

This now computes an MTRR solution based on coreboots allocation and
always uses that solution over the one set by AGESA.
Syncing MSR is now done in sipi_vector.S.

Change-Id: If3326c5fbd278f8f974d5408e16440e3b56b1b44
Signed-off-by: Arthur Heymans <arthur@aheymans.xyz>

3 files changed, 30 insertions, 170 deletions

diff --git a/src/cpu/amd/agesa/family16kb/model_16_init.c b/src/cpu/amd/agesa/family16kb/model_16_init.c
index 0f1113638c..50d591ce5f 100644
--- a/src/cpu/amd/agesa/family16kb/model_16_init.c
+++ b/src/cpu/amd/agesa/family16kb/model_16_init.c
@@ -1,15 +1,12 @@
 /* SPDX-License-Identifier: GPL-2.0-only */
 
-#include <acpi/acpi.h>
 #include <amdblocks/cpu.h>
 #include <amdblocks/smm.h>
 #include <console/console.h>
 #include <cpu/amd/msr.h>
 #include <cpu/amd/mtrr.h>
 #include <cpu/cpu.h>
-#include <cpu/x86/cache.h>
 #include <cpu/x86/msr.h>
-#include <cpu/x86/mtrr.h>
 #include <device/device.h>
 #include <northbridge/amd/agesa/agesa_helper.h>
 
@@ -18,42 +15,9 @@ static void model_16_init(struct device *dev)
 	printk(BIOS_DEBUG, "Model 16 Init.\n");
 
 	msr_t msr;
-	int msrno;
 #if CONFIG(LOGICAL_CPUS)
 	u32 siblings;
 #endif
-
-	/*
-	 * AGESA sets the MTRRs main MTRRs. The shadow area needs to be set
-	 * by coreboot.
-	 */
-	disable_cache();
-	/* Enable access to AMD RdDram and WrDram extension bits */
-	msr = rdmsr(SYSCFG_MSR);
-	msr.lo |= SYSCFG_MSR_MtrrFixDramModEn;
-	msr.lo &= ~SYSCFG_MSR_MtrrFixDramEn;
-	wrmsr(SYSCFG_MSR, msr);
-
-	// BSP: make a0000-bffff UC, c0000-fffff WB, same as OntarioApMtrrSettingsList for APs
-	msr.lo = msr.hi = 0;
-	wrmsr(MTRR_FIX_16K_A0000, msr);
-	msr.lo = msr.hi = 0x1e1e1e1e;
-	wrmsr(MTRR_FIX_64K_00000, msr);
-	wrmsr(MTRR_FIX_16K_80000, msr);
-	for (msrno = MTRR_FIX_4K_C0000; msrno <= MTRR_FIX_4K_F8000; msrno++)
-		wrmsr(msrno, msr);
-
-	msr = rdmsr(SYSCFG_MSR);
-	msr.lo &= ~SYSCFG_MSR_MtrrFixDramModEn;
-	msr.lo |= SYSCFG_MSR_MtrrFixDramEn;
-	wrmsr(SYSCFG_MSR, msr);
-
-	if (acpi_is_wakeup_s3())
-		restore_mtrr();
-
-	x86_mtrr_check();
-	enable_cache();
-
 	/* zero the machine check error status registers */
 	mca_clear_status();
 
diff --git a/src/northbridge/amd/agesa/family16kb/Kconfig b/src/northbridge/amd/agesa/family16kb/Kconfig
index 8cf919a2d5..faea30d6fc 100644
--- a/src/northbridge/amd/agesa/family16kb/Kconfig
+++ b/src/northbridge/amd/agesa/family16kb/Kconfig
@@ -2,7 +2,6 @@
 
 config NORTHBRIDGE_AMD_AGESA_FAMILY16_KB
 	bool
-	select LEGACY_SMP_INIT
 	select RESOURCE_ALLOCATOR_V3
 
 if NORTHBRIDGE_AMD_AGESA_FAMILY16_KB
diff --git a/src/northbridge/amd/agesa/family16kb/northbridge.c b/src/northbridge/amd/agesa/family16kb/northbridge.c
index 250c49105a..5135a3f7b3 100644
--- a/src/northbridge/amd/agesa/family16kb/northbridge.c
+++ b/src/northbridge/amd/agesa/family16kb/northbridge.c
@@ -1,26 +1,27 @@
 /* SPDX-License-Identifier: GPL-2.0-only */
 
-#include <console/console.h>
-#include <device/pci_ops.h>
+#include <AGESA.h>
+#include <Options.h>
+#include <Porting.h>
+#include <Topology.h>
 #include <acpi/acpi.h>
 #include <acpi/acpigen.h>
-#include <stdint.h>
+#include <console/console.h>
+#include <cpu/amd/msr.h>
+#include <cpu/amd/mtrr.h>
+#include <cpu/cpu.h>
+#include <cpu/x86/lapic.h>
+#include <cpu/x86/mp.h>
 #include <device/device.h>
 #include <device/pci.h>
 #include <device/pci_ids.h>
-#include <string.h>
+#include <device/pci_ops.h>
 #include <lib.h>
-#include <cpu/cpu.h>
-#include <cpu/x86/lapic.h>
-#include <cpu/amd/msr.h>
-#include <cpu/amd/mtrr.h>
-#include <Porting.h>
-#include <AGESA.h>
-#include <Options.h>
-#include <Topology.h>
-#include <northbridge/amd/nb_common.h>
-#include <northbridge/amd/agesa/state_machine.h>
 #include <northbridge/amd/agesa/agesa_helper.h>
+#include <northbridge/amd/agesa/state_machine.h>
+#include <northbridge/amd/nb_common.h>
+#include <stdint.h>
+#include <string.h>
 
 #define MAX_NODE_NUMS MAX_NODES
 
@@ -365,8 +366,18 @@ struct chip_operations northbridge_amd_agesa_family16kb_ops = {
 	.final = fam16_finalize,
 };
 
+static void sysconf_init(struct device *dev);
+
 static void domain_read_resources(struct device *dev)
 {
+
+	struct device *dev_mc = pcidev_on_root(DEV_CDB, 0);
+	if (!dev_mc) {
+		printk(BIOS_ERR, "0:%02x.0 not found", DEV_CDB);
+		die("");
+	}
+	sysconf_init(dev_mc);
+
 	unsigned int reg;
 
 	/* Find the already assigned resource pairs */
@@ -565,132 +576,18 @@ static void sysconf_init(struct device *dev) // first node
 	node_nums = ((pci_read_config32(dev, 0x60) >> 4) & 7) + 1; //NodeCnt[2:0]
 }
 
-static void cpu_bus_scan(struct device *dev)
+void mp_init_cpus(struct bus *cpu_bus)
 {
-	struct bus *cpu_bus;
-	struct device *dev_mc;
-	int i, j;
-	int coreid_bits;
-	int core_max = 0;
-	unsigned int ApicIdCoreIdSize;
-	unsigned int core_nums;
-	int siblings = 0;
-	unsigned int family;
-
-	dev_mc = pcidev_on_root(DEV_CDB, 0);
-	if (!dev_mc) {
-		printk(BIOS_ERR, "0:%02x.0 not found", DEV_CDB);
-		die("");
-	}
-	sysconf_init(dev_mc);
-
-	/* Get Max Number of cores(MNC) */
-	coreid_bits = (cpuid_ecx(0x80000008) & 0x0000F000) >> 12;
-	core_max = 1 << (coreid_bits & 0x000F); //mnc
+	extern const struct mp_ops amd_mp_ops_no_smm;
+	/* TODO: Handle mp_init_with_smm failure? */
+	mp_init_with_smm(cpu_bus, &amd_mp_ops_no_smm);
 
-	ApicIdCoreIdSize = ((cpuid_ecx(0x80000008) >> 12) & 0xF);
-	if (ApicIdCoreIdSize) {
-		core_nums = (1 << ApicIdCoreIdSize) - 1;
-	} else {
-		core_nums = 3; //quad core
-	}
-
-	/* Find which cpus are present */
-	cpu_bus = dev->link_list;
-	for (i = 0; i < node_nums; i++) {
-		struct device *cdb_dev;
-		unsigned int devn;
-		struct bus *pbus;
-
-		devn = DEV_CDB + i;
-		pbus = dev_mc->bus;
-
-		/* Find the cpu's pci device */
-		cdb_dev = pcidev_on_root(devn, 0);
-		if (!cdb_dev) {
-			/* If I am probing things in a weird order
-			 * ensure all of the cpu's pci devices are found.
-			 */
-			int fn;
-			for (fn = 0; fn <= 5; fn++) { //FBDIMM?
-				cdb_dev = pci_probe_dev(NULL, pbus,
-							PCI_DEVFN(devn, fn));
-			}
-			cdb_dev = pcidev_on_root(devn, 0);
-		} else {
-			/* Ok, We need to set the links for that device.
-			 * otherwise the device under it will not be scanned
-			 */
-			add_more_links(cdb_dev, 4);
-		}
-
-		family = cpuid_eax(1);
-		family = (family >> 20) & 0xFF;
-		if (family == 1) { //f10
-			u32 dword;
-			cdb_dev = pcidev_on_root(devn, 3);
-			dword = pci_read_config32(cdb_dev, 0xe8);
-			siblings = ((dword & BIT15) >> 13) | ((dword & (BIT13 | BIT12)) >> 12);
-		} else if (family == 7) {//f16
-			cdb_dev = pcidev_on_root(devn, 5);
-			if (cdb_dev && cdb_dev->enabled) {
-				siblings = pci_read_config32(cdb_dev, 0x84);
-				siblings &= 0xFF;
-			}
-		} else {
-			siblings = 0; //default one core
-		}
-		int enable_node = cdb_dev && cdb_dev->enabled;
-		printk(BIOS_SPEW, "%s family%xh, core_max=0x%x, core_nums=0x%x, siblings=0x%x\n",
-				dev_path(cdb_dev), 0x0f + family, core_max, core_nums, siblings);
-
-		for (j = 0; j <= siblings; j++) {
-			extern CONST OPTIONS_CONFIG_TOPOLOGY ROMDATA TopologyConfiguration;
-			u32 modules = TopologyConfiguration.PlatformNumberOfModules;
-			u32 lapicid_start = 0;
-
-			/*
-			 * APIC ID calucation is tightly coupled with AGESA v5 code.
-			 * This calculation MUST match the assignment calculation done
-			 * in LocalApicInitializationAtEarly() function.
-			 * And reference GetLocalApicIdForCore()
-			 *
-			 * Apply APIC enumeration rules
-			 * For systems with >= 16 APICs, put the IO-APICs at 0..n and
-			 * put the local-APICs at m..z
-			 *
-			 * This is needed because many IO-APIC devices only have 4 bits
-			 * for their APIC id and therefore must reside at 0..15
-			 */
-
-			u8 plat_num_io_apics = 3; /* FIXME */
-
-			if ((node_nums * core_max) + plat_num_io_apics >= 0x10) {
-				lapicid_start = (plat_num_io_apics - 1) / core_max;
-				lapicid_start = (lapicid_start + 1) * core_max;
-				printk(BIOS_SPEW, "lpaicid_start=0x%x ", lapicid_start);
-			}
-			u32 apic_id = (lapicid_start * (i / modules + 1)) + ((i % modules) ? (j + (siblings + 1)) : j);
-			printk(BIOS_SPEW, "node 0x%x core 0x%x apicid=0x%x\n",
-					i, j, apic_id);
-
-			struct device *cpu = add_cpu_device(cpu_bus, apic_id, enable_node);
-			if (cpu)
-				amd_cpu_topology(cpu, i, j);
-		} //j
-	}
-}
-
-static void cpu_bus_init(struct device *dev)
-{
-	initialize_cpus(dev->link_list);
 }
 
 static struct device_operations cpu_bus_ops = {
 	.read_resources	  = noop_read_resources,
 	.set_resources	  = noop_set_resources,
-	.init		  = cpu_bus_init,
-	.scan_bus	  = cpu_bus_scan,
+	.init		  = mp_cpu_bus_init,
 };
 
 static void root_complex_enable_dev(struct device *dev)