FDC: improve emulation

PATCH In review Hervé Poussineau 2 participants 6 messages Apr 16, 2008 – Apr 23, 2008

lore

Share & tools

lore

Hervé Poussineau

author

2008-04-16 19:43 UTC #0 lore raw

PATCH 1/1 FDC: improve emulation

Hi,

I've done big improvements on floppy drive controller.
For easier reviewing, I've tried to split them up in differents parts. Sorry if some parts are still big. Each patch applies on top of the previous one.

If only some of them are accepted, I can provide a global patch for only these ones. Patches which needs more work will be resubmitted later.

List of attached patches:

fdc_01_lookuptable.diff
- Adds a command lookup table, as suggested by Fabrice at http://lists.gnu.org/archive/html/qemu-devel/2008-04/msg00143.html
- This also moves initialization functions at the bottom of the file to prevent multiple forward declarations.

fdc_02_seek_next_sector.diff
- Extract seeking to next sector handling in a function. Add a sector seek in PIO read and write modes

fdc_03_status_ab.diff
- Fixes status A and status B registers. It removes one Sun4m mutation. Also removes the internal FD_CTRL_INTR flag.

fdc_04_status_012.diff
- Fixes status0, status1 and status2 handling, which were broken in read/write commands and in some subtle ways. Status are now calculated during command execution and not at the very end.
- This allows removing of one hack in fdctrl_handle_sense_interrupt_status().

fdc_05_dmaen.diff
- Handles correctly FD_MSR_NONDMA/FD_DOR_NONDMA flags, and uses them when possible. Fixes a problem with SPECIFY command. This allows removing of last Sun4m mutation.

fdc_06_dor_reg.diff
- Better handling of DOR register. DOR register drives external motors, but it not limited to existing drives.
- Use FD_DOR_nRESET flag instead of internal FD_CTRL_RESET flag.
- Support writing to DOR register even in reset mode (as said in specification)

fdc_07_msr_reg.diff
- Stores controller state in MSR register instead of internal state field. This simplifies the fdctrl_read_main_status() function, which may be called in some tight loops.

fdc_08_cleanup.diff
- Removes useless fields in fdrive_t structure.
- Adds a message when bdrv_read/bdrv_write calls fail.

fdc_09_tdr_reg.diff
- Replaces bootsel field by the whole tdr register. It may be easier if we want to later add support for tapes.

fdc_10_4_fds.diff
- Supports up to 4 floppy drives if MAX_FD is set to 4.

fdc_11_cur_drv.diff
- Replaces access to cur_drv field by macros.

fdc_12_pio_write.diff
- Fixes transfer data length.
- Fixes buffer overflow in PIO mode write.

Floppy controller works better than before in Sun4m. It now gets READ commands and correctly detects if floppy is write-protected or not. Unfortunately, no valid filesystem seems to be found on the floppy.

Please comment.

Hervé

fdc.c +9 -7

Index: hw/fdc.c
===================================================================
@@ -1117,13 +1117,13 @@
     else
         fdctrl->data_state &= ~FD_STATE_MULTI;
     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];

@@ -1335,7 +1335,7 @@
     kh = fdctrl->fifo[7];
     ks = fdctrl->fifo[8];
     FLOPPY_DPRINTF("format sector at %d %d %02x %02x (%d)\n",
-                   GET_CUR_DRV(fdctrl->dor), kh, kt, ks,
+                   GET_CUR_DRV(fdctrl), kh, kt, ks,
                    _fd_sector(kh, kt, ks, cur_drv->last_sect));
     switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & FD_CONFIG_EIS)) {
     case 2:

@@ -1643,7 +1643,7 @@
     } else if (fdctrl->data_len > 7) {
         /* ERROR */
         fdctrl->fifo[0] = 0x80 |
-            (cur_drv->head << 2) | GET_CUR_DRV(fdctrl->dor);
+            (cur_drv->head << 2) | GET_CUR_DRV(fdctrl);
         fdctrl_set_fifo(fdctrl, 1, 1);
     }
 }

@@ -1743,8 +1743,10 @@
     /* Is it write command time ? */
     if (fdctrl->msr & FD_MSR_NONDMA) {
         /* 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) {
             cur_drv = get_cur_drv(fdctrl);
             if (bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1) < 0) {

fdc.c +148 ~~-149~~

343 lines not on this page · raw message

fdc.c +48 ~~-33~~

111 lines not on this page · raw message

fdc.c +75 ~~-16~~

176 lines not on this page · raw message

fdc.c +105 ~~-89~~

502 lines not on this page · raw message

fdc.c +30 ~~-36~~

211 lines not on this page · raw message

fdc.c +13 ~~-76~~

218 lines not on this page · raw message

fdc.c +33 ~~-55~~

241 lines not on this page · raw message

fdc.c +22 ~~-38~~

170 lines not on this page · raw message

fdc.c +7 ~~-10~~

Index: hw/fdc.c
===================================================================
@@ -460,10 +460,10 @@
     uint8_t sra;
     uint8_t srb;
     uint8_t dor;
+    uint8_t tdr;
     uint8_t dsr;
     uint8_t msr;
     uint8_t cur_drv;
-    uint8_t bootsel;
     uint8_t status0;
     uint8_t status1;
     uint8_t status2;

@@ -598,10 +598,10 @@
     qemu_put_8s(f, &s->sra);
     qemu_put_8s(f, &s->srb);
     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_8s(f, &s->cur_drv);
-    qemu_put_8s(f, &s->bootsel);
     qemu_put_8s(f, &s->status0);
     qemu_put_8s(f, &s->status1);
     qemu_put_8s(f, &s->status2);

@@ -644,10 +644,10 @@
     qemu_get_8s(f, &s->sra);
     qemu_get_8s(f, &s->srb);
     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_8s(f, &s->cur_drv);
-    qemu_get_8s(f, &s->bootsel);
     qemu_get_8s(f, &s->status0);
     qemu_get_8s(f, &s->status1);
     qemu_get_8s(f, &s->status2);

@@ -747,12 +747,12 @@
 
 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];
+    return &fdctrl->drives[1 - ((fdctrl->tdr & FD_TDR_BOOTSEL) >> 2)];
 }
 
 static fdrive_t *get_cur_drv (fdctrl_t *fdctrl)

@@ -833,11 +833,8 @@
 /* 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;

@@ -852,7 +849,7 @@
     }
     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 */
 }

fdc.c +81 ~~-13~~

184 lines not on this page · raw message

fdc.c +22 ~~-19~~

149 lines not on this page · raw message

Blue Swirl

2008-04-18 19:26 UTC #1 lore raw

Quoted 64 lines · Hervé Poussineau

Hi,
I've done big improvements on floppy drive controller.
For easier reviewing, I've tried to split them up in differents parts.
Sorry if some parts are still big.
Each patch applies on top of the previous one.
If only some of them are accepted, I can provide a global patch for only
these ones. Patches which needs more work will be resubmitted later.
List of attached patches:
fdc_01_lookuptable.diff
- Adds a command lookup table, as suggested by Fabrice at
http://lists.gnu.org/archive/html/qemu-devel/2008-04/msg00143.html
- This also moves initialization functions at the bottom of the file to
prevent multiple forward declarations.
fdc_02_seek_next_sector.diff
- Extract seeking to next sector handling in a function. Add a sector seek
in PIO read and write modes
fdc_03_status_ab.diff
- Fixes status A and status B registers. It removes one Sun4m mutation.
Also removes the internal FD_CTRL_INTR flag.
fdc_04_status_012.diff
- Fixes status0, status1 and status2 handling, which were broken in
read/write commands and in some subtle ways. Status are now calculated during command execution and not at the very end.
- This allows removing of one hack in
fdctrl_handle_sense_interrupt_status().
fdc_05_dmaen.diff
- Handles correctly FD_MSR_NONDMA/FD_DOR_NONDMA flags, and uses them when
possible. Fixes a problem with SPECIFY command. This allows removing of last Sun4m mutation.
fdc_06_dor_reg.diff
- Better handling of DOR register. DOR register drives external motors, but
it not limited to existing drives.
- Use FD_DOR_nRESET flag instead of internal FD_CTRL_RESET flag.
- Support writing to DOR register even in reset mode (as said in
specification)
fdc_07_msr_reg.diff
- Stores controller state in MSR register instead of internal state field.
This simplifies the fdctrl_read_main_status() function, which may be called in some tight loops.
fdc_08_cleanup.diff
- Removes useless fields in fdrive_t structure.
- Adds a message when bdrv_read/bdrv_write calls fail.
fdc_09_tdr_reg.diff
- Replaces bootsel field by the whole tdr register. It may be easier if we
want to later add support for tapes.
fdc_10_4_fds.diff
- Supports up to 4 floppy drives if MAX_FD is set to 4.
fdc_11_cur_drv.diff
- Replaces access to cur_drv field by macros.

Quoted 7 lines

fdc_12_pio_write.diff
- Fixes transfer data length.
- Fixes buffer overflow in PIO mode write.
Floppy controller works better than before in Sun4m. It now gets READ
commands and correctly detects if floppy is write-protected or not. Unfortunately, no valid filesystem seems to be found on the floppy.

Maybe the TC signal should be used (fdctrl_handle_tc)? I didn't know what to do with it, but Linux uses it when transferring data.

Blue Swirl

2008-04-22 15:17 UTC #2 lore raw

In reply to Hervé Poussineau

Quoted 10 lines · Hervé Poussineau

Hi,
I've done big improvements on floppy drive controller.
For easier reviewing, I've tried to split them up in differents parts.
Sorry if some parts are still big.
Each patch applies on top of the previous one.
If only some of them are accepted, I can provide a global patch for only
these ones. Patches which needs more work will be resubmitted later.

Quoted 7 lines

List of attached patches:
fdc_01_lookuptable.diff
- Adds a command lookup table, as suggested by Fabrice at
http://lists.gnu.org/archive/html/qemu-devel/2008-04/msg00143.html
- This also moves initialization functions at the bottom of the file to
prevent multiple forward declarations.

The command_to_handler table size should be 256.

Quoted 3 lines

fdc_02_seek_next_sector.diff
- Extract seeking to next sector handling in a function. Add a sector seek
in PIO read and write modes

Looks OK.

Quoted 3 lines

fdc_03_status_ab.diff
- Fixes status A and status B registers. It removes one Sun4m mutation.
Also removes the internal FD_CTRL_INTR flag.

OK.

Quoted 6 lines

fdc_04_status_012.diff
- Fixes status0, status1 and status2 handling, which were broken in
read/write commands and in some subtle ways. Status are now calculated during command execution and not at the very end.
- This allows removing of one hack in
fdctrl_handle_sense_interrupt_status().

Not good, this makes Sparc64 print
SENSEI c0 00
SENSEI c0 00
forever.

Quoted 4 lines

fdc_05_dmaen.diff
- Handles correctly FD_MSR_NONDMA/FD_DOR_NONDMA flags, and uses them when
possible. Fixes a problem with SPECIFY command. This allows removing of last Sun4m mutation.

I'd still preserve the comment, because it's still true even if there are no more mutations. On Sun4m DMAGATE bit doesn't control DMA but enables interrupts, shouldn't that be kept?

Quoted 6 lines

fdc_06_dor_reg.diff
- Better handling of DOR register. DOR register drives external motors, but
it not limited to existing drives.
- Use FD_DOR_nRESET flag instead of internal FD_CTRL_RESET flag.
- Support writing to DOR register even in reset mode (as said in
specification)

No idea.

Quoted 4 lines

fdc_07_msr_reg.diff
- Stores controller state in MSR register instead of internal state field.
This simplifies the fdctrl_read_main_status() function, which may be called in some tight loops.

          */
-        if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA)
+        if (++fdctrl->data_pos == fdctrl->data_len)
             fdctrl_stop_transfer(fdctrl);

Would it be better to make the test >=?

Quoted 3 lines

fdc_08_cleanup.diff
- Removes useless fields in fdrive_t structure.
- Adds a message when bdrv_read/bdrv_write calls fail.

The version number could be handy if some machine wants a specific type of controller, I'd preserve it.

Quoted 3 lines

fdc_09_tdr_reg.diff
- Replaces bootsel field by the whole tdr register. It may be easier if we
want to later add support for tapes.

OK.

Quoted 2 lines

fdc_10_4_fds.diff
- Supports up to 4 floppy drives if MAX_FD is set to 4.

OK.

Quoted 2 lines

fdc_11_cur_drv.diff
- Replaces access to cur_drv field by macros.

OK.

Quoted 3 lines

fdc_12_pio_write.diff
- Fixes transfer data length.
- Fixes buffer overflow in PIO mode write.

The CUR_DRV changes could be merged to 11, other look OK. I'd separate the buffer overflow fix so that it can be applied to the stable version.

Hervé Poussineau

author

2008-04-23 10:10 UTC #3 lore raw

Re: [Qemu-devel] FDC: improve emulation [v2]

Hi,

First, Blue Swirl, thanks for the review.

Quoted 8 lines · Blue Swirl

Quoted 7 lines · Hervé Poussineau
I've done big improvements on floppy drive controller.
For easier reviewing, I've tried to split them up in differents parts.
Sorry if some parts are still big.
Each patch applies on top of the previous one.
If only some of them are accepted, I can provide a global patch for only
these ones. Patches which needs more work will be resubmitted later.

Here is a global patch after your comments.

Changelog:
- Adds a command lookup table, as suggested by Fabrice at http://lists.gnu.org/archive/html/qemu-devel/2008-04/msg00143.html
- This also moves initialization functions at the bottom of the file to prevent multiple forward declarations.
- Extract seeking to next sector handling in a function. Add a sector seek in PIO read and write modes
- Fixes status A and status B registers. It removes one Sun4m mutation. Also removes the internal FD_CTRL_INTR flag.
- Handles correctly FD_MSR_NONDMA/FD_DOR_NONDMA flags, and uses them when possible. Fixes a problem with SPECIFY command.
- Better handling of DOR register. DOR register shouldn't forget motor states for non-existing drives (drives 2 and 3), even if no physical pin for it exists.
- Use FD_DOR_nRESET flag instead of internal FD_CTRL_RESET flag.
- Support writing to DOR register even in reset mode (as said in specification)
- Stores controller state in MSR register instead of internal state field. This simplifies the fdctrl_read_main_status() function, which may be called in some tight loops.
- Replaces bootsel field by the whole tdr register. It may be easier if we want to later add support for tapes.
- Supports up to 4 floppy drives if MAX_FD is set to 4.
- Replaces access to cur_drv field by macros.

Changes from v1:
- patch 01: command_to_handler table size is now 256 (instead of 255)
- patch 04: not included in this serie
- patch 05: comment about Sun4m differences kept
- patch 07: fixed data_pos incrementation
- patch 08: version field kept
- patch 11: fixed compilation
- patch 12: not included in this serie

I'll rework patch 4 and send later patch 12, once this one is committed.

Quoted 6 lines

Quoted 1 line
fdc_04_status_012.diff
Not good, this makes Sparc64 print
SENSEI c0 00
SENSEI c0 00
forever.

I've removed patch 04 from this serie, so this shouldn't happen anymore. Blue Swirl, can you still please tell me how to reproduce the problem, so I can take a look?

Thanks,

Hervé

fdc.c +508 ~~-438~~

Index: hw/fdc.c
===================================================================
@@ -49,6 +49,9 @@
 /********************************************************/
 /* Floppy drive emulation                               */
 
+#define GET_CUR_DRV(fdctrl) ((fdctrl)->cur_drv)
+#define SET_CUR_DRV(fdctrl, drive) ((fdctrl)->cur_drv = (drive))
+
 /* Will always be a fixed parameter for us */
 #define FD_SECTOR_LEN 512
 #define FD_SECTOR_SC  2   /* Sector size code */

@@ -69,10 +72,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;

@@ -81,15 +80,11 @@
     BlockDriverState *bs;
     /* Drive status */
     fdrive_type_t drive;
-    fdrive_flags_t drflags;
     uint8_t perpendicular;    /* 2.88 MB access mode    */
     /* Position */
     uint8_t head;
     uint8_t track;
     uint8_t sect;
-    /* Last operation status */
-    uint8_t dir;              /* Direction              */
-    uint8_t rw;               /* Read/write             */
     /* Media */
     fdisk_flags_t flags;
     uint8_t last_sect;        /* Nb sector per track    */

@@ -103,7 +98,6 @@
     /* Drive */
     drv->bs = bs;
     drv->drive = FDRIVE_DRV_NONE;
-    drv->drflags = 0;
     drv->perpendicular = 0;
     /* Disk */
     drv->last_sect = 0;

@@ -122,6 +116,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)
 {

@@ -170,8 +171,6 @@
     drv->head = 0;
     drv->track = 0;
     drv->sect = 1;
-    drv->dir = 1;
-    drv->rw = 0;
 }
 
 /* Recognize floppy formats */

@@ -296,24 +295,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          */

@@ -321,9 +302,9 @@
 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_result_timer(void *opaque);
+static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status0);
 
+static uint32_t fdctrl_read_statusA (fdctrl_t *fdctrl);
 static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl);
 static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl);
 static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value);

@@ -336,14 +317,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,

@@ -352,18 +325,14 @@
 };
 
 enum {
-    FD_STATE_CMD    = 0x00,
-    FD_STATE_STATUS = 0x01,
-    FD_STATE_DATA   = 0x02,
-    FD_STATE_STATE  = 0x03,
-    FD_STATE_MULTI  = 0x10,
-    FD_STATE_SEEK   = 0x20,
-    FD_STATE_FORMAT = 0x40,
+    FD_STATE_MULTI  = 0x01,	/* multi track flag */
+    FD_STATE_FORMAT = 0x02,	/* format flag */
+    FD_STATE_SEEK   = 0x04,	/* seek flag */
 };
 
 enum {
-    FD_REG_0 = 0x00,
-    FD_REG_STATUSB = 0x01,
+    FD_REG_SRA = 0x00,
+    FD_REG_SRB = 0x01,
     FD_REG_DOR = 0x02,
     FD_REG_TDR = 0x03,
     FD_REG_MSR = 0x04,

@@ -422,7 +391,40 @@
 };
 
 enum {
+    FD_SR1_EC       = 0x80, /* End of cylinder */
+};
+
+enum {
+    FD_SR2_SNS      = 0x04, /* Scan not satisfied */
+    FD_SR2_SEH      = 0x08, /* Scan equal hit */
+};
+
+enum {
+    FD_SRA_DIR      = 0x01,
+    FD_SRA_nWP      = 0x02,
+    FD_SRA_nINDX    = 0x04,
+    FD_SRA_HDSEL    = 0x08,
+    FD_SRA_nTRK0    = 0x10,
+    FD_SRA_STEP     = 0x20,
+    FD_SRA_nDRV2    = 0x40,
+    FD_SRA_INTPEND  = 0x80,
+};
+
+enum {
+    FD_SRB_MTR0     = 0x01,
+    FD_SRB_MTR1     = 0x02,
+    FD_SRB_WGATE    = 0x04,
+    FD_SRB_RDATA    = 0x08,
+    FD_SRB_WDATA    = 0x10,
+    FD_SRB_DR0      = 0x20,
+};
+
+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 +434,11 @@
 };
 
 enum {
+#if MAX_FD == 4
     FD_TDR_BOOTSEL  = 0x0c,
+#else
+    FD_TDR_BOOTSEL  = 0x04,
+#endif
 };
 
 enum {

@@ -456,15 +462,11 @@
     FD_DIR_DSKCHG   = 0x80,
 };
 
-#define FD_STATE(state) ((state) & FD_STATE_STATE)
-#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 {
-    fdctrl_t *fdctrl;
     /* Controller's identification */
     uint8_t version;
     /* HW */

@@ -473,17 +475,22 @@
     target_phys_addr_t io_base;
     /* Controller state */
     QEMUTimer *result_timer;
-    uint8_t state;
-    uint8_t dma_en;
+    uint8_t sra;
+    uint8_t srb;
+    uint8_t dor;
+    uint8_t tdr;
+    uint8_t dsr;
+    uint8_t msr;
     uint8_t cur_drv;
-    uint8_t bootsel;
+    uint8_t status0;
+    uint8_t status1;
+    uint8_t status2;
     /* 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 eot; /* last wanted sector */
     /* States kept only to be returned back */
     /* Timers state */

@@ -498,7 +505,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)

@@ -507,15 +514,10 @@
     uint32_t retval;
 
     switch (reg & 0x07) {
-    case FD_REG_0:
-        if (fdctrl->sun4m) {
-            // Identify to Linux as S82078B
-            retval = fdctrl_read_statusB(fdctrl);
-        } else {
-            retval = (uint32_t)(-1);
-        }
+    case FD_REG_SRA:
+        retval = fdctrl_read_statusA(fdctrl);
         break;
-    case FD_REG_STATUSB:
+    case FD_REG_SRB:
         retval = fdctrl_read_statusB(fdctrl);
         break;
     case FD_REG_DOR:

@@ -603,53 +605,54 @@
 
 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);
-    qemu_put_8s(f, &fd->dir);
-    qemu_put_8s(f, &fd->rw);
 }
 
 static void fdc_save (QEMUFile *f, void *opaque)
 {
     fdctrl_t *s = opaque;
+    uint8_t tmp;
+    int i;
+    uint8_t dor = s->dor | GET_CUR_DRV(s);
 
-    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);
+    /* Controller state */
+    qemu_put_8s(f, &s->sra);
+    qemu_put_8s(f, &s->srb);
+    qemu_put_8s(f, &dor);
+    qemu_put_8s(f, &s->tdr);
+    qemu_put_8s(f, &s->dsr);
+    qemu_put_8s(f, &s->msr);
+    qemu_put_8s(f, &s->status0);
+    qemu_put_8s(f, &s->status1);
+    qemu_put_8s(f, &s->status2);
+    /* Command FIFO */
     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->eot);
+    /* States kept only to be returned back */
     qemu_put_8s(f, &s->timer0);
     qemu_put_8s(f, &s->timer1);
     qemu_put_8s(f, &s->precomp_trk);
     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);
-    qemu_get_8s(f, &fd->dir);
-    qemu_get_8s(f, &fd->rw);
 
     return 0;
 }

@@ -657,33 +660,49 @@
 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);
+    /* Controller state */
+    qemu_get_8s(f, &s->sra);
+    qemu_get_8s(f, &s->srb);
+    qemu_get_8s(f, &s->dor);
+    SET_CUR_DRV(s, s->dor & FD_DOR_SELMASK);
+    s->dor &= ~FD_DOR_SELMASK;
+    qemu_get_8s(f, &s->tdr);
+    qemu_get_8s(f, &s->dsr);
+    qemu_get_8s(f, &s->msr);
+    qemu_get_8s(f, &s->status0);
+    qemu_get_8s(f, &s->status1);
+    qemu_get_8s(f, &s->status2);
+    /* Command FIFO */
     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->eot);
+    /* States kept only to be returned back */
     qemu_get_8s(f, &s->timer0);
     qemu_get_8s(f, &s->timer1);
     qemu_get_8s(f, &s->precomp_trk);
     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;
 }

@@ -694,78 +713,6 @@
     fdctrl_reset(s, 0);
 }
 
-static fdctrl_t *fdctrl_init_common (qemu_irq irq, int dma_chann,
-                                     target_phys_addr_t io_base,
-                                     BlockDriverState **fds)
-{
-    fdctrl_t *fdctrl;
-    int i;
-
-    FLOPPY_DPRINTF("init controller\n");
-    fdctrl = qemu_mallocz(sizeof(fdctrl_t));
-    if (!fdctrl)
-        return NULL;
-    fdctrl->fifo = qemu_memalign(512, FD_SECTOR_LEN);
-    if (fdctrl->fifo == NULL) {
-        qemu_free(fdctrl);
-        return NULL;
-    }
-    fdctrl->result_timer = qemu_new_timer(vm_clock,
-                                          fdctrl_result_timer, fdctrl);
-
-    fdctrl->version = 0x90; /* Intel 82078 controller */
-    fdctrl->irq = irq;
-    fdctrl->dma_chann = dma_chann;
-    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;
-        DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl);
-    } else {
-        fdctrl->dma_en = 0;
-    }
-    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);
-    qemu_register_reset(fdctrl_external_reset, fdctrl);
-    for (i = 0; i < MAX_FD; i++) {
-        fd_revalidate(&fdctrl->drives[i]);
-    }
-
-    return fdctrl;
-}
-
-fdctrl_t *fdctrl_init (qemu_irq irq, int dma_chann, int mem_mapped,
-                       target_phys_addr_t io_base,
-                       BlockDriverState **fds)
-{
-    fdctrl_t *fdctrl;
-    int io_mem;
-
-    fdctrl = fdctrl_init_common(irq, dma_chann, io_base, fds);
-
-    fdctrl->sun4m = 0;
-    if (mem_mapped) {
-        io_mem = cpu_register_io_memory(0, fdctrl_mem_read, fdctrl_mem_write,
-                                        fdctrl);
-        cpu_register_physical_memory(io_base, 0x08, io_mem);
-    } else {
-        register_ioport_read((uint32_t)io_base + 0x01, 5, 1, &fdctrl_read,
-                             fdctrl);
-        register_ioport_read((uint32_t)io_base + 0x07, 1, 1, &fdctrl_read,
-                             fdctrl);
-        register_ioport_write((uint32_t)io_base + 0x01, 5, 1, &fdctrl_write,
-                              fdctrl);
-        register_ioport_write((uint32_t)io_base + 0x07, 1, 1, &fdctrl_write,
-                              fdctrl);
-    }
-
-    return fdctrl;
-}
-
 static void fdctrl_handle_tc(void *opaque, int irq, int level)
 {
     //fdctrl_t *s = opaque;

@@ -776,23 +723,6 @@
     }
 }
 
-fdctrl_t *sun4m_fdctrl_init (qemu_irq irq, target_phys_addr_t io_base,
-                             BlockDriverState **fds, qemu_irq *fdc_tc)
-{
-    fdctrl_t *fdctrl;
-    int io_mem;
-
-    fdctrl = fdctrl_init_common(irq, 0, io_base, fds);
-    fdctrl->sun4m = 1;
-    io_mem = cpu_register_io_memory(0, fdctrl_mem_read_strict,
-                                    fdctrl_mem_write_strict,
-                                    fdctrl);
-    cpu_register_physical_memory(io_base, 0x08, io_mem);
-    *fdc_tc = *qemu_allocate_irqs(fdctrl_handle_tc, fdctrl, 1);
-
-    return fdctrl;
-}
-
 /* XXX: may change if moved to bdrv */
 int fdctrl_get_drive_type(fdctrl_t *fdctrl, int drive_num)
 {

@@ -802,25 +732,28 @@
 /* Change IRQ state */
 static void fdctrl_reset_irq (fdctrl_t *fdctrl)
 {
+    if (!(fdctrl->sra & FD_SRA_INTPEND))
+        return;
     FLOPPY_DPRINTF("Reset interrupt\n");
     qemu_set_irq(fdctrl->irq, 0);
-    fdctrl->state &= ~FD_CTRL_INTR;
+    fdctrl->sra &= ~FD_SRA_INTPEND;
 }
 
-static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status)
+static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status0)
 {
-    // Sparc mutation
-    if (fdctrl->sun4m && !fdctrl->dma_en) {
-        fdctrl->state &= ~FD_CTRL_BUSY;
-        fdctrl->int_status = status;
+    /* Sparc mutation */
+    if (fdctrl->sun4m && (fdctrl->msr & FD_MSR_CMDBUSY)) {
+        fdctrl->msr &= ~FD_MSR_CMDBUSY;
+        fdctrl->msr |= FD_MSR_RQM | FD_MSR_DIO; /* XXX: not sure */
+        fdctrl->status0 = status0;
         return;
     }
-    if (~(fdctrl->state & FD_CTRL_INTR)) {
+    if (!(fdctrl->sra & FD_SRA_INTPEND)) {
         qemu_set_irq(fdctrl->irq, 1);
-        fdctrl->state |= FD_CTRL_INTR;
+        fdctrl->sra |= FD_SRA_INTPEND;
     }
-    FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", status);
-    fdctrl->int_status = status;
+    fdctrl->status0 = status0;
+    FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", fdctrl->status0);
 }
 
 /* Reset controller */

@@ -831,57 +764,96 @@
     FLOPPY_DPRINTF("reset controller\n");
     fdctrl_reset_irq(fdctrl);
     /* Initialise controller */
+    fdctrl->sra = 0;
+    fdctrl->srb = 0xc0;
+    if (!fdctrl->drives[1].bs)
+        fdctrl->sra |= FD_SRA_nDRV2;
     fdctrl->cur_drv = 0;
+    fdctrl->dor = FD_DOR_nRESET;
+    fdctrl->dor |= (fdctrl->dma_chann != -1) ? FD_DOR_DMAEN : 0;
+    fdctrl->msr = FD_MSR_RQM;
     /* FIFO state */
     fdctrl->data_pos = 0;
     fdctrl->data_len = 0;
-    fdctrl->data_state = FD_STATE_CMD;
+    fdctrl->data_state = 0;
     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)
+    if (do_irq) {
         fdctrl_raise_irq(fdctrl, FD_SR0_RDYCHG);
+    }
 }
 
 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 (GET_CUR_DRV(fdctrl)) {
+        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 A register : 0x00 (read-only) */
+static uint32_t fdctrl_read_statusA (fdctrl_t *fdctrl)
+{
+    uint32_t retval = fdctrl->sra;
+
+    FLOPPY_DPRINTF("status register A: 0x%02x\n", retval);
+
+    return retval;
+}
+
 /* Status B register : 0x01 (read-only) */
 static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl)
 {
-    FLOPPY_DPRINTF("status register: 0x00\n");
-    return 0;
+    uint32_t retval = fdctrl->srb;
+
+    FLOPPY_DPRINTF("status register B: 0x%02x\n", retval);
+
+    return retval;
 }
 
 /* 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);

@@ -891,53 +863,47 @@
 
 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 */
+
+    /* Motors */
+    if (value & FD_DOR_MOTEN0)
+        fdctrl->srb |= FD_SRB_MTR0;
+    else
+        fdctrl->srb &= ~FD_SRB_MTR0;
     if (value & FD_DOR_MOTEN1)
-        fd_start(drv1(fdctrl));
+        fdctrl->srb |= FD_SRB_MTR1;
     else
-        fd_stop(drv1(fdctrl));
-    if (value & FD_DOR_MOTEN0)
-        fd_start(drv0(fdctrl));
+        fdctrl->srb &= ~FD_SRB_MTR1;
+
+    /* Drive */
+    if (value & 1)
+        fdctrl->srb |= FD_SRB_DR0;
     else
-        fd_stop(drv0(fdctrl));
-    /* DMA enable */
-#if 0
-    if (fdctrl->dma_chann != -1)
-        fdctrl->dma_en = value & FD_DOR_DMAEN ? 1 : 0;
-#endif
+        fdctrl->srb &= ~FD_SRB_DR0;
+
     /* 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;

@@ -946,34 +912,24 @@
 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 */
 }
 
 /* Main status register : 0x04 (read) */
 static uint32_t fdctrl_read_main_status (fdctrl_t *fdctrl)
 {
-    uint32_t retval = 0;
+    uint32_t retval = fdctrl->msr;
 
-    fdctrl->state &= ~(FD_CTRL_SLEEP | FD_CTRL_RESET);
-    if (!(fdctrl->state & FD_CTRL_BUSY)) {
-        /* 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 */
-    /* Command busy indicator */
-    if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA ||
-        FD_STATE(fdctrl->data_state) == FD_STATE_STATUS)
-        retval |= FD_MSR_CMDBUSY;
+    fdctrl->dsr &= ~FD_DSR_PWRDOWN;
+    fdctrl->dor |= FD_DOR_nRESET;
+
     FLOPPY_DPRINTF("main status register: 0x%02x\n", retval);
 
     return retval;

@@ -983,21 +939,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)

@@ -1018,8 +974,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 +993,7 @@
 {
     fdctrl->data_dir = FD_DIR_WRITE;
     fdctrl->data_pos = 0;
-    FD_SET_STATE(fdctrl->data_state, FD_STATE_CMD);
+    fdctrl->msr &= ~(FD_MSR_CMDBUSY | FD_MSR_DIO);
 }
 
 /* Set FIFO status for the host to read */

@@ -1041,7 +1002,7 @@
     fdctrl->data_dir = FD_DIR_READ;
     fdctrl->data_len = fifo_len;
     fdctrl->data_pos = 0;
-    FD_SET_STATE(fdctrl->data_state, FD_STATE_STATUS);
+    fdctrl->msr |= FD_MSR_CMDBUSY | FD_MSR_RQM | FD_MSR_DIO;
     if (do_irq)
         fdctrl_raise_irq(fdctrl, 0x00);
 }

@@ -1049,21 +1010,45 @@
 /* Set an error: unimplemented/unknown command */
 static void fdctrl_unimplemented (fdctrl_t *fdctrl, int direction)
 {
-#if 0
-    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_set_fifo(fdctrl, 3, 1);
-#else
-    //    fdctrl_reset_fifo(fdctrl);
+    FLOPPY_ERROR("unimplemented command 0x%02x\n", fdctrl->fifo[0]);
     fdctrl->fifo[0] = FD_SR0_INVCMD;
     fdctrl_set_fifo(fdctrl, 1, 0);
-#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 1;
+        }
+        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++;
+    }
+    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)

@@ -1073,8 +1058,8 @@
     cur_drv = get_cur_drv(fdctrl);
     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;
+                   status0 | (cur_drv->head << 2) | GET_CUR_DRV(fdctrl));
+    fdctrl->fifo[0] = status0 | (cur_drv->head << 2) | GET_CUR_DRV(fdctrl);
     fdctrl->fifo[1] = status1;
     fdctrl->fifo[2] = status2;
     fdctrl->fifo[3] = cur_drv->track;

@@ -1082,10 +1067,11 @@
     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_NONDMA)) {
         DMA_release_DREQ(fdctrl->dma_chann);
-        fdctrl->state &= ~FD_CTRL_BUSY;
     }
+    fdctrl->msr |= FD_MSR_RQM | FD_MSR_DIO;
+    fdctrl->msr &= ~FD_MSR_NONDMA;
     fdctrl_set_fifo(fdctrl, 7, 1);
 }

@@ -1094,18 +1080,17 @@
 {
     fdrive_t *cur_drv;
     uint8_t kh, kt, ks;
-    int did_seek;
+    int did_seek = 0;
 
-    fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+    SET_CUR_DRV(fdctrl, 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,
+                   GET_CUR_DRV(fdctrl), 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);

@@ -1115,7 +1100,7 @@
         return;
     case 3:
         /* track too big */
-        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x80, 0x00);
+        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, FD_SR1_EC, 0x00);
         fdctrl->fifo[3] = kt;
         fdctrl->fifo[4] = kh;
         fdctrl->fifo[5] = ks;

@@ -1133,10 +1118,11 @@
     default:
         break;
     }
+
     /* Set the FIFO state */
     fdctrl->data_dir = direction;
     fdctrl->data_pos = 0;
-    FD_SET_STATE(fdctrl->data_state, FD_STATE_DATA); /* FIFO ready for data */
+    fdctrl->msr |= FD_MSR_CMDBUSY;
     if (fdctrl->fifo[0] & 0x80)
         fdctrl->data_state |= FD_STATE_MULTI;
     else

@@ -1156,7 +1142,7 @@
         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);

@@ -1170,7 +1156,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...
              */

@@ -1182,6 +1168,9 @@
         }
     }
     FLOPPY_DPRINTF("start non-DMA transfer\n");
+    fdctrl->msr |= FD_MSR_NONDMA;
+    if (direction != FD_DIR_WRITE)
+        fdctrl->msr |= FD_MSR_DIO;
     /* IO based transfer: calculate len */
     fdctrl_raise_irq(fdctrl, 0x00);

@@ -1191,6 +1180,8 @@
 /* Prepare a transfer of deleted data */
 static void fdctrl_start_transfer_del (fdctrl_t *fdctrl, int direction)
 {
+    FLOPPY_ERROR("fdctrl_start_transfer_del() unimplemented\n");
+
     /* We don't handle deleted data,
      * so we don't return *ANYTHING*
      */

@@ -1207,14 +1198,14 @@
     uint8_t status0 = 0x00, status1 = 0x00, 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;
     }
     cur_drv = get_cur_drv(fdctrl);
     if (fdctrl->data_dir == FD_DIR_SCANE || fdctrl->data_dir == FD_DIR_SCANL ||
         fdctrl->data_dir == FD_DIR_SCANH)
-        status2 = 0x04;
+        status2 = FD_SR2_SNS;
     if (dma_len > fdctrl->data_len)
         dma_len = fdctrl->data_len;
     if (cur_drv->bs == NULL) {

@@ -1232,7 +1223,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, GET_CUR_DRV(fdctrl), 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 ||

@@ -1258,7 +1249,7 @@
                              fdctrl->data_pos, len);
             if (bdrv_write(cur_drv->bs, fd_sector(cur_drv),
                            fdctrl->fifo, 1) < 0) {
-                FLOPPY_ERROR("writting sector %d\n", fd_sector(cur_drv));
+                FLOPPY_ERROR("writing sector %d\n", fd_sector(cur_drv));
                 fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00);
                 goto transfer_error;
             }

@@ -1271,7 +1262,7 @@
                 DMA_read_memory (nchan, tmpbuf, fdctrl->data_pos, len);
                 ret = memcmp(tmpbuf, fdctrl->fifo + rel_pos, len);
                 if (ret == 0) {
-                    status2 = 0x08;
+                    status2 = FD_SR2_SEH;
                     goto end_transfer;
                 }
                 if ((ret < 0 && fdctrl->data_dir == FD_DIR_SCANL) ||

@@ -1286,35 +1277,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:

@@ -1324,11 +1288,10 @@
     if (fdctrl->data_dir == FD_DIR_SCANE ||
         fdctrl->data_dir == FD_DIR_SCANL ||
         fdctrl->data_dir == FD_DIR_SCANH)
-        status2 = 0x08;
+        status2 = FD_SR2_SEH;
     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);
  transfer_error:

@@ -1340,22 +1303,30 @@
 {
     fdrive_t *cur_drv;
     uint32_t retval = 0;
-    int pos, len;
+    int pos;
 
     cur_drv = get_cur_drv(fdctrl);
-    fdctrl->state &= ~FD_CTRL_SLEEP;
-    if (FD_STATE(fdctrl->data_state) == FD_STATE_CMD) {
-        FLOPPY_ERROR("can't read data in CMD state\n");
+    fdctrl->dsr &= ~FD_DSR_PWRDOWN;
+    if (!(fdctrl->msr & FD_MSR_RQM) || !(fdctrl->msr & FD_MSR_DIO)) {
+        FLOPPY_ERROR("controller not ready for reading\n");
         return 0;
     }
     pos = fdctrl->data_pos;
-    if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
+    if (fdctrl->msr & FD_MSR_NONDMA) {
         pos %= FD_SECTOR_LEN;
         if (pos == 0) {
-            len = fdctrl->data_len - fdctrl->data_pos;
-            if (len > FD_SECTOR_LEN)
-                len = FD_SECTOR_LEN;
-            bdrv_read(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1);
+            if (fdctrl->data_pos != 0)
+                if (!fdctrl_seek_to_next_sect(fdctrl, cur_drv)) {
+                    FLOPPY_DPRINTF("error seeking to next sector %d\n",
+                                   fd_sector(cur_drv));
+                    return 0;
+                }
+            if (bdrv_read(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1) < 0) {
+                FLOPPY_DPRINTF("error getting sector %d\n",
+                               fd_sector(cur_drv));
+                /* Sure, image size is too small... */
+                memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
+            }
         }
     }
     retval = fdctrl->fifo[pos];

@@ -1364,7 +1335,7 @@
         /* Switch from transfer mode to status mode
          * then from status mode to command mode
          */
-        if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
+        if (fdctrl->msr & FD_MSR_NONDMA) {
             fdctrl_stop_transfer(fdctrl, FD_SR0_SEEK, 0x00, 0x00);
         } else {
             fdctrl_reset_fifo(fdctrl);

@@ -1380,17 +1351,15 @@
 {
     fdrive_t *cur_drv;
     uint8_t kh, kt, ks;
-    int did_seek;
 
-    fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+    SET_CUR_DRV(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,
+                   GET_CUR_DRV(fdctrl), 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 */

@@ -1401,7 +1370,7 @@
         return;
     case 3:
         /* track too big */
-        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x80, 0x00);
+        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, FD_SR1_EC, 0x00);
         fdctrl->fifo[3] = kt;
         fdctrl->fifo[4] = kh;
         fdctrl->fifo[5] = ks;

@@ -1414,7 +1383,6 @@
         fdctrl->fifo[5] = ks;
         return;
     case 1:
-        did_seek = 1;
         fdctrl->data_state |= FD_STATE_SEEK;
         break;
     default:

@@ -1455,11 +1423,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);

@@ -1470,7 +1443,6 @@
 
 static void fdctrl_handle_version (fdctrl_t *fdctrl, int direction)
 {
-    /* Controller's version */
     fdctrl->fifo[0] = fdctrl->version;
     fdctrl_set_fifo(fdctrl, 1, 1);
 }

@@ -1488,6 +1460,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];

@@ -1509,8 +1485,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;

@@ -1529,7 +1510,9 @@
 {
     fdrive_t *cur_drv = get_cur_drv(fdctrl);
 
-    /* XXX: should set main status register to busy */
+    /* Set main status register to busy */
+    fdctrl->msr &= ~FD_MSR_RQM;
+
     cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
     qemu_mod_timer(fdctrl->result_timer,
                    qemu_get_clock(vm_clock) + (ticks_per_sec / 50));

@@ -1539,14 +1522,13 @@
 {
     fdrive_t *cur_drv;
 
-    fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+    SET_CUR_DRV(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

@@ -1568,7 +1550,10 @@
 {
     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);
 }

@@ -1577,14 +1562,14 @@
 {
     fdrive_t *cur_drv;
 
-    fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+    SET_CUR_DRV(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 |
+        GET_CUR_DRV(fdctrl) |
         0x28;
     fdctrl_set_fifo(fdctrl, 1, 0);
 }

@@ -1593,7 +1578,7 @@
 {
     fdrive_t *cur_drv;
 
-    fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+    SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
     cur_drv = get_cur_drv(fdctrl);
     fd_recalibrate(cur_drv);
     fdctrl_reset_fifo(fdctrl);

@@ -1607,31 +1592,26 @@
 
 #if 0
     fdctrl->fifo[0] =
-        fdctrl->int_status | (cur_drv->head << 2) | fdctrl->cur_drv;
+        fdctrl->status0 | (cur_drv->head << 2) | GET_CUR_DRV(fdctrl);
 #else
-    /* XXX: int_status handling is broken for read/write
+    /* XXX: status0 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;
+        FD_SR0_SEEK | (cur_drv->head << 2) | GET_CUR_DRV(fdctrl);
 #endif
     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;
 }
 
 static void fdctrl_handle_seek (fdctrl_t *fdctrl, int direction)
 {
     fdrive_t *cur_drv;
 
-    fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+    SET_CUR_DRV(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);

@@ -1649,7 +1629,7 @@
     if (fdctrl->fifo[1] & 0x80)
         cur_drv->perpendicular = fdctrl->fifo[1] & 0x7;
     /* No result back */
-           fdctrl_reset_fifo(fdctrl);
+    fdctrl_reset_fifo(fdctrl);
 }
 
 static void fdctrl_handle_configure (fdctrl_t *fdctrl, int direction)

@@ -1690,36 +1670,33 @@
     } else if (fdctrl->data_len > 7) {
         /* ERROR */
         fdctrl->fifo[0] = 0x80 |
-            (cur_drv->head << 2) | fdctrl->cur_drv;
+            (cur_drv->head << 2) | GET_CUR_DRV(fdctrl);
         fdctrl_set_fifo(fdctrl, 1, 1);
     }
 }
 
 static void fdctrl_handle_relative_seek_out (fdctrl_t *fdctrl, int direction)
 {
-    fdrive_t *cur_drv = get_cur_drv(fdctrl);
+    fdrive_t *cur_drv;
 
-    fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+    SET_CUR_DRV(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;
     } else {
         cur_drv->track += fdctrl->fifo[2];
     }
     fdctrl_reset_fifo(fdctrl);
+    /* Raise Interrupt */
     fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
 }
 
 static void fdctrl_handle_relative_seek_in (fdctrl_t *fdctrl, int direction)
 {
-    fdrive_t *cur_drv = get_cur_drv(fdctrl);
+    fdrive_t *cur_drv;
 
-    fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+    SET_CUR_DRV(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;
     } else {

@@ -1730,96 +1707,99 @@
     fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
 }
 
+static const struct {
+    uint8_t value;
+    uint8_t mask;
+    const char* name;
+    int parameters;
+    void (*handler)(fdctrl_t *fdctrl, int direction);
+    int direction;
+} handlers[] = {
+    { FD_CMD_READ, 0x1f, "READ", 8, fdctrl_start_transfer, FD_DIR_READ },
+    { FD_CMD_WRITE, 0x3f, "WRITE", 8, fdctrl_start_transfer, FD_DIR_WRITE },
+    { FD_CMD_SEEK, 0xff, "SEEK", 2, fdctrl_handle_seek },
+    { FD_CMD_SENSE_INTERRUPT_STATUS, 0xff, "SENSE INTERRUPT STATUS", 0, fdctrl_handle_sense_interrupt_status },
+    { FD_CMD_RECALIBRATE, 0xff, "RECALIBRATE", 1, fdctrl_handle_recalibrate },
+    { FD_CMD_FORMAT_TRACK, 0xbf, "FORMAT TRACK", 5, fdctrl_handle_format_track },
+    { FD_CMD_READ_TRACK, 0xbf, "READ TRACK", 8, fdctrl_start_transfer, FD_DIR_READ },
+    { FD_CMD_RESTORE, 0xff, "RESTORE", 17, fdctrl_handle_restore }, /* part of READ DELETED DATA */
+    { FD_CMD_SAVE, 0xff, "SAVE", 0, fdctrl_handle_save }, /* part of READ DELETED DATA */
+    { FD_CMD_READ_DELETED, 0x1f, "READ DELETED DATA", 8, fdctrl_start_transfer_del, FD_DIR_READ },
+    { FD_CMD_SCAN_EQUAL, 0x1f, "SCAN EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANE },
+    { FD_CMD_VERIFY, 0x1f, "VERIFY", 8, fdctrl_unimplemented },
+    { FD_CMD_SCAN_LOW_OR_EQUAL, 0x1f, "SCAN LOW OR EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANL },
+    { FD_CMD_SCAN_HIGH_OR_EQUAL, 0x1f, "SCAN HIGH OR EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANH },
+    { FD_CMD_WRITE_DELETED, 0x3f, "WRITE DELETED DATA", 8, fdctrl_start_transfer_del, FD_DIR_WRITE },
+    { FD_CMD_READ_ID, 0xbf, "READ ID", 1, fdctrl_handle_readid },
+    { FD_CMD_SPECIFY, 0xff, "SPECIFY", 2, fdctrl_handle_specify },
+    { FD_CMD_SENSE_DRIVE_STATUS, 0xff, "SENSE DRIVE STATUS", 1, fdctrl_handle_sense_drive_status },
+    { FD_CMD_PERPENDICULAR_MODE, 0xff, "PERPENDICULAR MODE", 1, fdctrl_handle_perpendicular_mode },
+    { FD_CMD_CONFIGURE, 0xff, "CONFIGURE", 3, fdctrl_handle_configure },
+    { FD_CMD_POWERDOWN_MODE, 0xff, "POWERDOWN MODE", 2, fdctrl_handle_powerdown_mode },
+    { FD_CMD_OPTION, 0xff, "OPTION", 1, fdctrl_handle_option },
+    { FD_CMD_DRIVE_SPECIFICATION_COMMAND, 0xff, "DRIVE SPECIFICATION COMMAND", 5, fdctrl_handle_drive_specification_command },
+    { FD_CMD_RELATIVE_SEEK_OUT, 0xff, "RELATIVE SEEK OUT", 2, fdctrl_handle_relative_seek_out },
+    { FD_CMD_FORMAT_AND_WRITE, 0xff, "FORMAT AND WRITE", 10, fdctrl_unimplemented },
+    { FD_CMD_RELATIVE_SEEK_IN, 0xff, "RELATIVE SEEK IN", 2, fdctrl_handle_relative_seek_in },
+    { FD_CMD_LOCK, 0x7f, "LOCK", 0, fdctrl_handle_lock },
+    { FD_CMD_DUMPREG, 0xff, "DUMPREG", 0, fdctrl_handle_dumpreg },
+    { FD_CMD_VERSION, 0xff, "VERSION", 0, fdctrl_handle_version },
+    { FD_CMD_PART_ID, 0xff, "PART ID", 0, fdctrl_handle_partid },
+    { FD_CMD_WRITE, 0x1f, "WRITE (BeOS)", 8, fdctrl_start_transfer, FD_DIR_WRITE }, /* not in specification ; BeOS 4.5 bug */
+    { 0, 0, "unknown", 0, fdctrl_unimplemented }, /* default handler */
+};
+/* Associate command to an index in the 'handlers' array */
+static uint8_t command_to_handler[0xff];
+
 static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
 {
     fdrive_t *cur_drv;
     int pos;
-    static const struct {
-        uint8_t value;
-        uint8_t mask;
-        const char* name;
-        int parameters;
-        void (*handler)(fdctrl_t *fdctrl, int direction);
-        int parameter;
-    } commands[] = {
-        { FD_CMD_READ, 0x1f, "READ", 8, fdctrl_start_transfer, FD_DIR_READ },
-        { FD_CMD_WRITE, 0x3f, "WRITE", 8, fdctrl_start_transfer, FD_DIR_WRITE },
-        { FD_CMD_SEEK, 0xff, "SEEK", 2, fdctrl_handle_seek },
-        { FD_CMD_SENSE_INTERRUPT_STATUS, 0xff, "SENSE INTERRUPT STATUS", 0, fdctrl_handle_sense_interrupt_status },
-        { FD_CMD_RECALIBRATE, 0xff, "RECALIBRATE", 1, fdctrl_handle_recalibrate },
-        { FD_CMD_FORMAT_TRACK, 0xbf, "FORMAT TRACK", 5, fdctrl_handle_format_track },
-        { FD_CMD_READ_TRACK, 0xbf, "READ TRACK", 8, fdctrl_start_transfer, FD_DIR_READ },
-        { FD_CMD_RESTORE, 0xff, "RESTORE", 17, fdctrl_handle_restore }, /* part of READ DELETED DATA */
-        { FD_CMD_SAVE, 0xff, "SAVE", 0, fdctrl_handle_save }, /* part of READ DELETED DATA */
-        { FD_CMD_READ_DELETED, 0x1f, "READ DELETED DATA", 8, fdctrl_start_transfer_del, FD_DIR_READ },
-        { FD_CMD_SCAN_EQUAL, 0x1f, "SCAN EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANE },
-        { FD_CMD_VERIFY, 0x1f, "VERIFY", 8, fdctrl_unimplemented },
-        { FD_CMD_SCAN_LOW_OR_EQUAL, 0x1f, "SCAN LOW OR EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANL },
-        { FD_CMD_SCAN_HIGH_OR_EQUAL, 0x1f, "SCAN HIGH OR EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANH },
-        { FD_CMD_WRITE_DELETED, 0x3f, "WRITE DELETED DATA", 8, fdctrl_start_transfer_del, FD_DIR_WRITE },
-        { FD_CMD_READ_ID, 0xbf, "READ ID", 1, fdctrl_handle_readid },
-        { FD_CMD_SPECIFY, 0xff, "SPECIFY", 2, fdctrl_handle_specify },
-        { FD_CMD_SENSE_DRIVE_STATUS, 0xff, "SENSE DRIVE STATUS", 1, fdctrl_handle_sense_drive_status },
-        { FD_CMD_PERPENDICULAR_MODE, 0xff, "PERPENDICULAR MODE", 1, fdctrl_handle_perpendicular_mode },
-        { FD_CMD_CONFIGURE, 0xff, "CONFIGURE", 3, fdctrl_handle_configure },
-        { FD_CMD_POWERDOWN_MODE, 0xff, "POWERDOWN MODE", 2, fdctrl_handle_powerdown_mode },
-        { FD_CMD_OPTION, 0xff, "OPTION", 1, fdctrl_handle_option },
-        { FD_CMD_DRIVE_SPECIFICATION_COMMAND, 0xff, "DRIVE SPECIFICATION COMMAND", 5, fdctrl_handle_drive_specification_command },
-        { FD_CMD_RELATIVE_SEEK_OUT, 0xff, "RELATIVE SEEK OUT", 2, fdctrl_handle_relative_seek_out },
-        { FD_CMD_FORMAT_AND_WRITE, 0xff, "FORMAT AND WRITE", 10, fdctrl_unimplemented },
-        { FD_CMD_RELATIVE_SEEK_IN, 0xff, "RELATIVE SEEK IN", 2, fdctrl_handle_relative_seek_in },
-        { FD_CMD_LOCK, 0x7f, "LOCK", 0, fdctrl_handle_lock },
-        { FD_CMD_DUMPREG, 0xff, "DUMPREG", 0, fdctrl_handle_dumpreg },
-        { FD_CMD_VERSION, 0xff, "VERSION", 0, fdctrl_handle_version },
-        { FD_CMD_PART_ID, 0xff, "PART ID", 0, fdctrl_handle_partid },
-        { FD_CMD_WRITE, 0x1f, "WRITE (BeOS)", 8, fdctrl_start_transfer, FD_DIR_WRITE }, /* not in specification ; BeOS 4.5 bug */
-    };
 
-    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;
-    if (FD_STATE(fdctrl->data_state) == FD_STATE_STATUS) {
-        FLOPPY_ERROR("can't write data in status mode\n");
+    if (!(fdctrl->msr & FD_MSR_RQM) || (fdctrl->msr & FD_MSR_DIO)) {
+        FLOPPY_ERROR("controller not ready for writing\n");
         return;
     }
+    fdctrl->dsr &= ~FD_DSR_PWRDOWN;
     /* Is it write command time ? */
-    if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
+    if (fdctrl->msr & FD_MSR_NONDMA) {
         /* FIFO data write */
         fdctrl->fifo[fdctrl->data_pos++] = value;
         if (fdctrl->data_pos % FD_SECTOR_LEN == (FD_SECTOR_LEN - 1) ||
             fdctrl->data_pos == fdctrl->data_len) {
-            bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1);
+            cur_drv = get_cur_drv(fdctrl);
+            if (bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1) < 0) {
+                FLOPPY_ERROR("writing sector %d\n", fd_sector(cur_drv));
+                return;
+            }
+            if (!fdctrl_seek_to_next_sect(fdctrl, cur_drv)) {
+                FLOPPY_DPRINTF("error seeking to next sector %d\n",
+                               fd_sector(cur_drv));
+                return;
+            }
         }
         /* Switch from transfer mode to status mode
          * then from status mode to command mode
          */
-        if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA)
+        if (fdctrl->data_pos == fdctrl->data_len)
             fdctrl_stop_transfer(fdctrl, FD_SR0_SEEK, 0x00, 0x00);
         return;
     }
     if (fdctrl->data_pos == 0) {
         /* Command */
-        for (pos = 0; pos < sizeof(commands)/sizeof(commands[0]); pos++) {
-            if ((value & commands[pos].mask) == commands[pos].value) {
-                FLOPPY_DPRINTF("%s command\n", commands[pos].name);
-                fdctrl->data_len = commands[pos].parameters + 1;
-                goto enqueue;
-            }
-        }
+        pos = command_to_handler[value & 0xff];
+        FLOPPY_DPRINTF("%s command\n", handlers[pos].name);
+        fdctrl->data_len = handlers[pos].parameters + 1;
+    }
 
-        /* Unknown command */
-        FLOPPY_ERROR("unknown command: 0x%02x\n", value);
-        fdctrl_unimplemented(fdctrl, 0);
-        return;
-    }
- enqueue:
     FLOPPY_DPRINTF("%s: %02x\n", __func__, value);
-    fdctrl->fifo[fdctrl->data_pos] = value;
-    if (++fdctrl->data_pos == fdctrl->data_len) {
+    fdctrl->fifo[fdctrl->data_pos++] = value;
+    if (fdctrl->data_pos == fdctrl->data_len) {
         /* We now have all parameters
          * and will be able to treat the command
          */

@@ -1828,13 +1808,9 @@
             return;
         }
 
-        for (pos = 0; pos < sizeof(commands)/sizeof(commands[0]); pos++) {
-            if ((fdctrl->fifo[0] & commands[pos].mask) == commands[pos].value) {
-                FLOPPY_DPRINTF("treat %s command\n", commands[pos].name);
-                (*commands[pos].handler)(fdctrl, commands[pos].parameter);
-                break;
-            }
-        }
+        pos = command_to_handler[fdctrl->fifo[0] & 0xff];
+        FLOPPY_DPRINTF("treat %s command\n", handlers[pos].name);
+        (*handlers[pos].handler)(fdctrl, handlers[pos].direction);
     }
 }

@@ -1852,3 +1828,97 @@
     }
     fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
 }
+
+/* Init functions */
+static fdctrl_t *fdctrl_init_common (qemu_irq irq, int dma_chann,
+                                     target_phys_addr_t io_base,
+                                     BlockDriverState **fds)
+{
+    fdctrl_t *fdctrl;
+    int i, j;
+
+    /* Fill 'command_to_handler' lookup table */
+    for (i = sizeof(handlers)/sizeof(handlers[0]) - 1; i >= 0; i--) {
+        for (j = 0; j < sizeof(command_to_handler); j++) {
+            if ((j & handlers[i].mask) == handlers[i].value)
+                command_to_handler[j] = i;
+        }
+    }
+
+    FLOPPY_DPRINTF("init controller\n");
+    fdctrl = qemu_mallocz(sizeof(fdctrl_t));
+    if (!fdctrl)
+        return NULL;
+    fdctrl->fifo = qemu_memalign(512, FD_SECTOR_LEN);
+    if (fdctrl->fifo == NULL) {
+        qemu_free(fdctrl);
+        return NULL;
+    }
+    fdctrl->result_timer = qemu_new_timer(vm_clock,
+                                          fdctrl_result_timer, fdctrl);
+
+    fdctrl->version = 0x90; /* Intel 82078 controller */
+    fdctrl->irq = irq;
+    fdctrl->dma_chann = dma_chann;
+    fdctrl->io_base = io_base;
+    fdctrl->config = FD_CONFIG_EIS | FD_CONFIG_EFIFO; /* Implicit seek, polling & FIFO enabled */
+    if (fdctrl->dma_chann != -1) {
+        DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl);
+    }
+    for (i = 0; i < MAX_FD; i++) {
+        fd_init(&fdctrl->drives[i], fds[i]);
+    }
+    fdctrl_external_reset(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]);
+    }
+
+    return fdctrl;
+}
+
+fdctrl_t *fdctrl_init (qemu_irq irq, int dma_chann, int mem_mapped,
+                       target_phys_addr_t io_base,
+                       BlockDriverState **fds)
+{
+    fdctrl_t *fdctrl;
+    int io_mem;
+
+    fdctrl = fdctrl_init_common(irq, dma_chann, io_base, fds);
+
+    fdctrl->sun4m = 0;
+    if (mem_mapped) {
+        io_mem = cpu_register_io_memory(0, fdctrl_mem_read, fdctrl_mem_write,
+                                        fdctrl);
+        cpu_register_physical_memory(io_base, 0x08, io_mem);
+    } else {
+        register_ioport_read((uint32_t)io_base + 0x01, 5, 1, &fdctrl_read,
+                             fdctrl);
+        register_ioport_read((uint32_t)io_base + 0x07, 1, 1, &fdctrl_read,
+                             fdctrl);
+        register_ioport_write((uint32_t)io_base + 0x01, 5, 1, &fdctrl_write,
+                              fdctrl);
+        register_ioport_write((uint32_t)io_base + 0x07, 1, 1, &fdctrl_write,
+                              fdctrl);
+    }
+
+    return fdctrl;
+}
+
+fdctrl_t *sun4m_fdctrl_init (qemu_irq irq, target_phys_addr_t io_base,
+                             BlockDriverState **fds, qemu_irq *fdc_tc)
+{
+    fdctrl_t *fdctrl;
+    int io_mem;
+
+    fdctrl = fdctrl_init_common(irq, -1, io_base, fds);
+    fdctrl->sun4m = 1;
+    io_mem = cpu_register_io_memory(0, fdctrl_mem_read_strict,
+                                    fdctrl_mem_write_strict,
+                                    fdctrl);
+    cpu_register_physical_memory(io_base, 0x08, io_mem);
+    *fdc_tc = *qemu_allocate_irqs(fdctrl_handle_tc, fdctrl, 1);
+
+    return fdctrl;
+}

Blue Swirl

2008-04-23 16:52 UTC #4 lore raw

Re: [Qemu-devel] FDC: improve emulation [v2]

Quoted 56 lines · Hervé Poussineau

Hi,
First, Blue Swirl, thanks for the review.
Quoted 8 lines · Blue Swirl
Quoted 6 lines · Hervé Poussineau
I've done big improvements on floppy drive controller.
For easier reviewing, I've tried to split them up in differents parts.
Sorry if some parts are still big.
Each patch applies on top of the previous one.
If only some of them are accepted, I can provide a global patch for
only
Quoted 3 lines
Quoted 2 lines
these ones. Patches which needs more work will be resubmitted later.
Here is a global patch after your comments.
Changelog:
- Adds a command lookup table, as suggested by Fabrice at
http://lists.gnu.org/archive/html/qemu-devel/2008-04/msg00143.html
- This also moves initialization functions at the bottom of the file to
prevent multiple forward declarations.
- Extract seeking to next sector handling in a function. Add a sector seek
in PIO read and write modes
- Fixes status A and status B registers. It removes one Sun4m mutation.
Also removes the internal FD_CTRL_INTR flag.
- Handles correctly FD_MSR_NONDMA/FD_DOR_NONDMA flags, and uses them when
possible. Fixes a problem with SPECIFY command.
- Better handling of DOR register. DOR register shouldn't forget motor
states for non-existing drives (drives 2 and 3), even if no physical pin for it exists.
- Use FD_DOR_nRESET flag instead of internal FD_CTRL_RESET flag.
- Support writing to DOR register even in reset mode (as said in
specification)
- Stores controller state in MSR register instead of internal state field.
This simplifies the fdctrl_read_main_status() function, which may be called in some tight loops.
- Replaces bootsel field by the whole tdr register. It may be easier if we
want to later add support for tapes.
- Supports up to 4 floppy drives if MAX_FD is set to 4.
- Replaces access to cur_drv field by macros.
Changes from v1:
- patch 01: command_to_handler table size is now 256 (instead of 255)
- patch 04: not included in this serie
- patch 05: comment about Sun4m differences kept
- patch 07: fixed data_pos incrementation
- patch 08: version field kept
- patch 11: fixed compilation
- patch 12: not included in this serie

Quoted 1 line

I'll rework patch 4 and send later patch 12, once this one is committed.

These changes pass my Sparc32 and Sparc64 tests. On Sparc32 floppy handling is improved, so I'd like to commit these. If there are any objections, please let me know soon.

But it's better to commit many small patches rather than one big patch, because if there are problems, the culprit patch can be identified and fixed. So could you still rework the patch set?

Quoted 10 lines

Quoted 6 lines
Quoted 1 line
fdc_04_status_012.diff
Not good, this makes Sparc64 print
SENSEI c0 00
SENSEI c0 00
forever.
I've removed patch 04 from this serie, so this shouldn't happen anymore.
Blue Swirl, can you still please tell me how to reproduce the problem, so I can take a look?

./sparc64-softmmu/qemu-system-sparc64 -kernel /dev/null -L pc-bios

Without the patch, emulator exits quickly because of error state, you may want to add -nographic and start Qemu in a terminal.

Hervé Poussineau

author

2008-04-23 20:22 UTC #5 lore raw

Re: [Qemu-devel] FDC: improve emulation [v2]

Quoted 3 lines · Blue Swirl

But it's better to commit many small patches rather than one big patch, because if there are problems, the culprit patch can be identified and fixed. So could you still rework the patch set?

Here is the version with multiple small patches.

fdc_02_seek_next_sector.diff
- Extract seeking to next sector handling in a function. Add a sector seek in PIO read and write modes

fdc_03_status_ab.diff
- Fixes status A and status B registers. It removes one Sun4m mutation. Also removes the internal FD_CTRL_INTR flag.

fdc_04_dmaen.diff
- Handles correctly FD_MSR_NONDMA/FD_DOR_NONDMA flags, and uses them when possible. Fixes a problem with SPECIFY command.

fdc_05_dor_reg.diff
- Better handling of DOR register. DOR register drives external motors, but it not limited to existing drives.
- Use FD_DOR_nRESET flag instead of internal FD_CTRL_RESET flag.
- Support writing to DOR register even in reset mode (as said in specification)

fdc_06_msr_reg.diff
- Stores controller state in MSR register instead of internal state field. This simplifies the fdctrl_read_main_status() function, which may be called in some tight loops.

fdc_07_cleanup.diff
- Removes useless fields in fdrive_t structure.
- Adds a message when bdrv_read/bdrv_write calls fail.
- Rename int_status to status0.
- Replace some constants by value names.

fdc_08_tdr_reg.diff
- Replaces bootsel field by the whole tdr register. It may be easier if we want to later add support for tapes.

fdc_09_4_fds.diff
- Supports up to 4 floppy drives if MAX_FD is set to 4.

fdc_10_cur_drv.diff
- Replaces access to cur_drv field by macros.

Hervé

fdc.c +149 ~~-150~~

Index: hw/fdc.c
===================================================================
@@ -322,7 +322,6 @@
 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_result_timer(void *opaque);
 
 static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl);
 static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl);

@@ -694,78 +693,6 @@
     fdctrl_reset(s, 0);
 }
 
-static fdctrl_t *fdctrl_init_common (qemu_irq irq, int dma_chann,
-                                     target_phys_addr_t io_base,
-                                     BlockDriverState **fds)
-{
-    fdctrl_t *fdctrl;
-    int i;
-
-    FLOPPY_DPRINTF("init controller\n");
-    fdctrl = qemu_mallocz(sizeof(fdctrl_t));
-    if (!fdctrl)
-        return NULL;
-    fdctrl->fifo = qemu_memalign(512, FD_SECTOR_LEN);
-    if (fdctrl->fifo == NULL) {
-        qemu_free(fdctrl);
-        return NULL;
-    }
-    fdctrl->result_timer = qemu_new_timer(vm_clock,
-                                          fdctrl_result_timer, fdctrl);
-
-    fdctrl->version = 0x90; /* Intel 82078 controller */
-    fdctrl->irq = irq;
-    fdctrl->dma_chann = dma_chann;
-    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;
-        DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl);
-    } else {
-        fdctrl->dma_en = 0;
-    }
-    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);
-    qemu_register_reset(fdctrl_external_reset, fdctrl);
-    for (i = 0; i < MAX_FD; i++) {
-        fd_revalidate(&fdctrl->drives[i]);
-    }
-
-    return fdctrl;
-}
-
-fdctrl_t *fdctrl_init (qemu_irq irq, int dma_chann, int mem_mapped,
-                       target_phys_addr_t io_base,
-                       BlockDriverState **fds)
-{
-    fdctrl_t *fdctrl;
-    int io_mem;
-
-    fdctrl = fdctrl_init_common(irq, dma_chann, io_base, fds);
-
-    fdctrl->sun4m = 0;
-    if (mem_mapped) {
-        io_mem = cpu_register_io_memory(0, fdctrl_mem_read, fdctrl_mem_write,
-                                        fdctrl);
-        cpu_register_physical_memory(io_base, 0x08, io_mem);
-    } else {
-        register_ioport_read((uint32_t)io_base + 0x01, 5, 1, &fdctrl_read,
-                             fdctrl);
-        register_ioport_read((uint32_t)io_base + 0x07, 1, 1, &fdctrl_read,
-                             fdctrl);
-        register_ioport_write((uint32_t)io_base + 0x01, 5, 1, &fdctrl_write,
-                              fdctrl);
-        register_ioport_write((uint32_t)io_base + 0x07, 1, 1, &fdctrl_write,
-                              fdctrl);
-    }
-
-    return fdctrl;
-}
-
 static void fdctrl_handle_tc(void *opaque, int irq, int level)
 {
     //fdctrl_t *s = opaque;

@@ -776,23 +703,6 @@
     }
 }
 
-fdctrl_t *sun4m_fdctrl_init (qemu_irq irq, target_phys_addr_t io_base,
-                             BlockDriverState **fds, qemu_irq *fdc_tc)
-{
-    fdctrl_t *fdctrl;
-    int io_mem;
-
-    fdctrl = fdctrl_init_common(irq, 0, io_base, fds);
-    fdctrl->sun4m = 1;
-    io_mem = cpu_register_io_memory(0, fdctrl_mem_read_strict,
-                                    fdctrl_mem_write_strict,
-                                    fdctrl);
-    cpu_register_physical_memory(io_base, 0x08, io_mem);
-    *fdc_tc = *qemu_allocate_irqs(fdctrl_handle_tc, fdctrl, 1);
-
-    return fdctrl;
-}
-
 /* XXX: may change if moved to bdrv */
 int fdctrl_get_drive_type(fdctrl_t *fdctrl, int drive_num)
 {

@@ -1730,50 +1640,54 @@
     fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
 }
 
+static const struct {
+    uint8_t value;
+    uint8_t mask;
+    const char* name;
+    int parameters;
+    void (*handler)(fdctrl_t *fdctrl, int direction);
+    int direction;
+} handlers[] = {
+    { FD_CMD_READ, 0x1f, "READ", 8, fdctrl_start_transfer, FD_DIR_READ },
+    { FD_CMD_WRITE, 0x3f, "WRITE", 8, fdctrl_start_transfer, FD_DIR_WRITE },
+    { FD_CMD_SEEK, 0xff, "SEEK", 2, fdctrl_handle_seek },
+    { FD_CMD_SENSE_INTERRUPT_STATUS, 0xff, "SENSE INTERRUPT STATUS", 0, fdctrl_handle_sense_interrupt_status },
+    { FD_CMD_RECALIBRATE, 0xff, "RECALIBRATE", 1, fdctrl_handle_recalibrate },
+    { FD_CMD_FORMAT_TRACK, 0xbf, "FORMAT TRACK", 5, fdctrl_handle_format_track },
+    { FD_CMD_READ_TRACK, 0xbf, "READ TRACK", 8, fdctrl_start_transfer, FD_DIR_READ },
+    { FD_CMD_RESTORE, 0xff, "RESTORE", 17, fdctrl_handle_restore }, /* part of READ DELETED DATA */
+    { FD_CMD_SAVE, 0xff, "SAVE", 0, fdctrl_handle_save }, /* part of READ DELETED DATA */
+    { FD_CMD_READ_DELETED, 0x1f, "READ DELETED DATA", 8, fdctrl_start_transfer_del, FD_DIR_READ },
+    { FD_CMD_SCAN_EQUAL, 0x1f, "SCAN EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANE },
+    { FD_CMD_VERIFY, 0x1f, "VERIFY", 8, fdctrl_unimplemented },
+    { FD_CMD_SCAN_LOW_OR_EQUAL, 0x1f, "SCAN LOW OR EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANL },
+    { FD_CMD_SCAN_HIGH_OR_EQUAL, 0x1f, "SCAN HIGH OR EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANH },
+    { FD_CMD_WRITE_DELETED, 0x3f, "WRITE DELETED DATA", 8, fdctrl_start_transfer_del, FD_DIR_WRITE },
+    { FD_CMD_READ_ID, 0xbf, "READ ID", 1, fdctrl_handle_readid },
+    { FD_CMD_SPECIFY, 0xff, "SPECIFY", 2, fdctrl_handle_specify },
+    { FD_CMD_SENSE_DRIVE_STATUS, 0xff, "SENSE DRIVE STATUS", 1, fdctrl_handle_sense_drive_status },
+    { FD_CMD_PERPENDICULAR_MODE, 0xff, "PERPENDICULAR MODE", 1, fdctrl_handle_perpendicular_mode },
+    { FD_CMD_CONFIGURE, 0xff, "CONFIGURE", 3, fdctrl_handle_configure },
+    { FD_CMD_POWERDOWN_MODE, 0xff, "POWERDOWN MODE", 2, fdctrl_handle_powerdown_mode },
+    { FD_CMD_OPTION, 0xff, "OPTION", 1, fdctrl_handle_option },
+    { FD_CMD_DRIVE_SPECIFICATION_COMMAND, 0xff, "DRIVE SPECIFICATION COMMAND", 5, fdctrl_handle_drive_specification_command },
+    { FD_CMD_RELATIVE_SEEK_OUT, 0xff, "RELATIVE SEEK OUT", 2, fdctrl_handle_relative_seek_out },
+    { FD_CMD_FORMAT_AND_WRITE, 0xff, "FORMAT AND WRITE", 10, fdctrl_unimplemented },
+    { FD_CMD_RELATIVE_SEEK_IN, 0xff, "RELATIVE SEEK IN", 2, fdctrl_handle_relative_seek_in },
+    { FD_CMD_LOCK, 0x7f, "LOCK", 0, fdctrl_handle_lock },
+    { FD_CMD_DUMPREG, 0xff, "DUMPREG", 0, fdctrl_handle_dumpreg },
+    { FD_CMD_VERSION, 0xff, "VERSION", 0, fdctrl_handle_version },
+    { FD_CMD_PART_ID, 0xff, "PART ID", 0, fdctrl_handle_partid },
+    { FD_CMD_WRITE, 0x1f, "WRITE (BeOS)", 8, fdctrl_start_transfer, FD_DIR_WRITE }, /* not in specification ; BeOS 4.5 bug */
+    { 0, 0, "unknown", 0, fdctrl_unimplemented }, /* default handler */
+};
+/* Associate command to an index in the 'handlers' array */
+static uint8_t command_to_handler[256];
+
 static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
 {
     fdrive_t *cur_drv;
     int pos;
-    static const struct {
-        uint8_t value;
-        uint8_t mask;
-        const char* name;
-        int parameters;
-        void (*handler)(fdctrl_t *fdctrl, int direction);
-        int parameter;
-    } commands[] = {
-        { FD_CMD_READ, 0x1f, "READ", 8, fdctrl_start_transfer, FD_DIR_READ },
-        { FD_CMD_WRITE, 0x3f, "WRITE", 8, fdctrl_start_transfer, FD_DIR_WRITE },
-        { FD_CMD_SEEK, 0xff, "SEEK", 2, fdctrl_handle_seek },
-        { FD_CMD_SENSE_INTERRUPT_STATUS, 0xff, "SENSE INTERRUPT STATUS", 0, fdctrl_handle_sense_interrupt_status },
-        { FD_CMD_RECALIBRATE, 0xff, "RECALIBRATE", 1, fdctrl_handle_recalibrate },
-        { FD_CMD_FORMAT_TRACK, 0xbf, "FORMAT TRACK", 5, fdctrl_handle_format_track },
-        { FD_CMD_READ_TRACK, 0xbf, "READ TRACK", 8, fdctrl_start_transfer, FD_DIR_READ },
-        { FD_CMD_RESTORE, 0xff, "RESTORE", 17, fdctrl_handle_restore }, /* part of READ DELETED DATA */
-        { FD_CMD_SAVE, 0xff, "SAVE", 0, fdctrl_handle_save }, /* part of READ DELETED DATA */
-        { FD_CMD_READ_DELETED, 0x1f, "READ DELETED DATA", 8, fdctrl_start_transfer_del, FD_DIR_READ },
-        { FD_CMD_SCAN_EQUAL, 0x1f, "SCAN EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANE },
-        { FD_CMD_VERIFY, 0x1f, "VERIFY", 8, fdctrl_unimplemented },
-        { FD_CMD_SCAN_LOW_OR_EQUAL, 0x1f, "SCAN LOW OR EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANL },
-        { FD_CMD_SCAN_HIGH_OR_EQUAL, 0x1f, "SCAN HIGH OR EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANH },
-        { FD_CMD_WRITE_DELETED, 0x3f, "WRITE DELETED DATA", 8, fdctrl_start_transfer_del, FD_DIR_WRITE },
-        { FD_CMD_READ_ID, 0xbf, "READ ID", 1, fdctrl_handle_readid },
-        { FD_CMD_SPECIFY, 0xff, "SPECIFY", 2, fdctrl_handle_specify },
-        { FD_CMD_SENSE_DRIVE_STATUS, 0xff, "SENSE DRIVE STATUS", 1, fdctrl_handle_sense_drive_status },
-        { FD_CMD_PERPENDICULAR_MODE, 0xff, "PERPENDICULAR MODE", 1, fdctrl_handle_perpendicular_mode },
-        { FD_CMD_CONFIGURE, 0xff, "CONFIGURE", 3, fdctrl_handle_configure },
-        { FD_CMD_POWERDOWN_MODE, 0xff, "POWERDOWN MODE", 2, fdctrl_handle_powerdown_mode },
-        { FD_CMD_OPTION, 0xff, "OPTION", 1, fdctrl_handle_option },
-        { FD_CMD_DRIVE_SPECIFICATION_COMMAND, 0xff, "DRIVE SPECIFICATION COMMAND", 5, fdctrl_handle_drive_specification_command },
-        { FD_CMD_RELATIVE_SEEK_OUT, 0xff, "RELATIVE SEEK OUT", 2, fdctrl_handle_relative_seek_out },
-        { FD_CMD_FORMAT_AND_WRITE, 0xff, "FORMAT AND WRITE", 10, fdctrl_unimplemented },
-        { FD_CMD_RELATIVE_SEEK_IN, 0xff, "RELATIVE SEEK IN", 2, fdctrl_handle_relative_seek_in },
-        { FD_CMD_LOCK, 0x7f, "LOCK", 0, fdctrl_handle_lock },
-        { FD_CMD_DUMPREG, 0xff, "DUMPREG", 0, fdctrl_handle_dumpreg },
-        { FD_CMD_VERSION, 0xff, "VERSION", 0, fdctrl_handle_version },
-        { FD_CMD_PART_ID, 0xff, "PART ID", 0, fdctrl_handle_partid },
-        { FD_CMD_WRITE, 0x1f, "WRITE (BeOS)", 8, fdctrl_start_transfer, FD_DIR_WRITE }, /* not in specification ; BeOS 4.5 bug */
-    };
 
     cur_drv = get_cur_drv(fdctrl);
     /* Reset mode */

@@ -1803,20 +1717,11 @@
     }
     if (fdctrl->data_pos == 0) {
         /* Command */
-        for (pos = 0; pos < sizeof(commands)/sizeof(commands[0]); pos++) {
-            if ((value & commands[pos].mask) == commands[pos].value) {
-                FLOPPY_DPRINTF("%s command\n", commands[pos].name);
-                fdctrl->data_len = commands[pos].parameters + 1;
-                goto enqueue;
-            }
-        }
-
-        /* Unknown command */
-        FLOPPY_ERROR("unknown command: 0x%02x\n", value);
-        fdctrl_unimplemented(fdctrl, 0);
-        return;
+        pos = command_to_handler[value & 0xff];
+        FLOPPY_DPRINTF("%s command\n", handlers[pos].name);
+        fdctrl->data_len = handlers[pos].parameters + 1;
     }
- enqueue:
+
     FLOPPY_DPRINTF("%s: %02x\n", __func__, value);
     fdctrl->fifo[fdctrl->data_pos] = value;
     if (++fdctrl->data_pos == fdctrl->data_len) {

@@ -1828,13 +1733,9 @@
             return;
         }
 
-        for (pos = 0; pos < sizeof(commands)/sizeof(commands[0]); pos++) {
-            if ((fdctrl->fifo[0] & commands[pos].mask) == commands[pos].value) {
-                FLOPPY_DPRINTF("treat %s command\n", commands[pos].name);
-                (*commands[pos].handler)(fdctrl, commands[pos].parameter);
-                break;
-            }
-        }
+        pos = command_to_handler[fdctrl->fifo[0] & 0xff];
+        FLOPPY_DPRINTF("treat %s command\n", handlers[pos].name);
+        (*handlers[pos].handler)(fdctrl, handlers[pos].direction);
     }
 }

@@ -1851,4 +1752,102 @@
         cur_drv->sect = (cur_drv->sect % cur_drv->last_sect) + 1;
     }
     fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
+}
+
+/* Init functions */
+static fdctrl_t *fdctrl_init_common (qemu_irq irq, int dma_chann,
+                                     target_phys_addr_t io_base,
+                                     BlockDriverState **fds)
+{
+    fdctrl_t *fdctrl;
+    int i, j;
+
+    /* Fill 'command_to_handler' lookup table */
+    for (i = sizeof(handlers)/sizeof(handlers[0]) - 1; i >= 0; i--) {
+        for (j = 0; j < sizeof(command_to_handler); j++) {
+            if ((j & handlers[i].mask) == handlers[i].value)
+                command_to_handler[j] = i;
+        }
+    }
+
+    FLOPPY_DPRINTF("init controller\n");
+    fdctrl = qemu_mallocz(sizeof(fdctrl_t));
+    if (!fdctrl)
+        return NULL;
+    fdctrl->fifo = qemu_memalign(512, FD_SECTOR_LEN);
+    if (fdctrl->fifo == NULL) {
+        qemu_free(fdctrl);
+        return NULL;
+    }
+    fdctrl->result_timer = qemu_new_timer(vm_clock,
+                                          fdctrl_result_timer, fdctrl);
+
+    fdctrl->version = 0x90; /* Intel 82078 controller */
+    fdctrl->irq = irq;
+    fdctrl->dma_chann = dma_chann;
+    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;
+        DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl);
+    } else {
+        fdctrl->dma_en = 0;
+    }
+    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);
+    qemu_register_reset(fdctrl_external_reset, fdctrl);
+    for (i = 0; i < MAX_FD; i++) {
+        fd_revalidate(&fdctrl->drives[i]);
+    }
+
+    return fdctrl;
+}
+
+fdctrl_t *fdctrl_init (qemu_irq irq, int dma_chann, int mem_mapped,
+                       target_phys_addr_t io_base,
+                       BlockDriverState **fds)
+{
+    fdctrl_t *fdctrl;
+    int io_mem;
+
+    fdctrl = fdctrl_init_common(irq, dma_chann, io_base, fds);
+
+    fdctrl->sun4m = 0;
+    if (mem_mapped) {
+        io_mem = cpu_register_io_memory(0, fdctrl_mem_read, fdctrl_mem_write,
+                                        fdctrl);
+        cpu_register_physical_memory(io_base, 0x08, io_mem);
+    } else {
+        register_ioport_read((uint32_t)io_base + 0x01, 5, 1, &fdctrl_read,
+                             fdctrl);
+        register_ioport_read((uint32_t)io_base + 0x07, 1, 1, &fdctrl_read,
+                             fdctrl);
+        register_ioport_write((uint32_t)io_base + 0x01, 5, 1, &fdctrl_write,
+                              fdctrl);
+        register_ioport_write((uint32_t)io_base + 0x07, 1, 1, &fdctrl_write,
+                              fdctrl);
+    }
+
+    return fdctrl;
+}
+
+fdctrl_t *sun4m_fdctrl_init (qemu_irq irq, target_phys_addr_t io_base,
+                             BlockDriverState **fds, qemu_irq *fdc_tc)
+{
+    fdctrl_t *fdctrl;
+    int io_mem;
+
+    fdctrl = fdctrl_init_common(irq, 0, io_base, fds);
+    fdctrl->sun4m = 1;
+    io_mem = cpu_register_io_memory(0, fdctrl_mem_read_strict,
+                                    fdctrl_mem_write_strict,
+                                    fdctrl);
+    cpu_register_physical_memory(io_base, 0x08, io_mem);
+    *fdc_tc = *qemu_allocate_irqs(fdctrl_handle_tc, fdctrl, 1);
+
+    return fdctrl;
 }

fdc.c +48 ~~-33~~

111 lines not on this page · raw message

fdc.c +75 ~~-16~~

176 lines not on this page · raw message

fdc.c +21 ~~-12~~

Index: hw/fdc.c
===================================================================
@@ -494,6 +494,8 @@
     QEMUTimer *result_timer;
     uint8_t sra;
     uint8_t srb;
+    uint8_t dor;
+    uint8_t msr;
     uint8_t state;
     uint8_t dma_en;
     uint8_t cur_drv;

@@ -771,6 +773,9 @@
     if (!fdctrl->drives[1].bs)
         fdctrl->sra |= FD_SRA_nDRV2;
     fdctrl->cur_drv = 0;
+    fdctrl->dor = 0;
+    fdctrl->dor |= (fdctrl->dma_chann != -1) ? FD_DOR_DMAEN : 0;
+    fdctrl->msr = 0;
     /* FIFO state */
     fdctrl->data_pos = 0;
     fdctrl->data_len = 0;

@@ -829,7 +834,7 @@
     if (drv1(fdctrl)->drflags & FDRIVE_MOTOR_ON)
         retval |= FD_DOR_MOTEN1;
     /* DMA enable */
-    if (fdctrl->dma_en)
+    if (fdctrl->dor & FD_DOR_DMAEN)
         retval |= FD_DOR_DMAEN;
     /* Reset indicator */
     if (!(fdctrl->state & FD_CTRL_RESET))

@@ -897,6 +902,8 @@
     }
     /* Selected drive */
     fdctrl->cur_drv = value & FD_DOR_SELMASK;
+
+    fdctrl->dor = value;
 }
 
 /* Tape drive register : 0x03 */

@@ -1085,10 +1092,11 @@
     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_NONDMA)) {
         DMA_release_DREQ(fdctrl->dma_chann);
         fdctrl->state &= ~FD_CTRL_BUSY;
     }
+    fdctrl->msr &= ~FD_MSR_NONDMA;
     fdctrl_set_fifo(fdctrl, 7, 1);
 }

@@ -1159,7 +1167,7 @@
         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);

@@ -1185,6 +1193,7 @@
         }
     }
     FLOPPY_DPRINTF("start non-DMA transfer\n");
+    fdctrl->msr |= FD_MSR_NONDMA;
     /* IO based transfer: calculate len */
     fdctrl_raise_irq(fdctrl, 0x00);

@@ -1325,7 +1334,7 @@
         return 0;
     }
     pos = fdctrl->data_pos;
-    if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
+    if (fdctrl->msr & FD_MSR_NONDMA) {
         pos %= FD_SECTOR_LEN;
         if (pos == 0) {
             if (fdctrl->data_pos != 0)

@@ -1343,7 +1352,7 @@
         /* Switch from transfer mode to status mode
          * then from status mode to command mode
          */
-        if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
+        if (fdctrl->msr & FD_MSR_NONDMA) {
             fdctrl_stop_transfer(fdctrl, FD_SR0_SEEK, 0x00, 0x00);
         } else {
             fdctrl_reset_fifo(fdctrl);

@@ -1438,7 +1447,7 @@
     fdctrl->fifo[3] = 0;
     /* 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);

@@ -1547,7 +1556,10 @@
 {
     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);
 }

@@ -1770,7 +1782,7 @@
         return;
     }
     /* Is it write command time ? */
-    if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
+    if (fdctrl->msr & FD_MSR_NONDMA) {
         /* FIFO data write */
         fdctrl->fifo[fdctrl->data_pos++] = value;
         if (fdctrl->data_pos % FD_SECTOR_LEN == (FD_SECTOR_LEN - 1) ||

@@ -1862,10 +1874,7 @@
     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;
         DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl);
-    } else {
-        fdctrl->dma_en = 0;
     }
     for (i = 0; i < MAX_FD; i++) {
         fd_init(&fdctrl->drives[i], fds[i]);

@@ -1915,7 +1924,7 @@
     fdctrl_t *fdctrl;
     int io_mem;
 
-    fdctrl = fdctrl_init_common(irq, 0, io_base, fds);
+    fdctrl = fdctrl_init_common(irq, -1, io_base, fds);
     fdctrl->sun4m = 1;
     io_mem = cpu_register_io_memory(0, fdctrl_mem_read_strict,
                                     fdctrl_mem_write_strict,

fdc.c +14 ~~-82~~

231 lines not on this page · raw message

fdc.c +32 ~~-51~~

216 lines not on this page · raw message

fdc.c +74 ~~-69~~

419 lines not on this page · raw message

fdc.c +10 ~~-10~~

Index: hw/fdc.c
===================================================================
@@ -467,10 +467,10 @@
     uint8_t sra;
     uint8_t srb;
     uint8_t dor;
+    uint8_t tdr;
     uint8_t dsr;
     uint8_t msr;
     uint8_t cur_drv;
-    uint8_t bootsel;
     uint8_t status0;
     uint8_t status1;
     uint8_t status2;

@@ -606,13 +606,13 @@
     /* Controller state */
     qemu_put_8s(f, &s->sra);
     qemu_put_8s(f, &s->srb);
+    qemu_put_8s(f, &s->tdr);
     qemu_put_8s(f, &s->dsr);
     qemu_put_8s(f, &s->msr);
     qemu_put_8s(f, &s->status0);
     qemu_put_8s(f, &s->status1);
     qemu_put_8s(f, &s->status2);
     qemu_put_8s(f, &s->cur_drv);
-    qemu_put_8s(f, &s->bootsel);
     /* Command FIFO */
     qemu_put_buffer(f, s->fifo, FD_SECTOR_LEN);
     qemu_put_be32s(f, &s->data_pos);

@@ -651,13 +651,13 @@
     /* Controller state */
     qemu_get_8s(f, &s->sra);
     qemu_get_8s(f, &s->srb);
+    qemu_get_8s(f, &s->tdr);
     qemu_get_8s(f, &s->dsr);
     qemu_get_8s(f, &s->msr);
     qemu_get_8s(f, &s->status0);
     qemu_get_8s(f, &s->status1);
     qemu_get_8s(f, &s->status2);
     qemu_get_8s(f, &s->cur_drv);
-    qemu_get_8s(f, &s->bootsel);
     /* Command FIFO */
     qemu_get_buffer(f, s->fifo, FD_SECTOR_LEN);
     qemu_get_be32s(f, &s->data_pos);

@@ -762,12 +762,15 @@
 
 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];
 }
 
 static fdrive_t *get_cur_drv (fdctrl_t *fdctrl)

@@ -848,11 +851,8 @@
 /* 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;

@@ -867,7 +867,7 @@
     }
     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 */
 }

fdc.c +75 ~~-10~~

166 lines not on this page · raw message

fdc.c +25 ~~-19~~

171 lines not on this page · raw message