# HG changeset patch
# User Jonathan Pevarnek <pevarnj@gmail.com>
# Date 1305392087 14400
# Node ID a480d02a10c8c92d26cc260cf5352669aaa0427a
# Parent  191e99dffd6ca5bd76492417ef01d68c953a31d4# Parent  cabcdc3727e3577dcd645d47c0596dee7f29764f
merge

diff -r 191e99dffd6c -r a480d02a10c8 arch/io.c
--- a/arch/io.c	Sat May 14 12:54:29 2011 -0400
+++ b/arch/io.c	Sat May 14 12:54:47 2011 -0400
@@ -70,7 +70,7 @@
 
 	devst = *((u8*) 0x210);
 
-	if (*((u8*) 0x210) & 0x84)
+	if (devst & 0x84)
 		return;
 
 	die();
diff -r 191e99dffd6c -r a480d02a10c8 arch/svc.c
--- a/arch/svc.c	Sat May 14 12:54:29 2011 -0400
+++ b/arch/svc.c	Sat May 14 12:54:47 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;
 
diff -r 191e99dffd6c -r a480d02a10c8 arch/svcint.S
--- a/arch/svcint.S	Sat May 14 12:54:29 2011 -0400
+++ b/arch/svcint.S	Sat May 14 12:54:47 2011 -0400
@@ -20,7 +20,8 @@
 	lg	%r14,0(%r14)
 	basr	%r14,%r14
 
-	stmg	%r2,%r3,_PSA_GPRS+16	# store the return value
+	sr	%r1,%r1
+	sr	%r3,%r3
+	sigp	%r1,%r3,0x05
 
-	lmg	%r0,%r15,_PSA_GPRS
-	lpswe	_PSA_OLD_SVC_PSW
+	.byte 0x00, 0x00, 0x00, 0x00
diff -r 191e99dffd6c -r a480d02a10c8 include/svc.h
--- a/include/svc.h	Sat May 14 12:54:29 2011 -0400
+++ b/include/svc.h	Sat May 14 12:54:47 2011 -0400
@@ -5,6 +5,8 @@
 
 extern void setcontext(struct psw *psw, u64 *regs);
 extern void savecontext(struct psw *psw, u64 *regs);
-extern void set_svc_handler(u64(*f)(u64, u64, u64, u64), void *stack);
+extern void swapcontext(struct psw *oldpsw, u64 *oldregs,
+			struct psw *newpsw, u64 *newregs);
+extern void set_svc_handler(u64(*f)(u64, u64, u64, u64, u64), void *stack);
 
 #endif