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

9 files changed, 329 insertions, 54 deletions

diff --git a/sys/powerpc/aim/machdep.c b/sys/powerpc/aim/machdep.c
index 2199cb1..da8f35f 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 0f59e73..672fe6b 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 0a3827d..4b54093 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 ba72a65..82cdf32 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 */
diff --git a/sys/powerpc/include/altivec.h b/sys/powerpc/include/altivec.h
new file mode 100644
index 0000000..ba16a0b
--- /dev/null
+++ b/sys/powerpc/include/altivec.h
@@ -0,0 +1,36 @@
+/*-
+ * Copyright (c) 2008 Nathan Whitehorn
+ * All rights reserved
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ */
+
+#ifndef	_MACHINE_ALTIVEC_H_
+#define	_MACHINE_ALTIVEC_H_
+
+void    enable_vec(struct thread *);
+void    save_vec(struct thread *);
+
+#endif	/* _MACHINE_ALTIVEC_H_ */
+
diff --git a/sys/powerpc/include/pcb.h b/sys/powerpc/include/pcb.h
index 3f68899..a57cef1 100644
--- a/sys/powerpc/include/pcb.h
+++ b/sys/powerpc/include/pcb.h
@@ -47,12 +47,21 @@ struct pcb {
 						    copyin/copyout */
 	int		pcb_flags;
 #define	PCB_FPU		1	/* Process had FPU initialized */
+#define	PCB_VEC		2	/* Process had Altivec initialized */
 	struct fpu {
 		double	fpr[32];
 		double	fpscr;	/* FPSCR stored as double for easier access */
 	} pcb_fpu;		/* Floating point processor */
 	unsigned int	pcb_fpcpu;		/* which CPU had our FPU
 							stuff. */
+	struct vec {
+		uint32_t vr[32][4];
+		register_t vrsave;
+		register_t spare[2];
+		register_t vscr;
+	} pcb_vec __attribute__((aligned(16)));	/* Vector processor */
+	unsigned int	pcb_veccpu;		/* which CPU had our vector
+							stuff. */
 
 	union {
 		struct {
diff --git a/sys/powerpc/include/pcpu.h b/sys/powerpc/include/pcpu.h
index 0384902..a0a3e51 100644
--- a/sys/powerpc/include/pcpu.h
+++ b/sys/powerpc/include/pcpu.h
@@ -39,6 +39,7 @@ struct pmap;
 	int		pc_inside_intr;					\
 	struct pmap	*pc_curpmap;		/* current pmap */	\
 	struct thread	*pc_fputhread;		/* current fpu user */  \
+	struct thread	*pc_vecthread;		/* current vec user */  \
 	uintptr_t	pc_hwref;					\
 	uint32_t	pc_pir;						\
 	int		pc_bsp:1;					\
diff --git a/sys/powerpc/powerpc/altivec.c b/sys/powerpc/powerpc/altivec.c
new file mode 100644
index 0000000..eeec2bb
--- /dev/null
+++ b/sys/powerpc/powerpc/altivec.c
@@ -0,0 +1,152 @@
+/*-
+ * Copyright (C) 1996 Wolfgang Solfrank.
+ * Copyright (C) 1996 TooLs GmbH.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. All advertising materials mentioning features or use of this software
+ *    must display the following acknowledgement:
+ *	This product includes software developed by TooLs GmbH.
+ * 4. The name of TooLs GmbH may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY TOOLS GMBH ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL TOOLS GMBH BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
+ * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
+ * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
+ * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *	$NetBSD: fpu.c,v 1.5 2001/07/22 11:29:46 wiz Exp $
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/proc.h>
+#include <sys/systm.h>
+#include <sys/limits.h>
+
+#include <machine/altivec.h>
+#include <machine/pcb.h>
+#include <machine/psl.h>
+
+void
+enable_vec(struct thread *td)
+{
+	int	msr;
+	struct	pcb *pcb;
+	struct	trapframe *tf;
+
+	pcb = td->td_pcb;
+	tf = trapframe(td);
+
+	/*
+	 * Save the thread's Altivec CPU number, and set the CPU's current
+	 * vector thread
+	 */
+	td->td_pcb->pcb_veccpu = PCPU_GET(cpuid);
+	PCPU_SET(vecthread, td);
+
+	/*
+	 * Enable the vector unit for when the thread returns from the
+	 * exception. If this is the first time the unit has been used by
+	 * the thread, initialise the vector registers and VSCR to 0, and
+	 * set the flag to indicate that the vector unit is in use.
+	 */
+	tf->srr1 |= PSL_VEC;
+	if (!(pcb->pcb_flags & PCB_VEC)) {
+		memset(&pcb->pcb_vec, 0, sizeof pcb->pcb_vec);
+		pcb->pcb_flags |= PCB_VEC;
+	}
+
+	/*
+	 * Temporarily enable the vector unit so the registers
+	 * can be restored.
+	 */
+	msr = mfmsr();
+	mtmsr(msr | PSL_VEC);
+	isync();
+
+	/*
+	 * Restore VSCR by first loading it into a vector and then into VSCR.
+	 * (this needs to done before loading the user's vector registers
+	 * since we need to use a scratch vector register)
+	 */
+	__asm __volatile("vxor 0,0,0; lvewx 0,0,%0; mtvscr 0" \
+			  :: "b"(&pcb->pcb_vec.vscr));
+
+#define LVX(n)   __asm ("lvx " #n ",0,%0" \
+		:: "b"(&pcb->pcb_vec.vr[n]));
+	LVX(0);		LVX(1);		LVX(2);		LVX(3);
+	LVX(4);		LVX(5);		LVX(6);		LVX(7);
+	LVX(8);		LVX(9);		LVX(10);	LVX(11);
+	LVX(12);	LVX(13);	LVX(14);	LVX(15);
+	LVX(16);	LVX(17);	LVX(18);	LVX(19);
+	LVX(20);	LVX(21);	LVX(22);	LVX(23);
+	LVX(24);	LVX(25);	LVX(26);	LVX(27);
+	LVX(28);	LVX(29);	LVX(30);	LVX(31);
+#undef LVX
+
+	isync();
+	mtmsr(msr);
+}
+
+void
+save_vec(struct thread *td)
+{
+	int	msr;
+	struct	pcb *pcb;
+
+	pcb = td->td_pcb;
+
+	/*
+	 * Temporarily re-enable the vector unit during the save
+	 */
+	msr = mfmsr();
+	mtmsr(msr | PSL_VEC);
+	isync();
+
+	/*
+	 * Save the vector registers and VSCR to the PCB
+	 */
+#define STVX(n)   __asm ("stvx %1,0,%0" \
+		:: "b"(pcb->pcb_vec.vr[n]), "n"(n));
+	STVX(0);	STVX(1);	STVX(2);	STVX(3);
+	STVX(4);	STVX(5);	STVX(6);	STVX(7);
+	STVX(8);	STVX(9);	STVX(10);	STVX(11);
+	STVX(12);	STVX(13);	STVX(14);	STVX(15);
+	STVX(16);	STVX(17);	STVX(18);	STVX(19);
+	STVX(20);	STVX(21);	STVX(22);	STVX(23);
+	STVX(24);	STVX(25);	STVX(26);	STVX(27);
+	STVX(28);	STVX(29);	STVX(30);	STVX(31);
+#undef STVX
+
+	__asm __volatile("mfvscr 0; stvewx 0,0,%0" :: "b"(&pcb->pcb_vec.vscr));
+
+	/*
+	 * Disable vector unit again
+	 */
+	isync();
+	mtmsr(msr);
+
+	/*
+	 * Clear the current vec thread and pcb's CPU id
+	 * XXX should this be left clear to allow lazy save/restore ?
+	 */
+	pcb->pcb_veccpu = INT_MAX;
+	PCPU_SET(vecthread, NULL);
+}
+
diff --git a/sys/powerpc/powerpc/genassym.c b/sys/powerpc/powerpc/genassym.c
index 1c4f4fb..1cb79f9 100644
--- a/sys/powerpc/powerpc/genassym.c
+++ b/sys/powerpc/powerpc/genassym.c
@@ -171,6 +171,7 @@ ASSYM(PCB_LR, offsetof(struct pcb, pcb_lr));
 ASSYM(PCB_ONFAULT, offsetof(struct pcb, pcb_onfault));
 ASSYM(PCB_FLAGS, offsetof(struct pcb, pcb_flags));
 ASSYM(PCB_FPU, PCB_FPU);
+ASSYM(PCB_VEC, PCB_VEC);
 
 ASSYM(PCB_AIM_USR, offsetof(struct pcb, pcb_cpu.aim.usr));
 ASSYM(PCB_BOOKE_CTR, offsetof(struct pcb, pcb_cpu.booke.ctr));