[Qemu-devel] [PATCH] FDC: fix status0/1/2 handling

[Hervé Poussineau]

Hi,

Attached patch fixes handling of status0, status1 and status2 values in 
the floppy controller. Those values are not calculated anymore at the 
end of the command, but during its execution.
Incidentally, this also removes a hack from 
fdctrl_handle_sense_interrupt_status().

Hervé

Index: fdc.c
===================================================================
--- fdc.c       (revision 4175)
+++ fdc.c       (working copy)
@@ -321,7 +321,7 @@
 static void fdctrl_reset_fifo (fdctrl_t *fdctrl);
 static int fdctrl_transfer_handler (void *opaque, int nchan,
                                     int dma_pos, int dma_len);
-static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status);
+static void fdctrl_raise_irq (fdctrl_t *fdctrl);
 static void fdctrl_result_timer(void *opaque);
 
 static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl);
@@ -357,7 +357,6 @@
     FD_STATE_DATA   = 0x02,
     FD_STATE_STATE  = 0x03,
     FD_STATE_MULTI  = 0x10,
-    FD_STATE_SEEK   = 0x20,
     FD_STATE_FORMAT = 0x40,
 };
 
@@ -421,6 +420,15 @@
     FD_SR0_RDYCHG   = 0xc0,
 };
 
+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_DOR_SELMASK  = 0x01,
     FD_DOR_nRESET   = 0x04,
@@ -460,7 +468,6 @@
 #define FD_SET_STATE(state, new_state) \
 do { (state) = ((state) & ~FD_STATE_STATE) | (new_state); } while (0)
 #define FD_MULTI_TRACK(state) ((state) & FD_STATE_MULTI)
-#define FD_DID_SEEK(state) ((state) & FD_STATE_SEEK)
 #define FD_FORMAT_CMD(state) ((state) & FD_STATE_FORMAT)
 
 struct fdctrl_t {
@@ -477,13 +484,15 @@
     uint8_t dma_en;
     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 */
@@ -622,13 +631,17 @@
     qemu_put_8s(f, &s->dma_en);
     qemu_put_8s(f, &s->cur_drv);
     qemu_put_8s(f, &s->bootsel);
+    qemu_put_8s(f, &s->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);
@@ -666,13 +679,17 @@
     qemu_get_8s(f, &s->dma_en);
     qemu_get_8s(f, &s->cur_drv);
     qemu_get_8s(f, &s->bootsel);
+    qemu_get_8s(f, &s->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);
@@ -807,20 +824,18 @@
     fdctrl->state &= ~FD_CTRL_INTR;
 }
 
-static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status)
+static void fdctrl_raise_irq (fdctrl_t *fdctrl)
 {
     // Sparc mutation
     if (fdctrl->sun4m && !fdctrl->dma_en) {
         fdctrl->state &= ~FD_CTRL_BUSY;
-        fdctrl->int_status = status;
         return;
     }
     if (~(fdctrl->state & FD_CTRL_INTR)) {
         qemu_set_irq(fdctrl->irq, 1);
         fdctrl->state |= FD_CTRL_INTR;
     }
-    FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", status);
-    fdctrl->int_status = status;
+    FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", fdctrl->status0);
 }
 
 /* Reset controller */
@@ -840,8 +855,10 @@
     for (i = 0; i < MAX_FD; i++)
         fd_reset(&fdctrl->drives[i]);
     fdctrl_reset_fifo(fdctrl);
-    if (do_irq)
-        fdctrl_raise_irq(fdctrl, FD_SR0_RDYCHG);
+    if (do_irq) {
+        fdctrl->status0 |= FD_SR0_RDYCHG;
+        fdctrl_raise_irq(fdctrl);
+    }
 }
 
 static inline fdrive_t *drv0 (fdctrl_t *fdctrl)
@@ -1043,7 +1060,7 @@
     fdctrl->data_pos = 0;
     FD_SET_STATE(fdctrl->data_state, FD_STATE_STATUS);
     if (do_irq)
-        fdctrl_raise_irq(fdctrl, 0x00);
+        fdctrl_raise_irq(fdctrl);
 }
 
 /* Set an error: unimplemented/unknown command */
@@ -1065,18 +1082,21 @@
 }
 
 /* Callback for transfer end (stop or abort) */
-static void fdctrl_stop_transfer (fdctrl_t *fdctrl, uint8_t status0,
-                                  uint8_t status1, uint8_t status2)
+static void fdctrl_stop_transfer (fdctrl_t *fdctrl)
 {
     fdrive_t *cur_drv;
+    uint8_t status0 = fdctrl->status0;
 
     cur_drv = get_cur_drv(fdctrl);
+    /* Add head and drive to status0 */
+    status0 |= (cur_drv->head << 2) | fdctrl->cur_drv;
+
     FLOPPY_DPRINTF("transfer status: %02x %02x %02x (%02x)\n",
-                   status0, status1, status2,
-                   status0 | (cur_drv->head << 2) | fdctrl->cur_drv);
-    fdctrl->fifo[0] = status0 | (cur_drv->head << 2) | fdctrl->cur_drv;
-    fdctrl->fifo[1] = status1;
-    fdctrl->fifo[2] = status2;
+                   fdctrl->status0, fdctrl->status1, fdctrl->status2,
+                   status0);
+    fdctrl->fifo[0] = status0;
+    fdctrl->fifo[1] = fdctrl->status1;
+    fdctrl->fifo[2] = fdctrl->status2;
     fdctrl->fifo[3] = cur_drv->track;
     fdctrl->fifo[4] = cur_drv->head;
     fdctrl->fifo[5] = cur_drv->sect;
@@ -1094,7 +1114,6 @@
 {
     fdrive_t *cur_drv;
     uint8_t kh, kt, ks;
-    int did_seek;
 
     fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
     cur_drv = get_cur_drv(fdctrl);
@@ -1104,31 +1123,35 @@
     FLOPPY_DPRINTF("Start transfer at %d %d %02x %02x (%d)\n",
                    fdctrl->cur_drv, kh, kt, ks,
                    _fd_sector(kh, kt, ks, cur_drv->last_sect));
-    did_seek = 0;
-    switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & 0x40)) {
+    switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & FD_CONFIG_EIS)) {
     case 2:
         /* sect too big */
-        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
+        fdctrl->status0 |= FD_SR0_ABNTERM;
+        fdctrl_stop_transfer(fdctrl);
         fdctrl->fifo[3] = kt;
         fdctrl->fifo[4] = kh;
         fdctrl->fifo[5] = ks;
         return;
     case 3:
         /* track too big */
-        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x80, 0x00);
+        fdctrl->status0 |= FD_SR0_ABNTERM;
+        fdctrl->status1 |= FD_SR1_EC;
+        fdctrl_stop_transfer(fdctrl);
         fdctrl->fifo[3] = kt;
         fdctrl->fifo[4] = kh;
         fdctrl->fifo[5] = ks;
         return;
     case 4:
         /* No seek enabled */
-        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
+        fdctrl->status0 |= FD_SR0_ABNTERM;
+        fdctrl_stop_transfer(fdctrl);
         fdctrl->fifo[3] = kt;
         fdctrl->fifo[4] = kh;
         fdctrl->fifo[5] = ks;
         return;
+    case 0:
     case 1:
-        did_seek = 1;
+        fdctrl->status0 |= FD_SR0_SEEK;
         break;
     default:
         break;
@@ -1141,10 +1164,6 @@
         fdctrl->data_state |= FD_STATE_MULTI;
     else
         fdctrl->data_state &= ~FD_STATE_MULTI;
-    if (did_seek)
-        fdctrl->data_state |= FD_STATE_SEEK;
-    else
-        fdctrl->data_state &= ~FD_STATE_SEEK;
     if (fdctrl->fifo[5] == 00) {
         fdctrl->data_len = fdctrl->fifo[8];
     } else {
@@ -1183,7 +1202,7 @@
     }
     FLOPPY_DPRINTF("start non-DMA transfer\n");
     /* IO based transfer: calculate len */
-    fdctrl_raise_irq(fdctrl, 0x00);
+    fdctrl_raise_irq(fdctrl);
 
     return;
 }
@@ -1194,7 +1213,8 @@
     /* We don't handle deleted data,
      * so we don't return *ANYTHING*
      */
-    fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00);
+    fdctrl->status0 |= FD_SR0_ABNTERM;
+    fdctrl_stop_transfer(fdctrl);
 }
 
 /* handlers for DMA transfers */
@@ -1204,7 +1224,6 @@
     fdctrl_t *fdctrl;
     fdrive_t *cur_drv;
     int len, start_pos, rel_pos;
-    uint8_t status0 = 0x00, status1 = 0x00, status2 = 0x00;
 
     fdctrl = opaque;
     if (!(fdctrl->state & FD_CTRL_BUSY)) {
@@ -1212,16 +1231,11 @@
         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;
     if (dma_len > fdctrl->data_len)
         dma_len = fdctrl->data_len;
     if (cur_drv->bs == NULL) {
-        if (fdctrl->data_dir == FD_DIR_WRITE)
-            fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 
0x00);
-        else
-            fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
+        fdctrl->status0 |= FD_SR0_ABNTERM;
+        fdctrl_stop_transfer(fdctrl);
         len = 0;
         goto transfer_error;
     }
@@ -1258,8 +1272,9 @@
                              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));
-                fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 
0x00, 0x00);
+                FLOPPY_ERROR("writing sector %d\n", fd_sector(cur_drv));
+                fdctrl->status0 |= FD_SR0_ABNTERM;
+                fdctrl_stop_transfer(fdctrl);
                 goto transfer_error;
             }
             break;
@@ -1271,14 +1286,16 @@
                 DMA_read_memory (nchan, tmpbuf, fdctrl->data_pos, len);
                 ret = memcmp(tmpbuf, fdctrl->fifo + rel_pos, len);
                 if (ret == 0) {
-                    status2 = 0x08;
+                    fdctrl->status2 |= FD_SR2_SEH;
+                    fdctrl->status2 &= ~FD_SR2_SNS;
                     goto end_transfer;
                 }
                 if ((ret < 0 && fdctrl->data_dir == FD_DIR_SCANL) ||
                     (ret > 0 && fdctrl->data_dir == FD_DIR_SCANH)) {
-                    status2 = 0x00;
+                    fdctrl->status2 &= ~FD_SR2_SNS;
                     goto end_transfer;
                 }
+                fdctrl->status2 |= FD_SR2_SNS;
             }
             break;
         }
@@ -1315,21 +1332,15 @@
             } else {
                 cur_drv->sect++;
             }
+            fdctrl->status0 |= FD_SR0_SEEK;
         }
     }
  end_transfer:
     len = fdctrl->data_pos - start_pos;
     FLOPPY_DPRINTF("end transfer %d %d %d\n",
                    fdctrl->data_pos, len, fdctrl->data_len);
-    if (fdctrl->data_dir == FD_DIR_SCANE ||
-        fdctrl->data_dir == FD_DIR_SCANL ||
-        fdctrl->data_dir == FD_DIR_SCANH)
-        status2 = 0x08;
-    if (FD_DID_SEEK(fdctrl->data_state))
-        status0 |= FD_SR0_SEEK;
     fdctrl->data_len -= len;
-    //    if (fdctrl->data_len == 0)
-    fdctrl_stop_transfer(fdctrl, status0, status1, status2);
+    fdctrl_stop_transfer(fdctrl);
  transfer_error:
 
     return len;
@@ -1365,7 +1376,7 @@
          * then from status mode to command mode
          */
         if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
-            fdctrl_stop_transfer(fdctrl, FD_SR0_SEEK, 0x00, 0x00);
+            fdctrl_stop_transfer(fdctrl);
         } else {
             fdctrl_reset_fifo(fdctrl);
             fdctrl_reset_irq(fdctrl);
@@ -1380,7 +1391,6 @@
 {
     fdrive_t *cur_drv;
     uint8_t kh, kt, ks;
-    int did_seek;
 
     fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
     cur_drv = get_cur_drv(fdctrl);
@@ -1390,32 +1400,35 @@
     FLOPPY_DPRINTF("format sector at %d %d %02x %02x (%d)\n",
                    fdctrl->cur_drv, kh, kt, ks,
                    _fd_sector(kh, kt, ks, cur_drv->last_sect));
-    did_seek = 0;
     switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & FD_CONFIG_EIS)) {
     case 2:
         /* sect too big */
-        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
+        fdctrl->status0 |= FD_SR0_ABNTERM;
+        fdctrl_stop_transfer(fdctrl);
         fdctrl->fifo[3] = kt;
         fdctrl->fifo[4] = kh;
         fdctrl->fifo[5] = ks;
         return;
     case 3:
         /* track too big */
-        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x80, 0x00);
+        fdctrl->status0 |= FD_SR0_ABNTERM;
+        fdctrl->status1 |= FD_SR1_EC;
+        fdctrl_stop_transfer(fdctrl);
         fdctrl->fifo[3] = kt;
         fdctrl->fifo[4] = kh;
         fdctrl->fifo[5] = ks;
         return;
     case 4:
         /* No seek enabled */
-        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
+        fdctrl->status0 |= FD_SR0_ABNTERM;
+        fdctrl_stop_transfer(fdctrl);
         fdctrl->fifo[3] = kt;
         fdctrl->fifo[4] = kh;
         fdctrl->fifo[5] = ks;
         return;
+    case 0:
     case 1:
-        did_seek = 1;
-        fdctrl->data_state |= FD_STATE_SEEK;
+        fdctrl->status0 |= FD_SR0_SEEK;
         break;
     default:
         break;
@@ -1424,15 +1437,13 @@
     if (cur_drv->bs == NULL ||
         bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1) < 0) {
         FLOPPY_ERROR("formatting sector %d\n", fd_sector(cur_drv));
-        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00);
+        fdctrl->status0 |= FD_SR0_ABNTERM;
+        fdctrl_stop_transfer(fdctrl);
     } else {
         if (cur_drv->sect == cur_drv->last_sect) {
             fdctrl->data_state &= ~FD_STATE_FORMAT;
             /* Last sector done */
-            if (FD_DID_SEEK(fdctrl->data_state))
-                fdctrl_stop_transfer(fdctrl, FD_SR0_SEEK, 0x00, 0x00);
-            else
-                fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
+            fdctrl_stop_transfer(fdctrl);
         } else {
             /* More to do */
             fdctrl->data_pos = 0;
@@ -1546,7 +1557,6 @@
         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
@@ -1561,7 +1571,7 @@
      * the sector with the specified fill byte
      */
     fdctrl->data_state &= ~FD_STATE_FORMAT;
-    fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
+    fdctrl_stop_transfer(fdctrl);
 }
 
 static void fdctrl_handle_specify (fdctrl_t *fdctrl, int direction)
@@ -1598,27 +1608,20 @@
     fd_recalibrate(cur_drv);
     fdctrl_reset_fifo(fdctrl);
     /* Raise Interrupt */
-    fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
+    fdctrl->status0 |= FD_SR0_SEEK;
+    fdctrl_raise_irq(fdctrl);
 }
 
 static void fdctrl_handle_sense_interrupt_status (fdctrl_t *fdctrl, int 
direction)
 {
     fdrive_t *cur_drv = get_cur_drv(fdctrl);
 
-#if 0
     fdctrl->fifo[0] =
-        fdctrl->int_status | (cur_drv->head << 2) | fdctrl->cur_drv;
-#else
-    /* XXX: int_status handling is broken for read/write
-       commands, so we do this hack. It should be suppressed
-       ASAP */
-    fdctrl->fifo[0] =
-        0x20 | (cur_drv->head << 2) | fdctrl->cur_drv;
-#endif
+        fdctrl->status0 | (cur_drv->head << 2) | fdctrl->cur_drv;
     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)
@@ -1633,13 +1636,13 @@
     else
         cur_drv->dir = 0;
     fdctrl_reset_fifo(fdctrl);
-    if (fdctrl->fifo[2] > cur_drv->max_track) {
-        fdctrl_raise_irq(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK);
-    } else {
+    fdctrl->status0 |= FD_SR0_SEEK;
+    if (fdctrl->fifo[2] > cur_drv->max_track)
+        fdctrl->status0 |= FD_SR0_ABNTERM;
+    else
         cur_drv->track = fdctrl->fifo[2];
-        /* Raise Interrupt */
-        fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
-    }
+    /* Raise Interrupt */
+    fdctrl_raise_irq(fdctrl);
 }
 
 static void fdctrl_handle_perpendicular_mode (fdctrl_t *fdctrl, int direction)
@@ -1709,7 +1712,8 @@
         cur_drv->track += fdctrl->fifo[2];
     }
     fdctrl_reset_fifo(fdctrl);
-    fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
+    fdctrl->status0 |= FD_SR0_SEEK;
+    fdctrl_raise_irq(fdctrl);
 }
 
 static void fdctrl_handle_relative_seek_in (fdctrl_t *fdctrl, int direction)
@@ -1727,7 +1731,8 @@
     }
     fdctrl_reset_fifo(fdctrl);
     /* Raise Interrupt */
-    fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
+    fdctrl->status0 |= FD_SR0_SEEK;
+    fdctrl_raise_irq(fdctrl);
 }
 
 static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
@@ -1798,7 +1803,7 @@
          * then from status mode to command mode
          */
         if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA)
-            fdctrl_stop_transfer(fdctrl, FD_SR0_SEEK, 0x00, 0x00);
+            fdctrl_stop_transfer(fdctrl);
         return;
     }
     if (fdctrl->data_pos == 0) {
@@ -1828,6 +1833,15 @@
             return;
         }
 
+        if (fdctrl->fifo[0] != FD_CMD_SENSE_INTERRUPT_STATUS)
+        {
+            /* Reset status0, except for SENSE_INTERRUPT_STATUS
+             * which returns it */
+            fdctrl->status0 = 0;
+        }
+        fdctrl->status1 = 0;
+        fdctrl->status2 = 0;
+
         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);
@@ -1850,5 +1864,5 @@
     if (cur_drv->last_sect != 0) {
         cur_drv->sect = (cur_drv->sect % cur_drv->last_sect) + 1;
     }
-    fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
+    fdctrl_stop_transfer(fdctrl);
 }