grml...
[seabios.git] / src / floppy.c
index 6aa83e6c09e98f7d65969cb902b3dde59d75e404..383744a22d9d8fa5debc025053d4d2af2463de5c 100644 (file)
@@ -9,30 +9,38 @@
 #include "disk.h" // DISK_RET_SUCCESS
 #include "config.h" // CONFIG_FLOPPY
 #include "biosvar.h" // SET_BDA
-#include "util.h" // irq_disable
+#include "util.h" // wait_irq
 #include "cmos.h" // inb_cmos
 #include "pic.h" // eoi_pic1
 #include "bregs.h" // struct bregs
+#include "boot.h" // boot_add_floppy
+#include "pci.h" // pci_to_bdf
+#include "pci_ids.h" // PCI_CLASS_BRIDGE_ISA
 
-#define BX_FLOPPY_ON_CNT 37   /* 2 seconds */
+#define FLOPPY_SIZE_CODE 0x02 // 512 byte sectors
+#define FLOPPY_DATALEN 0xff   // Not used - because size code is 0x02
+#define FLOPPY_MOTOR_TICKS 37 // ~2 seconds
+#define FLOPPY_FILLBYTE 0xf6
+#define FLOPPY_GAPLEN 0x1B
+#define FLOPPY_FORMAT_GAPLEN 0x6c
 
 // New diskette parameter table adding 3 parameters from IBM
 // Since no provisions are made for multiple drive types, most
 // values in this table are ignored.  I set parameters for 1.44M
 // floppy here
-struct floppy_ext_dbt_s diskette_param_table2 VAR16_32 = {
+struct floppy_ext_dbt_s diskette_param_table2 VAR16VISIBLE = {
     .dbt = {
-        .specify1       = 0xAF,
-        .specify2       = 0x02, // head load time 0000001, DMA used
-        .shutoff_ticks  = 0x25,
-        .bps_code       = 0x02,
+        .specify1       = 0xAF, // step rate 12ms, head unload 240ms
+        .specify2       = 0x02, // head load time 4ms, DMA used
+        .shutoff_ticks  = FLOPPY_MOTOR_TICKS, // ~2 seconds
+        .bps_code       = FLOPPY_SIZE_CODE,
         .sectors        = 18,
-        .interblock_len = 0x1B,
-        .data_len       = 0xFF,
-        .gap_len        = 0x6C,
-        .fill_byte      = 0xF6,
-        .settle_time    = 0x0F,
-        .startup_time   = 0x08,
+        .interblock_len = FLOPPY_GAPLEN,
+        .data_len       = FLOPPY_DATALEN,
+        .gap_len        = FLOPPY_FORMAT_GAPLEN,
+        .fill_byte      = FLOPPY_FILLBYTE,
+        .settle_time    = 0x0F, // 15ms
+        .startup_time   = 0x08, // 1 second
     },
     .max_track      = 79,   // maximum track
     .data_rate      = 0,    // data transfer rate
@@ -44,27 +52,25 @@ struct floppy_ext_dbt_s diskette_param_table2 VAR16_32 = {
 // floppy here
 struct floppy_dbt_s diskette_param_table VAR16FIXED(0xefc7) = {
     .specify1       = 0xAF,
-    .specify2       = 0x02, // head load time 0000001, DMA used
-    .shutoff_ticks  = 0x25,
-    .bps_code       = 0x02,
+    .specify2       = 0x02,
+    .shutoff_ticks  = FLOPPY_MOTOR_TICKS,
+    .bps_code       = FLOPPY_SIZE_CODE,
     .sectors        = 18,
-    .interblock_len = 0x1B,
-    .data_len       = 0xFF,
-    .gap_len        = 0x6C,
-    .fill_byte      = 0xF6,
+    .interblock_len = FLOPPY_GAPLEN,
+    .data_len       = FLOPPY_DATALEN,
+    .gap_len        = FLOPPY_FORMAT_GAPLEN,
+    .fill_byte      = FLOPPY_FILLBYTE,
     .settle_time    = 0x0F,
     .startup_time   = 0x08,
 };
 
-u8 FloppyTypes[2] VAR16_32;
-
 struct floppyinfo_s {
     struct chs_s chs;
     u8 config_data;
     u8 media_state;
 };
 
-struct floppyinfo_s FloppyInfo[] VAR16_32 = {
+struct floppyinfo_s FloppyInfo[] VAR16VISIBLE = {
     // Unknown
     { {0, 0, 0}, 0x00, 0x00},
     // 1 - 360KB, 5.25" - 2 heads, 40 tracks, 9 sectors
@@ -85,35 +91,49 @@ struct floppyinfo_s FloppyInfo[] VAR16_32 = {
     { {2, 40, 8}, 0x00, 0x27},
 };
 
-static void
-addFloppy(int floppyid, int ftype)
+struct drive_s *
+init_floppy(int floppyid, int ftype)
 {
     if (ftype <= 0 || ftype >= ARRAY_SIZE(FloppyInfo)) {
         dprintf(1, "Bad floppy type %d\n", ftype);
-        return;
+        return NULL;
     }
 
-    int driveid = Drives.drivecount;
-    if (driveid >= ARRAY_SIZE(Drives.drives))
-        return;
-    Drives.drivecount++;
-    memset(&Drives.drives[driveid], 0, sizeof(Drives.drives[0]));
-    Drives.drives[driveid].cntl_id = floppyid;
-    FloppyTypes[floppyid] = ftype;
-
-    memcpy(&Drives.drives[driveid].lchs, &FloppyInfo[ftype].chs
+    struct drive_s *drive_g = malloc_fseg(sizeof(*drive_g));
+    if (!drive_g) {
+        warn_noalloc();
+        return NULL;
+    }
+    memset(drive_g, 0, sizeof(*drive_g));
+    drive_g->cntl_id = floppyid;
+    drive_g->type = DTYPE_FLOPPY;
+    drive_g->blksize = DISK_SECTOR_SIZE;
+    drive_g->floppy_type = ftype;
+    drive_g->sectors = (u64)-1;
+
+    memcpy(&drive_g->lchs, &FloppyInfo[ftype].chs
            , sizeof(FloppyInfo[ftype].chs));
+    return drive_g;
+}
 
-    map_floppy_drive(driveid);
+static void
+addFloppy(int floppyid, int ftype)
+{
+    struct drive_s *drive_g = init_floppy(floppyid, ftype);
+    if (!drive_g)
+        return;
+    char *desc = znprintf(MAXDESCSIZE, "Floppy [drive %c]", 'A' + floppyid);
+    struct pci_device *pci = pci_find_class(PCI_CLASS_BRIDGE_ISA); /* isa-to-pci bridge */
+    int prio = bootprio_find_fdc_device(pci, PORT_FD_BASE, floppyid);
+    boot_add_floppy(drive_g, desc, prio);
 }
 
 void
-floppy_setup()
+floppy_setup(void)
 {
     if (! CONFIG_FLOPPY)
         return;
     dprintf(3, "init floppy drives\n");
-    FloppyTypes[0] = FloppyTypes[1] = 0;
 
     if (CONFIG_COREBOOT) {
         // XXX - disable floppies on coreboot for now.
@@ -127,21 +147,20 @@ floppy_setup()
 
     outb(0x02, PORT_DMA1_MASK_REG);
 
-    enable_hwirq(6, entry_0e);
+    enable_hwirq(6, FUNC16(entry_0e));
 }
 
-#define floppy_ret(regs, code)                                  \
-    __floppy_ret((regs), (code) | (__LINE__ << 8), __func__)
-
-void
-__floppy_ret(struct bregs *regs, u32 linecode, const char *fname)
+// Find a floppy type that matches a given image size.
+int
+find_floppy_type(u32 size)
 {
-    u8 code = linecode;
-    SET_BDA(floppy_last_status, code);
-    if (code)
-        __set_code_fail(regs, linecode, fname);
-    else
-        set_code_success(regs);
+    int i;
+    for (i=1; i<ARRAY_SIZE(FloppyInfo); i++) {
+        struct chs_s *c = &FloppyInfo[i].chs;
+        if (c->cylinders * c->heads * c->spt * DISK_SECTOR_SIZE == size)
+            return i;
+    }
+    return -1;
 }
 
 
@@ -150,7 +169,7 @@ __floppy_ret(struct bregs *regs, u32 linecode, const char *fname)
  ****************************************************************/
 
 static void
-floppy_reset_controller()
+floppy_reset_controller(void)
 {
     // Reset controller
     u8 val8 = inb(PORT_FD_DOR);
@@ -163,21 +182,20 @@ floppy_reset_controller()
 }
 
 static int
-wait_floppy_irq()
+wait_floppy_irq(void)
 {
-    irq_enable();
+    ASSERT16();
     u8 v;
     for (;;) {
-        if (!GET_BDA(floppy_motor_counter)) {
-            irq_disable();
+        if (!GET_BDA(floppy_motor_counter))
             return -1;
-        }
         v = GET_BDA(floppy_recalibration_status);
         if (v & FRS_TIMEOUT)
             break;
-        cpu_relax();
+        // Could use wait_irq() here, but that causes issues on
+        // bochs, so use yield() instead.
+        yield();
     }
-    irq_disable();
 
     v &= ~FRS_TIMEOUT;
     SET_BDA(floppy_recalibration_status, v);
@@ -199,7 +217,7 @@ floppy_prepare_controller(u8 floppyid)
     outb(dor, PORT_FD_DOR);
 
     // reset the disk motor timeout value of INT 08
-    SET_BDA(floppy_motor_counter, BX_FLOPPY_ON_CNT);
+    SET_BDA(floppy_motor_counter, FLOPPY_MOTOR_TICKS);
 
     // wait for drive readiness
     while ((inb(PORT_FD_STATUS) & 0xc0) != 0x80)
@@ -229,17 +247,16 @@ floppy_pio(u8 *cmd, u8 cmdlen)
 }
 
 static int
-floppy_cmd(struct bregs *regs, u16 count, u8 *cmd, u8 cmdlen)
+floppy_cmd(struct disk_op_s *op, u16 count, u8 *cmd, u8 cmdlen)
 {
     // es:bx = pointer to where to place information from diskette
-    u32 addr = (u32)MAKE_FLATPTR(regs->es, regs->bx);
+    u32 addr = (u32)op->buf_fl;
 
     // check for 64K boundary overrun
-    u32 last_addr = addr + count;
-    if ((addr >> 16) != (last_addr >> 16)) {
-        floppy_ret(regs, DISK_RET_EBOUNDARY);
-        return -1;
-    }
+    u16 end = count - 1;
+    u32 last_addr = addr + end;
+    if ((addr >> 16) != (last_addr >> 16))
+        return DISK_RET_EBOUNDARY;
 
     u8 mode_register = 0x4a; // single mode, increment, autoinit disable,
     if (cmd[0] == 0xe6)
@@ -252,8 +269,8 @@ floppy_cmd(struct bregs *regs, u16 count, u8 *cmd, u8 cmdlen)
     outb(addr, PORT_DMA_ADDR_2);
     outb(addr>>8, PORT_DMA_ADDR_2);
     outb(0x00, PORT_DMA1_CLEAR_FF_REG); // clear flip-flop
-    outb(count, PORT_DMA_CNT_2);
-    outb(count>>8, PORT_DMA_CNT_2);
+    outb(end, PORT_DMA_CNT_2);
+    outb(end>>8, PORT_DMA_CNT_2);
 
     // port 0b: DMA-1 Mode Register
     // transfer type=write, channel 2
@@ -265,16 +282,12 @@ floppy_cmd(struct bregs *regs, u16 count, u8 *cmd, u8 cmdlen)
     outb(0x02, PORT_DMA1_MASK_REG); // unmask channel 2
 
     int ret = floppy_pio(cmd, cmdlen);
-    if (ret) {
-        floppy_ret(regs, DISK_RET_ETIMEOUT);
-        return -1;
-    }
+    if (ret)
+        return DISK_RET_ETIMEOUT;
 
     // check port 3f4 for accessibility to status bytes
-    if ((inb(PORT_FD_STATUS) & 0xc0) != 0xc0) {
-        floppy_ret(regs, DISK_RET_ECONTROLLER);
-        return -1;
-    }
+    if ((inb(PORT_FD_STATUS) & 0xc0) != 0xc0)
+        return DISK_RET_ECONTROLLER;
 
     // read 7 return status bytes from controller
     u8 i;
@@ -284,7 +297,7 @@ floppy_cmd(struct bregs *regs, u16 count, u8 *cmd, u8 cmdlen)
         SET_BDA(floppy_return_status[i], v);
     }
 
-    return 0;
+    return DISK_RET_SUCCESS;
 }
 
 
@@ -312,7 +325,7 @@ floppy_drive_recal(u8 floppyid)
 }
 
 static int
-floppy_media_sense(u8 floppyid)
+floppy_media_sense(struct drive_s *drive_g)
 {
     // for now cheat and get drive type from CMOS,
     // assume media is same as drive type
@@ -345,294 +358,220 @@ floppy_media_sense(u8 floppyid)
     //    110 reserved
     //    111 all other formats/drives
 
-    u8 ftype = GET_GLOBAL(FloppyTypes[floppyid]);
+    u8 ftype = GET_GLOBAL(drive_g->floppy_type);
     SET_BDA(floppy_last_data_rate, GET_GLOBAL(FloppyInfo[ftype].config_data));
+    u8 floppyid = GET_GLOBAL(drive_g->cntl_id);
     SET_BDA(floppy_media_state[floppyid]
             , GET_GLOBAL(FloppyInfo[ftype].media_state));
-    return 0;
+    return DISK_RET_SUCCESS;
 }
 
 static int
-check_recal_drive(struct bregs *regs, u8 floppyid)
+check_recal_drive(struct drive_s *drive_g)
 {
+    u8 floppyid = GET_GLOBAL(drive_g->cntl_id);
     if ((GET_BDA(floppy_recalibration_status) & (1<<floppyid))
         && (GET_BDA(floppy_media_state[floppyid]) & FMS_MEDIA_DRIVE_ESTABLISHED))
         // Media is known.
-        return 0;
+        return DISK_RET_SUCCESS;
 
     // Recalibrate drive.
     floppy_drive_recal(floppyid);
 
     // Sense media.
-    int ret = floppy_media_sense(floppyid);
-    if (ret) {
-        floppy_ret(regs, DISK_RET_EMEDIA);
-        return -1;
-    }
-    return 0;
+    return floppy_media_sense(drive_g);
 }
 
 
 /****************************************************************
- * Floppy int13 handlers
+ * Floppy handlers
  ****************************************************************/
 
-// diskette controller reset
 static void
-floppy_1300(struct bregs *regs, u8 driveid)
+lba2chs(struct disk_op_s *op, u8 *track, u8 *sector, u8 *head)
 {
-    u8 floppyid = GET_GLOBAL(Drives.drives[driveid].cntl_id);
-    set_diskette_current_cyl(floppyid, 0); // current cylinder
-    floppy_ret(regs, DISK_RET_SUCCESS);
+    u32 lba = op->lba;
+
+    u32 tmp = lba + 1;
+    u16 nlspt = GET_GLOBAL(op->drive_g->lchs.spt);
+    *sector = tmp % nlspt;
+
+    tmp /= nlspt;
+    u16 nlh = GET_GLOBAL(op->drive_g->lchs.heads);
+    *head = tmp % nlh;
+
+    tmp /= nlh;
+    *track = tmp;
 }
 
-// Read Diskette Status
-static void
-floppy_1301(struct bregs *regs, u8 driveid)
+// diskette controller reset
+static int
+floppy_reset(struct disk_op_s *op)
 {
-    u8 v = GET_BDA(floppy_last_status);
-    regs->ah = v;
-    set_cf(regs, v);
+    u8 floppyid = GET_GLOBAL(op->drive_g->cntl_id);
+    set_diskette_current_cyl(floppyid, 0); // current cylinder
+    return DISK_RET_SUCCESS;
 }
 
 // Read Diskette Sectors
-static void
-floppy_1302(struct bregs *regs, u8 driveid)
+static int
+floppy_read(struct disk_op_s *op)
 {
-    u8 floppyid = GET_GLOBAL(Drives.drives[driveid].cntl_id);
-    if (check_recal_drive(regs, floppyid))
+    int res = check_recal_drive(op->drive_g);
+    if (res)
         goto fail;
 
-    u8 num_sectors = regs->al;
-    u8 track       = regs->ch;
-    u8 sector      = regs->cl;
-    u8 head        = regs->dh;
-
-    if (head > 1 || sector == 0 || num_sectors == 0
-        || track > 79 || num_sectors > 72) {
-        floppy_ret(regs, DISK_RET_EPARAM);
-        goto fail;
-    }
+    u8 track, sector, head;
+    lba2chs(op, &track, &sector, &head);
 
     // send read-normal-data command (9 bytes) to controller
+    u8 floppyid = GET_GLOBAL(op->drive_g->cntl_id);
     u8 data[12];
     data[0] = 0xe6; // e6: read normal data
     data[1] = (head << 2) | floppyid; // HD DR1 DR2
     data[2] = track;
     data[3] = head;
     data[4] = sector;
-    data[5] = 2; // 512 byte sector size
-    data[6] = sector + num_sectors - 1; // last sector to read on track
-    data[7] = 0; // Gap length
-    data[8] = 0xff; // Gap length
+    data[5] = FLOPPY_SIZE_CODE;
+    data[6] = sector + op->count - 1; // last sector to read on track
+    data[7] = FLOPPY_GAPLEN;
+    data[8] = FLOPPY_DATALEN;
 
-    int ret = floppy_cmd(regs, (num_sectors * 512) - 1, data, 9);
-    if (ret)
+    res = floppy_cmd(op, op->count * DISK_SECTOR_SIZE, data, 9);
+    if (res)
         goto fail;
 
     if (data[0] & 0xc0) {
-        floppy_ret(regs, DISK_RET_ECONTROLLER);
+        res = DISK_RET_ECONTROLLER;
         goto fail;
     }
 
     // ??? should track be new val from return_status[3] ?
     set_diskette_current_cyl(floppyid, track);
-    // AL = number of sectors read (same value as passed)
-    floppy_ret(regs, DISK_RET_SUCCESS);
-    return;
+    return DISK_RET_SUCCESS;
 fail:
-    regs->al = 0; // no sectors read
+    op->count = 0; // no sectors read
+    return res;
 }
 
 // Write Diskette Sectors
-static void
-floppy_1303(struct bregs *regs, u8 driveid)
+static int
+floppy_write(struct disk_op_s *op)
 {
-    u8 floppyid = GET_GLOBAL(Drives.drives[driveid].cntl_id);
-    if (check_recal_drive(regs, floppyid))
+    int res = check_recal_drive(op->drive_g);
+    if (res)
         goto fail;
 
-    u8 num_sectors = regs->al;
-    u8 track       = regs->ch;
-    u8 sector      = regs->cl;
-    u8 head        = regs->dh;
-
-    if (head > 1 || sector == 0 || num_sectors == 0
-        || track > 79 || num_sectors > 72) {
-        floppy_ret(regs, DISK_RET_EPARAM);
-        goto fail;
-    }
+    u8 track, sector, head;
+    lba2chs(op, &track, &sector, &head);
 
     // send write-normal-data command (9 bytes) to controller
+    u8 floppyid = GET_GLOBAL(op->drive_g->cntl_id);
     u8 data[12];
     data[0] = 0xc5; // c5: write normal data
     data[1] = (head << 2) | floppyid; // HD DR1 DR2
     data[2] = track;
     data[3] = head;
     data[4] = sector;
-    data[5] = 2; // 512 byte sector size
-    data[6] = sector + num_sectors - 1; // last sector to write on track
-    data[7] = 0; // Gap length
-    data[8] = 0xff; // Gap length
+    data[5] = FLOPPY_SIZE_CODE;
+    data[6] = sector + op->count - 1; // last sector to write on track
+    data[7] = FLOPPY_GAPLEN;
+    data[8] = FLOPPY_DATALEN;
 
-    int ret = floppy_cmd(regs, (num_sectors * 512) - 1, data, 9);
-    if (ret)
+    res = floppy_cmd(op, op->count * DISK_SECTOR_SIZE, data, 9);
+    if (res)
         goto fail;
 
     if (data[0] & 0xc0) {
         if (data[1] & 0x02)
-            floppy_ret(regs, DISK_RET_EWRITEPROTECT);
+            res = DISK_RET_EWRITEPROTECT;
         else
-            floppy_ret(regs, DISK_RET_ECONTROLLER);
+            res = DISK_RET_ECONTROLLER;
         goto fail;
     }
 
     // ??? should track be new val from return_status[3] ?
     set_diskette_current_cyl(floppyid, track);
-    // AL = number of sectors read (same value as passed)
-    floppy_ret(regs, DISK_RET_SUCCESS);
-    return;
+    return DISK_RET_SUCCESS;
 fail:
-    regs->al = 0; // no sectors read
+    op->count = 0; // no sectors read
+    return res;
 }
 
 // Verify Diskette Sectors
-static void
-floppy_1304(struct bregs *regs, u8 driveid)
+static int
+floppy_verify(struct disk_op_s *op)
 {
-    u8 floppyid = GET_GLOBAL(Drives.drives[driveid].cntl_id);
-    if (check_recal_drive(regs, floppyid))
+    int res = check_recal_drive(op->drive_g);
+    if (res)
         goto fail;
 
-    u8 num_sectors = regs->al;
-    u8 track       = regs->ch;
-    u8 sector      = regs->cl;
-    u8 head        = regs->dh;
-
-    if (head > 1 || sector == 0 || num_sectors == 0
-        || track > 79 || num_sectors > 72) {
-        floppy_ret(regs, DISK_RET_EPARAM);
-        goto fail;
-    }
+    u8 track, sector, head;
+    lba2chs(op, &track, &sector, &head);
 
     // ??? should track be new val from return_status[3] ?
+    u8 floppyid = GET_GLOBAL(op->drive_g->cntl_id);
     set_diskette_current_cyl(floppyid, track);
-    // AL = number of sectors verified (same value as passed)
-    floppy_ret(regs, DISK_RET_SUCCESS);
-    return;
+    return DISK_RET_SUCCESS;
 fail:
-    regs->al = 0; // no sectors read
+    op->count = 0; // no sectors read
+    return res;
 }
 
 // format diskette track
-static void
-floppy_1305(struct bregs *regs, u8 driveid)
+static int
+floppy_format(struct disk_op_s *op)
 {
-    u8 floppyid = GET_GLOBAL(Drives.drives[driveid].cntl_id);
-    dprintf(3, "floppy f05\n");
-
-    if (check_recal_drive(regs, floppyid))
-        return;
-
-    u8 num_sectors = regs->al;
-    u8 head        = regs->dh;
+    int ret = check_recal_drive(op->drive_g);
+    if (ret)
+        return ret;
 
-    if (head > 1 || num_sectors == 0 || num_sectors > 18) {
-        floppy_ret(regs, DISK_RET_EPARAM);
-        return;
-    }
+    u8 head = op->lba;
 
     // send format-track command (6 bytes) to controller
+    u8 floppyid = GET_GLOBAL(op->drive_g->cntl_id);
     u8 data[12];
     data[0] = 0x4d; // 4d: format track
     data[1] = (head << 2) | floppyid; // HD DR1 DR2
-    data[2] = 2; // 512 byte sector size
-    data[3] = num_sectors; // number of sectors per track
-    data[4] = 0; // Gap length
-    data[5] = 0xf6; // Fill byte
+    data[2] = FLOPPY_SIZE_CODE;
+    data[3] = op->count; // number of sectors per track
+    data[4] = FLOPPY_FORMAT_GAPLEN;
+    data[5] = FLOPPY_FILLBYTE;
 
-    int ret = floppy_cmd(regs, (num_sectors * 4) - 1, data, 6);
+    ret = floppy_cmd(op, op->count * 4, data, 6);
     if (ret)
-        return;
+        return ret;
 
     if (data[0] & 0xc0) {
         if (data[1] & 0x02)
-            floppy_ret(regs, DISK_RET_EWRITEPROTECT);
-        else
-            floppy_ret(regs, DISK_RET_ECONTROLLER);
-        return;
+            return DISK_RET_EWRITEPROTECT;
+        return DISK_RET_ECONTROLLER;
     }
 
     set_diskette_current_cyl(floppyid, 0);
-    floppy_ret(regs, 0);
+    return DISK_RET_SUCCESS;
 }
 
-// read diskette drive parameters
-static void
-floppy_1308(struct bregs *regs, u8 driveid)
+int
+process_floppy_op(struct disk_op_s *op)
 {
-    dprintf(3, "floppy f08\n");
-
-    regs->ax = 0;
-    regs->dx = GET_GLOBAL(Drives.floppycount);
-
-    u8 floppyid = GET_GLOBAL(Drives.drives[driveid].cntl_id);
-    u8 ftype = GET_GLOBAL(FloppyTypes[floppyid]);
-    regs->bx = ftype;
-
-    u16 nlc = GET_GLOBAL(Drives.drives[driveid].lchs.cylinders);
-    u16 nlh = GET_GLOBAL(Drives.drives[driveid].lchs.heads);
-    u16 nlspt = GET_GLOBAL(Drives.drives[driveid].lchs.spt);
-    nlc -= 1; // 0 based
-    nlh -= 1;
-
-    regs->ch = nlc & 0xff;
-    regs->cl = ((nlc >> 2) & 0xc0) | (nlspt & 0x3f);
-    regs->dh = nlh;
-
-    /* set es & di to point to 11 byte diskette param table in ROM */
-    regs->es = SEG_BIOS;
-    regs->di = (u32)&diskette_param_table2;
-    /* disk status not changed upon success */
-    set_success(regs);
-}
-
-// read diskette drive type
-static void
-floppy_1315(struct bregs *regs, u8 driveid)
-{
-    dprintf(6, "floppy f15\n");
-    regs->ah = 1;
-    set_success(regs);
-}
-
-// get diskette change line status
-static void
-floppy_1316(struct bregs *regs, u8 driveid)
-{
-    floppy_ret(regs, DISK_RET_ECHANGED);
-}
-
-static void
-floppy_13XX(struct bregs *regs, u8 driveid)
-{
-    floppy_ret(regs, DISK_RET_EPARAM);
-}
+    if (!CONFIG_FLOPPY)
+        return 0;
 
-void
-floppy_13(struct bregs *regs, u8 driveid)
-{
-    switch (regs->ah) {
-    case 0x00: floppy_1300(regs, driveid); break;
-    case 0x01: floppy_1301(regs, driveid); break;
-    case 0x02: floppy_1302(regs, driveid); break;
-    case 0x03: floppy_1303(regs, driveid); break;
-    case 0x04: floppy_1304(regs, driveid); break;
-    case 0x05: floppy_1305(regs, driveid); break;
-    case 0x08: floppy_1308(regs, driveid); break;
-    case 0x15: floppy_1315(regs, driveid); break;
-    case 0x16: floppy_1316(regs, driveid); break;
-    default:   floppy_13XX(regs, driveid); break;
+    switch (op->command) {
+    case CMD_RESET:
+        return floppy_reset(op);
+    case CMD_READ:
+        return floppy_read(op);
+    case CMD_WRITE:
+        return floppy_write(op);
+    case CMD_VERIFY:
+        return floppy_verify(op);
+    case CMD_FORMAT:
+        return floppy_format(op);
+    default:
+        op->count = 0;
+        return DISK_RET_EPARAM;
     }
 }
 
@@ -643,7 +582,7 @@ floppy_13(struct bregs *regs, u8 driveid)
 
 // INT 0Eh Diskette Hardware ISR Entry Point
 void VISIBLE16
-handle_0e()
+handle_0e(void)
 {
     debug_isr(DEBUG_ISR_0e);
     if (! CONFIG_FLOPPY)
@@ -666,7 +605,7 @@ done:
 
 // Called from int08 handler.
 void
-floppy_tick()
+floppy_tick(void)
 {
     if (! CONFIG_FLOPPY)
         return;