Add enable_fpu/save_fpu for handling the floating point registers in the PCB.

Obtained from:	NetBSD

6 files changed, 235 insertions, 0 deletions

diff --git a/sys/powerpc/aim/machdep.c b/sys/powerpc/aim/machdep.c
index 9432ddb..b141ce1 100644
--- a/sys/powerpc/aim/machdep.c
+++ b/sys/powerpc/aim/machdep.c
@@ -425,6 +425,7 @@ powerpc_init(u_int startkernel, u_int endkernel, u_int basekernel, char *args)
 	thread0->td_kstack = proc0kstack;
 	thread0->td_pcb = (struct pcb *)
 	    (thread0->td_kstack + KSTACK_PAGES * PAGE_SIZE) - 1;
+	enable_fpu(thread0->td_pcb);
 
 	globalp = pmap_steal_memory(round_page(sizeof(struct globaldata)));
 
@@ -918,3 +919,95 @@ globaldata_init(struct globaldata *globaldata, int cpuid, size_t sz)
         globaldata->gd_next_asn = 0;
         globaldata->gd_current_asngen = 1;
 }
+
+void
+enable_fpu(pcb)
+	struct pcb *pcb;
+{
+	int msr, scratch;
+
+	if (!(pcb->pcb_flags & PCB_FPU)) {
+		bzero(&pcb->pcb_fpu, sizeof pcb->pcb_fpu);
+		pcb->pcb_flags |= PCB_FPU;
+	}
+	__asm volatile ("mfmsr %0; ori %1,%0,%2; mtmsr %1; isync"
+		      : "=r"(msr), "=r"(scratch) : "K"(PSL_FP));
+	__asm volatile ("lfd 0,0(%0); mtfsf 0xff,0" :: "b"(&pcb->pcb_fpu.fpscr));
+	__asm ("lfd 0,0(%0);"
+	     "lfd 1,8(%0);"
+	     "lfd 2,16(%0);"
+	     "lfd 3,24(%0);"
+	     "lfd 4,32(%0);"
+	     "lfd 5,40(%0);"
+	     "lfd 6,48(%0);"
+	     "lfd 7,56(%0);"
+	     "lfd 8,64(%0);"
+	     "lfd 9,72(%0);"
+	     "lfd 10,80(%0);"
+	     "lfd 11,88(%0);"
+	     "lfd 12,96(%0);"
+	     "lfd 13,104(%0);"
+	     "lfd 14,112(%0);"
+	     "lfd 15,120(%0);"
+	     "lfd 16,128(%0);"
+	     "lfd 17,136(%0);"
+	     "lfd 18,144(%0);"
+	     "lfd 19,152(%0);"
+	     "lfd 20,160(%0);"
+	     "lfd 21,168(%0);"
+	     "lfd 22,176(%0);"
+	     "lfd 23,184(%0);"
+	     "lfd 24,192(%0);"
+	     "lfd 25,200(%0);"
+	     "lfd 26,208(%0);"
+	     "lfd 27,216(%0);"
+	     "lfd 28,224(%0);"
+	     "lfd 29,232(%0);"
+	     "lfd 30,240(%0);"
+	     "lfd 31,248(%0)" :: "b"(&pcb->pcb_fpu.fpr[0]));
+	__asm volatile ("mtmsr %0; isync" :: "r"(msr));
+}
+
+void
+save_fpu(pcb)
+	struct pcb *pcb;
+{
+	int msr, scratch;
+	
+	__asm volatile ("mfmsr %0; ori %1,%0,%2; mtmsr %1; isync"
+		      : "=r"(msr), "=r"(scratch) : "K"(PSL_FP));
+	__asm ("stfd 0,0(%0);"
+	     "stfd 1,8(%0);"
+	     "stfd 2,16(%0);"
+	     "stfd 3,24(%0);"
+	     "stfd 4,32(%0);"
+	     "stfd 5,40(%0);"
+	     "stfd 6,48(%0);"
+	     "stfd 7,56(%0);"
+	     "stfd 8,64(%0);"
+	     "stfd 9,72(%0);"
+	     "stfd 10,80(%0);"
+	     "stfd 11,88(%0);"
+	     "stfd 12,96(%0);"
+	     "stfd 13,104(%0);"
+	     "stfd 14,112(%0);"
+	     "stfd 15,120(%0);"
+	     "stfd 16,128(%0);"
+	     "stfd 17,136(%0);"
+	     "stfd 18,144(%0);"
+	     "stfd 19,152(%0);"
+	     "stfd 20,160(%0);"
+	     "stfd 21,168(%0);"
+	     "stfd 22,176(%0);"
+	     "stfd 23,184(%0);"
+	     "stfd 24,192(%0);"
+	     "stfd 25,200(%0);"
+	     "stfd 26,208(%0);"
+	     "stfd 27,216(%0);"
+	     "stfd 28,224(%0);"
+	     "stfd 29,232(%0);"
+	     "stfd 30,240(%0);"
+	     "stfd 31,248(%0)" :: "b"(&pcb->pcb_fpu.fpr[0]));
+	__asm volatile ("mffs 0; stfd 0,0(%0)" :: "b"(&pcb->pcb_fpu.fpscr));
+	__asm volatile ("mtmsr %0; isync" :: "r"(msr));
+}
diff --git a/sys/powerpc/aim/swtch.S b/sys/powerpc/aim/swtch.S
index 97c15a5..c800264 100644
--- a/sys/powerpc/aim/swtch.S
+++ b/sys/powerpc/aim/swtch.S
@@ -80,6 +80,14 @@ ENTRY(cpu_switch)
 	mfcr	%r4			/* Save the condition register */
 	stw	%r4,PCB_CR(%r3)
 
+	lwz	%r29,PCB_FLAGS(%r3)
+	andi.	%r9, %r29, 1		/* XXX - don't hard code */
+	beq	.L1
+	mr	%r29, %r3		/* Save the PCB pointer */
+	bl	save_fpu
+	mr	%r3, %r29		/* and restore it */
+
+.L1:
 	bl	choosethread		/* Find a new thread to run */
 
 	mr	%r14,%r3		/* Save off the (struct thread *) */
@@ -91,6 +99,14 @@ ENTRY(cpu_switch)
 	stw	%r14,GD_CURTHREAD(%r4)	/* Store new current thread */
 	lwz	%r4,TD_PCB(%r14)	/* Grab the new PCB */
 
+	lwz	%r29, PCB_FLAGS(%r4)	/* Restore FPU regs if needed */
+	andi.	%r9, %r29, 1
+	beq	.L2
+	mr	%r29, %r4
+	bl	enable_fpu
+	mr	%r4, %r29
+
+.L2:
 	lmw	%r14,PCB_CONTEXT(%r4)	/* Load the non-volatile GP regs */
 	lwz	%r5,PCB_CR(%r4)		/* Load the condition register */
 	mtcr	%r5
diff --git a/sys/powerpc/powerpc/genassym.c b/sys/powerpc/powerpc/genassym.c
index 229d839..0b031c1 100644
--- a/sys/powerpc/powerpc/genassym.c
+++ b/sys/powerpc/powerpc/genassym.c
@@ -94,6 +94,7 @@ ASSYM(PCB_PMR, offsetof(struct pcb, pcb_pmreal));
 ASSYM(PCB_SP, offsetof(struct pcb, pcb_sp));
 ASSYM(PCB_SPL, offsetof(struct pcb, pcb_spl));
 ASSYM(PCB_ONFAULT, offsetof(struct pcb, pcb_onfault));
+ASSYM(PCB_FLAGS, offsetof(struct pcb, pcb_flags));
 
 ASSYM(TD_PROC, offsetof(struct thread, td_proc));
 ASSYM(TD_PCB, offsetof(struct thread, td_pcb));
diff --git a/sys/powerpc/powerpc/machdep.c b/sys/powerpc/powerpc/machdep.c
index 9432ddb..b141ce1 100644
--- a/sys/powerpc/powerpc/machdep.c
+++ b/sys/powerpc/powerpc/machdep.c
@@ -425,6 +425,7 @@ powerpc_init(u_int startkernel, u_int endkernel, u_int basekernel, char *args)
 	thread0->td_kstack = proc0kstack;
 	thread0->td_pcb = (struct pcb *)
 	    (thread0->td_kstack + KSTACK_PAGES * PAGE_SIZE) - 1;
+	enable_fpu(thread0->td_pcb);
 
 	globalp = pmap_steal_memory(round_page(sizeof(struct globaldata)));
 
@@ -918,3 +919,95 @@ globaldata_init(struct globaldata *globaldata, int cpuid, size_t sz)
         globaldata->gd_next_asn = 0;
         globaldata->gd_current_asngen = 1;
 }
+
+void
+enable_fpu(pcb)
+	struct pcb *pcb;
+{
+	int msr, scratch;
+
+	if (!(pcb->pcb_flags & PCB_FPU)) {
+		bzero(&pcb->pcb_fpu, sizeof pcb->pcb_fpu);
+		pcb->pcb_flags |= PCB_FPU;
+	}
+	__asm volatile ("mfmsr %0; ori %1,%0,%2; mtmsr %1; isync"
+		      : "=r"(msr), "=r"(scratch) : "K"(PSL_FP));
+	__asm volatile ("lfd 0,0(%0); mtfsf 0xff,0" :: "b"(&pcb->pcb_fpu.fpscr));
+	__asm ("lfd 0,0(%0);"
+	     "lfd 1,8(%0);"
+	     "lfd 2,16(%0);"
+	     "lfd 3,24(%0);"
+	     "lfd 4,32(%0);"
+	     "lfd 5,40(%0);"
+	     "lfd 6,48(%0);"
+	     "lfd 7,56(%0);"
+	     "lfd 8,64(%0);"
+	     "lfd 9,72(%0);"
+	     "lfd 10,80(%0);"
+	     "lfd 11,88(%0);"
+	     "lfd 12,96(%0);"
+	     "lfd 13,104(%0);"
+	     "lfd 14,112(%0);"
+	     "lfd 15,120(%0);"
+	     "lfd 16,128(%0);"
+	     "lfd 17,136(%0);"
+	     "lfd 18,144(%0);"
+	     "lfd 19,152(%0);"
+	     "lfd 20,160(%0);"
+	     "lfd 21,168(%0);"
+	     "lfd 22,176(%0);"
+	     "lfd 23,184(%0);"
+	     "lfd 24,192(%0);"
+	     "lfd 25,200(%0);"
+	     "lfd 26,208(%0);"
+	     "lfd 27,216(%0);"
+	     "lfd 28,224(%0);"
+	     "lfd 29,232(%0);"
+	     "lfd 30,240(%0);"
+	     "lfd 31,248(%0)" :: "b"(&pcb->pcb_fpu.fpr[0]));
+	__asm volatile ("mtmsr %0; isync" :: "r"(msr));
+}
+
+void
+save_fpu(pcb)
+	struct pcb *pcb;
+{
+	int msr, scratch;
+	
+	__asm volatile ("mfmsr %0; ori %1,%0,%2; mtmsr %1; isync"
+		      : "=r"(msr), "=r"(scratch) : "K"(PSL_FP));
+	__asm ("stfd 0,0(%0);"
+	     "stfd 1,8(%0);"
+	     "stfd 2,16(%0);"
+	     "stfd 3,24(%0);"
+	     "stfd 4,32(%0);"
+	     "stfd 5,40(%0);"
+	     "stfd 6,48(%0);"
+	     "stfd 7,56(%0);"
+	     "stfd 8,64(%0);"
+	     "stfd 9,72(%0);"
+	     "stfd 10,80(%0);"
+	     "stfd 11,88(%0);"
+	     "stfd 12,96(%0);"
+	     "stfd 13,104(%0);"
+	     "stfd 14,112(%0);"
+	     "stfd 15,120(%0);"
+	     "stfd 16,128(%0);"
+	     "stfd 17,136(%0);"
+	     "stfd 18,144(%0);"
+	     "stfd 19,152(%0);"
+	     "stfd 20,160(%0);"
+	     "stfd 21,168(%0);"
+	     "stfd 22,176(%0);"
+	     "stfd 23,184(%0);"
+	     "stfd 24,192(%0);"
+	     "stfd 25,200(%0);"
+	     "stfd 26,208(%0);"
+	     "stfd 27,216(%0);"
+	     "stfd 28,224(%0);"
+	     "stfd 29,232(%0);"
+	     "stfd 30,240(%0);"
+	     "stfd 31,248(%0)" :: "b"(&pcb->pcb_fpu.fpr[0]));
+	__asm volatile ("mffs 0; stfd 0,0(%0)" :: "b"(&pcb->pcb_fpu.fpscr));
+	__asm volatile ("mtmsr %0; isync" :: "r"(msr));
+}
diff --git a/sys/powerpc/powerpc/swtch.S b/sys/powerpc/powerpc/swtch.S
index 97c15a5..c800264 100644
--- a/sys/powerpc/powerpc/swtch.S
+++ b/sys/powerpc/powerpc/swtch.S
@@ -80,6 +80,14 @@ ENTRY(cpu_switch)
 	mfcr	%r4			/* Save the condition register */
 	stw	%r4,PCB_CR(%r3)
 
+	lwz	%r29,PCB_FLAGS(%r3)
+	andi.	%r9, %r29, 1		/* XXX - don't hard code */
+	beq	.L1
+	mr	%r29, %r3		/* Save the PCB pointer */
+	bl	save_fpu
+	mr	%r3, %r29		/* and restore it */
+
+.L1:
 	bl	choosethread		/* Find a new thread to run */
 
 	mr	%r14,%r3		/* Save off the (struct thread *) */
@@ -91,6 +99,14 @@ ENTRY(cpu_switch)
 	stw	%r14,GD_CURTHREAD(%r4)	/* Store new current thread */
 	lwz	%r4,TD_PCB(%r14)	/* Grab the new PCB */
 
+	lwz	%r29, PCB_FLAGS(%r4)	/* Restore FPU regs if needed */
+	andi.	%r9, %r29, 1
+	beq	.L2
+	mr	%r29, %r4
+	bl	enable_fpu
+	mr	%r4, %r29
+
+.L2:
 	lmw	%r14,PCB_CONTEXT(%r4)	/* Load the non-volatile GP regs */
 	lwz	%r5,PCB_CR(%r4)		/* Load the condition register */
 	mtcr	%r5
diff --git a/sys/powerpc/powerpc/swtch.s b/sys/powerpc/powerpc/swtch.s
index 97c15a5..c800264 100644
--- a/sys/powerpc/powerpc/swtch.s
+++ b/sys/powerpc/powerpc/swtch.s
@@ -80,6 +80,14 @@ ENTRY(cpu_switch)
 	mfcr	%r4			/* Save the condition register */
 	stw	%r4,PCB_CR(%r3)
 
+	lwz	%r29,PCB_FLAGS(%r3)
+	andi.	%r9, %r29, 1		/* XXX - don't hard code */
+	beq	.L1
+	mr	%r29, %r3		/* Save the PCB pointer */
+	bl	save_fpu
+	mr	%r3, %r29		/* and restore it */
+
+.L1:
 	bl	choosethread		/* Find a new thread to run */
 
 	mr	%r14,%r3		/* Save off the (struct thread *) */
@@ -91,6 +99,14 @@ ENTRY(cpu_switch)
 	stw	%r14,GD_CURTHREAD(%r4)	/* Store new current thread */
 	lwz	%r4,TD_PCB(%r14)	/* Grab the new PCB */
 
+	lwz	%r29, PCB_FLAGS(%r4)	/* Restore FPU regs if needed */
+	andi.	%r9, %r29, 1
+	beq	.L2
+	mr	%r29, %r4
+	bl	enable_fpu
+	mr	%r4, %r29
+
+.L2:
 	lmw	%r14,PCB_CONTEXT(%r4)	/* Load the non-volatile GP regs */
 	lwz	%r5,PCB_CR(%r4)		/* Load the condition register */
 	mtcr	%r5