FPU support.

Obtained from:	NetBSD (portions)

2 files changed, 16 insertions, 110 deletions

diff --git a/sys/powerpc/aim/machdep.c b/sys/powerpc/aim/machdep.c
index 1330e0f..ac35d8f 100644
--- a/sys/powerpc/aim/machdep.c
+++ b/sys/powerpc/aim/machdep.c
@@ -350,6 +350,7 @@ powerpc_init(u_int startkernel, u_int endkernel, u_int basekernel, char *args)
 	bcopy(&dsitrap, (void *)EXC_DSI, (size_t)&dsisize);
 	bcopy(&isitrap, (void *)EXC_ISI, (size_t)&isisize);
 	bcopy(&trapcode, (void *)EXC_SC, (size_t)&trapsize);
+	bcopy(&trapcode, (void *)EXC_FPU, (size_t)&trapsize);
 
 	/*
 	 * Start initializing proc0 and thread0.
@@ -960,93 +961,3 @@ cpu_pcpu_init(struct pcpu *pcpu, int cpuid, size_t sz)
 
 	pcpu->pc_current_asngen = 1;
 }
-
-void
-enable_fpu(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(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/trap.c b/sys/powerpc/aim/trap.c
index 13f8646..cdceed3 100644
--- a/sys/powerpc/aim/trap.c
+++ b/sys/powerpc/aim/trap.c
@@ -94,11 +94,14 @@ int setfault(faultbuf);	/* defined in locore.S */
 int badaddr(void *, size_t);
 int badaddr_read(void *, size_t, int *);
 
+extern char	*syscallnames[];
+
 void
 trap(frame)
 	struct trapframe *frame;
 {
 	struct thread *td = PCPU_GET(curthread);
+	struct thread *fputhread;
 	struct proc *p = td->td_proc;
 	int type = frame->exc;
 	int ftype, rv;
@@ -363,24 +366,15 @@ syscall_bad:
 		break;
 
 	case EXC_FPU|EXC_USER:
-#if 0
-		curcpu()->ci_ev_fpu.ev_count++;
-#endif
-#if 0
-		if (fpuproc) {
-			curcpu()->ci_ev_fpusw.ev_count++;
-			save_fpu(fpuproc);
+		if ((fputhread = PCPU_GET(fputhread)) != NULL) {
+			KASSERT(fputhread != td,
+			    ("floating-point already enabled"));
+			save_fpu(fputhread);
 		}
-#endif
-#if defined(MULTIPROCESSOR)
-		if (p->p_addr->u_pcb.pcb_fpcpu)
-			save_fpu_proc(p);
-#endif
-#if 0
-		fpuproc = p;
-		p->p_addr->u_pcb.pcb_fpcpu = curcpu();
-		enable_fpu(p);
-#endif
+		PCPU_SET(fputhread, td);
+		td->td_pcb->pcb_fpcpu = PCPU_GET(cpuid);
+		enable_fpu(td);
+		frame->srr1 |= PSL_FP;
 		break;
 
 #ifdef ALTIVEC
@@ -483,6 +477,7 @@ brain_damage2:
 		panic("trap");
 	}
 
+#if 0
 	/* Take pending signals. */
 	{
 		int sig;
@@ -490,15 +485,15 @@ brain_damage2:
 		while ((sig = CURSIG(p)) != 0)
 			postsig(sig);
 	}
+#endif
 
 	/*
 	 * If someone stole the fp or vector unit while we were away,
 	 * disable it
 	 */
-#if 0
-	if (p != fpuproc || p->p_addr->u_pcb.pcb_fpcpu != curcpu())
+	if (td != PCPU_GET(fputhread) ||
+	    td->td_pcb->pcb_fpcpu != PCPU_GET(cpuid))
 		frame->srr1 &= ~PSL_FP;
-#endif
 #ifdef ALTIVEC
 	if (p != vecproc)
 		frame->srr1 &= ~PSL_VEC;