4 files changed, 39 insertions, 11 deletions

diff --git a/src/cpu/x86/lapic/lapic.c b/src/cpu/x86/lapic/lapic.c
index b4d3c4de42..9003534485 100644
--- a/src/cpu/x86/lapic/lapic.c
+++ b/src/cpu/x86/lapic/lapic.c
@@ -1,5 +1,6 @@
 /* SPDX-License-Identifier: GPL-2.0-only */
 
+#include <assert.h>
 #include <cpu/cpu.h>
 #include <cpu/x86/lapic.h>
 #include <cpu/x86/lapic_def.h>
@@ -10,23 +11,52 @@
 
 void enable_lapic(void)
 {
+	uintptr_t apic_base;
+	bool use_x2apic;
 	msr_t msr;
 
 	msr = rdmsr(LAPIC_BASE_MSR);
-	msr.hi &= 0xffffff00;
-	msr.lo &= ~LAPIC_BASE_MSR_ADDR_MASK;
-	msr.lo |= LAPIC_DEFAULT_BASE;
-	msr.lo |= LAPIC_BASE_MSR_ENABLE;
-	wrmsr(LAPIC_BASE_MSR, msr);
+	if (!(msr.lo & LAPIC_BASE_MSR_ENABLE)) {
+		msr.hi &= 0xffffff00;
+		msr.lo &= ~LAPIC_BASE_MSR_ADDR_MASK;
+		msr.lo |= LAPIC_DEFAULT_BASE;
+		msr.lo |= LAPIC_BASE_MSR_ENABLE;
+		wrmsr(LAPIC_BASE_MSR, msr);
+		msr = rdmsr(LAPIC_BASE_MSR);
+	}
+
+	ASSERT(msr.lo & LAPIC_BASE_MSR_ENABLE);
+
+	apic_base = msr.lo & LAPIC_BASE_MSR_ADDR_MASK;
+	ASSERT(apic_base == LAPIC_DEFAULT_BASE);
+
+	if (CONFIG(XAPIC_ONLY)) {
+		use_x2apic = false;
+	} else {
+		use_x2apic = !!(cpu_get_feature_flags_ecx() & CPUID_X2APIC);
+		ASSERT(CONFIG(X2APIC_RUNTIME) || use_x2apic);
+	}
+
+	if (use_x2apic == !!(msr.lo & LAPIC_BASE_MSR_X2APIC_MODE)) {
+		printk(BIOS_INFO, "LAPIC 0x%x in %s mode.\n", lapicid(),
+				  use_x2apic ? "X2APIC" : "XAPIC");
+	} else if (use_x2apic) {
+		msr.lo |= LAPIC_BASE_MSR_X2APIC_MODE;
+		wrmsr(LAPIC_BASE_MSR, msr);
+		msr = rdmsr(LAPIC_BASE_MSR);
+		ASSERT(!!(msr.lo & LAPIC_BASE_MSR_X2APIC_MODE));
+		printk(BIOS_INFO, "LAPIC 0x%x switched to X2APIC mode.\n", lapicid());
+	} else {
+		die("Switching from X2APIC to XAPIC mode is not implemented.");
+	}
 
-	printk(BIOS_INFO, "Setting up local APIC 0x%x\n", lapicid());
 }
 
 void disable_lapic(void)
 {
 	msr_t msr;
 	msr = rdmsr(LAPIC_BASE_MSR);
-	msr.lo &= ~LAPIC_BASE_MSR_ENABLE;
+	msr.lo &= ~(LAPIC_BASE_MSR_ENABLE | LAPIC_BASE_MSR_X2APIC_MODE);
 	wrmsr(LAPIC_BASE_MSR, msr);
 }
 
diff --git a/src/cpu/x86/mp_init.c b/src/cpu/x86/mp_init.c
index 888c97fff2..e1979c81cb 100644
--- a/src/cpu/x86/mp_init.c
+++ b/src/cpu/x86/mp_init.c
@@ -454,9 +454,6 @@ static enum cb_err start_aps(struct bus *cpu_bus, int ap_count, atomic_t *num_ap
 
 	printk(BIOS_DEBUG, "Attempting to start %d APs\n", ap_count);
 
-	int x2apic_mode = is_x2apic_mode();
-	printk(BIOS_DEBUG, "Starting CPUs in %s mode\n", x2apic_mode ? "x2apic" : "xapic");
-
 	if (lapic_busy()) {
 		printk(BIOS_DEBUG, "Waiting for ICR not to be busy...\n");
 		if (apic_wait_timeout(1000 /* 1 ms */, 50) != CB_SUCCESS) {
diff --git a/src/include/cpu/x86/lapic.h b/src/include/cpu/x86/lapic.h
index 537fa97afe..af0793b7b8 100644
--- a/src/include/cpu/x86/lapic.h
+++ b/src/include/cpu/x86/lapic.h
@@ -59,7 +59,7 @@ static __always_inline void x2apic_send_ipi(uint32_t icrlow, uint32_t apicid)
 	wrmsr(X2APIC_MSR_ICR_ADDRESS, icr);
 }
 
-static inline bool is_x2apic_mode(void)
+static __always_inline bool is_x2apic_mode(void)
 {
 	if (CONFIG(XAPIC_ONLY))
 		return false;
diff --git a/src/include/cpu/x86/msr.h b/src/include/cpu/x86/msr.h
index a8d5e2211b..4d1cb68279 100644
--- a/src/include/cpu/x86/msr.h
+++ b/src/include/cpu/x86/msr.h
@@ -24,6 +24,7 @@
 #define  CPUID_VMX			(1 << 5)
 #define  CPUID_SMX			(1 << 6)
 #define  CPUID_DCA			(1 << 18)
+#define  CPUID_X2APIC			(1 << 21)
 #define  CPUID_AES			(1 << 25)
 #define  SGX_GLOBAL_ENABLE		(1 << 18)
 #define  PLATFORM_INFO_SET_TDP		(1 << 29)