3 files changed, 75 insertions, 9 deletions

diff --git a/pc-bios/s390-ccw.img b/pc-bios/s390-ccw.img
index f6223e77c2..1c7f7640fc 100644
--- a/pc-bios/s390-ccw.img
+++ b/pc-bios/s390-ccw.img
diff --git a/pc-bios/s390-ccw/bootmap.c b/pc-bios/s390-ccw/bootmap.c
index 53a460df84..5ee3fcb38f 100644
--- a/pc-bios/s390-ccw/bootmap.c
+++ b/pc-bios/s390-ccw/bootmap.c
@@ -58,6 +58,52 @@ struct mbr {
 /* Scratch space */
 static uint8_t sec[SECTOR_SIZE] __attribute__((__aligned__(SECTOR_SIZE)));
 
+typedef struct ResetInfo {
+    uint32_t ipl_mask;
+    uint32_t ipl_addr;
+    uint32_t ipl_continue;
+} ResetInfo;
+
+ResetInfo save;
+
+static void jump_to_IPL_2(void)
+{
+    ResetInfo *current = 0;
+
+    void (*ipl)(void) = (void *) (uint64_t) current->ipl_continue;
+    debug_print_addr("set IPL addr to", ipl);
+
+    /* Ensure the guest output starts fresh */
+    sclp_print("\n");
+
+    *current = save;
+    ipl(); /* should not return */
+}
+
+static void jump_to_IPL_code(uint64_t address)
+{
+    /*
+     * The IPL PSW is at address 0. We also must not overwrite the
+     * content of non-BIOS memory after we loaded the guest, so we
+     * save the original content and restore it in jump_to_IPL_2.
+     */
+    ResetInfo *current = 0;
+
+    save = *current;
+    current->ipl_addr = (uint32_t) (uint64_t) &jump_to_IPL_2;
+    current->ipl_continue = address & 0x7fffffff;
+
+    /*
+     * HACK ALERT.
+     * We use the load normal reset to keep r15 unchanged. jump_to_IPL_2
+     * can then use r15 as its stack pointer.
+     */
+    asm volatile("lghi 1,1\n\t"
+                 "diag 1,1,0x308\n\t"
+                 : : : "1", "memory");
+    virtio_panic("\n! IPL returns !\n");
+}
+
 /* Check for ZIPL magic. Returns 0 if not matched. */
 static int zipl_magic(uint8_t *ptr)
 {
@@ -72,10 +118,26 @@ static int zipl_magic(uint8_t *ptr)
     return 1;
 }
 
+#define FREE_SPACE_FILLER '\xAA'
+
+static inline bool unused_space(const void *p, unsigned int size)
+{
+    int i;
+    const unsigned char *m = p;
+
+    for (i = 0; i < size; i++) {
+        if (m[i] != FREE_SPACE_FILLER) {
+            return false;
+        }
+    }
+    return true;
+}
+
 static int zipl_load_segment(struct component_entry *entry)
 {
     const int max_entries = (SECTOR_SIZE / sizeof(struct scsi_blockptr));
     struct scsi_blockptr *bprs = (void*)sec;
+    const int bprs_size = sizeof(sec);
     uint64_t blockno;
     long address;
     int i;
@@ -87,6 +149,7 @@ static int zipl_load_segment(struct component_entry *entry)
     debug_print_int("addr", address);
 
     do {
+        memset(bprs, FREE_SPACE_FILLER, bprs_size);
         if (virtio_read(blockno, (uint8_t *)bprs)) {
             debug_print_int("failed reading bprs at", blockno);
             goto fail;
@@ -104,6 +167,16 @@ static int zipl_load_segment(struct component_entry *entry)
             if (i == (max_entries - 1))
                 break;
 
+            if (bprs[i].blockct == 0 && unused_space(&bprs[i + 1],
+                sizeof(struct scsi_blockptr))) {
+                /* This is a "continue" pointer.
+                 * This ptr is the last one in the current script section.
+                 * I.e. the next ptr must point to the unused memory area.
+                 * The blockno is not zero, so the upper loop must continue
+                 * reading next section of BPRS.
+                 */
+                break;
+            }
             address = virtio_load_direct(cur_desc[0], cur_desc[1], 0,
                                          (void*)address);
             if (address == -1)
@@ -123,7 +196,6 @@ static int zipl_run(struct scsi_blockptr *pte)
 {
     struct component_header *header;
     struct component_entry *entry;
-    void (*ipl)(void);
     uint8_t tmp_sec[SECTOR_SIZE];
 
     virtio_read(pte->blockno, tmp_sec);
@@ -157,14 +229,8 @@ static int zipl_run(struct scsi_blockptr *pte)
         goto fail;
     }
 
-    /* Ensure the guest output starts fresh */
-    sclp_print("\n");
-
-    /* And run the OS! */
-    ipl = (void*)(entry->load_address & 0x7fffffff);
-    debug_print_addr("set IPL addr to", ipl);
     /* should not return */
-    ipl();
+    jump_to_IPL_code(entry->load_address);
 
     return 0;
 
diff --git a/pc-bios/s390-ccw/virtio.c b/pc-bios/s390-ccw/virtio.c
index a46914dd68..bbb3c4f36d 100644
--- a/pc-bios/s390-ccw/virtio.c
+++ b/pc-bios/s390-ccw/virtio.c
@@ -240,7 +240,7 @@ unsigned long virtio_load_direct(ulong rec_list1, ulong rec_list2,
 {
     u8 status;
     int sec = rec_list1;
-    int sec_num = (((rec_list2 >> 32)+ 1) & 0xffff);
+    int sec_num = ((rec_list2 >> 32) & 0xffff) + 1;
     int sec_len = rec_list2 >> 48;
     ulong addr = (ulong)load_addr;