v86 0.4.0 → 0.5.11

package/bios/seabios/src/hw/floppy.c

@@ -1,741 +0,0 @@
-// 16bit code to access floppy drives.
-//
-// Copyright (C) 2008,2009  Kevin O'Connor <kevin@koconnor.net>
-// Copyright (C) 2002  MandrakeSoft S.A.
-//
-// This file may be distributed under the terms of the GNU LGPLv3 license.
-
-#include "biosvar.h" // SET_BDA
-#include "block.h" // struct drive_s
-#include "bregs.h" // struct bregs
-#include "config.h" // CONFIG_FLOPPY
-#include "malloc.h" // malloc_fseg
-#include "output.h" // dprintf
-#include "pcidevice.h" // pci_find_class
-#include "pci_ids.h" // PCI_CLASS_BRIDGE_ISA
-#include "pic.h" // pic_eoi1
-#include "romfile.h" // romfile_loadint
-#include "rtc.h" // rtc_read
-#include "stacks.h" // yield
-#include "std/disk.h" // DISK_RET_SUCCESS
-#include "string.h" // memset
-#include "util.h" // timer_calc
-
-#define PORT_FD_BASE           0x03f0
-#define PORT_FD_DOR            0x03f2
-#define PORT_FD_STATUS         0x03f4
-#define PORT_FD_DATA           0x03f5
-#define PORT_FD_DIR            0x03f7
-
-#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
-#define FLOPPY_PIO_TIMEOUT 1000
-#define FLOPPY_IRQ_TIMEOUT 5000
-#define FLOPPY_SPECIFY1 0xAF  // step rate 12ms, head unload 240ms
-#define FLOPPY_SPECIFY2 0x02  // head load time 4ms, DMA used
-#define FLOPPY_STARTUP_TIME 8 // 1 second
-
-#define FLOPPY_DOR_MOTOR_D     0x80 // Set to turn drive 3's motor ON
-#define FLOPPY_DOR_MOTOR_C     0x40 // Set to turn drive 2's motor ON
-#define FLOPPY_DOR_MOTOR_B     0x20 // Set to turn drive 1's motor ON
-#define FLOPPY_DOR_MOTOR_A     0x10 // Set to turn drive 0's motor ON
-#define FLOPPY_DOR_MOTOR_MASK  0xf0
-#define FLOPPY_DOR_IRQ         0x08 // Set to enable IRQs and DMA
-#define FLOPPY_DOR_RESET       0x04 // Clear = enter reset mode, Set = normal operation
-#define FLOPPY_DOR_DSEL_MASK   0x03 // "Select" drive number for next access
-
-// 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 VARFSEG = {
-    .dbt = {
-        .specify1       = FLOPPY_SPECIFY1,
-        .specify2       = FLOPPY_SPECIFY2,
-        .shutoff_ticks  = FLOPPY_MOTOR_TICKS, // ~2 seconds
-        .bps_code       = FLOPPY_SIZE_CODE,
-        .sectors        = 18,
-        .interblock_len = FLOPPY_GAPLEN,
-        .data_len       = FLOPPY_DATALEN,
-        .gap_len        = FLOPPY_FORMAT_GAPLEN,
-        .fill_byte      = FLOPPY_FILLBYTE,
-        .settle_time    = 0x0F, // 15ms
-        .startup_time   = FLOPPY_STARTUP_TIME,
-    },
-    .max_track      = 79,   // maximum track
-    .data_rate      = 0,    // data transfer rate
-    .drive_type     = 4,    // drive type in cmos
-};
-
-struct floppyinfo_s {
-    struct chs_s chs;
-    u8 floppy_size;
-    u8 data_rate;
-};
-
-#define FLOPPY_SIZE_525 0x01
-#define FLOPPY_SIZE_350 0x02
-
-#define FLOPPY_RATE_500K 0x00
-#define FLOPPY_RATE_300K 0x01
-#define FLOPPY_RATE_250K 0x02
-#define FLOPPY_RATE_1M   0x03
-
-struct floppyinfo_s FloppyInfo[] VARFSEG = {
-    // Unknown
-    { {0, 0, 0}, 0x00, 0x00},
-    // 1 - 360KB, 5.25" - 2 heads, 40 tracks, 9 sectors
-    { {2, 40, 9}, FLOPPY_SIZE_525, FLOPPY_RATE_300K},
-    // 2 - 1.2MB, 5.25" - 2 heads, 80 tracks, 15 sectors
-    { {2, 80, 15}, FLOPPY_SIZE_525, FLOPPY_RATE_500K},
-    // 3 - 720KB, 3.5"  - 2 heads, 80 tracks, 9 sectors
-    { {2, 80, 9}, FLOPPY_SIZE_350, FLOPPY_RATE_250K},
-    // 4 - 1.44MB, 3.5" - 2 heads, 80 tracks, 18 sectors
-    { {2, 80, 18}, FLOPPY_SIZE_350, FLOPPY_RATE_500K},
-    // 5 - 2.88MB, 3.5" - 2 heads, 80 tracks, 36 sectors
-    { {2, 80, 36}, FLOPPY_SIZE_350, FLOPPY_RATE_1M},
-    // 6 - 160k, 5.25"  - 1 heads, 40 tracks, 8 sectors
-    { {1, 40, 8}, FLOPPY_SIZE_525, FLOPPY_RATE_250K},
-    // 7 - 180k, 5.25"  - 1 heads, 40 tracks, 9 sectors
-    { {1, 40, 9}, FLOPPY_SIZE_525, FLOPPY_RATE_300K},
-    // 8 - 320k, 5.25"  - 2 heads, 40 tracks, 8 sectors
-    { {2, 40, 8}, FLOPPY_SIZE_525, FLOPPY_RATE_250K},
-};
-
-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 NULL;
-    }
-
-    struct drive_s *drive = malloc_fseg(sizeof(*drive));
-    if (!drive) {
-        warn_noalloc();
-        return NULL;
-    }
-    memset(drive, 0, sizeof(*drive));
-    drive->cntl_id = floppyid;
-    drive->type = DTYPE_FLOPPY;
-    drive->blksize = DISK_SECTOR_SIZE;
-    drive->floppy_type = ftype;
-    drive->sectors = (u64)-1;
-
-    memcpy(&drive->lchs, &FloppyInfo[ftype].chs
-           , sizeof(FloppyInfo[ftype].chs));
-    return drive;
-}
-
-static void
-addFloppy(int floppyid, int ftype)
-{
-    struct drive_s *drive = init_floppy(floppyid, ftype);
-    if (!drive)
-        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, desc, prio);
-}
-
-void
-floppy_setup(void)
-{
-    memcpy(&diskette_param_table, &diskette_param_table2
-           , sizeof(diskette_param_table));
-    SET_IVT(0x1E, SEGOFF(SEG_BIOS
-                         , (u32)&diskette_param_table2 - BUILD_BIOS_ADDR));
-
-    if (! CONFIG_FLOPPY)
-        return;
-    dprintf(3, "init floppy drives\n");
-
-    if (CONFIG_QEMU) {
-        u8 type = rtc_read(CMOS_FLOPPY_DRIVE_TYPE);
-        if (type & 0xf0)
-            addFloppy(0, type >> 4);
-        if (type & 0x0f)
-            addFloppy(1, type & 0x0f);
-    } else {
-        u8 type = romfile_loadint("etc/floppy0", 0);
-        if (type)
-            addFloppy(0, type);
-        type = romfile_loadint("etc/floppy1", 0);
-        if (type)
-            addFloppy(1, type);
-    }
-
-    enable_hwirq(6, FUNC16(entry_0e));
-}
-
-// Find a floppy type that matches a given image size.
-int
-find_floppy_type(u32 size)
-{
-    int i;
-    for (i=1; i<ARRAY_SIZE(FloppyInfo); i++) {
-        struct chs_s *c = &FloppyInfo[i].chs;
-        if (c->cylinder * c->head * c->sector * DISK_SECTOR_SIZE == size)
-            return i;
-    }
-    return -1;
-}
-
-
-/****************************************************************
- * Low-level floppy IO
- ****************************************************************/
-
-u8 FloppyDOR VARLOW;
-
-static inline u8
-floppy_dor_read(void)
-{
-    return GET_LOW(FloppyDOR);
-}
-
-static inline void
-floppy_dor_write(u8 val)
-{
-    outb(val, PORT_FD_DOR);
-    SET_LOW(FloppyDOR, val);
-}
-
-static inline void
-floppy_dor_mask(u8 off, u8 on)
-{
-    floppy_dor_write((floppy_dor_read() & ~off) | on);
-}
-
-static void
-floppy_disable_controller(void)
-{
-    dprintf(2, "Floppy_disable_controller\n");
-    // Clear the reset bit (enter reset state) and clear 'enable IRQ and DMA'
-    floppy_dor_mask(FLOPPY_DOR_IRQ | FLOPPY_DOR_RESET, 0);
-}
-
-static int
-floppy_wait_irq(void)
-{
-    u8 frs = GET_BDA(floppy_recalibration_status);
-    SET_BDA(floppy_recalibration_status, frs & ~FRS_IRQ);
-    u32 end = timer_calc(FLOPPY_IRQ_TIMEOUT);
-    for (;;) {
-        if (timer_check(end)) {
-            warn_timeout();
-            floppy_disable_controller();
-            return DISK_RET_ETIMEOUT;
-        }
-        frs = GET_BDA(floppy_recalibration_status);
-        if (frs & FRS_IRQ)
-            break;
-        // Could use yield_toirq() here, but that causes issues on
-        // bochs, so use yield() instead.
-        yield();
-    }
-
-    SET_BDA(floppy_recalibration_status, frs & ~FRS_IRQ);
-    return DISK_RET_SUCCESS;
-}
-
-// Floppy commands
-#define FCF_WAITIRQ 0x10000
-#define FC_CHECKIRQ    (0x08 | (0<<8) | (2<<12))
-#define FC_SEEK        (0x0f | (2<<8) | (0<<12) | FCF_WAITIRQ)
-#define FC_RECALIBRATE (0x07 | (1<<8) | (0<<12) | FCF_WAITIRQ)
-#define FC_READID      (0x4a | (1<<8) | (7<<12) | FCF_WAITIRQ)
-#define FC_READ        (0xe6 | (8<<8) | (7<<12) | FCF_WAITIRQ)
-#define FC_WRITE       (0xc5 | (8<<8) | (7<<12) | FCF_WAITIRQ)
-#define FC_FORMAT      (0x4d | (5<<8) | (7<<12) | FCF_WAITIRQ)
-#define FC_SPECIFY     (0x03 | (2<<8) | (0<<12))
-
-// Send the specified command and it's parameters to the floppy controller.
-static int
-floppy_pio(int command, u8 *param)
-{
-    dprintf(9, "Floppy pio command %x\n", command);
-    // Send command and parameters to controller.
-    u32 end = timer_calc(FLOPPY_PIO_TIMEOUT);
-    int send = (command >> 8) & 0xf;
-    int i = 0;
-    for (;;) {
-        u8 sts = inb(PORT_FD_STATUS);
-        if (!(sts & 0x80)) {
-            if (timer_check(end)) {
-                warn_timeout();
-                floppy_disable_controller();
-                return DISK_RET_ETIMEOUT;
-            }
-            yield();
-            continue;
-        }
-        if (sts & 0x40) {
-            floppy_disable_controller();
-            return DISK_RET_ECONTROLLER;
-        }
-        if (i == 0)
-            outb(command & 0xff, PORT_FD_DATA);
-        else
-            outb(param[i-1], PORT_FD_DATA);
-        if (i++ >= send)
-            break;
-    }
-
-    // Wait for command to complete.
-    if (command & FCF_WAITIRQ) {
-        int ret = floppy_wait_irq();
-        if (ret)
-            return ret;
-    }
-
-    // Read response from controller.
-    end = timer_calc(FLOPPY_PIO_TIMEOUT);
-    int receive = (command >> 12) & 0xf;
-    i = 0;
-    for (;;) {
-        u8 sts = inb(PORT_FD_STATUS);
-        if (!(sts & 0x80)) {
-            if (timer_check(end)) {
-                warn_timeout();
-                floppy_disable_controller();
-                return DISK_RET_ETIMEOUT;
-            }
-            yield();
-            continue;
-        }
-        if (i >= receive) {
-            if (sts & 0x40) {
-                floppy_disable_controller();
-                return DISK_RET_ECONTROLLER;
-            }
-            break;
-        }
-        if (!(sts & 0x40)) {
-            floppy_disable_controller();
-            return DISK_RET_ECONTROLLER;
-        }
-        param[i++] = inb(PORT_FD_DATA);
-    }
-
-    return DISK_RET_SUCCESS;
-}
-
-static int
-floppy_enable_controller(void)
-{
-    dprintf(2, "Floppy_enable_controller\n");
-    // Clear the reset bit (enter reset state), but set 'enable IRQ and DMA'
-    floppy_dor_mask(FLOPPY_DOR_RESET, FLOPPY_DOR_IRQ);
-    // Real hardware needs a 4 microsecond delay
-    usleep(4);
-    // Set the reset bit (normal operation) and keep 'enable IRQ and DMA' on
-    floppy_dor_mask(0, FLOPPY_DOR_IRQ | FLOPPY_DOR_RESET);
-    int ret = floppy_wait_irq();
-    if (ret)
-        return ret;
-
-    // After the interrupt is received, send 4 SENSE INTERRUPT commands to
-    // clear the interrupt status for each of the four logical drives,
-    // supported by the controller.
-    // See section 7.4 - "Drive Polling" of the Intel 82077AA datasheet for
-    // a more detailed description of why this voodoo needs to be done.
-    // Without this, initialization fails on real controllers (but still works
-    // in QEMU)
-    u8 param[2];
-    int i;
-    for (i=0; i<4; i++) {
-        ret = floppy_pio(FC_CHECKIRQ, param);
-        if (ret)
-            return ret;
-    }
-    return DISK_RET_SUCCESS;
-}
-
-// Activate a drive and send a command to it.
-static int
-floppy_drive_pio(u8 floppyid, int command, u8 *param)
-{
-    // Enable controller if it isn't running.
-    if (!(floppy_dor_read() & FLOPPY_DOR_RESET)) {
-        int ret = floppy_enable_controller();
-        if (ret)
-            return ret;
-    }
-
-    // set the disk motor timeout value of INT 08 to the highest value
-    SET_BDA(floppy_motor_counter, 255);
-
-    // Check if the motor is already running
-    u8 motor_mask = FLOPPY_DOR_MOTOR_A << floppyid;
-    int motor_already_running = floppy_dor_read() & motor_mask;
-
-    // Turn on motor of selected drive, DMA & int enabled, normal operation
-    floppy_dor_write(motor_mask | FLOPPY_DOR_IRQ | FLOPPY_DOR_RESET | floppyid);
-
-    // If the motor was just started, wait for it to get up to speed
-    if (!motor_already_running && !CONFIG_QEMU)
-        msleep(FLOPPY_STARTUP_TIME * 125);
-
-    // Send command.
-    int ret = floppy_pio(command, param);
-    SET_BDA(floppy_motor_counter, FLOPPY_MOTOR_TICKS); // reset motor timeout
-    if (ret)
-        return ret;
-
-    // Check IRQ command is needed after irq commands with no results
-    if ((command & FCF_WAITIRQ) && ((command >> 12) & 0xf) == 0)
-        return floppy_pio(FC_CHECKIRQ, param);
-    return DISK_RET_SUCCESS;
-}
-
-
-/****************************************************************
- * Floppy media sense and seeking
- ****************************************************************/
-
-static int
-floppy_drive_recal(u8 floppyid)
-{
-    dprintf(2, "Floppy_drive_recal %d\n", floppyid);
-    // send Recalibrate command to controller
-    u8 param[2];
-    param[0] = floppyid;
-    int ret = floppy_drive_pio(floppyid, FC_RECALIBRATE, param);
-    if (ret)
-        return ret;
-
-    u8 frs = GET_BDA(floppy_recalibration_status);
-    SET_BDA(floppy_recalibration_status, frs | (1<<floppyid));
-    SET_BDA(floppy_track[floppyid], 0);
-    return DISK_RET_SUCCESS;
-}
-
-static int
-floppy_drive_specify(void)
-{
-    u8 param[2];
-    param[0] = FLOPPY_SPECIFY1;
-    param[1] = FLOPPY_SPECIFY2;
-    return floppy_pio(FC_SPECIFY, param);
-}
-
-static int
-floppy_drive_readid(u8 floppyid, u8 data_rate, u8 head)
-{
-    // Set data rate.
-    outb(data_rate, PORT_FD_DIR);
-
-    // send Read Sector Id command
-    u8 param[7];
-    param[0] = (head << 2) | floppyid; // HD DR1 DR2
-    int ret = floppy_drive_pio(floppyid, FC_READID, param);
-    if (ret)
-        return ret;
-    if (param[0] & 0xc0)
-        return -1;
-    return 0;
-}
-
-static int
-floppy_media_sense(struct drive_s *drive_gf)
-{
-    u8 ftype = GET_GLOBALFLAT(drive_gf->floppy_type), stype = ftype;
-    u8 floppyid = GET_GLOBALFLAT(drive_gf->cntl_id);
-
-    u8 data_rate = GET_GLOBAL(FloppyInfo[stype].data_rate);
-    int ret = floppy_drive_readid(floppyid, data_rate, 0);
-    if (ret) {
-        // Attempt media sense.
-        for (stype=1; ; stype++) {
-            if (stype >= ARRAY_SIZE(FloppyInfo))
-                return DISK_RET_EMEDIA;
-            if (stype==ftype
-                || (GET_GLOBAL(FloppyInfo[stype].floppy_size)
-                    != GET_GLOBAL(FloppyInfo[ftype].floppy_size))
-                || (GET_GLOBAL(FloppyInfo[stype].chs.head)
-                    > GET_GLOBAL(FloppyInfo[ftype].chs.head))
-                || (GET_GLOBAL(FloppyInfo[stype].chs.cylinder)
-                    > GET_GLOBAL(FloppyInfo[ftype].chs.cylinder))
-                || (GET_GLOBAL(FloppyInfo[stype].chs.sector)
-                    > GET_GLOBAL(FloppyInfo[ftype].chs.sector)))
-                continue;
-            data_rate = GET_GLOBAL(FloppyInfo[stype].data_rate);
-            ret = floppy_drive_readid(floppyid, data_rate, 0);
-            if (!ret)
-                break;
-        }
-    }
-    dprintf(2, "Floppy_media_sense on drive %d found rate %d\n"
-            , floppyid, data_rate);
-
-    u8 old_data_rate = GET_BDA(floppy_media_state[floppyid]) >> 6;
-    SET_BDA(floppy_last_data_rate, (old_data_rate<<2) | (data_rate<<6));
-    u8 media = (stype == 1 ? 0x04 : (stype == 2 ? 0x05 : 0x07));
-    u8 fms = (data_rate<<6) | FMS_MEDIA_DRIVE_ESTABLISHED | media;
-    if (GET_GLOBAL(FloppyInfo[stype].chs.cylinder)
-        < GET_GLOBAL(FloppyInfo[ftype].chs.cylinder))
-        fms |= FMS_DOUBLE_STEPPING;
-    SET_BDA(floppy_media_state[floppyid], fms);
-
-    return DISK_RET_SUCCESS;
-}
-
-// Prepare a floppy for a data transfer.
-static int
-floppy_prep(struct drive_s *drive_gf, u8 cylinder)
-{
-    u8 floppyid = GET_GLOBALFLAT(drive_gf->cntl_id);
-    if (!(GET_BDA(floppy_recalibration_status) & (1<<floppyid)) ||
-        !(GET_BDA(floppy_media_state[floppyid]) & FMS_MEDIA_DRIVE_ESTABLISHED)) {
-        // Recalibrate drive.
-        int ret = floppy_drive_recal(floppyid);
-        if (ret)
-            return ret;
-
-        // Sense media.
-        ret = floppy_media_sense(drive_gf);
-        if (ret)
-            return ret;
-
-        // Execute a SPECIFY command (sets the Step Rate Time,
-        // Head Load Time, Head Unload Time and the DMA enable/disable bit).
-        ret = floppy_drive_specify();
-        if (ret)
-            return ret;
-    }
-
-    // Seek to cylinder if needed.
-    u8 lastcyl = GET_BDA(floppy_track[floppyid]);
-    if (cylinder != lastcyl) {
-        u8 param[2];
-        param[0] = floppyid;
-        param[1] = cylinder;
-        int ret = floppy_drive_pio(floppyid, FC_SEEK, param);
-        if (ret)
-            return ret;
-        SET_BDA(floppy_track[floppyid], cylinder);
-    }
-
-    return DISK_RET_SUCCESS;
-}
-
-
-/****************************************************************
- * Floppy DMA transfer
- ****************************************************************/
-
-// Perform a floppy transfer command (setup DMA and issue PIO).
-static int
-floppy_dma_cmd(struct disk_op_s *op, int count, int command, u8 *param)
-{
-    // Setup DMA controller
-    int isWrite = command != FC_READ;
-    int ret = dma_floppy((u32)op->buf_fl, count, isWrite);
-    if (ret)
-        return DISK_RET_EBOUNDARY;
-
-    // Invoke floppy controller
-    u8 floppyid = GET_GLOBALFLAT(op->drive_fl->cntl_id);
-    ret = floppy_drive_pio(floppyid, command, param);
-    if (ret)
-        return ret;
-
-    // Populate floppy_return_status in BDA
-    int i;
-    for (i=0; i<7; i++)
-        SET_BDA(floppy_return_status[i], param[i]);
-
-    if (param[0] & 0xc0) {
-        if (param[1] & 0x02)
-            return DISK_RET_EWRITEPROTECT;
-        dprintf(1, "floppy error: %02x %02x %02x %02x %02x %02x %02x\n"
-                , param[0], param[1], param[2], param[3]
-                , param[4], param[5], param[6]);
-        return DISK_RET_ECONTROLLER;
-    }
-
-    return DISK_RET_SUCCESS;
-}
-
-
-/****************************************************************
- * Floppy handlers
- ****************************************************************/
-
-static struct chs_s
-lba2chs(struct disk_op_s *op)
-{
-    struct chs_s res = { };
-
-    u32 tmp = op->lba;
-    u16 nls = GET_GLOBALFLAT(op->drive_fl->lchs.sector);
-    res.sector = (tmp % nls) + 1;
-
-    tmp /= nls;
-    u16 nlh = GET_GLOBALFLAT(op->drive_fl->lchs.head);
-    res.head = tmp % nlh;
-
-    tmp /= nlh;
-    res.cylinder = tmp;
-
-    return res;
-}
-
-// diskette controller reset
-static int
-floppy_reset(struct disk_op_s *op)
-{
-    SET_BDA(floppy_recalibration_status, 0);
-    SET_BDA(floppy_media_state[0], 0);
-    SET_BDA(floppy_media_state[1], 0);
-    SET_BDA(floppy_track[0], 0);
-    SET_BDA(floppy_track[1], 0);
-    SET_BDA(floppy_last_data_rate, 0);
-    floppy_disable_controller();
-    return floppy_enable_controller();
-}
-
-// Read Diskette Sectors
-static int
-floppy_read(struct disk_op_s *op)
-{
-    struct chs_s chs = lba2chs(op);
-    int ret = floppy_prep(op->drive_fl, chs.cylinder);
-    if (ret)
-        return ret;
-
-    // send read-normal-data command to controller
-    u8 floppyid = GET_GLOBALFLAT(op->drive_fl->cntl_id);
-    u8 param[8];
-    param[0] = (chs.head << 2) | floppyid; // HD DR1 DR2
-    param[1] = chs.cylinder;
-    param[2] = chs.head;
-    param[3] = chs.sector;
-    param[4] = FLOPPY_SIZE_CODE;
-    param[5] = chs.sector + op->count - 1; // last sector to read on track
-    param[6] = FLOPPY_GAPLEN;
-    param[7] = FLOPPY_DATALEN;
-    return floppy_dma_cmd(op, op->count * DISK_SECTOR_SIZE, FC_READ, param);
-}
-
-// Write Diskette Sectors
-static int
-floppy_write(struct disk_op_s *op)
-{
-    struct chs_s chs = lba2chs(op);
-    int ret = floppy_prep(op->drive_fl, chs.cylinder);
-    if (ret)
-        return ret;
-
-    // send write-normal-data command to controller
-    u8 floppyid = GET_GLOBALFLAT(op->drive_fl->cntl_id);
-    u8 param[8];
-    param[0] = (chs.head << 2) | floppyid; // HD DR1 DR2
-    param[1] = chs.cylinder;
-    param[2] = chs.head;
-    param[3] = chs.sector;
-    param[4] = FLOPPY_SIZE_CODE;
-    param[5] = chs.sector + op->count - 1; // last sector to write on track
-    param[6] = FLOPPY_GAPLEN;
-    param[7] = FLOPPY_DATALEN;
-    return floppy_dma_cmd(op, op->count * DISK_SECTOR_SIZE, FC_WRITE, param);
-}
-
-// Verify Diskette Sectors
-static int
-floppy_verify(struct disk_op_s *op)
-{
-    struct chs_s chs = lba2chs(op);
-    int ret = floppy_prep(op->drive_fl, chs.cylinder);
-    if (ret)
-        return ret;
-
-    // This command isn't implemented - just return success.
-    return DISK_RET_SUCCESS;
-}
-
-// format diskette track
-static int
-floppy_format(struct disk_op_s *op)
-{
-    struct chs_s chs = lba2chs(op);
-    int ret = floppy_prep(op->drive_fl, chs.cylinder);
-    if (ret)
-        return ret;
-
-    // send format-track command to controller
-    u8 floppyid = GET_GLOBALFLAT(op->drive_fl->cntl_id);
-    u8 param[7];
-    param[0] = (chs.head << 2) | floppyid; // HD DR1 DR2
-    param[1] = FLOPPY_SIZE_CODE;
-    param[2] = op->count; // number of sectors per track
-    param[3] = FLOPPY_FORMAT_GAPLEN;
-    param[4] = FLOPPY_FILLBYTE;
-    return floppy_dma_cmd(op, op->count * 4, FC_FORMAT, param);
-}
-
-int
-floppy_process_op(struct disk_op_s *op)
-{
-    if (!CONFIG_FLOPPY)
-        return 0;
-
-    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:
-        return DISK_RET_EPARAM;
-    }
-}
-
-
-/****************************************************************
- * HW irqs
- ****************************************************************/
-
-// INT 0Eh Diskette Hardware ISR Entry Point
-void VISIBLE16
-handle_0e(void)
-{
-    if (! CONFIG_FLOPPY)
-        return;
-    debug_isr(DEBUG_ISR_0e);
-
-    // diskette interrupt has occurred
-    u8 frs = GET_BDA(floppy_recalibration_status);
-    SET_BDA(floppy_recalibration_status, frs | FRS_IRQ);
-
-    pic_eoi1();
-}
-
-// Called from int08 handler.
-void
-floppy_tick(void)
-{
-    if (! CONFIG_FLOPPY)
-        return;
-
-    // time to turn off drive(s)?
-    u8 fcount = GET_BDA(floppy_motor_counter);
-    if (fcount) {
-        fcount--;
-        SET_BDA(floppy_motor_counter, fcount);
-        if (fcount == 0)
-            // turn motor(s) off
-            floppy_dor_mask(FLOPPY_DOR_MOTOR_MASK, 0);
-    }
-}