4 files changed, 46 insertions, 0 deletions
diff --git a/pc-bios/s390-ccw/cio.h b/pc-bios/s390-ccw/cio.h
index cb5815accd..f5b4549ea3 100644
--- a/pc-bios/s390-ccw/cio.h
+++ b/pc-bios/s390-ccw/cio.h
@@ -93,6 +93,26 @@ struct subchannel_id {
         __u32 sch_no : 16;
 } __attribute__ ((packed, aligned(4)));
 
+struct chsc_header {
+    __u16 length;
+    __u16 code;
+} __attribute__((packed));
+
+struct chsc_area_sda {
+    struct chsc_header request;
+    __u8 reserved1:4;
+    __u8 format:4;
+    __u8 reserved2;
+    __u16 operation_code;
+    __u32 reserved3;
+    __u32 reserved4;
+    __u32 operation_data_area[252];
+    struct chsc_header response;
+    __u32 reserved5:4;
+    __u32 format2:4;
+    __u32 reserved6:24;
+} __attribute__((packed));
+
 /*
  * TPI info structure
  */
diff --git a/pc-bios/s390-ccw/main.c b/pc-bios/s390-ccw/main.c
index 1665c57225..c5d533231b 100644
--- a/pc-bios/s390-ccw/main.c
+++ b/pc-bios/s390-ccw/main.c
@@ -35,6 +35,13 @@ static void virtio_setup(uint64_t dev_info)
         check_devno = true;
         dev_no = dev_info & 0xffff;
         debug_print_int("device no. ", dev_no);
+        blk_schid.ssid = (dev_info >> 16) & 0x3;
+        if (blk_schid.ssid != 0) {
+            debug_print_int("ssid ", blk_schid.ssid);
+            if (enable_mss_facility() != 0) {
+                virtio_panic("Failed to enable mss facility\n");
+            }
+        }
     }
 
     for (i = 0; i < 0x10000; i++) {
diff --git a/pc-bios/s390-ccw/s390-ccw.h b/pc-bios/s390-ccw/s390-ccw.h
index 8241b0af05..5e871ac84c 100644
--- a/pc-bios/s390-ccw/s390-ccw.h
+++ b/pc-bios/s390-ccw/s390-ccw.h
@@ -61,6 +61,7 @@ unsigned long virtio_load_direct(ulong rec_list1, ulong rec_list2,
 bool virtio_is_blk(struct subchannel_id schid);
 void virtio_setup_block(struct subchannel_id schid);
 int virtio_read(ulong sector, void *load_addr);
+int enable_mss_facility(void);
 
 /* bootmap.c */
 int zipl_load(void);
diff --git a/pc-bios/s390-ccw/virtio.c b/pc-bios/s390-ccw/virtio.c
index f438af15aa..49f2d291fc 100644
--- a/pc-bios/s390-ccw/virtio.c
+++ b/pc-bios/s390-ccw/virtio.c
@@ -13,6 +13,8 @@
 
 struct vring block;
 
+static char chsc_page[PAGE_SIZE] __attribute__((__aligned__(PAGE_SIZE)));
+
 static long kvm_hypercall(unsigned long nr, unsigned long param1,
                           unsigned long param2)
 {
@@ -301,3 +303,19 @@ bool virtio_is_blk(struct subchannel_id schid)
     return true;
 }
 
+int enable_mss_facility(void)
+{
+    int ret;
+    struct chsc_area_sda *sda_area = (struct chsc_area_sda *) chsc_page;
+
+    memset(sda_area, 0, PAGE_SIZE);
+    sda_area->request.length = 0x0400;
+    sda_area->request.code = 0x0031;
+    sda_area->operation_code = 0x2;
+
+    ret = chsc(sda_area);
+    if ((ret == 0) && (sda_area->response.code == 0x0001)) {
+        return 0;
+    }
+    return -EIO;
+}