Fix floppy controller issues v2
PATCH 1/1 Fix floppy controller issues v2
Hi,
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...
Hervé
Index: fdc.c
===================================================================
RCS file: /sources/qemu/qemu/hw/fdc.c,v
retrieving revision 1.37
fdc.c +55 -21
@@ -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); }
@@ -402,7 +401,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)
@@ -622,20 +621,16 @@ fdctrl->dma_chann = dma_chann; fdctrl->io_base = io_base; fdctrl->config =0x60 ;/* Implicit seek, polling & FIFO enabled */ -if (fdctrl->dma_chann !=-1 ) { - fdctrl->dma_en =1 ; +if (fdctrl->dma_chann !=-1 )DMA_register_channel (dma_chann, &fdctrl_transfer_handler, fdctrl); - }else { - fdctrl->dma_en =0 ; - } -for (i =0 ; i <2 ; i++) { +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 <2 ; i++) { +for (i =0 ; i <MAX_FD ; i++) {fd_revalidate (&fdctrl->drives[i]); }
@@ -735,6 +730,7 @@fdctrl_reset_fifo (fdctrl);if (do_irq)fdctrl_raise_irq (fdctrl,0xc0 ); + fdctrl->dma_en = (fdctrl->dma_chann !=-1 ) ?1 :0 ; }static inline fdrive_t *drv0 (fdctrl_t *fdctrl)
@@ -744,12 +740,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) */
@@ -765,9 +790,15 @@uint32_t retval =0 ;/* Drive motors state indicators */ -if (drv0 (fdctrl)->drflags &FDRIVE_MOTOR_ON ) - retval |=1 <<5 ; +#if MAX_FD >=4 +if (drv3 (fdctrl)->drflags &FDRIVE_MOTOR_ON ) + retval |=1 <<7 ; +if (drv2 (fdctrl)->drflags &FDRIVE_MOTOR_ON ) + retval |=1 <<6 ; +#endif if (drv1 (fdctrl)->drflags &FDRIVE_MOTOR_ON ) + retval |=1 <<5 ; +if (drv0 (fdctrl)->drflags &FDRIVE_MOTOR_ON ) retval |=1 <<4 ;/* DMA enable */ retval |= fdctrl->dma_en <<3 ;
@@ -782,15 +813,18 @@static void fdctrl_write_dor (fdctrl_t *fdctrl,uint32_t value) { -/* Reset mode */ -if (fdctrl->state &FD_CTRL_RESET ) { -if (!(value &0x04 )) { -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 &0x80 ) +fd_start (drv3 (fdctrl)); +else +fd_stop (drv3 (fdctrl)); +if (value &0x40 ) +fd_start (drv2 (fdctrl)); +else +fd_stop (drv2 (fdctrl)); +#endif if (value &0x20 )fd_start (drv1 (fdctrl));
else
@@ -818,7 +852,11 @@
}
}
/* Selected drive */
+#if MAX_FD >= 4
+ fdctrl->cur_drv = value & 3;
+#else
fdctrl->cur_drv = value & 1;+#endif
}
/* Tape drive register : 0x03 */
@@ -843,7 +881,11 @@
}
FLOPPY_DPRINTF("tape drive register set to 0x%02x\n", value);
/* Disk boot selection indicator */
+#if MAX_FD >= 4
+ fdctrl->bootsel = (value >> 2) & 3;
+#else
fdctrl->bootsel = (value >> 2) & 1;
+#endif
/* Tape indicators: never allow */}
@@ -860,7 +902,10 @@
if (fdctrl->data_dir == FD_DIR_READ)
retval |= 0x40;
}
- /* Should handle 0x20 for SPECIFY command */
+ /* Non-DMA indicator */
+ if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA &&
+ !fdctrl->dma_en)
+ retval |= 0x20;
/* Command busy indicator */
if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA ||
FD_STATE(fdctrl->data_state) == FD_STATE_STATUS)@@ -956,6 +1001,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)
@@ -1042,9 +1121,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];
@@ -1178,35 +1257,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:
@@ -1244,6 +1296,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;
@@ -1336,6 +1390,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 */
@@ -1351,16 +1406,26 @@
/* 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);
+ }
+ if (++fdctrl->data_pos == fdctrl->data_len) {
+ fdctrl->data_pos = 0;
+ /* Switch from transfer mode to status mode
+ * then from status mode to command mode
+ */
+ if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
+ fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
+ } else {
+ fdctrl_reset_fifo(fdctrl);
+ fdctrl_reset_irq(fdctrl);
+ }
}
- /* Switch from transfer mode to status mode
- * then from status mode to command mode
- */
- if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA)
- fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
return;
}
if (fdctrl->data_pos == 0) {