Attached patch fixes some issues in the floppy disk controller: - Enhance reset support (external and software) - Use MAX_FD constant when possible - Support up to 4 drives if MAX_FD is set to 4 - Fix DOR register, which should be writable at any time - Let MSR return 0x20 when non-DMA transfer is happening - Don't assume caller wants to read whole track at once - Add seek to next sector when in non-DMA mode - Fix non-DMA write, which was stopping after only 1 byte
Credits to Stuart Brady to help me to debug some issues... Changelog v1 to v2: - Fix non-DMA write, which was stopping after only 1 byte Changelog v2 to v3: - Update to current CVS Programs can now be started from the floppy To Blue Swirl, I don't forget the floppy problem with SPARC.
Index: hw/fdc.c =================================================================== RCS file: /sources/qemu/qemu/hw/fdc.c,v retrieving revision 1.38 diff -u -r1.38 fdc.c --- hw/fdc.c 29 Feb 2008 19:24:00 -0000 1.38 +++ hw/fdc.c 3 Mar 2008 10:15:28 -0000 @@ -306,10 +306,9 @@ drv->drflags &= ~FDRIVE_MOTOR_ON; } -/* Re-initialise a drives (motor off, repositioned) */ +/* Re-initialise a drive (repositioned) */ static void fd_reset (fdrive_t *drv) { - fd_stop(drv); fd_recalibrate(drv); } @@ -422,7 +421,11 @@ }; 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 +435,11 @@ }; enum { +#if MAX_FD == 4 FD_TDR_BOOTSEL = 0x0c, +#else + FD_TDR_BOOTSEL = 0x04, +#endif }; enum { @@ -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) @@ -831,6 +838,7 @@ fdctrl_reset_fifo(fdctrl); if (do_irq) fdctrl_raise_irq(fdctrl, FD_SR0_RDYCHG); + fdctrl->dma_en = (fdctrl->dma_chann != -1) ? 1 : 0; } static inline fdrive_t *drv0 (fdctrl_t *fdctrl) @@ -840,12 +848,41 @@ static inline fdrive_t *drv1 (fdctrl_t *fdctrl) { - return &fdctrl->drives[1 - fdctrl->bootsel]; + if (fdctrl->bootsel < 1) + return &fdctrl->drives[1]; + else + return &fdctrl->drives[0]; +} + +#if MAX_FD == 4 +static inline fdrive_t *drv2 (fdctrl_t *fdctrl) +{ + if (fdctrl->bootsel < 2) + return &fdctrl->drives[2]; + else + return &fdctrl->drives[1]; +} + +static inline fdrive_t *drv3 (fdctrl_t *fdctrl) +{ + if (fdctrl->bootsel < 3) + return &fdctrl->drives[3]; + else + return &fdctrl->drives[2]; } +#endif static fdrive_t *get_cur_drv (fdctrl_t *fdctrl) { - return fdctrl->cur_drv == 0 ? drv0(fdctrl) : drv1(fdctrl); + switch (fdctrl->cur_drv) { + case 0: return drv0(fdctrl); + case 1: return drv1(fdctrl); +#if MAX_FD == 4 + case 2: return drv2(fdctrl); + case 3: return drv3(fdctrl); +#endif + default: return NULL; + } } /* Status B register : 0x01 (read-only) */ @@ -865,6 +902,12 @@ retval |= FD_DOR_MOTEN0; if (drv1(fdctrl)->drflags & FDRIVE_MOTOR_ON) retval |= FD_DOR_MOTEN1; +#if MAX_FD == 4 + if (drv2(fdctrl)->drflags & FDRIVE_MOTOR_ON) + retval |= FD_DOR_MOTEN2; + if (drv3(fdctrl)->drflags & FDRIVE_MOTOR_ON) + retval |= FD_DOR_MOTEN3; +#endif /* DMA enable */ if (fdctrl->dma_en) retval |= FD_DOR_DMAEN; @@ -880,15 +923,18 @@ static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value) { - /* Reset mode */ - if (fdctrl->state & FD_CTRL_RESET) { - if (!(value & FD_DOR_nRESET)) { - FLOPPY_DPRINTF("Floppy controller in RESET state !\n"); - return; - } - } FLOPPY_DPRINTF("digital output register set to 0x%02x\n", value); /* Drive motors state indicators */ +#if MAX_FD == 4 + if (value & FD_DOR_MOTEN3) + fd_start(drv3(fdctrl)); + else + fd_stop(drv3(fdctrl)); + if (value & FD_DOR_MOTEN2) + fd_start(drv2(fdctrl)); + else + fd_stop(drv2(fdctrl)); +#endif if (value & FD_DOR_MOTEN1) fd_start(drv1(fdctrl)); else @@ -958,7 +1004,10 @@ if (fdctrl->data_dir == FD_DIR_READ) retval |= FD_MSR_DIO; } - /* Should handle FD_MSR_NONDMA for SPECIFY command */ + /* Non-DMA indicator */ + if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA && + !fdctrl->dma_en) + retval |= FD_MSR_NONDMA; /* Command busy indicator */ if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA || FD_STATE(fdctrl->data_state) == FD_STATE_STATUS) @@ -1053,6 +1102,40 @@ #endif } +/* Seek to next sector */ +static int fdctrl_seek_to_next_sect (fdctrl_t *fdctrl, fdrive_t *cur_drv) +{ + FLOPPY_DPRINTF("seek to next sector (%d %02x %02x => %d)\n", + cur_drv->head, cur_drv->track, cur_drv->sect, + fd_sector(cur_drv)); + /* XXX: cur_drv->sect >= cur_drv->last_sect should be an + error in fact */ + if (cur_drv->sect >= cur_drv->last_sect || + cur_drv->sect == fdctrl->eot) { + cur_drv->sect = 1; + if (FD_MULTI_TRACK(fdctrl->data_state)) { + if (cur_drv->head == 0 && + (cur_drv->flags & FDISK_DBL_SIDES) != 0) { + cur_drv->head = 1; + } else { + cur_drv->head = 0; + cur_drv->track++; + if ((cur_drv->flags & FDISK_DBL_SIDES) == 0) + return 0; + } + } else { + cur_drv->track++; + return 0; + } + FLOPPY_DPRINTF("seek to next track (%d %02x %02x => %d)\n", + cur_drv->head, cur_drv->track, + cur_drv->sect, fd_sector(cur_drv)); + } else { + cur_drv->sect++; + } + 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) @@ -1139,9 +1222,9 @@ } 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]; @@ -1275,35 +1358,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: @@ -1341,6 +1397,8 @@ if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) { pos %= FD_SECTOR_LEN; if (pos == 0) { + if (fdctrl->data_pos != 0) + fdctrl_seek_to_next_sect(fdctrl, cur_drv); len = fdctrl->data_len - fdctrl->data_pos; if (len > FD_SECTOR_LEN) len = FD_SECTOR_LEN; @@ -1433,6 +1491,7 @@ static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value) { fdrive_t *cur_drv; + int pos; cur_drv = get_cur_drv(fdctrl); /* Reset mode */ @@ -1448,15 +1507,18 @@ /* Is it write command time ? */ if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) { /* FIFO data write */ - fdctrl->fifo[fdctrl->data_pos++] = value; - if (fdctrl->data_pos % FD_SECTOR_LEN == (FD_SECTOR_LEN - 1) || + pos = fdctrl->data_pos; + pos %= FD_SECTOR_LEN; + fdctrl->fifo[pos] = value; + if (pos == FD_SECTOR_LEN - 1 || fdctrl->data_pos == fdctrl->data_len) { bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1); + fdctrl_seek_to_next_sect(fdctrl, cur_drv); } /* Switch from transfer mode to status mode * then from status mode to command mode */ - if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) + if (++fdctrl->data_pos == fdctrl->data_len) fdctrl_stop_transfer(fdctrl, FD_SR0_SEEK, 0x00, 0x00); return; }