Index: hw/fdc.c
===================================================================
RCS file: /sources/qemu/qemu/hw/fdc.c,v
retrieving revision 1.38
diff -u -r1.38 hw/fdc.c
--- hw/fdc.c	29 Feb 2008 19:24:00 -0000	1.38
+++ hw/fdc.c	13 Mar 2008 09:13:19 -0000
@@ -2,6 +2,7 @@
  * QEMU Floppy disk emulator (Intel 82078)
  *
  * Copyright (c) 2003, 2007 Jocelyn Mayer
+ * Copyright (c) 2008 Hervé Poussineau
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -21,10 +22,7 @@
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  * THE SOFTWARE.
  */
-/*
- * The controller is used in Sun4m systems in a slightly different
- * way. There are changes in DOR register and DMA is not available.
- */
+
 #include "hw.h"
 #include "fdc.h"
 #include "block.h"
@@ -48,6 +46,8 @@
 /********************************************************/
 /* Floppy drive emulation                               */
 
+#define SELECT_FD(fdctrl, drive) ((fdctrl)->dor = ((fdctrl)->dor & ~FD_DOR_SELMASK) | ((drive) & FD_DOR_SELMASK))
+
 /* Will always be a fixed parameter for us */
 #define FD_SECTOR_LEN 512
 #define FD_SECTOR_SC  2   /* Sector size code */
@@ -68,10 +68,6 @@
     FDRIVE_DRV_NONE = 0x03,   /* No drive connected     */
 } fdrive_type_t;
 
-typedef enum fdrive_flags_t {
-    FDRIVE_MOTOR_ON   = 0x01, /* motor on/off           */
-} fdrive_flags_t;
-
 typedef enum fdisk_flags_t {
     FDISK_DBL_SIDES  = 0x01,
 } fdisk_flags_t;
@@ -80,7 +76,6 @@
     BlockDriverState *bs;
     /* Drive status */
     fdrive_type_t drive;
-    fdrive_flags_t drflags;
     uint8_t perpendicular;    /* 2.88 MB access mode    */
     /* Position */
     uint8_t head;
@@ -102,7 +97,6 @@
     /* Drive */
     drv->bs = bs;
     drv->drive = FDRIVE_DRV_NONE;
-    drv->drflags = 0;
     drv->perpendicular = 0;
     /* Disk */
     drv->last_sect = 0;
@@ -121,6 +115,13 @@
     return _fd_sector(drv->head, drv->track, drv->sect, drv->last_sect);
 }
 
+/* Seek to a new position:
+ * returns 0 if already on right track
+ * returns 1 if track changed
+ * returns 2 if track is invalid
+ * returns 3 if sector is invalid
+ * returns 4 if seek is disabled
+ */
 static int fd_seek (fdrive_t *drv, uint8_t head, uint8_t track, uint8_t sect,
                     int enable_seek)
 {
@@ -144,14 +145,16 @@
     }
     sector = _fd_sector(head, track, sect, drv->last_sect);
     ret = 0;
-    if (sector != fd_sector(drv)) {
 #if 0
-        if (!enable_seek) {
-            FLOPPY_ERROR("no implicit seek %d %02x %02x (max=%d %02x %02x)\n",
-                         head, track, sect, 1, drv->max_track, drv->last_sect);
-            return 4;
-        }
+    if (!enable_seek && (head != drv->head || track != drv->track)) {
+        FLOPPY_ERROR("no implicit seek %d %02x %02x (current=%d %02x %02x, max=%d %02x %02x)\n",
+                     head, track, sect,
+                     drv->head, drv->track, drv->sect,
+                     1, drv->max_track, drv->last_sect);
+        return 4;
+    }
 #endif
+    if (sector != fd_sector(drv)) {
         drv->head = head;
         if (drv->track != track)
             ret = 1;
@@ -295,24 +298,6 @@
     }
 }
 
-/* Motor control */
-static void fd_start (fdrive_t *drv)
-{
-    drv->drflags |= FDRIVE_MOTOR_ON;
-}
-
-static void fd_stop (fdrive_t *drv)
-{
-    drv->drflags &= ~FDRIVE_MOTOR_ON;
-}
-
-/* Re-initialise a drives (motor off, repositioned) */
-static void fd_reset (fdrive_t *drv)
-{
-    fd_stop(drv);
-    fd_recalibrate(drv);
-}
-
 /********************************************************/
 /* Intel 82078 floppy disk controller emulation          */
 
@@ -320,7 +305,7 @@
 static void fdctrl_reset_fifo (fdctrl_t *fdctrl);
 static int fdctrl_transfer_handler (void *opaque, int nchan,
                                     int dma_pos, int dma_len);
-static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status);
+static void fdctrl_raise_irq (fdctrl_t *fdctrl);
 static void fdctrl_result_timer(void *opaque);
 
 static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl);
@@ -335,14 +320,6 @@
 static uint32_t fdctrl_read_dir (fdctrl_t *fdctrl);
 
 enum {
-    FD_CTRL_ACTIVE = 0x01, /* XXX: suppress that */
-    FD_CTRL_RESET  = 0x02,
-    FD_CTRL_SLEEP  = 0x04, /* XXX: suppress that */
-    FD_CTRL_BUSY   = 0x08, /* dma transfer in progress */
-    FD_CTRL_INTR   = 0x10,
-};
-
-enum {
     FD_DIR_WRITE   = 0,
     FD_DIR_READ    = 1,
     FD_DIR_SCANE   = 2,
@@ -356,7 +333,6 @@
     FD_STATE_DATA   = 0x02,
     FD_STATE_STATE  = 0x03,
     FD_STATE_MULTI  = 0x10,
-    FD_STATE_SEEK   = 0x20,
     FD_STATE_FORMAT = 0x40,
 };
 
@@ -398,6 +374,8 @@
     FD_CMD_VERIFY = 0x56,
     FD_CMD_SCAN_LOW_OR_EQUAL = 0x59,
     FD_CMD_SCAN_HIGH_OR_EQUAL = 0x5d,
+    FD_CMD_WRITE_SECTOR = 0x65,
+    FD_CMD_READ_SECTOR = 0x66,
     FD_CMD_DRIVE_SPECIFICATION_COMMAND = 0x8e,
     FD_CMD_RELATIVE_SEEK_OUT = 0x8f,
     FD_CMD_LOCK = 0x94,
@@ -422,7 +400,15 @@
 };
 
 enum {
+    FD_SR1_EC       = 0x80, /* End of cylinder */
+};
+
+enum {
+#if MAX_FD == 4
+    FD_DOR_SELMASK  = 0x03,
+#else
     FD_DOR_SELMASK  = 0x01,
+#endif
     FD_DOR_nRESET   = 0x04,
     FD_DOR_DMAEN    = 0x08,
     FD_DOR_MOTEN0   = 0x10,
@@ -432,7 +418,11 @@
 };
 
 enum {
+#if MAX_FD == 4
     FD_TDR_BOOTSEL  = 0x0c,
+#else
+    FD_TDR_BOOTSEL  = 0x04,
+#endif
 };
 
 enum {
@@ -460,7 +450,6 @@
 #define FD_SET_STATE(state, new_state) \
 do { (state) = ((state) & ~FD_STATE_STATE) | (new_state); } while (0)
 #define FD_MULTI_TRACK(state) ((state) & FD_STATE_MULTI)
-#define FD_DID_SEEK(state) ((state) & FD_STATE_SEEK)
 #define FD_FORMAT_CMD(state) ((state) & FD_STATE_FORMAT)
 
 struct fdctrl_t {
@@ -473,17 +462,19 @@
     target_phys_addr_t io_base;
     /* Controller state */
     QEMUTimer *result_timer;
-    uint8_t state;
-    uint8_t dma_en;
-    uint8_t cur_drv;
-    uint8_t bootsel;
+    uint8_t dor;
+    uint8_t tdr;
+    uint8_t dsr;
+    uint8_t msr;
     /* Command FIFO */
     uint8_t *fifo;
     uint32_t data_pos;
     uint32_t data_len;
     uint8_t data_state;
     uint8_t data_dir;
-    uint8_t int_status;
+    uint8_t status0;
+    uint8_t status1;
+    uint8_t status2;
     uint8_t eot; /* last wanted sector */
     /* States kept only to be returned back */
     /* Timers state */
@@ -498,7 +489,7 @@
     /* Sun4m quirks? */
     int sun4m;
     /* Floppy drives */
-    fdrive_t drives[2];
+    fdrive_t drives[MAX_FD];
 };
 
 static uint32_t fdctrl_read (void *opaque, uint32_t reg)
@@ -603,10 +594,6 @@
 
 static void fd_save (QEMUFile *f, fdrive_t *fd)
 {
-    uint8_t tmp;
-
-    tmp = fd->drflags;
-    qemu_put_8s(f, &tmp);
     qemu_put_8s(f, &fd->head);
     qemu_put_8s(f, &fd->track);
     qemu_put_8s(f, &fd->sect);
@@ -617,17 +604,21 @@
 static void fdc_save (QEMUFile *f, void *opaque)
 {
     fdctrl_t *s = opaque;
+    uint8_t tmp;
+    int i;
 
-    qemu_put_8s(f, &s->state);
-    qemu_put_8s(f, &s->dma_en);
-    qemu_put_8s(f, &s->cur_drv);
-    qemu_put_8s(f, &s->bootsel);
+    qemu_put_8s(f, &s->dor);
+    qemu_put_8s(f, &s->tdr);
+    qemu_put_8s(f, &s->dsr);
+    qemu_put_8s(f, &s->msr);
     qemu_put_buffer(f, s->fifo, FD_SECTOR_LEN);
     qemu_put_be32s(f, &s->data_pos);
     qemu_put_be32s(f, &s->data_len);
     qemu_put_8s(f, &s->data_state);
     qemu_put_8s(f, &s->data_dir);
-    qemu_put_8s(f, &s->int_status);
+    qemu_put_8s(f, &s->status0);
+    qemu_put_8s(f, &s->status1);
+    qemu_put_8s(f, &s->status2);
     qemu_put_8s(f, &s->eot);
     qemu_put_8s(f, &s->timer0);
     qemu_put_8s(f, &s->timer1);
@@ -635,16 +626,15 @@
     qemu_put_8s(f, &s->config);
     qemu_put_8s(f, &s->lock);
     qemu_put_8s(f, &s->pwrd);
-    fd_save(f, &s->drives[0]);
-    fd_save(f, &s->drives[1]);
+
+    tmp = MAX_FD;
+    qemu_put_8s(f, &tmp);
+    for (i = 0; i < MAX_FD; i++)
+        fd_save(f, &s->drives[i]);
 }
 
 static int fd_load (QEMUFile *f, fdrive_t *fd)
 {
-    uint8_t tmp;
-
-    qemu_get_8s(f, &tmp);
-    fd->drflags = tmp;
     qemu_get_8s(f, &fd->head);
     qemu_get_8s(f, &fd->track);
     qemu_get_8s(f, &fd->sect);
@@ -657,21 +647,24 @@
 static int fdc_load (QEMUFile *f, void *opaque, int version_id)
 {
     fdctrl_t *s = opaque;
-    int ret;
+    int i, ret = 0;
+    uint8_t n;
 
-    if (version_id != 1)
+    if (version_id != 2)
         return -EINVAL;
 
-    qemu_get_8s(f, &s->state);
-    qemu_get_8s(f, &s->dma_en);
-    qemu_get_8s(f, &s->cur_drv);
-    qemu_get_8s(f, &s->bootsel);
+    qemu_get_8s(f, &s->dor);
+    qemu_get_8s(f, &s->tdr);
+    qemu_get_8s(f, &s->dsr);
+    qemu_get_8s(f, &s->msr);
     qemu_get_buffer(f, s->fifo, FD_SECTOR_LEN);
     qemu_get_be32s(f, &s->data_pos);
     qemu_get_be32s(f, &s->data_len);
     qemu_get_8s(f, &s->data_state);
     qemu_get_8s(f, &s->data_dir);
-    qemu_get_8s(f, &s->int_status);
+    qemu_get_8s(f, &s->status0);
+    qemu_get_8s(f, &s->status1);
+    qemu_get_8s(f, &s->status2);
     qemu_get_8s(f, &s->eot);
     qemu_get_8s(f, &s->timer0);
     qemu_get_8s(f, &s->timer1);
@@ -679,10 +672,16 @@
     qemu_get_8s(f, &s->config);
     qemu_get_8s(f, &s->lock);
     qemu_get_8s(f, &s->pwrd);
+    qemu_get_8s(f, &n);
 
-    ret = fd_load(f, &s->drives[0]);
-    if (ret == 0)
-        ret = fd_load(f, &s->drives[1]);
+    if (n > MAX_FD)
+        return -EINVAL;
+
+    for (i = 0; i < n; i++) {
+        ret = fd_load(f, &s->drives[i]);
+        if (ret != 0)
+            break;
+    }
 
     return ret;
 }
@@ -719,17 +718,16 @@
     fdctrl->io_base = io_base;
     fdctrl->config = FD_CONFIG_EIS | FD_CONFIG_EFIFO; /* Implicit seek, polling & FIFO enabled */
     if (fdctrl->dma_chann != -1) {
-        fdctrl->dma_en = 1;
+        fdctrl->dor |= FD_DOR_DMAEN;
         DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl);
     } else {
-        fdctrl->dma_en = 0;
+        fdctrl->dor &= FD_DOR_DMAEN;
     }
     for (i = 0; i < MAX_FD; i++) {
         fd_init(&fdctrl->drives[i], fds[i]);
     }
     fdctrl_reset(fdctrl, 0);
-    fdctrl->state = FD_CTRL_ACTIVE;
-    register_savevm("fdc", io_base, 1, fdc_save, fdc_load, fdctrl);
+    register_savevm("fdc", io_base, 2, fdc_save, fdc_load, fdctrl);
     qemu_register_reset(fdctrl_external_reset, fdctrl);
     for (i = 0; i < MAX_FD; i++) {
         fd_revalidate(&fdctrl->drives[i]);
@@ -793,23 +791,12 @@
 {
     FLOPPY_DPRINTF("Reset interrupt\n");
     qemu_set_irq(fdctrl->irq, 0);
-    fdctrl->state &= ~FD_CTRL_INTR;
 }
 
-static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status)
+static void fdctrl_raise_irq (fdctrl_t *fdctrl)
 {
-    // Sparc mutation
-    if (fdctrl->sun4m && !fdctrl->dma_en) {
-        fdctrl->state &= ~FD_CTRL_BUSY;
-        fdctrl->int_status = status;
-        return;
-    }
-    if (~(fdctrl->state & FD_CTRL_INTR)) {
-        qemu_set_irq(fdctrl->irq, 1);
-        fdctrl->state |= FD_CTRL_INTR;
-    }
-    FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", status);
-    fdctrl->int_status = status;
+    qemu_set_irq(fdctrl->irq, 1);
+    FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", fdctrl->status0);
 }
 
 /* Reset controller */
@@ -820,32 +807,65 @@
     FLOPPY_DPRINTF("reset controller\n");
     fdctrl_reset_irq(fdctrl);
     /* Initialise controller */
-    fdctrl->cur_drv = 0;
+    fdctrl->dor = FD_DOR_nRESET;
+    fdctrl->msr = FD_MSR_RQM;
     /* FIFO state */
     fdctrl->data_pos = 0;
     fdctrl->data_len = 0;
     fdctrl->data_state = FD_STATE_CMD;
     fdctrl->data_dir = FD_DIR_WRITE;
     for (i = 0; i < MAX_FD; i++)
-        fd_reset(&fdctrl->drives[i]);
+        fd_recalibrate(&fdctrl->drives[i]);
     fdctrl_reset_fifo(fdctrl);
-    if (do_irq)
-        fdctrl_raise_irq(fdctrl, FD_SR0_RDYCHG);
+    if (do_irq) {
+        fdctrl->status0 |= FD_SR0_RDYCHG;
+        fdctrl_raise_irq(fdctrl);
+    }
+    fdctrl->dor |= (fdctrl->dma_chann != -1) ? FD_DOR_DMAEN : 0;
 }
 
 static inline fdrive_t *drv0 (fdctrl_t *fdctrl)
 {
-    return &fdctrl->drives[fdctrl->bootsel];
+    return &fdctrl->drives[(fdctrl->tdr & FD_TDR_BOOTSEL) >> 2];
 }
 
 static inline fdrive_t *drv1 (fdctrl_t *fdctrl)
 {
-    return &fdctrl->drives[1 - fdctrl->bootsel];
+    if ((fdctrl->tdr & FD_TDR_BOOTSEL) < (1 << 2))
+        return &fdctrl->drives[1];
+    else
+        return &fdctrl->drives[0];
+}
+
+#if MAX_FD == 4
+static inline fdrive_t *drv2 (fdctrl_t *fdctrl)
+{
+    if ((fdctrl->tdr & FD_TDR_BOOTSEL) < (2 << 2))
+        return &fdctrl->drives[2];
+    else
+        return &fdctrl->drives[1];
 }
 
+static inline fdrive_t *drv3 (fdctrl_t *fdctrl)
+{
+    if ((fdctrl->tdr & FD_TDR_BOOTSEL) < (3 << 2))
+        return &fdctrl->drives[3];
+    else
+        return &fdctrl->drives[2];
+}
+#endif
+
 static fdrive_t *get_cur_drv (fdctrl_t *fdctrl)
 {
-    return fdctrl->cur_drv == 0 ? drv0(fdctrl) : drv1(fdctrl);
+    switch (fdctrl->dor & FD_DOR_SELMASK) {
+        case 0: return drv0(fdctrl);
+        case 1: return drv1(fdctrl);
+#if MAX_FD == 4
+        case 2: return drv2(fdctrl);
+        case 3: return drv3(fdctrl);
+#endif
+        default: return NULL;
+    }
 }
 
 /* Status B register : 0x01 (read-only) */
@@ -858,21 +878,8 @@
 /* Digital output register : 0x02 */
 static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl)
 {
-    uint32_t retval = 0;
+    uint32_t retval = fdctrl->dor;
 
-    /* Drive motors state indicators */
-    if (drv0(fdctrl)->drflags & FDRIVE_MOTOR_ON)
-        retval |= FD_DOR_MOTEN0;
-    if (drv1(fdctrl)->drflags & FDRIVE_MOTOR_ON)
-        retval |= FD_DOR_MOTEN1;
-    /* DMA enable */
-    if (fdctrl->dma_en)
-        retval |= FD_DOR_DMAEN;
-    /* Reset indicator */
-    if (!(fdctrl->state & FD_CTRL_RESET))
-        retval |= FD_DOR_nRESET;
-    /* Selected drive */
-    retval |= fdctrl->cur_drv;
     FLOPPY_DPRINTF("digital output register: 0x%02x\n", retval);
 
     return retval;
@@ -880,53 +887,29 @@
 
 static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value)
 {
-    /* Reset mode */
-    if (fdctrl->state & FD_CTRL_RESET) {
-        if (!(value & FD_DOR_nRESET)) {
-            FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
-            return;
-        }
-    }
     FLOPPY_DPRINTF("digital output register set to 0x%02x\n", value);
-    /* Drive motors state indicators */
-    if (value & FD_DOR_MOTEN1)
-        fd_start(drv1(fdctrl));
-    else
-        fd_stop(drv1(fdctrl));
-    if (value & FD_DOR_MOTEN0)
-        fd_start(drv0(fdctrl));
-    else
-        fd_stop(drv0(fdctrl));
-    /* DMA enable */
-#if 0
-    if (fdctrl->dma_chann != -1)
-        fdctrl->dma_en = value & FD_DOR_DMAEN ? 1 : 0;
-#endif
+
     /* Reset */
     if (!(value & FD_DOR_nRESET)) {
-        if (!(fdctrl->state & FD_CTRL_RESET)) {
+        if (fdctrl->dor & FD_DOR_nRESET) {
             FLOPPY_DPRINTF("controller enter RESET state\n");
-            fdctrl->state |= FD_CTRL_RESET;
         }
     } else {
-        if (fdctrl->state & FD_CTRL_RESET) {
+        if (!(fdctrl->dor & FD_DOR_nRESET)) {
             FLOPPY_DPRINTF("controller out of RESET state\n");
             fdctrl_reset(fdctrl, 1);
-            fdctrl->state &= ~(FD_CTRL_RESET | FD_CTRL_SLEEP);
+            fdctrl->dsr &= ~FD_DSR_PWRDOWN;
         }
     }
-    /* Selected drive */
-    fdctrl->cur_drv = value & FD_DOR_SELMASK;
+
+    fdctrl->dor = value;
 }
 
 /* Tape drive register : 0x03 */
 static uint32_t fdctrl_read_tape (fdctrl_t *fdctrl)
 {
-    uint32_t retval = 0;
+    uint32_t retval = fdctrl->tdr;
 
-    /* Disk boot selection indicator */
-    retval |= fdctrl->bootsel << 2;
-    /* Tape indicators: never allowed */
     FLOPPY_DPRINTF("tape drive register: 0x%02x\n", retval);
 
     return retval;
@@ -935,13 +918,13 @@
 static void fdctrl_write_tape (fdctrl_t *fdctrl, uint32_t value)
 {
     /* Reset mode */
-    if (fdctrl->state & FD_CTRL_RESET) {
+    if (!(fdctrl->dor & FD_DOR_nRESET)) {
         FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
         return;
     }
     FLOPPY_DPRINTF("tape drive register set to 0x%02x\n", value);
     /* Disk boot selection indicator */
-    fdctrl->bootsel = (value & FD_TDR_BOOTSEL) >> 2;
+    fdctrl->tdr = value & FD_TDR_BOOTSEL;
     /* Tape indicators: never allow */
 }
 
@@ -950,15 +933,19 @@
 {
     uint32_t retval = 0;
 
-    fdctrl->state &= ~(FD_CTRL_SLEEP | FD_CTRL_RESET);
-    if (!(fdctrl->state & FD_CTRL_BUSY)) {
+    fdctrl->dsr &= ~FD_DSR_PWRDOWN;
+    fdctrl->dor |= FD_DOR_nRESET;
+    if (fdctrl->msr & FD_MSR_RQM) {
         /* Data transfer allowed */
         retval |= FD_MSR_RQM;
         /* Data transfer direction indicator */
         if (fdctrl->data_dir == FD_DIR_READ)
             retval |= FD_MSR_DIO;
     }
-    /* Should handle FD_MSR_NONDMA for SPECIFY command */
+    /* Non-DMA indicator */
+    if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA &&
+        !(fdctrl->dor & FD_DOR_DMAEN))
+        retval |= FD_MSR_NONDMA;
     /* Command busy indicator */
     if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA ||
         FD_STATE(fdctrl->data_state) == FD_STATE_STATUS)
@@ -972,21 +959,21 @@
 static void fdctrl_write_rate (fdctrl_t *fdctrl, uint32_t value)
 {
     /* Reset mode */
-    if (fdctrl->state & FD_CTRL_RESET) {
+    if (!(fdctrl->dor & FD_DOR_nRESET)) {
         FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
         return;
     }
     FLOPPY_DPRINTF("select rate register set to 0x%02x\n", value);
     /* Reset: autoclear */
     if (value & FD_DSR_SWRESET) {
-        fdctrl->state |= FD_CTRL_RESET;
+        fdctrl->dor &= ~FD_DOR_nRESET;
         fdctrl_reset(fdctrl, 1);
-        fdctrl->state &= ~FD_CTRL_RESET;
+        fdctrl->dor |= FD_DOR_nRESET;
     }
     if (value & FD_DSR_PWRDOWN) {
-        fdctrl->state |= FD_CTRL_SLEEP;
         fdctrl_reset(fdctrl, 1);
     }
+    fdctrl->dsr = value;
 }
 
 static int fdctrl_media_changed(fdrive_t *drv)
@@ -1007,8 +994,13 @@
 {
     uint32_t retval = 0;
 
-    if (fdctrl_media_changed(drv0(fdctrl)) ||
-        fdctrl_media_changed(drv1(fdctrl)))
+    if (fdctrl_media_changed(drv0(fdctrl))
+     || fdctrl_media_changed(drv1(fdctrl))
+#if MAX_FD == 4
+     || fdctrl_media_changed(drv2(fdctrl))
+     || fdctrl_media_changed(drv3(fdctrl))
+#endif
+        )
         retval |= FD_DIR_DSKCHG;
     if (retval != 0)
         FLOPPY_DPRINTF("Floppy digital input register: 0x%02x\n", retval);
@@ -1032,7 +1024,7 @@
     fdctrl->data_pos = 0;
     FD_SET_STATE(fdctrl->data_state, FD_STATE_STATUS);
     if (do_irq)
-        fdctrl_raise_irq(fdctrl, 0x00);
+        fdctrl_raise_irq(fdctrl);
 }
 
 /* Set an error: unimplemented/unknown command */
@@ -1042,9 +1034,9 @@
     fdrive_t *cur_drv;
 
     cur_drv = get_cur_drv(fdctrl);
-    fdctrl->fifo[0] = FD_SR0_ABNTERM | FD_SR0_SEEK | (cur_drv->head << 2) | fdctrl->cur_drv;
-    fdctrl->fifo[1] = 0x00;
-    fdctrl->fifo[2] = 0x00;
+    fdctrl->fifo[0] = FD_SR0_ABNTERM | FD_SR0_SEEK | (cur_drv->head << 2) | (fdctrl->dor & FD_DOR_SELMASK);
+    fdctrl->fifo[1] = fdctrl->status1;
+    fdctrl->fifo[2] = fdctrl->status2;
     fdctrl_set_fifo(fdctrl, 3, 1);
 #else
     //    fdctrl_reset_fifo(fdctrl);
@@ -1053,27 +1045,65 @@
 #endif
 }
 
+/* Seek to next sector */
+static int fdctrl_seek_to_next_sect (fdctrl_t *fdctrl, fdrive_t *cur_drv)
+{
+    FLOPPY_DPRINTF("seek to next sector (%d %02x %02x => %d)\n",
+                   cur_drv->head, cur_drv->track, cur_drv->sect,
+                   fd_sector(cur_drv));
+    /* XXX: cur_drv->sect >= cur_drv->last_sect should be an
+       error in fact */
+    if (cur_drv->sect >= cur_drv->last_sect ||
+        cur_drv->sect == fdctrl->eot) {
+        cur_drv->sect = 1;
+        if (FD_MULTI_TRACK(fdctrl->data_state)) {
+            if (cur_drv->head == 0 &&
+                (cur_drv->flags & FDISK_DBL_SIDES) != 0) {
+                cur_drv->head = 1;
+            } else {
+                cur_drv->head = 0;
+                cur_drv->track++;
+                if ((cur_drv->flags & FDISK_DBL_SIDES) == 0)
+                    return 0;
+            }
+        } else {
+            cur_drv->track++;
+            return 0;
+        }
+        FLOPPY_DPRINTF("seek to next track (%d %02x %02x => %d)\n",
+                       cur_drv->head, cur_drv->track,
+                       cur_drv->sect, fd_sector(cur_drv));
+    } else {
+        cur_drv->sect++;
+    }
+    fdctrl->status0 |= FD_SR0_SEEK;
+    return 1;
+}
+
 /* Callback for transfer end (stop or abort) */
-static void fdctrl_stop_transfer (fdctrl_t *fdctrl, uint8_t status0,
-                                  uint8_t status1, uint8_t status2)
+static void fdctrl_stop_transfer (fdctrl_t *fdctrl)
 {
     fdrive_t *cur_drv;
+    uint8_t status0 = fdctrl->status0;
 
     cur_drv = get_cur_drv(fdctrl);
+    /* Add head and drive to status0 */
+    status0 |= (cur_drv->head << 2) | (fdctrl->dor & FD_DOR_SELMASK);
+
     FLOPPY_DPRINTF("transfer status: %02x %02x %02x (%02x)\n",
-                   status0, status1, status2,
-                   status0 | (cur_drv->head << 2) | fdctrl->cur_drv);
-    fdctrl->fifo[0] = status0 | (cur_drv->head << 2) | fdctrl->cur_drv;
-    fdctrl->fifo[1] = status1;
-    fdctrl->fifo[2] = status2;
+                   fdctrl->status0, fdctrl->status1, fdctrl->status2,
+                   status0);
+    fdctrl->fifo[0] = status0;
+    fdctrl->fifo[1] = fdctrl->status1;
+    fdctrl->fifo[2] = fdctrl->status2;
     fdctrl->fifo[3] = cur_drv->track;
     fdctrl->fifo[4] = cur_drv->head;
     fdctrl->fifo[5] = cur_drv->sect;
     fdctrl->fifo[6] = FD_SECTOR_SC;
     fdctrl->data_dir = FD_DIR_READ;
-    if (fdctrl->state & FD_CTRL_BUSY) {
+    if (!(fdctrl->msr & FD_MSR_RQM)) {
         DMA_release_DREQ(fdctrl->dma_chann);
-        fdctrl->state &= ~FD_CTRL_BUSY;
+        fdctrl->msr |= FD_MSR_RQM;
     }
     fdctrl_set_fifo(fdctrl, 7, 1);
 }
@@ -1083,41 +1113,43 @@
 {
     fdrive_t *cur_drv;
     uint8_t kh, kt, ks;
-    int did_seek;
 
-    fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
     cur_drv = get_cur_drv(fdctrl);
     kt = fdctrl->fifo[2];
     kh = fdctrl->fifo[3];
     ks = fdctrl->fifo[4];
     FLOPPY_DPRINTF("Start transfer at %d %d %02x %02x (%d)\n",
-                   fdctrl->cur_drv, kh, kt, ks,
+                   fdctrl->dor & FD_DOR_SELMASK, kh, kt, ks,
                    _fd_sector(kh, kt, ks, cur_drv->last_sect));
-    did_seek = 0;
-    switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & 0x40)) {
+    switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & FD_CONFIG_EIS)) {
     case 2:
         /* sect too big */
-        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
+        fdctrl->status0 |= FD_SR0_ABNTERM;
+        fdctrl_stop_transfer(fdctrl);
         fdctrl->fifo[3] = kt;
         fdctrl->fifo[4] = kh;
         fdctrl->fifo[5] = ks;
         return;
     case 3:
         /* track too big */
-        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x80, 0x00);
+        fdctrl->status0 |= FD_SR0_ABNTERM;
+        fdctrl->status1 |= FD_SR1_EC;
+        fdctrl_stop_transfer(fdctrl);
         fdctrl->fifo[3] = kt;
         fdctrl->fifo[4] = kh;
         fdctrl->fifo[5] = ks;
         return;
     case 4:
         /* No seek enabled */
-        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
+        fdctrl->status0 |= FD_SR0_ABNTERM;
+        fdctrl_stop_transfer(fdctrl);
         fdctrl->fifo[3] = kt;
         fdctrl->fifo[4] = kh;
         fdctrl->fifo[5] = ks;
         return;
+    case 0:
     case 1:
-        did_seek = 1;
+        fdctrl->status0 |= FD_SR0_SEEK;
         break;
     default:
         break;
@@ -1130,22 +1162,18 @@
         fdctrl->data_state |= FD_STATE_MULTI;
     else
         fdctrl->data_state &= ~FD_STATE_MULTI;
-    if (did_seek)
-        fdctrl->data_state |= FD_STATE_SEEK;
-    else
-        fdctrl->data_state &= ~FD_STATE_SEEK;
     if (fdctrl->fifo[5] == 00) {
-        fdctrl->data_len = fdctrl->fifo[8];
+        fdctrl->data_len = fdctrl->fifo[7];
     } else {
         int tmp;
         fdctrl->data_len = 128 << (fdctrl->fifo[5] > 7 ? 7 : fdctrl->fifo[5]);
-        tmp = (cur_drv->last_sect - ks + 1);
+        tmp = (fdctrl->fifo[6] - ks + 1);
         if (fdctrl->fifo[0] & 0x80)
-            tmp += cur_drv->last_sect;
+            tmp += fdctrl->fifo[6];
         fdctrl->data_len *= tmp;
     }
     fdctrl->eot = fdctrl->fifo[6];
-    if (fdctrl->dma_en) {
+    if (fdctrl->dor & FD_DOR_DMAEN) {
         int dma_mode;
         /* DMA transfer are enabled. Check if DMA channel is well programmed */
         dma_mode = DMA_get_channel_mode(fdctrl->dma_chann);
@@ -1159,7 +1187,7 @@
             (direction == FD_DIR_WRITE && dma_mode == 2) ||
             (direction == FD_DIR_READ && dma_mode == 1)) {
             /* No access is allowed until DMA transfer has completed */
-            fdctrl->state |= FD_CTRL_BUSY;
+            fdctrl->msr &= ~FD_MSR_RQM;
             /* Now, we just have to wait for the DMA controller to
              * recall us...
              */
@@ -1172,7 +1200,7 @@
     }
     FLOPPY_DPRINTF("start non-DMA transfer\n");
     /* IO based transfer: calculate len */
-    fdctrl_raise_irq(fdctrl, 0x00);
+    fdctrl_raise_irq(fdctrl);
 
     return;
 }
@@ -1183,7 +1211,8 @@
     /* We don't handle deleted data,
      * so we don't return *ANYTHING*
      */
-    fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00);
+    fdctrl->status0 |= FD_SR0_ABNTERM;
+    fdctrl_stop_transfer(fdctrl);
 }
 
 /* handlers for DMA transfers */
@@ -1193,10 +1222,10 @@
     fdctrl_t *fdctrl;
     fdrive_t *cur_drv;
     int len, start_pos, rel_pos;
-    uint8_t status0 = 0x00, status1 = 0x00, status2 = 0x00;
+    uint8_t status2 = 0x00;
 
     fdctrl = opaque;
-    if (!(fdctrl->state & FD_CTRL_BUSY)) {
+    if (fdctrl->msr & FD_MSR_RQM) {
         FLOPPY_DPRINTF("Not in DMA transfer mode !\n");
         return 0;
     }
@@ -1207,10 +1236,8 @@
     if (dma_len > fdctrl->data_len)
         dma_len = fdctrl->data_len;
     if (cur_drv->bs == NULL) {
-        if (fdctrl->data_dir == FD_DIR_WRITE)
-            fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00);
-        else
-            fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
+        fdctrl->status0 |= FD_SR0_ABNTERM;
+        fdctrl_stop_transfer(fdctrl);
         len = 0;
         goto transfer_error;
     }
@@ -1221,7 +1248,7 @@
             len = FD_SECTOR_LEN - rel_pos;
         FLOPPY_DPRINTF("copy %d bytes (%d %d %d) %d pos %d %02x "
                        "(%d-0x%08x 0x%08x)\n", len, dma_len, fdctrl->data_pos,
-                       fdctrl->data_len, fdctrl->cur_drv, cur_drv->head,
+                       fdctrl->data_len, fdctrl->dor & FD_DOR_SELMASK, cur_drv->head,
                        cur_drv->track, cur_drv->sect, fd_sector(cur_drv),
                        fd_sector(cur_drv) * FD_SECTOR_LEN);
         if (fdctrl->data_dir != FD_DIR_WRITE ||
@@ -1248,7 +1275,8 @@
             if (bdrv_write(cur_drv->bs, fd_sector(cur_drv),
                            fdctrl->fifo, 1) < 0) {
                 FLOPPY_ERROR("writting sector %d\n", fd_sector(cur_drv));
-                fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00);
+                fdctrl->status0 |= FD_SR0_ABNTERM;
+                fdctrl_stop_transfer(fdctrl);
                 goto transfer_error;
             }
             break;
@@ -1275,35 +1303,8 @@
         rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;
         if (rel_pos == 0) {
             /* Seek to next sector */
-            FLOPPY_DPRINTF("seek to next sector (%d %02x %02x => %d) (%d)\n",
-                           cur_drv->head, cur_drv->track, cur_drv->sect,
-                           fd_sector(cur_drv),
-                           fdctrl->data_pos - len);
-            /* XXX: cur_drv->sect >= cur_drv->last_sect should be an
-               error in fact */
-            if (cur_drv->sect >= cur_drv->last_sect ||
-                cur_drv->sect == fdctrl->eot) {
-                cur_drv->sect = 1;
-                if (FD_MULTI_TRACK(fdctrl->data_state)) {
-                    if (cur_drv->head == 0 &&
-                        (cur_drv->flags & FDISK_DBL_SIDES) != 0) {
-                        cur_drv->head = 1;
-                    } else {
-                        cur_drv->head = 0;
-                        cur_drv->track++;
-                        if ((cur_drv->flags & FDISK_DBL_SIDES) == 0)
-                            break;
-                    }
-                } else {
-                    cur_drv->track++;
-                    break;
-                }
-                FLOPPY_DPRINTF("seek to next track (%d %02x %02x => %d)\n",
-                               cur_drv->head, cur_drv->track,
-                               cur_drv->sect, fd_sector(cur_drv));
-            } else {
-                cur_drv->sect++;
-            }
+            if (!fdctrl_seek_to_next_sect(fdctrl, cur_drv))
+                break;
         }
     }
  end_transfer:
@@ -1314,11 +1315,9 @@
         fdctrl->data_dir == FD_DIR_SCANL ||
         fdctrl->data_dir == FD_DIR_SCANH)
         status2 = 0x08;
-    if (FD_DID_SEEK(fdctrl->data_state))
-        status0 |= FD_SR0_SEEK;
     fdctrl->data_len -= len;
-    //    if (fdctrl->data_len == 0)
-    fdctrl_stop_transfer(fdctrl, status0, status1, status2);
+    fdctrl->status2 |= status2;
+    fdctrl_stop_transfer(fdctrl);
  transfer_error:
 
     return len;
@@ -1332,7 +1331,7 @@
     int pos, len;
 
     cur_drv = get_cur_drv(fdctrl);
-    fdctrl->state &= ~FD_CTRL_SLEEP;
+    fdctrl->dsr &= ~FD_DSR_PWRDOWN;
     if (FD_STATE(fdctrl->data_state) == FD_STATE_CMD) {
         FLOPPY_ERROR("can't read data in CMD state\n");
         return 0;
@@ -1341,6 +1340,8 @@
     if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
         pos %= FD_SECTOR_LEN;
         if (pos == 0) {
+            if (fdctrl->data_pos != 0)
+                fdctrl_seek_to_next_sect(fdctrl, cur_drv);
             len = fdctrl->data_len - fdctrl->data_pos;
             if (len > FD_SECTOR_LEN)
                 len = FD_SECTOR_LEN;
@@ -1354,7 +1355,7 @@
          * then from status mode to command mode
          */
         if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
-            fdctrl_stop_transfer(fdctrl, FD_SR0_SEEK, 0x00, 0x00);
+            fdctrl_stop_transfer(fdctrl);
         } else {
             fdctrl_reset_fifo(fdctrl);
             fdctrl_reset_irq(fdctrl);
@@ -1369,42 +1370,44 @@
 {
     fdrive_t *cur_drv;
     uint8_t kh, kt, ks;
-    int did_seek;
 
-    fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+    SELECT_FD(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
     cur_drv = get_cur_drv(fdctrl);
     kt = fdctrl->fifo[6];
     kh = fdctrl->fifo[7];
     ks = fdctrl->fifo[8];
     FLOPPY_DPRINTF("format sector at %d %d %02x %02x (%d)\n",
-                   fdctrl->cur_drv, kh, kt, ks,
+                   fdctrl->dor & FD_DOR_SELMASK, kh, kt, ks,
                    _fd_sector(kh, kt, ks, cur_drv->last_sect));
-    did_seek = 0;
     switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & FD_CONFIG_EIS)) {
     case 2:
         /* sect too big */
-        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
+        fdctrl->status0 |= FD_SR0_ABNTERM;
+        fdctrl_stop_transfer(fdctrl);
         fdctrl->fifo[3] = kt;
         fdctrl->fifo[4] = kh;
         fdctrl->fifo[5] = ks;
         return;
     case 3:
         /* track too big */
-        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x80, 0x00);
+        fdctrl->status0 |= FD_SR0_ABNTERM;
+        fdctrl->status1 |= FD_SR1_EC;
+        fdctrl_stop_transfer(fdctrl);
         fdctrl->fifo[3] = kt;
         fdctrl->fifo[4] = kh;
         fdctrl->fifo[5] = ks;
         return;
     case 4:
         /* No seek enabled */
-        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
+        fdctrl->status0 |= FD_SR0_ABNTERM;
+        fdctrl_stop_transfer(fdctrl);
         fdctrl->fifo[3] = kt;
         fdctrl->fifo[4] = kh;
         fdctrl->fifo[5] = ks;
         return;
+    case 0:
     case 1:
-        did_seek = 1;
-        fdctrl->data_state |= FD_STATE_SEEK;
+        fdctrl->status0 |= FD_SR0_SEEK;
         break;
     default:
         break;
@@ -1413,15 +1416,13 @@
     if (cur_drv->bs == NULL ||
         bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1) < 0) {
         FLOPPY_ERROR("formatting sector %d\n", fd_sector(cur_drv));
-        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00);
+        fdctrl->status0 |= FD_SR0_ABNTERM;
+        fdctrl_stop_transfer(fdctrl);
     } else {
         if (cur_drv->sect == cur_drv->last_sect) {
             fdctrl->data_state &= ~FD_STATE_FORMAT;
             /* Last sector done */
-            if (FD_DID_SEEK(fdctrl->data_state))
-                fdctrl_stop_transfer(fdctrl, FD_SR0_SEEK, 0x00, 0x00);
-            else
-                fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
+            fdctrl_stop_transfer(fdctrl);
         } else {
             /* More to do */
             fdctrl->data_pos = 0;
@@ -1433,14 +1434,15 @@
 static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
 {
     fdrive_t *cur_drv;
+    int pos;
 
     cur_drv = get_cur_drv(fdctrl);
     /* Reset mode */
-    if (fdctrl->state & FD_CTRL_RESET) {
+    if (!(fdctrl->dor & FD_DOR_nRESET)) {
         FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
         return;
     }
-    fdctrl->state &= ~FD_CTRL_SLEEP;
+    fdctrl->dsr &= ~FD_DSR_PWRDOWN;
     if (FD_STATE(fdctrl->data_state) == FD_STATE_STATUS) {
         FLOPPY_ERROR("can't write data in status mode\n");
         return;
@@ -1448,16 +1450,19 @@
     /* Is it write command time ? */
     if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
         /* FIFO data write */
-        fdctrl->fifo[fdctrl->data_pos++] = value;
-        if (fdctrl->data_pos % FD_SECTOR_LEN == (FD_SECTOR_LEN - 1) ||
+        pos = fdctrl->data_pos;
+        pos %= FD_SECTOR_LEN;
+        fdctrl->fifo[pos] = value;
+        if (pos == FD_SECTOR_LEN - 1 ||
             fdctrl->data_pos == fdctrl->data_len) {
             bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1);
+            fdctrl_seek_to_next_sect(fdctrl, cur_drv);
         }
         /* Switch from transfer mode to status mode
          * then from status mode to command mode
          */
-        if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA)
-            fdctrl_stop_transfer(fdctrl, FD_SR0_SEEK, 0x00, 0x00);
+        if (++fdctrl->data_pos == fdctrl->data_len)
+            fdctrl_stop_transfer(fdctrl);
         return;
     }
     if (fdctrl->data_pos == 0) {
@@ -1469,6 +1474,12 @@
             /* 8 parameters cmd */
             fdctrl->data_len = 9;
             goto enqueue;
+        case FD_CMD_READ_SECTOR:
+            /* READ_SECTOR variants */
+            FLOPPY_DPRINTF("READ_SECTOR command\n");
+            /* 8 parameters cmd */
+            fdctrl->data_len = 9;
+            goto enqueue;
         case FD_CMD_READ_DELETED:
             /* READ_DELETED variants */
             FLOPPY_DPRINTF("READ_DELETED command\n");
@@ -1509,6 +1520,12 @@
             /* 8 parameters cmd */
             fdctrl->data_len = 9;
             goto enqueue;
+        case FD_CMD_WRITE_SECTOR:
+            /* FD_CMD_WRITE_SECTOR variants */
+            FLOPPY_DPRINTF("FD_CMD_WRITE_SECTOR command\n");
+            /* 8 parameters cmd */
+            fdctrl->data_len = 9;
+            goto enqueue;
         case FD_CMD_WRITE_DELETED:
             /* WRITE_DELETED variants */
             FLOPPY_DPRINTF("WRITE_DELETED command\n");
@@ -1540,22 +1557,14 @@
         case FD_CMD_SENSE_INTERRUPT_STATUS:
             /* SENSE_INTERRUPT_STATUS */
             FLOPPY_DPRINTF("SENSE_INTERRUPT_STATUS command (%02x)\n",
-                           fdctrl->int_status);
+                           fdctrl->status0);
             /* No parameters cmd: returns status if no interrupt */
-#if 0
-            fdctrl->fifo[0] =
-                fdctrl->int_status | (cur_drv->head << 2) | fdctrl->cur_drv;
-#else
-            /* XXX: int_status handling is broken for read/write
-               commands, so we do this hack. It should be suppressed
-               ASAP */
             fdctrl->fifo[0] =
-                0x20 | (cur_drv->head << 2) | fdctrl->cur_drv;
-#endif
+                fdctrl->status0 | (cur_drv->head << 2) | (fdctrl->dor & FD_DOR_SELMASK);
             fdctrl->fifo[1] = cur_drv->track;
             fdctrl_set_fifo(fdctrl, 2, 0);
             fdctrl_reset_irq(fdctrl);
-            fdctrl->int_status = FD_SR0_RDYCHG;
+            fdctrl->status0 |= FD_SR0_RDYCHG;
             return;
         case FD_CMD_DUMPREG:
             /* DUMPREG */
@@ -1563,11 +1572,16 @@
             /* Drives position */
             fdctrl->fifo[0] = drv0(fdctrl)->track;
             fdctrl->fifo[1] = drv1(fdctrl)->track;
+#if MAX_FD == 4
+            fdctrl->fifo[2] = drv2(fdctrl)->track;
+            fdctrl->fifo[3] = drv3(fdctrl)->track;
+#else
             fdctrl->fifo[2] = 0;
             fdctrl->fifo[3] = 0;
+#endif
             /* timers */
             fdctrl->fifo[4] = fdctrl->timer0;
-            fdctrl->fifo[5] = (fdctrl->timer1 << 1) | fdctrl->dma_en;
+            fdctrl->fifo[5] = (fdctrl->timer1 << 1) | (fdctrl->dor & FD_DOR_DMAEN ? 1 : 0);
             fdctrl->fifo[6] = cur_drv->last_sect;
             fdctrl->fifo[7] = (fdctrl->lock << 7) |
                 (cur_drv->perpendicular << 2);
@@ -1631,8 +1645,13 @@
             /* Drives position */
             fdctrl->fifo[2] = drv0(fdctrl)->track;
             fdctrl->fifo[3] = drv1(fdctrl)->track;
+#if MAX_FD == 4
+            fdctrl->fifo[4] = drv2(fdctrl)->track;
+            fdctrl->fifo[5] = drv3(fdctrl)->track;
+#else
             fdctrl->fifo[4] = 0;
             fdctrl->fifo[5] = 0;
+#endif
             /* timers */
             fdctrl->fifo[6] = fdctrl->timer0;
             fdctrl->fifo[7] = fdctrl->timer1;
@@ -1722,18 +1741,19 @@
         /* We now have all parameters
          * and will be able to treat the command
          */
+        fdctrl->status0 = 0;
+        fdctrl->status1 = 0;
+        fdctrl->status2 = 0;
         if (fdctrl->data_state & FD_STATE_FORMAT) {
             fdctrl_format_sector(fdctrl);
             return;
         }
         switch (fdctrl->fifo[0] & 0x1F) {
         case FD_CMD_READ & 0x1F:
-            {
-                /* READ variants */
-                FLOPPY_DPRINTF("treat READ command\n");
-                fdctrl_start_transfer(fdctrl, FD_DIR_READ);
-                return;
-            }
+            /* READ variants */
+            FLOPPY_DPRINTF("treat READ command\n");
+            fdctrl_start_transfer(fdctrl, FD_DIR_READ);
+            return;
         case FD_CMD_READ_DELETED & 0x1F:
             /* READ_DELETED variants */
 //            FLOPPY_DPRINTF("treat READ_DELETED command\n");
@@ -1744,7 +1764,7 @@
             /* VERIFY variants */
 //            FLOPPY_DPRINTF("treat VERIFY command\n");
             FLOPPY_ERROR("treat VERIFY command\n");
-            fdctrl_stop_transfer(fdctrl, FD_SR0_SEEK, 0x00, 0x00);
+            fdctrl_stop_transfer(fdctrl);
             return;
         case FD_CMD_SCAN_EQUAL & 0x1F:
             /* SCAN_EQUAL variants */
@@ -1773,6 +1793,11 @@
             FLOPPY_DPRINTF("treat WRITE command (%02x)\n", fdctrl->fifo[0]);
             fdctrl_start_transfer(fdctrl, FD_DIR_WRITE);
             return;
+        case FD_CMD_WRITE_SECTOR & 0x3F:
+            /* WRITE_SECTOR variants */
+            FLOPPY_DPRINTF("treat WRITE_SECTOR command (%02x)\n", fdctrl->fifo[0]);
+            fdctrl_start_transfer(fdctrl, FD_DIR_WRITE);
+            return;
         case FD_CMD_WRITE_DELETED & 0x3F:
             /* WRITE_DELETED variants */
 //            FLOPPY_DPRINTF("treat WRITE_DELETED command\n");
@@ -1788,52 +1813,55 @@
             FLOPPY_DPRINTF("treat SPECIFY command\n");
             fdctrl->timer0 = (fdctrl->fifo[1] >> 4) & 0xF;
             fdctrl->timer1 = fdctrl->fifo[2] >> 1;
-            fdctrl->dma_en = 1 - (fdctrl->fifo[2] & 1) ;
+            if (fdctrl->fifo[2] & 1)
+                fdctrl->dor &= ~FD_DOR_DMAEN;
+            else
+                fdctrl->dor |= FD_DOR_DMAEN;
             /* No result back */
             fdctrl_reset_fifo(fdctrl);
             break;
         case FD_CMD_SENSE_DRIVE_STATUS:
             /* SENSE_DRIVE_STATUS */
             FLOPPY_DPRINTF("treat SENSE_DRIVE_STATUS command\n");
-            fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+            SELECT_FD(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
             cur_drv = get_cur_drv(fdctrl);
             cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
             /* 1 Byte status back */
             fdctrl->fifo[0] = (cur_drv->ro << 6) |
                 (cur_drv->track == 0 ? 0x10 : 0x00) |
                 (cur_drv->head << 2) |
-                fdctrl->cur_drv |
+                (fdctrl->dor & FD_DOR_SELMASK) |
                 0x28;
             fdctrl_set_fifo(fdctrl, 1, 0);
             break;
         case FD_CMD_RECALIBRATE:
             /* RECALIBRATE */
             FLOPPY_DPRINTF("treat RECALIBRATE command\n");
-            fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+            SELECT_FD(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
             cur_drv = get_cur_drv(fdctrl);
             fd_recalibrate(cur_drv);
             fdctrl_reset_fifo(fdctrl);
             /* Raise Interrupt */
-            fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
+            fdctrl->status0 |= FD_SR0_SEEK;
+            fdctrl_raise_irq(fdctrl);
             break;
         case FD_CMD_SEEK:
             /* SEEK */
             FLOPPY_DPRINTF("treat SEEK command\n");
-            fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+            SELECT_FD(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
             cur_drv = get_cur_drv(fdctrl);
-            fd_start(cur_drv);
             if (fdctrl->fifo[2] <= cur_drv->track)
                 cur_drv->dir = 1;
             else
                 cur_drv->dir = 0;
             fdctrl_reset_fifo(fdctrl);
-            if (fdctrl->fifo[2] > cur_drv->max_track) {
-                fdctrl_raise_irq(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK);
-            } else {
+            fdctrl->status0 |= FD_SR0_SEEK;
+            if (fdctrl->fifo[2] > cur_drv->max_track)
+                fdctrl->status0 |= FD_SR0_ABNTERM;
+            else
                 cur_drv->track = fdctrl->fifo[2];
-                /* Raise Interrupt */
-                fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
-            }
+            /* Raise Interrupt */
+            fdctrl_raise_irq(fdctrl);
             break;
         case FD_CMD_PERPENDICULAR_MODE:
             /* PERPENDICULAR_MODE */
@@ -1884,6 +1912,10 @@
             /* Drives position */
             drv0(fdctrl)->track = fdctrl->fifo[3];
             drv1(fdctrl)->track = fdctrl->fifo[4];
+#if MAX_FD == 4
+            drv2(fdctrl)->track = fdctrl->fifo[5];
+            drv3(fdctrl)->track = fdctrl->fifo[6];
+#endif
             /* timers */
             fdctrl->timer0 = fdctrl->fifo[7];
             fdctrl->timer1 = fdctrl->fifo[8];
@@ -1898,14 +1930,13 @@
         case FD_CMD_FORMAT_TRACK:
             /* FORMAT_TRACK */
             FLOPPY_DPRINTF("treat FORMAT_TRACK command\n");
-            fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+            SELECT_FD(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
             cur_drv = get_cur_drv(fdctrl);
             fdctrl->data_state |= FD_STATE_FORMAT;
             if (fdctrl->fifo[0] & 0x80)
                 fdctrl->data_state |= FD_STATE_MULTI;
             else
                 fdctrl->data_state &= ~FD_STATE_MULTI;
-            fdctrl->data_state &= ~FD_STATE_SEEK;
             cur_drv->bps =
                 fdctrl->fifo[2] > 7 ? 16384 : 128 << fdctrl->fifo[2];
 #if 0
@@ -1920,7 +1951,7 @@
              * the sector with the specified fill byte
              */
             fdctrl->data_state &= ~FD_STATE_FORMAT;
-            fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
+            fdctrl_stop_transfer(fdctrl);
             break;
         case FD_CMD_DRIVE_SPECIFICATION_COMMAND:
             /* DRIVE_SPECIFICATION_COMMAND */
@@ -1938,16 +1969,15 @@
             } else if (fdctrl->data_len > 7) {
                 /* ERROR */
                 fdctrl->fifo[0] = 0x80 |
-                    (cur_drv->head << 2) | fdctrl->cur_drv;
+                    (cur_drv->head << 2) | (fdctrl->dor & FD_DOR_SELMASK);
                 fdctrl_set_fifo(fdctrl, 1, 1);
             }
             break;
         case FD_CMD_RELATIVE_SEEK_OUT:
             /* RELATIVE_SEEK_OUT */
             FLOPPY_DPRINTF("treat RELATIVE_SEEK_OUT command\n");
-            fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+            SELECT_FD(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
             cur_drv = get_cur_drv(fdctrl);
-            fd_start(cur_drv);
             cur_drv->dir = 0;
             if (fdctrl->fifo[2] + cur_drv->track >= cur_drv->max_track) {
                 cur_drv->track = cur_drv->max_track - 1;
@@ -1955,7 +1985,8 @@
                 cur_drv->track += fdctrl->fifo[2];
             }
             fdctrl_reset_fifo(fdctrl);
-            fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
+            fdctrl->status0 |= FD_SR0_SEEK;
+            fdctrl_raise_irq(fdctrl);
             break;
         case FD_CMD_FORMAT_AND_WRITE:
             /* FORMAT_AND_WRITE */
@@ -1966,9 +1997,8 @@
         case FD_CMD_RELATIVE_SEEK_IN:
             /* RELATIVE_SEEK_IN */
             FLOPPY_DPRINTF("treat RELATIVE_SEEK_IN command\n");
-            fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+            SELECT_FD(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
             cur_drv = get_cur_drv(fdctrl);
-            fd_start(cur_drv);
             cur_drv->dir = 1;
             if (fdctrl->fifo[2] > cur_drv->track) {
                 cur_drv->track = 0;
@@ -1976,8 +2006,9 @@
                 cur_drv->track -= fdctrl->fifo[2];
             }
             fdctrl_reset_fifo(fdctrl);
+            fdctrl->status0 |= FD_SR0_SEEK;
             /* Raise Interrupt */
-            fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
+            fdctrl_raise_irq(fdctrl);
             break;
         }
     }
@@ -1995,5 +2026,5 @@
     if (cur_drv->last_sect != 0) {
         cur_drv->sect = (cur_drv->sect % cur_drv->last_sect) + 1;
     }
-    fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
+    fdctrl_stop_transfer(fdctrl);
 }