3 files changed, 24 insertions, 2 deletions
diff --git a/sys/amd64/acpica/acpi_switch.S b/sys/amd64/acpica/acpi_switch.S
index 45bad1f8f9cf..6091e2a45c18 100644
--- a/sys/amd64/acpica/acpi_switch.S
+++ b/sys/amd64/acpica/acpi_switch.S
@@ -146,11 +146,22 @@ ENTRY(acpi_restorecpu)
 
 	/* Restore FPU state. */
 	fninit
-	fxrstor	PCB_USERFPU(%rdi)
+	movq	WAKEUP_CTX(fpusave),%rdi
+	cmpl	$0,use_xsave
+	jne	1f
+	fxrstor	(%rdi)
+	jmp	2f
+1:	movl	xsave_mask,%eax
+	movl	xsave_mask+4,%edx
+/*	xrstor	(%rdi) */
+	.byte	0x0f,0xae,0x2f
+2:
 
 	/* Reload CR0. */
 	movq	%rcx, %cr0
 
+	movq	WAKEUP_CTX(pcb),%rdi
+
 	/* Restore return address. */
 	movq	PCB_RIP(%rdi), %rax
 	movq	%rax, (%rsp)
diff --git a/sys/amd64/acpica/acpi_wakecode.S b/sys/amd64/acpica/acpi_wakecode.S
index 49d14f5cdf2f..6e44e6c5b818 100644
--- a/sys/amd64/acpica/acpi_wakecode.S
+++ b/sys/amd64/acpica/acpi_wakecode.S
@@ -270,6 +270,8 @@ wakeup_pcb:
 wakeup_gdt:
 	.word	0
 	.quad	0
+wakeup_fpusave:
+	.quad	0
 
 	ALIGN_DATA
 wakeup_efer:
diff --git a/sys/amd64/acpica/acpi_wakeup.c b/sys/amd64/acpica/acpi_wakeup.c
index 43aeec319508..f86243547b5d 100644
--- a/sys/amd64/acpica/acpi_wakeup.c
+++ b/sys/amd64/acpica/acpi_wakeup.c
@@ -45,6 +45,7 @@ __FBSDID("$FreeBSD$");
 #include <machine/pcb.h>
 #include <machine/pmap.h>
 #include <machine/specialreg.h>
+#include <machine/md_var.h>
 
 #ifdef SMP
 #include <x86/apicreg.h>
@@ -67,8 +68,10 @@ extern int		acpi_reset_video;
 
 #ifdef SMP
 extern struct pcb	**susppcbs;
+extern void		**suspfpusave;
 #else
 static struct pcb	**susppcbs;
+static void		**suspfpusave;
 #endif
 
 int			acpi_restorecpu(vm_offset_t, struct pcb *);
@@ -105,6 +108,7 @@ acpi_wakeup_ap(struct acpi_softc *sc, int cpu)
 	int		ms;
 
 	WAKECODE_FIXUP(wakeup_pcb, struct pcb *, susppcbs[cpu]);
+	WAKECODE_FIXUP(wakeup_fpusave, void *, suspfpusave[cpu]);
 	WAKECODE_FIXUP(wakeup_gdt, uint16_t, susppcbs[cpu]->pcb_gdt.rd_limit);
 	WAKECODE_FIXUP(wakeup_gdt + 2, uint64_t,
 	    susppcbs[cpu]->pcb_gdt.rd_base);
@@ -244,6 +248,7 @@ acpi_sleep_machdep(struct acpi_softc *sc, int state)
 	load_cr3(KPML4phys);
 
 	if (savectx(susppcbs[0])) {
+		ctx_fpusave(suspfpusave[0]);
 #ifdef SMP
 		if (!CPU_EMPTY(&wakeup_cpus) &&
 		    suspend_cpus(wakeup_cpus) == 0) {
@@ -256,6 +261,7 @@ acpi_sleep_machdep(struct acpi_softc *sc, int state)
 		WAKECODE_FIXUP(reset_video, uint8_t, (acpi_reset_video != 0));
 
 		WAKECODE_FIXUP(wakeup_pcb, struct pcb *, susppcbs[0]);
+		WAKECODE_FIXUP(wakeup_fpusave, void *, suspfpusave[0]);
 		WAKECODE_FIXUP(wakeup_gdt, uint16_t,
 		    susppcbs[0]->pcb_gdt.rd_limit);
 		WAKECODE_FIXUP(wakeup_gdt + 2, uint64_t,
@@ -333,8 +339,11 @@ acpi_alloc_wakeup_handler(void)
 		return (NULL);
 	}
 	susppcbs = malloc(mp_ncpus * sizeof(*susppcbs), M_DEVBUF, M_WAITOK);
-	for (i = 0; i < mp_ncpus; i++)
+	suspfpusave = malloc(mp_ncpus * sizeof(void *), M_DEVBUF, M_WAITOK);
+	for (i = 0; i < mp_ncpus; i++) {
 		susppcbs[i] = malloc(sizeof(**susppcbs), M_DEVBUF, M_WAITOK);
+		suspfpusave[i] = alloc_fpusave(M_WAITOK);
+	}
 
 	return (wakeaddr);
 }