Add Altivec support for supported CPUs. This is derived from the FPU support

code, and also reducing the size of trapcode to fit inside a 32 byte handler
slot.

Reviewed by:	grehan
MFC after:	2 weeks

4 files changed, 130 insertions, 54 deletions

diff --git a/sys/powerpc/aim/machdep.c b/sys/powerpc/aim/machdep.c
index 2199cb136c3b..da8f35f7f3b3 100644
--- a/sys/powerpc/aim/machdep.c
+++ b/sys/powerpc/aim/machdep.c
@@ -103,6 +103,7 @@ __FBSDID("$FreeBSD$");
 #include <vm/vm_object.h>
 #include <vm/vm_pager.h>
 
+#include <machine/altivec.h>
 #include <machine/bat.h>
 #include <machine/cpu.h>
 #include <machine/elf.h>
@@ -238,7 +239,6 @@ extern void	*dsitrap, *dsisize;
 extern void	*decrint, *decrsize;
 extern void     *extint, *extsize;
 extern void	*dblow, *dbsize;
-extern void	*vectrap, *vectrapsize;
 
 u_int
 powerpc_init(u_int startkernel, u_int endkernel, u_int basekernel, void *mdp)
@@ -340,7 +340,7 @@ powerpc_init(u_int startkernel, u_int endkernel, u_int basekernel, void *mdp)
 	bcopy(&trapcode, (void *)EXC_SC,   (size_t)&trapsize);
 	bcopy(&trapcode, (void *)EXC_TRC,  (size_t)&trapsize);
 	bcopy(&trapcode, (void *)EXC_FPA,  (size_t)&trapsize);
-	bcopy(&vectrap,  (void *)EXC_VEC,  (size_t)&vectrapsize);
+	bcopy(&trapcode, (void *)EXC_VEC,  (size_t)&trapsize);
 	bcopy(&trapcode, (void *)EXC_VECAST, (size_t)&trapsize);
 	bcopy(&trapcode, (void *)EXC_THRM, (size_t)&trapsize);
 	bcopy(&trapcode, (void *)EXC_BPT,  (size_t)&trapsize);
@@ -647,7 +647,21 @@ grab_mcontext(struct thread *td, mcontext_t *mcp, int flags)
 		memcpy(mcp->mc_fpreg, pcb->pcb_fpu.fpr, 32*sizeof(double));
 	}
 
-	/* XXX Altivec context ? */
+	/*
+	 * Repeat for Altivec context
+	 */
+
+	if (pcb->pcb_flags & PCB_VEC) {
+		KASSERT(td == curthread,
+			("get_mcontext: fp save not curthread"));
+		critical_enter();
+		save_vec(td);
+		critical_exit();
+		mcp->mc_flags |= _MC_AV_VALID;
+		mcp->mc_vscr  = pcb->pcb_vec.vscr;
+		mcp->mc_vrsave =  pcb->pcb_vec.vrsave;
+		memcpy(mcp->mc_avec, pcb->pcb_vec.vr, sizeof(mcp->mc_avec));
+	}
 
 	mcp->mc_len = sizeof(*mcp);
 
@@ -701,7 +715,17 @@ set_mcontext(struct thread *td, const mcontext_t *mcp)
 		memcpy(pcb->pcb_fpu.fpr, mcp->mc_fpreg, 32*sizeof(double));
 	}
 
-	/* XXX Altivec context? */
+	if (mcp->mc_flags & _MC_AV_VALID) {
+		if ((pcb->pcb_flags & PCB_VEC) != PCB_VEC) {
+			critical_enter();
+			enable_vec(td);
+			critical_exit();
+		}
+		pcb->pcb_vec.vscr = mcp->mc_vscr;
+		pcb->pcb_vec.vrsave = mcp->mc_vrsave;
+		memcpy(pcb->pcb_vec.vr, mcp->mc_avec, sizeof(mcp->mc_avec));
+	}
+
 
 	return (0);
 }
diff --git a/sys/powerpc/aim/swtch.S b/sys/powerpc/aim/swtch.S
index 0f59e7312e24..672fe6b1161c 100644
--- a/sys/powerpc/aim/swtch.S
+++ b/sys/powerpc/aim/swtch.S
@@ -106,8 +106,16 @@ ENTRY(cpu_switch)
 	andi.	%r6, %r6, PCB_FPU
 	beq	.L1
 	bl	save_fpu
-	mr	%r3,%r14		/* restore old thread ptr */
+
 .L1:
+	lwz	%r6,PCB_FLAGS(%r5)
+	/* Save Altivec context if needed */
+	andi.	%r6, %r6, PCB_VEC
+	beq	.L2
+	bl	save_vec
+	
+.L2:
+	mr	%r3,%r14		/* restore old thread ptr */
 	bl	pmap_deactivate		/* Deactivate the current pmap */
 
 cpu_switchin:
@@ -122,12 +130,20 @@ cpu_switchin:
 	lwz	%r6, PCB_FLAGS(%r17)
 	/* Restore FPU context if needed */
 	andi.	%r6, %r6, PCB_FPU
-	beq	.L2
+	beq	.L3
 	mr	%r3,%r15		/* Pass curthread to enable_fpu */
 	bl	enable_fpu
 
+.L3:
+	lwz	%r6, PCB_FLAGS(%r17)
+	/* Restore Altivec context if needed */
+	andi.	%r6, %r6, PCB_VEC
+	beq	.L4
+	mr	%r3,%r15		/* Pass curthread to enable_vec */
+	bl	enable_vec
+
 	/* thread to restore is in r3 */
-.L2:
+.L4:
 	mr	%r3,%r17		/* Recover PCB ptr */
 	lmw	%r12,PCB_CONTEXT(%r3)	/* Load the non-volatile GP regs */
 	mr	%r2,%r12
diff --git a/sys/powerpc/aim/trap.c b/sys/powerpc/aim/trap.c
index 0a3827d03398..4b540937fa8a 100644
--- a/sys/powerpc/aim/trap.c
+++ b/sys/powerpc/aim/trap.c
@@ -65,6 +65,7 @@ __FBSDID("$FreeBSD$");
 #include <vm/vm_map.h>
 #include <vm/vm_page.h>
 
+#include <machine/altivec.h>
 #include <machine/cpu.h>
 #include <machine/db_machdep.h>
 #include <machine/fpu.h>
@@ -188,24 +189,16 @@ trap(struct trapframe *frame)
 			enable_fpu(td);
 			break;
 
-#ifdef	ALTIVEC
 		case EXC_VEC:
-			if ((vecthread = PCPU_GET(vecthread)) != NULL) {
-				KASSERT(vecthread != td,
-				    ("altivec already enabled"));
-				save_vec(vecthread);
-			}
-			PCPU_SET(vecthread, td);
-			td->td_pcb->pcb_veccpu = PCPU_GET(cpuid);
+			KASSERT((td->td_pcb->pcb_flags & PCB_VEC) != PCB_VEC,
+			    ("Altivec already enabled for thread"));
 			enable_vec(td);
-			frame->srr1 |= PSL_VEC;
 			break;
-#else
-		case EXC_VEC:
+
 		case EXC_VECAST:
+			printf("Vector assist exception!\n");
 			sig = SIGILL;
 			break;
-#endif /* ALTIVEC */
 
 		case EXC_ALI:
 			if (fix_unaligned(td, frame) != 0)
diff --git a/sys/powerpc/aim/trap_subr.S b/sys/powerpc/aim/trap_subr.S
index ba72a6557e35..82cdf3237b8d 100644
--- a/sys/powerpc/aim/trap_subr.S
+++ b/sys/powerpc/aim/trap_subr.S
@@ -79,13 +79,13 @@
 /*
  * FRAME_SETUP assumes:
  *	SPRG1		SP (1)
+ * 	SPRG3		trap type
  *	savearea	r28-r31,DAR,DSISR   (DAR & DSISR only for DSI traps)
  *	r28		LR
  *	r29		CR
  *	r30		scratch
  *	r31		scratch
  *	r1		kernel stack
- *	LR		trap type (from calling address, mask with 0xff00)
  *	SRR0/1		as at start of trap
  */
 #define	FRAME_SETUP(savearea)						\
@@ -146,8 +146,7 @@
 	lwz	%r31,(savearea+CPUSAVE_SRR1)(%r2); /* saved SRR1 */	\
 	mfxer	%r3;							\
 	mfctr	%r4;							\
-	mflr	%r5;							\
-	andi.	%r5,%r5,0xff00;		/* convert LR to exc # */	\
+	mfsprg3	%r5;							\
 	stw	%r3, FRAME_XER+8(1);	/* save xer/ctr/exc */		\
 	stw	%r4, FRAME_CTR+8(1);					\
 	stw	%r5, FRAME_EXC+8(1);					\
@@ -264,18 +263,10 @@ CNAME(rstsize) = . - CNAME(rstcode)
 	.globl	CNAME(trapcode),CNAME(trapsize)
 CNAME(trapcode):
 	mtsprg1	%r1			/* save SP */
-	GET_CPUINFO(%r1)
-	stw	%r28,(PC_TEMPSAVE+CPUSAVE_R28)(%r1)	/* free r28-r31 */
-	stw	%r29,(PC_TEMPSAVE+CPUSAVE_R29)(%r1)
-	stw	%r30,(PC_TEMPSAVE+CPUSAVE_R30)(%r1)
-	stw	%r31,(PC_TEMPSAVE+CPUSAVE_R31)(%r1)
-	mfsprg1	%r1			/* restore SP, in case of branch */
-	mflr	%r28			/* save LR */
-	mfcr	%r29			/* save CR */
-/* Test whether we already had PR set */
-	mfsrr1	%r31
-	mtcr	%r31
-	bla	s_trap			/* LR & 0xff00 is exception # */
+	mflr	%r1			/* Save the old LR in r1 */
+	mtsprg2 %r1			/* And then in SPRG2 */
+	li	%r1, 0x20		/* How to get the vector from LR */
+	bla	generictrap		/* LR & SPRG3 is exception # */
 CNAME(trapsize) = .-CNAME(trapcode)
 
 /*
@@ -296,10 +287,15 @@ CNAME(alitrap):
 	mfsprg1	%r1			/* restore SP, in case of branch */
 	mflr	%r28			/* save LR */
 	mfcr	%r29			/* save CR */
-/* Test whether we already had PR set */
+
+	/* Put our exception vector in SPRG0 */
+	li	%r31, EXC_ALI
+	mtsprg3	%r31
+
+	/* Test whether we already had PR set */
 	mfsrr1	%r31
 	mtcr	%r31
-	bla	s_trap			/* LR & 0xff00 is exception # */
+	bla	s_trap
 CNAME(alisize) = .-CNAME(alitrap)
 
 /*
@@ -364,6 +360,11 @@ CNAME(dsisize) = .-CNAME(dsitrap)
  * Preamble code for DSI/ISI traps
  */
 disitrap:
+	/* Write the trap vector to SPRG0 by computing LR & 0xff00 */
+	mflr	%r1
+	andi.	%r1,%r1,0xff00
+	mtsprg3	%r1
+	
 	GET_CPUINFO(%r1)
 	lwz	%r30,(PC_DISISAVE+CPUSAVE_R28)(%r1)
 	stw	%r30,(PC_TEMPSAVE+CPUSAVE_R28)(%r1)
@@ -416,6 +417,47 @@ realtrap:
 	mtcr	%r1
 	mfsprg1	%r1			/* restore SP (might have been
 					   overwritten) */
+	bf	17,k_trap		/* branch if PSL_PR is false */
+	GET_CPUINFO(%r1)
+	lwz	%r1,PC_CURPCB(%r1)
+	RESTORE_KERN_SRS(%r30,%r31)	/* enable kernel mapping */
+	ba s_trap
+
+/*
+ * generictrap does some standard setup for trap handling to minimize
+ * the code that need be installed in the actual vectors. It expects
+ * the following conditions.
+ * 
+ * R1 - Trap vector = LR & (0xff00 | R1)
+ * SPRG1 - Original R1 contents
+ * SPRG2 - Original LR
+ */
+
+generictrap:
+	/* Save R1 for computing the exception vector */
+	mtsprg3 %r1
+
+	/* Save interesting registers */
+	GET_CPUINFO(%r1)
+	stw	%r28,(PC_TEMPSAVE+CPUSAVE_R28)(%r1)	/* free r28-r31 */
+	stw	%r29,(PC_TEMPSAVE+CPUSAVE_R29)(%r1)
+	stw	%r30,(PC_TEMPSAVE+CPUSAVE_R30)(%r1)
+	stw	%r31,(PC_TEMPSAVE+CPUSAVE_R31)(%r1)
+	mfsprg1	%r1			/* restore SP, in case of branch */
+	mfsprg2	%r28			/* save LR */
+	mfcr	%r29			/* save CR */
+
+	/* Compute the exception vector from the link register */
+	mfsprg3 %r31
+	ori	%r31,%r31,0xff00
+	mflr	%r30
+	and	%r30,%r30,%r31
+	mtsprg3	%r30
+
+	/* Test whether we already had PR set */
+	mfsrr1	%r31
+	mtcr	%r31
+
 s_trap:
 	bf	17,k_trap		/* branch if PSL_PR is false */
 	GET_CPUINFO(%r1)
@@ -464,14 +506,6 @@ CNAME(asttrapexit):
 	FRAME_LEAVE(PC_TEMPSAVE)
 	rfi
 
-/*
- *     Temporary: vector-unavailable traps are directed to vector-assist traps
- */
-	.globl  CNAME(vectrap),CNAME(vectrapsize)
-CNAME(vectrap):
-	ba	EXC_VECAST
-CNAME(vectrapsize) = .-CNAME(vectrap)
-
 #if defined(KDB)
 /*
  * Deliberate entry to dbtrap
@@ -499,6 +533,14 @@ CNAME(breakpoint):
  * Now the kdb trap catching code.
  */
 dbtrap:
+	/* Write the trap vector to SPRG0 by computing LR & 0xff00 */
+	mflr	%r1
+	andi.	%r1,%r1,0xff00
+	mtsprg3	%r1
+
+	lis	%r1,(tmpstk+TMPSTKSZ-16)@ha	/* get new SP */
+	addi	%r1,%r1,(tmpstk+TMPSTKSZ-16)@l
+
 	FRAME_SETUP(PC_DBSAVE)
 /* Call C trap code: */
 	addi	%r3,%r1,8
@@ -536,24 +578,25 @@ CNAME(dblow):
 	mfcr	%r29			/* save CR in r29 */
 	mfsrr1	%r1
 	mtcr	%r1
-	GET_CPUINFO(%r1)
 	bf	17,1f			/* branch if privileged */
-	stw	%r28,(PC_TEMPSAVE+CPUSAVE_R28)(%r1)	/* free r28 */
-        mfsprg2	%r28				/* r29 holds cr ... */
-        stw	%r28,(PC_TEMPSAVE+CPUSAVE_R29)(%r1)	/* free r29 */
-        stw	%r30,(PC_TEMPSAVE+CPUSAVE_R30)(%r1)	/* free r30 */
-        stw	%r31,(PC_TEMPSAVE+CPUSAVE_R31)(%r1)	/* free r31 */
-        mflr	%r28					/* save LR */
-        bla     u_trap
+
+	/* Unprivileged case */
+	mtcr	%r29			/* put the condition register back */
+        mfsprg2	%r29			/* ... and r29 */
+        mflr	%r1			/* save LR */
+	mtsprg2 %r1			/* And then in SPRG2 */
+	li	%r1, 0	 		/* How to get the vector from LR */
+
+        bla     generictrap		/* and we look like a generic trap */
 1:
+	/* Privileged, so drop to KDB */
+	GET_CPUINFO(%r1)
 	stw	%r28,(PC_DBSAVE+CPUSAVE_R28)(%r1)	/* free r28 */
         mfsprg2	%r28				/* r29 holds cr...  */
         stw	%r28,(PC_DBSAVE+CPUSAVE_R29)(%r1)	/* free r29 */
         stw	%r30,(PC_DBSAVE+CPUSAVE_R30)(%r1)	/* free r30 */
         stw	%r31,(PC_DBSAVE+CPUSAVE_R31)(%r1)	/* free r31 */
         mflr	%r28					/* save LR */
-	lis	%r1,(tmpstk+TMPSTKSZ-16)@ha	/* get new SP */
-	addi	%r1,%r1,(tmpstk+TMPSTKSZ-16)@l
 	bla	dbtrap
 CNAME(dbsize) = .-CNAME(dblow)
 #endif /* KDB */