3 files changed, 58 insertions, 4 deletions
diff --git a/src/include/cpu/x86/lapic.h b/src/include/cpu/x86/lapic.h
index f4291ab35b..e2ca297659 100644
--- a/src/include/cpu/x86/lapic.h
+++ b/src/include/cpu/x86/lapic.h
@@ -2,19 +2,54 @@
 #define CPU_X86_LAPIC_H
 
 #include <arch/mmio.h>
+#include <arch/cpu.h>
 #include <cpu/x86/lapic_def.h>
 #include <cpu/x86/msr.h>
 #include <halt.h>
 #include <stdint.h>
 
+static inline bool is_x2apic_mode(void)
+{
+	msr_t msr;
+	msr = rdmsr(LAPIC_BASE_MSR);
+	return (msr.lo & LAPIC_BASE_MSR_X2APIC_MODE);
+}
+
+static inline void x2apic_send_ipi(uint32_t icrlow, uint32_t apicid)
+{
+	msr_t icr;
+	icr.hi = apicid;
+	icr.lo = icrlow;
+	wrmsr(X2APIC_MSR_ICR_ADDRESS, icr);
+}
+
 static __always_inline uint32_t lapic_read(unsigned int reg)
 {
-	return read32((volatile void *)(uintptr_t)(LAPIC_DEFAULT_BASE + reg));
+	uint32_t value, index;
+	msr_t msr;
+
+	if (is_x2apic_mode()) {
+		index = X2APIC_MSR_BASE_ADDRESS + (uint32_t)(reg >> 4);
+		msr = rdmsr(index);
+		value = msr.lo;
+	} else {
+		value = read32((volatile void *)(uintptr_t)(LAPIC_DEFAULT_BASE + reg));
+	}
+	return value;
 }
 
 static __always_inline void lapic_write(unsigned int reg, uint32_t v)
 {
-	write32((volatile void *)(uintptr_t)(LAPIC_DEFAULT_BASE + reg), v);
+	msr_t msr;
+	uint32_t index;
+	if (is_x2apic_mode()) {
+		index = X2APIC_MSR_BASE_ADDRESS + (uint32_t)(reg >> 4);
+		msr.hi = 0x0;
+		msr.lo = v;
+		wrmsr(index, msr);
+	} else {
+		write32((volatile void *)(uintptr_t)(LAPIC_DEFAULT_BASE + reg), v);
+	}
 }
 
 static __always_inline void lapic_wait_icr_idle(void)
@@ -41,9 +76,24 @@ static inline void disable_lapic(void)
 	wrmsr(LAPIC_BASE_MSR, msr);
 }
 
+static __always_inline unsigned int initial_lapicid(void)
+{
+	uint32_t lapicid;
+	if (is_x2apic_mode())
+		lapicid = lapic_read(LAPIC_ID);
+	else
+		lapicid = cpuid_ebx(1) >> 24;
+	return lapicid;
+}
+
 static __always_inline unsigned int lapicid(void)
 {
-	return lapic_read(LAPIC_ID) >> 24;
+	uint32_t lapicid = lapic_read(LAPIC_ID);
+
+	/* check x2apic mode and return accordingly */
+	if (!is_x2apic_mode())
+		lapicid >>= 24;
+	return lapicid;
 }
 
 #if !CONFIG(AP_IN_SIPI_WAIT)
diff --git a/src/include/cpu/x86/lapic_def.h b/src/include/cpu/x86/lapic_def.h
index 9da89ee873..5b25e5a968 100644
--- a/src/include/cpu/x86/lapic_def.h
+++ b/src/include/cpu/x86/lapic_def.h
@@ -3,6 +3,7 @@
 
 #define LAPIC_BASE_MSR 0x1B
 #define LAPIC_BASE_MSR_BOOTSTRAP_PROCESSOR (1 << 8)
+#define LAPIC_BASE_MSR_X2APIC_MODE (1 << 10)
 #define LAPIC_BASE_MSR_ENABLE (1 << 11)
 #define LAPIC_BASE_MSR_ADDR_MASK 0xFFFFF000
 
@@ -94,4 +95,7 @@
 #define		LAPIC_TDR_DIV_64		0x9
 #define		LAPIC_TDR_DIV_128	0xA
 
+#define X2APIC_MSR_BASE_ADDRESS	0x800
+#define X2APIC_LAPIC_ID		(X2APIC_MSR_BASE_ADDRESS | (LAPIC_ID >> 4))
+#define	X2APIC_MSR_ICR_ADDRESS	0x830
 #endif /* CPU_X86_LAPIC_DEF_H */
diff --git a/src/include/cpu/x86/smm.h b/src/include/cpu/x86/smm.h
index b42f3faef4..c42eb8cf7f 100644
--- a/src/include/cpu/x86/smm.h
+++ b/src/include/cpu/x86/smm.h
@@ -85,7 +85,7 @@ struct smm_stub_params {
 	 * initializes this array with a 1:1 mapping. If the APIC ids are not
 	 * contiguous like the 1:1 mapping it is up to the caller of the stub
 	 * loader to adjust this mapping. */
-	u8 apic_id_to_cpu[CONFIG_MAX_CPUS];
+	u16 apic_id_to_cpu[CONFIG_MAX_CPUS];
 	/* STM's 32bit entry into SMI handler */
 	u32 start32_offset;
 } __packed;