From bdc7fe3638fa7693eed5886b5b2afe0858d875fc Mon Sep 17 00:00:00 2001
From: Christian Borntraeger <borntraeger@de.ibm.com>
Date: Fri, 19 Jun 2015 15:40:45 +0200
Subject: [PATCH] s390-ccw.img: Consume service interrupts

We have to consume the outstanding service interrupt after each
service call, otherwise a correct implementation will return
CC=2 on subsequent service calls.

Signed-off-by: Christian Borntraeger <borntraeger@de.ibm.com>
Reviewed-by: David Hildenbrand <dahi@linux.vnet.ibm.com>
Signed-off-by: Cornelia Huck <cornelia.huck@de.ibm.com>
---
 pc-bios/s390-ccw/s390-ccw.h   |  1 +
 pc-bios/s390-ccw/sclp-ascii.c |  1 +
 pc-bios/s390-ccw/start.S      | 32 ++++++++++++++++++++++++++++++++
 3 files changed, 34 insertions(+)

diff --git a/pc-bios/s390-ccw/s390-ccw.h b/pc-bios/s390-ccw/s390-ccw.h
index 9b3868bd6e..5484c2a45c 100644
--- a/pc-bios/s390-ccw/s390-ccw.h
+++ b/pc-bios/s390-ccw/s390-ccw.h
@@ -47,6 +47,7 @@ typedef unsigned long long __u64;
 
 /* start.s */
 void disabled_wait(void);
+void consume_sclp_int(void);
 
 /* main.c */
 void virtio_panic(const char *string);
diff --git a/pc-bios/s390-ccw/sclp-ascii.c b/pc-bios/s390-ccw/sclp-ascii.c
index 761fb44ff5..dc1c3e4f4d 100644
--- a/pc-bios/s390-ccw/sclp-ascii.c
+++ b/pc-bios/s390-ccw/sclp-ascii.c
@@ -24,6 +24,7 @@ static int sclp_service_call(unsigned int command, void *sccb)
                 "       srl     %0,28"
                 : "=&d" (cc) : "d" (command), "a" (__pa(sccb))
                 : "cc", "memory");
+        consume_sclp_int();
         if (cc == 3)
                 return -EIO;
         if (cc == 2)
diff --git a/pc-bios/s390-ccw/start.S b/pc-bios/s390-ccw/start.S
index 5d5df0d616..b6dd8c2fbe 100644
--- a/pc-bios/s390-ccw/start.S
+++ b/pc-bios/s390-ccw/start.S
@@ -28,6 +28,38 @@ disabled_wait:
         larl %r1,disabled_wait_psw
         lpswe   0(%r1)
 
+
+/*
+ * void consume_sclp_int(void)
+ *
+ * eats one sclp interrupt
+ */
+        .globl consume_sclp_int
+consume_sclp_int:
+        /* enable service interrupts in cr0 */
+        stctg 0,0,0(15)
+        oi 6(15), 0x2
+        lctlg 0,0,0(15)
+        /* prepare external call handler */
+        larl %r1, external_new_code
+        stg %r1, 0x1b8
+        larl %r1, external_new_mask
+        mvc 0x1b0(8),0(%r1)
+        /* load enabled wait PSW */
+        larl %r1, enabled_wait_psw
+        lpswe 0(%r1)
+
+external_new_code:
+        /* disable service interrupts in cr0 */
+        stctg 0,0,0(15)
+        ni 6(15), 0xfd
+        lctlg 0,0,0(15)
+        br 14
+
         .align  8
 disabled_wait_psw:
         .quad   0x0002000180000000,0x0000000000000000
+enabled_wait_psw:
+        .quad   0x0302000180000000,0x0000000000000000
+external_new_mask:
+        .quad   0x0000000180000000
-- 
2.11.4.GIT