[SeaBIOS] [PATCH 1/3] edd: Move EDD get drive parameters (int 1348) logic from disk.c to block.c.

[Kevin O'Connor]

Signed-off-by: Kevin O'Connor 
---
 src/block.c | 179 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 src/block.h |   3 +-
 src/disk.c  | 181 +-----------------------------------------------------------
 3 files changed, 184 insertions(+), 179 deletions(-)

diff --git a/src/block.c b/src/block.c
index 264f376..c8cdf57 100644
--- a/src/block.c
+++ b/src/block.c
@@ -11,6 +11,7 @@
 #include "hw/ata.h" // process_ata_op
 #include "hw/ahci.h" // process_ahci_op
 #include "hw/blockcmd.h" // cdb_*
+#include "hw/pci.h" // pci_bdf_to_bus
 #include "hw/rtc.h" // rtc_read
 #include "hw/virtio-blk.h" // process_virtio_blk_op
 #include "malloc.h" // malloc_low
@@ -310,6 +311,184 @@ __disk_ret_unimplemented(struct bregs *regs, u32 linecode, const char *fname)

/****************************************************************
+ * Extended Disk Drive (EDD) get drive parameters
+ ****************************************************************/
+
+int noinline
+fill_edd(u16 seg, struct int13dpt_s *param_far, struct drive_s *drive_gf)
+{
+    u16 size = GET_FARVAR(seg, param_far->size);
+    u16 t13 = size == 74;
+
+    // Buffer is too small
+    if (size < 26)
+        return DISK_RET_EPARAM;
+
+    // EDD 1.x
+
+    u8  type    = GET_GLOBALFLAT(drive_gf->type);
+    u16 npc     = GET_GLOBALFLAT(drive_gf->pchs.cylinder);
+    u16 nph     = GET_GLOBALFLAT(drive_gf->pchs.head);
+    u16 nps     = GET_GLOBALFLAT(drive_gf->pchs.sector);
+    u64 lba     = GET_GLOBALFLAT(drive_gf->sectors);
+    u16 blksize = GET_GLOBALFLAT(drive_gf->blksize);
+
+    dprintf(DEBUG_HDL_13, "disk_1348 size=%d t=%d chs=%d,%d,%d lba=%d bs=%d\n"
+            , size, type, npc, nph, nps, (u32)lba, blksize);
+
+    SET_FARVAR(seg, param_far->size, 26);
+    if (type == DTYPE_ATA_ATAPI) {
+        // 0x74 = removable, media change, lockable, max values
+        SET_FARVAR(seg, param_far->infos, 0x74);
+        SET_FARVAR(seg, param_far->cylinders, 0xffffffff);
+        SET_FARVAR(seg, param_far->heads, 0xffffffff);
+        SET_FARVAR(seg, param_far->spt, 0xffffffff);
+        SET_FARVAR(seg, param_far->sector_count, (u64)-1);
+    } else {
+        if (lba > (u64)nps*nph*0x3fff) {
+            SET_FARVAR(seg, param_far->infos, 0x00); // geometry is invalid
+            SET_FARVAR(seg, param_far->cylinders, 0x3fff);
+        } else {
+            SET_FARVAR(seg, param_far->infos, 0x02); // geometry is valid
+            SET_FARVAR(seg, param_far->cylinders, (u32)npc);
+        }
+        SET_FARVAR(seg, param_far->heads, (u32)nph);
+        SET_FARVAR(seg, param_far->spt, (u32)nps);
+        SET_FARVAR(seg, param_far->sector_count, lba);
+    }
+    SET_FARVAR(seg, param_far->blksize, blksize);
+
+    if (size < 30 ||
+        (type != DTYPE_ATA && type != DTYPE_ATA_ATAPI &&
+         type != DTYPE_VIRTIO_BLK && type != DTYPE_VIRTIO_SCSI))
+        return DISK_RET_SUCCESS;
+
+    // EDD 2.x
+
+    int bdf;
+    u16 iobase1 = 0;
+    u64 device_path = 0;
+    u8 channel = 0;
+    SET_FARVAR(seg, param_far->size, 30);
+    if (type == DTYPE_ATA || type == DTYPE_ATA_ATAPI) {
+        SET_FARVAR(seg, param_far->dpte, SEGOFF(SEG_LOW, (u32)&DefaultDPTE));
+
+        // Fill in dpte
+        struct atadrive_s *adrive_gf = container_of(
+            drive_gf, struct atadrive_s, drive);
+        struct ata_channel_s *chan_gf = GET_GLOBALFLAT(adrive_gf->chan_gf);
+        u8 slave = GET_GLOBALFLAT(adrive_gf->slave);
+        u16 iobase2 = GET_GLOBALFLAT(chan_gf->iobase2);
+        u8 irq = GET_GLOBALFLAT(chan_gf->irq);
+        iobase1 = GET_GLOBALFLAT(chan_gf->iobase1);
+        bdf = GET_GLOBALFLAT(chan_gf->pci_bdf);
+        device_path = slave;
+        channel = GET_GLOBALFLAT(chan_gf->chanid);
+
+        u16 options = 0;
+        if (type == DTYPE_ATA) {
+            u8 translation = GET_GLOBALFLAT(drive_gf->translation);
+            if (translation != TRANSLATION_NONE) {
+                options |= 1<<3; // CHS translation
+                if (translation == TRANSLATION_LBA)
+                    options |= 1<<9;
+                if (translation == TRANSLATION_RECHS)
+                    options |= 3<<9;
+            }
+        } else {
+            // ATAPI
+            options |= 1<<5; // removable device
+            options |= 1<<6; // atapi device
+        }
+        options |= 1<<4; // lba translation
+        if (CONFIG_ATA_PIO32)
+            options |= 1<<7;
+
+        SET_LOW(DefaultDPTE.iobase1, iobase1);
+        SET_LOW(DefaultDPTE.iobase2, iobase2 + ATA_CB_DC);
+        SET_LOW(DefaultDPTE.prefix, ((slave ? ATA_CB_DH_DEV1 : ATA_CB_DH_DEV0)
+                                  | ATA_CB_DH_LBA));
+        SET_LOW(DefaultDPTE.unused, 0xcb);
+        SET_LOW(DefaultDPTE.irq, irq);
+        SET_LOW(DefaultDPTE.blkcount, 1);
+        SET_LOW(DefaultDPTE.dma, 0);
+        SET_LOW(DefaultDPTE.pio, 0);
+        SET_LOW(DefaultDPTE.options, options);
+        SET_LOW(DefaultDPTE.reserved, 0);
+        SET_LOW(DefaultDPTE.revision, 0x11);
+
+        u8 sum = checksum_far(SEG_LOW, &DefaultDPTE, 15);
+        SET_LOW(DefaultDPTE.checksum, -sum);
+    } else {
+        SET_FARVAR(seg, param_far->dpte.segoff, 0xffffffff);
+        bdf = GET_GLOBALFLAT(drive_gf->cntl_id);
+    }
+
+    if (size < 66)
+        return DISK_RET_SUCCESS;
+
+    // EDD 3.x
+    SET_FARVAR(seg, param_far->key, 0xbedd);
+    SET_FARVAR(seg, param_far->dpi_length, t13 ? 44 : 36);
+    SET_FARVAR(seg, param_far->reserved1, 0);
+    SET_FARVAR(seg, param_far->reserved2, 0);
+
+    if (bdf != -1) {
+        SET_FARVAR(seg, param_far->host_bus[0], 'P');
+        SET_FARVAR(seg, param_far->host_bus[1], 'C');
+        SET_FARVAR(seg, param_far->host_bus[2], 'I');
+        SET_FARVAR(seg, param_far->host_bus[3], ' ');
+
+        u32 path = (pci_bdf_to_bus(bdf) | (pci_bdf_to_dev(bdf) << 8)
+                    | (pci_bdf_to_fn(bdf) << 16));
+        if (t13)
+            path |= channel << 24;
+
+        SET_FARVAR(seg, param_far->iface_path, path);
+    } else {
+        // ISA
+        SET_FARVAR(seg, param_far->host_bus[0], 'I');
+        SET_FARVAR(seg, param_far->host_bus[1], 'S');
+        SET_FARVAR(seg, param_far->host_bus[2], 'A');
+        SET_FARVAR(seg, param_far->host_bus[3], ' ');
+
+        SET_FARVAR(seg, param_far->iface_path, iobase1);
+    }
+
+    if (type != DTYPE_VIRTIO_BLK) {
+        SET_FARVAR(seg, param_far->iface_type[0], 'A');
+        SET_FARVAR(seg, param_far->iface_type[1], 'T');
+        SET_FARVAR(seg, param_far->iface_type[2], 'A');
+        SET_FARVAR(seg, param_far->iface_type[3], ' ');
+    } else {
+        SET_FARVAR(seg, param_far->iface_type[0], 'S');
+        SET_FARVAR(seg, param_far->iface_type[1], 'C');
+        SET_FARVAR(seg, param_far->iface_type[2], 'S');
+        SET_FARVAR(seg, param_far->iface_type[3], 'I');
+    }
+    SET_FARVAR(seg, param_far->iface_type[4], ' ');
+    SET_FARVAR(seg, param_far->iface_type[5], ' ');
+    SET_FARVAR(seg, param_far->iface_type[6], ' ');
+    SET_FARVAR(seg, param_far->iface_type[7], ' ');
+
+    if (t13) {
+        SET_FARVAR(seg, param_far->t13.device_path[0], device_path);
+        SET_FARVAR(seg, param_far->t13.device_path[1], 0);
+
+        SET_FARVAR(seg, param_far->t13.checksum
+                   , -checksum_far(seg, (void*)param_far+30, 43));
+    } else {
+        SET_FARVAR(seg, param_far->phoenix.device_path, device_path);
+
+        SET_FARVAR(seg, param_far->phoenix.checksum
+                   , -checksum_far(seg, (void*)param_far+30, 35));
+    }
+
+    return DISK_RET_SUCCESS;
+}
+
+
+/****************************************************************
  * 16bit calling interface
  ****************************************************************/

diff --git a/src/block.h b/src/block.h
index 5d0afb5..c870e13 100644
--- a/src/block.h
+++ b/src/block.h
@@ -107,7 +107,6 @@ struct drive_s {
  ****************************************************************/

// block.c
-extern struct dpte_s DefaultDPTE;
 extern u8 FloppyCount, CDCount;
 extern u8 *bounce_buf_fl;
 struct drive_s *getDrive(u8 exttype, u8 extdriveoffset);
@@ -119,6 +118,8 @@ struct bregs;
 void __disk_ret(struct bregs *regs, u32 linecode, const char *fname);
 void __disk_ret_unimplemented(struct bregs *regs, u32 linecode
                               , const char *fname);
+struct int13dpt_s;
+int fill_edd(u16 seg, struct int13dpt_s *param_far, struct drive_s *drive_gf);
 int process_op(struct disk_op_s *op);
 int send_disk_op(struct disk_op_s *op);
 int create_bounce_buf(void);
diff --git a/src/disk.c b/src/disk.c
index 4421d9d..7e42103 100644
--- a/src/disk.c
+++ b/src/disk.c
@@ -9,7 +9,6 @@
 #include "bregs.h" // struct bregs
 #include "config.h" // CONFIG_*
 #include "hw/ata.h" // ATA_CB_DC
-#include "hw/pci.h" // pci_bdf_to_bus
 #include "hw/pic.h" // pic_eoi2
 #include "output.h" // debug_enter
 #include "stacks.h" // call16_int
@@ -480,185 +479,11 @@ disk_1347(struct bregs *regs, struct drive_s *drive_gf)
 }

// IBM/MS get drive parameters
-static void noinline
+static void
 disk_1348(struct bregs *regs, struct drive_s *drive_gf)
 {
-    u16 seg = regs->ds;
-    struct int13dpt_s *param_far = (struct int13dpt_s*)(regs->si+0);
-    u16 size = GET_FARVAR(seg, param_far->size);
-    u16 t13 = size == 74;
-
-    // Buffer is too small
-    if (size < 26) {
-        disk_ret(regs, DISK_RET_EPARAM);
-        return;
-    }
-
-    // EDD 1.x
-
-    u8  type    = GET_GLOBALFLAT(drive_gf->type);
-    u16 npc     = GET_GLOBALFLAT(drive_gf->pchs.cylinder);
-    u16 nph     = GET_GLOBALFLAT(drive_gf->pchs.head);
-    u16 nps     = GET_GLOBALFLAT(drive_gf->pchs.sector);
-    u64 lba     = GET_GLOBALFLAT(drive_gf->sectors);
-    u16 blksize = GET_GLOBALFLAT(drive_gf->blksize);
-
-    dprintf(DEBUG_HDL_13, "disk_1348 size=%d t=%d chs=%d,%d,%d lba=%d bs=%d\n"
-            , size, type, npc, nph, nps, (u32)lba, blksize);
-
-    SET_FARVAR(seg, param_far->size, 26);
-    if (type == DTYPE_ATA_ATAPI) {
-        // 0x74 = removable, media change, lockable, max values
-        SET_FARVAR(seg, param_far->infos, 0x74);
-        SET_FARVAR(seg, param_far->cylinders, 0xffffffff);
-        SET_FARVAR(seg, param_far->heads, 0xffffffff);
-        SET_FARVAR(seg, param_far->spt, 0xffffffff);
-        SET_FARVAR(seg, param_far->sector_count, (u64)-1);
-    } else {
-        if (lba > (u64)nps*nph*0x3fff) {
-            SET_FARVAR(seg, param_far->infos, 0x00); // geometry is invalid
-            SET_FARVAR(seg, param_far->cylinders, 0x3fff);
-        } else {
-            SET_FARVAR(seg, param_far->infos, 0x02); // geometry is valid
-            SET_FARVAR(seg, param_far->cylinders, (u32)npc);
-        }
-        SET_FARVAR(seg, param_far->heads, (u32)nph);
-        SET_FARVAR(seg, param_far->spt, (u32)nps);
-        SET_FARVAR(seg, param_far->sector_count, lba);
-    }
-    SET_FARVAR(seg, param_far->blksize, blksize);
-
-    if (size < 30 ||
-        (type != DTYPE_ATA && type != DTYPE_ATA_ATAPI &&
-         type != DTYPE_VIRTIO_BLK && type != DTYPE_VIRTIO_SCSI)) {
-        disk_ret(regs, DISK_RET_SUCCESS);
-        return;
-    }
-
-    // EDD 2.x
-
-    int bdf;
-    u16 iobase1 = 0;
-    u64 device_path = 0;
-    u8 channel = 0;
-    SET_FARVAR(seg, param_far->size, 30);
-    if (type == DTYPE_ATA || type == DTYPE_ATA_ATAPI) {
-        SET_FARVAR(seg, param_far->dpte, SEGOFF(SEG_LOW, (u32)&DefaultDPTE));
-
-        // Fill in dpte
-        struct atadrive_s *adrive_gf = container_of(
-            drive_gf, struct atadrive_s, drive);
-        struct ata_channel_s *chan_gf = GET_GLOBALFLAT(adrive_gf->chan_gf);
-        u8 slave = GET_GLOBALFLAT(adrive_gf->slave);
-        u16 iobase2 = GET_GLOBALFLAT(chan_gf->iobase2);
-        u8 irq = GET_GLOBALFLAT(chan_gf->irq);
-        iobase1 = GET_GLOBALFLAT(chan_gf->iobase1);
-        bdf = GET_GLOBALFLAT(chan_gf->pci_bdf);
-        device_path = slave;
-        channel = GET_GLOBALFLAT(chan_gf->chanid);
-
-        u16 options = 0;
-        if (type == DTYPE_ATA) {
-            u8 translation = GET_GLOBALFLAT(drive_gf->translation);
-            if (translation != TRANSLATION_NONE) {
-                options |= 1<<3; // CHS translation
-                if (translation == TRANSLATION_LBA)
-                    options |= 1<<9;
-                if (translation == TRANSLATION_RECHS)
-                    options |= 3<<9;
-            }
-        } else {
-            // ATAPI
-            options |= 1<<5; // removable device
-            options |= 1<<6; // atapi device
-        }
-        options |= 1<<4; // lba translation
-        if (CONFIG_ATA_PIO32)
-            options |= 1<<7;
-
-        SET_LOW(DefaultDPTE.iobase1, iobase1);
-        SET_LOW(DefaultDPTE.iobase2, iobase2 + ATA_CB_DC);
-        SET_LOW(DefaultDPTE.prefix, ((slave ? ATA_CB_DH_DEV1 : ATA_CB_DH_DEV0)
-                                  | ATA_CB_DH_LBA));
-        SET_LOW(DefaultDPTE.unused, 0xcb);
-        SET_LOW(DefaultDPTE.irq, irq);
-        SET_LOW(DefaultDPTE.blkcount, 1);
-        SET_LOW(DefaultDPTE.dma, 0);
-        SET_LOW(DefaultDPTE.pio, 0);
-        SET_LOW(DefaultDPTE.options, options);
-        SET_LOW(DefaultDPTE.reserved, 0);
-        SET_LOW(DefaultDPTE.revision, 0x11);
-
-        u8 sum = checksum_far(SEG_LOW, &DefaultDPTE, 15);
-        SET_LOW(DefaultDPTE.checksum, -sum);
-    } else {
-        SET_FARVAR(seg, param_far->dpte.segoff, 0xffffffff);
-        bdf = GET_GLOBALFLAT(drive_gf->cntl_id);
-    }
-
-    if (size < 66) {
-        disk_ret(regs, DISK_RET_SUCCESS);
-        return;
-    }
-
-    // EDD 3.x
-    SET_FARVAR(seg, param_far->key, 0xbedd);
-    SET_FARVAR(seg, param_far->dpi_length, t13 ? 44 : 36);
-    SET_FARVAR(seg, param_far->reserved1, 0);
-    SET_FARVAR(seg, param_far->reserved2, 0);
-
-    if (bdf != -1) {
-        SET_FARVAR(seg, param_far->host_bus[0], 'P');
-        SET_FARVAR(seg, param_far->host_bus[1], 'C');
-        SET_FARVAR(seg, param_far->host_bus[2], 'I');
-        SET_FARVAR(seg, param_far->host_bus[3], ' ');
-
-        u32 path = (pci_bdf_to_bus(bdf) | (pci_bdf_to_dev(bdf) << 8)
-                    | (pci_bdf_to_fn(bdf) << 16));
-        if (t13)
-            path |= channel << 24;
-
-        SET_FARVAR(seg, param_far->iface_path, path);
-    } else {
-        // ISA
-        SET_FARVAR(seg, param_far->host_bus[0], 'I');
-        SET_FARVAR(seg, param_far->host_bus[1], 'S');
-        SET_FARVAR(seg, param_far->host_bus[2], 'A');
-        SET_FARVAR(seg, param_far->host_bus[3], ' ');
-
-        SET_FARVAR(seg, param_far->iface_path, iobase1);
-    }
-
-    if (type != DTYPE_VIRTIO_BLK) {
-        SET_FARVAR(seg, param_far->iface_type[0], 'A');
-        SET_FARVAR(seg, param_far->iface_type[1], 'T');
-        SET_FARVAR(seg, param_far->iface_type[2], 'A');
-        SET_FARVAR(seg, param_far->iface_type[3], ' ');
-    } else {
-        SET_FARVAR(seg, param_far->iface_type[0], 'S');
-        SET_FARVAR(seg, param_far->iface_type[1], 'C');
-        SET_FARVAR(seg, param_far->iface_type[2], 'S');
-        SET_FARVAR(seg, param_far->iface_type[3], 'I');
-    }
-    SET_FARVAR(seg, param_far->iface_type[4], ' ');
-    SET_FARVAR(seg, param_far->iface_type[5], ' ');
-    SET_FARVAR(seg, param_far->iface_type[6], ' ');
-    SET_FARVAR(seg, param_far->iface_type[7], ' ');
-
-    if (t13) {
-        SET_FARVAR(seg, param_far->t13.device_path[0], device_path);
-        SET_FARVAR(seg, param_far->t13.device_path[1], 0);
-
-        SET_FARVAR(seg, param_far->t13.checksum
-                   , -checksum_far(seg, (void*)param_far+30, 43));
-    } else {
-        SET_FARVAR(seg, param_far->phoenix.device_path, device_path);
-
-        SET_FARVAR(seg, param_far->phoenix.checksum
-                   , -checksum_far(seg, (void*)param_far+30, 35));
-    }
-
-    disk_ret(regs, DISK_RET_SUCCESS);
+    int ret = fill_edd(regs->ds, (void*)(regs->si+0), drive_gf);
+    disk_ret(regs, ret);
 }

// IBM/MS extended media change