[PATCH 1/2] x86: Enable x2apic during resume from suspend if used previously

[Mario Limonciello <mario.limonciello@amd.com>]

If x2apic was enabled during boot with parallel startup
it will be needed during resume from suspend to ram as well.

Store whether to enable into the smpboot_control global variable
and during startup re-enable it if necessary.

Cc: stable@vger.kernel.org # 6.5+
Fixes: 0c7ffa32dbd6 ("x86/smpboot/64: Implement arch_cpuhp_init_parallel_bringup() and enable it")
Signed-off-by: Mario Limonciello <mario.limonciello@amd.com>
---
 arch/x86/include/asm/smp.h   |  1 +
 arch/x86/kernel/acpi/sleep.c | 12 ++++++++----
 arch/x86/kernel/head_64.S    | 15 +++++++++++++++
 3 files changed, 24 insertions(+), 4 deletions(-)

diff --git a/arch/x86/include/asm/smp.h b/arch/x86/include/asm/smp.h
index c31c633419fe..86584ffaebc3 100644
--- a/arch/x86/include/asm/smp.h
+++ b/arch/x86/include/asm/smp.h
@@ -190,6 +190,7 @@ extern unsigned long apic_mmio_base;
 #endif /* !__ASSEMBLY__ */
 
 /* Control bits for startup_64 */
+#define STARTUP_ENABLE_X2APIC	0x40000000
 #define STARTUP_READ_APICID	0x80000000
 
 /* Top 8 bits are reserved for control */
diff --git a/arch/x86/kernel/acpi/sleep.c b/arch/x86/kernel/acpi/sleep.c
index 6dfecb27b846..29734a1299f6 100644
--- a/arch/x86/kernel/acpi/sleep.c
+++ b/arch/x86/kernel/acpi/sleep.c
@@ -11,6 +11,7 @@
 #include <linux/dmi.h>
 #include <linux/cpumask.h>
 #include <linux/pgtable.h>
+#include <asm/apic.h>
 #include <asm/segment.h>
 #include <asm/desc.h>
 #include <asm/cacheflush.h>
@@ -129,11 +130,14 @@ int x86_acpi_suspend_lowlevel(void)
 	 */
 	current->thread.sp = (unsigned long)temp_stack + sizeof(temp_stack);
 	/*
-	 * Ensure the CPU knows which one it is when it comes back, if
-	 * it isn't in parallel mode and expected to work that out for
-	 * itself.
+	 * Ensure x2apic is re-enabled if necessary and the CPU knows which
+	 * one it is when it comes back, if it isn't in parallel mode and
+	 * expected to work that out for itself.
 	 */
-	if (!(smpboot_control & STARTUP_PARALLEL_MASK))
+	if (smpboot_control & STARTUP_PARALLEL_MASK) {
+		if (x2apic_enabled())
+			smpboot_control |= STARTUP_ENABLE_X2APIC;
+	} else
 		smpboot_control = smp_processor_id();
 #endif
 	initial_code = (unsigned long)wakeup_long64;
diff --git a/arch/x86/kernel/head_64.S b/arch/x86/kernel/head_64.S
index ea6995920b7a..fcfa79105928 100644
--- a/arch/x86/kernel/head_64.S
+++ b/arch/x86/kernel/head_64.S
@@ -236,10 +236,15 @@ SYM_INNER_LABEL(secondary_startup_64_no_verify, SYM_L_GLOBAL)
 	 * used to look up the CPU number.  For booting a single CPU, the
 	 * CPU number is encoded in smpboot_control.
 	 *
+	 * Bit 30	STARTUP_ENABLE_X2APIC (Enable X2APIC mode)
 	 * Bit 31	STARTUP_READ_APICID (Read APICID from APIC)
 	 * Bit 0-23	CPU# if STARTUP_xx flags are not set
 	 */
 	movl	smpboot_control(%rip), %ecx
+
+	testl	$STARTUP_ENABLE_X2APIC, %ecx
+	jnz	.Lenable_x2apic
+
 	testl	$STARTUP_READ_APICID, %ecx
 	jnz	.Lread_apicid
 	/*
@@ -249,6 +254,16 @@ SYM_INNER_LABEL(secondary_startup_64_no_verify, SYM_L_GLOBAL)
 	andl	$(~STARTUP_PARALLEL_MASK), %ecx
 	jmp	.Lsetup_cpu
 
+.Lenable_x2apic:
+	/* Enable X2APIC if disabled */
+	mov	$MSR_IA32_APICBASE, %ecx
+	rdmsr
+	testl	$X2APIC_ENABLE, %eax
+	jnz	.Lread_apicid_msr
+	orl	$X2APIC_ENABLE, %eax
+	wrmsr
+	jmp	.Lread_apicid_msr
+
 .Lread_apicid:
 	/* Check whether X2APIC mode is already enabled */
 	mov	$MSR_IA32_APICBASE, %ecx
-- 
2.34.1