Re-implement FPU suspend/resume for amd64. This removes superfluous uses

of critical_enter(9) and critical_exit(9) by fpugetregs() and fpusetregs().
Also, we do not touch PCB flags any more.

MFC after:	1 month

5 files changed, 25 insertions, 11 deletions

diff --git a/sys/amd64/acpica/acpi_switch.S b/sys/amd64/acpica/acpi_switch.S
index 39f18e5..57a3aa8 100644
--- a/sys/amd64/acpica/acpi_switch.S
+++ b/sys/amd64/acpica/acpi_switch.S
@@ -102,9 +102,12 @@ ENTRY(acpi_restorecpu)
 	movl	WAKEUP_CTX(sfmask), %eax
 	wrmsr
 
-	/* Restore CR0, CR2 and CR4. */
+	/* Restore CR0 except for FPU mode. */
 	movq	WAKEUP_XPCB(CR0), %rax
+	andq	$~(CR0_EM | CR0_TS), %rax
 	movq	%rax, %cr0
+
+	/* Restore CR2 and CR4. */
 	movq	WAKEUP_XPCB(CR2), %rax
 	movq	%rax, %cr2
 	movq	WAKEUP_XPCB(CR4), %rax
@@ -149,6 +152,17 @@ ENTRY(acpi_restorecpu)
 	movq	WAKEUP_PCB(DR7), %rax
 	movq	%rax, %dr7
 
+	/* Restore FPU state. */
+	movq	PCPU(FPCURTHREAD), %rax
+	testq	%rax, %rax
+	je	1f
+	fxrstor	WAKEUP_PCB(USER_FPU)
+1:
+
+	/* Restore CR0 with FPU mode. */
+	movq	WAKEUP_XPCB(CR0), %rax
+	movq	%rax, %cr0
+
 	/* Restore return address. */
 	movq	WAKEUP_PCB(RIP), %rax
 	movq	%rax, (%rsp)
diff --git a/sys/amd64/acpica/acpi_wakeup.c b/sys/amd64/acpica/acpi_wakeup.c
index 5c35593..982809b 100644
--- a/sys/amd64/acpica/acpi_wakeup.c
+++ b/sys/amd64/acpica/acpi_wakeup.c
@@ -216,7 +216,6 @@ acpi_wakeup_cpus(struct acpi_softc *sc, cpumask_t wakeup_cpus)
 int
 acpi_sleep_machdep(struct acpi_softc *sc, int state)
 {
-	struct savefpu	*stopfpu;
 #ifdef SMP
 	cpumask_t	wakeup_cpus;
 #endif
@@ -246,10 +245,7 @@ acpi_sleep_machdep(struct acpi_softc *sc, int state)
 	cr3 = rcr3();
 	load_cr3(KPML4phys);
 
-	stopfpu = &stopxpcbs[0]->xpcb_pcb.pcb_user_save;
 	if (acpi_savecpu(stopxpcbs[0])) {
-		fpugetregs(curthread, stopfpu);
-
 #ifdef SMP
 		if (wakeup_cpus != 0 && suspend_cpus(wakeup_cpus) == 0) {
 			device_printf(sc->acpi_dev,
@@ -285,7 +281,6 @@ acpi_sleep_machdep(struct acpi_softc *sc, int state)
 		for (;;)
 			ia32_pause();
 	} else {
-		fpusetregs(curthread, stopfpu);
 #ifdef SMP
 		if (wakeup_cpus != 0)
 			acpi_wakeup_cpus(sc, wakeup_cpus);
diff --git a/sys/amd64/amd64/cpu_switch.S b/sys/amd64/amd64/cpu_switch.S
index 6b5c663..bba435d 100644
--- a/sys/amd64/amd64/cpu_switch.S
+++ b/sys/amd64/amd64/cpu_switch.S
@@ -417,6 +417,13 @@ ENTRY(savectx2)
 	leaq	(%rax,%rdx),%rax
 	movq	%rax,XPCB_KGSBASE(%r8)
 
+	movq	PCPU(FPCURTHREAD),%rax
+	testq	%rax,%rax
+	je	1f
+	clts
+	fxsave	PCB_USER_FPU(%r8)
+1:
+
 	movl	$1, %eax
 	ret
 END(savectx2)
diff --git a/sys/amd64/amd64/genassym.c b/sys/amd64/amd64/genassym.c
index 9d78309..95a0ef0 100644
--- a/sys/amd64/amd64/genassym.c
+++ b/sys/amd64/amd64/genassym.c
@@ -140,6 +140,7 @@ ASSYM(PCB_DR2, offsetof(struct pcb, pcb_dr2));
 ASSYM(PCB_DR3, offsetof(struct pcb, pcb_dr3));
 ASSYM(PCB_DR6, offsetof(struct pcb, pcb_dr6));
 ASSYM(PCB_DR7, offsetof(struct pcb, pcb_dr7));
+ASSYM(PCB_USER_FPU, offsetof(struct pcb, pcb_user_save));
 ASSYM(PCB_TSSP, offsetof(struct pcb, pcb_tssp));
 ASSYM(PCB_FULL_IRET, offsetof(struct pcb, pcb_full_iret));
 ASSYM(PCB_DBREGS, PCB_DBREGS);
diff --git a/sys/amd64/amd64/mp_machdep.c b/sys/amd64/amd64/mp_machdep.c
index 02e7505..0bda9fe 100644
--- a/sys/amd64/amd64/mp_machdep.c
+++ b/sys/amd64/amd64/mp_machdep.c
@@ -1329,20 +1329,17 @@ cpustop_handler(void)
 void
 cpususpend_handler(void)
 {
-	struct savefpu *stopfpu;
 	register_t cr3, rf;
 	int cpu = PCPU_GET(cpuid);
 	int cpumask = PCPU_GET(cpumask);
 
 	rf = intr_disable();
 	cr3 = rcr3();
-	stopfpu = &stopxpcbs[cpu]->xpcb_pcb.pcb_user_save;
+
 	if (savectx2(stopxpcbs[cpu])) {
-		fpugetregs(curthread, stopfpu);
 		wbinvd();
 		atomic_set_int(&stopped_cpus, cpumask);
-	} else
-		fpusetregs(curthread, stopfpu);
+	}
 
 	/* Wait for resume */
 	while (!(started_cpus & cpumask))