--- a/arch/svc.c	Thu May 12 13:27:53 2011 -0400
+++ b/arch/svc.c	Thu May 12 15:56:46 2011 -0400
@@ -20,6 +20,33 @@
 	die();
 }
 
+void swapcontext(struct psw *oldpsw, u64 *oldregs,
+		 struct psw *newpsw, u64 *newregs)
+{
+	__builtin_memcpy(PSA_PSW_TMP, newpsw, sizeof(struct psw));
+
+	__builtin_memset(oldpsw, 0, sizeof(struct psw));
+	oldpsw->ea = 1;
+	oldpsw->ba = 1;
+
+	asm volatile(
+		"stmg	%%r0,%%r15,%0\n"	/* store gpr */
+		"larl	%%r1,0f\n"
+		"stg	%%r1,%1\n"		/* store the IA */
+
+		"lmg	%%r0,%%r15,%2\n"	/* load gpr */
+		"lpswe	%3\n"			/* load new psw */
+
+		"0:\n"
+	: /* output */
+	  "=m" (*oldregs),
+	  "=m" (oldpsw->ptr)
+	: /* input */
+	  "m" (*newregs),
+	  "m" (*PSA_PSW_TMP)
+	);
+}
+
 void savecontext(struct psw *psw, u64 *regs)
 {
 	__builtin_memcpy(psw, PSA_OLD_SVC_PSW, sizeof(struct psw));
@@ -30,7 +57,7 @@
 u64 _SVC_HANDLER;
 u64 _SVC_STACK;
 
-void set_svc_handler(u64(*f)(u64, u64, u64, u64), void *stack)
+void set_svc_handler(u64(*f)(u64, u64, u64, u64, u64), void *stack)
 {
 	struct psw psw;