[SeaBIOS] [PATCHv2 1/3] floppy: Encode command and flags into single value in floppy pio code.

Kevin O'Connor kevin at koconnor.net
Thu Dec 12 18:06:35 CET 2013


Eliminate struct floppy_pio_s and change floppy_pio() to take the
command type and parameters in a single encoded value.  This makes the
code similar to the ps2port.c pio code.  It also reduces some of the
boilerplate code in the callers of floppy_pio().

Also, collapse pairs of floppy_select_drive() and floppy_pio() calls
into new call floppy_drive_pio().

Signed-off-by: Kevin O'Connor <kevin at koconnor.net>
---
The v2 of this patch introduces floppy_drive_pio() which provides
further simplification to the code.

The rest of this patch series is largely unchanged from v1.
---
 src/hw/floppy.c | 205 +++++++++++++++++++++++++-------------------------------
 1 file changed, 93 insertions(+), 112 deletions(-)

diff --git a/src/hw/floppy.c b/src/hw/floppy.c
index 48958e6..7d38b12 100644
--- a/src/hw/floppy.c
+++ b/src/hw/floppy.c
@@ -219,18 +219,23 @@ floppy_wait_irq(void)
     return DISK_RET_SUCCESS;
 }
 
-struct floppy_pio_s {
-    u8 cmdlen;
-    u8 resplen;
-    u8 waitirq;
-    u8 data[9];
-};
-
+// Floppy commands
+#define FCF_WAITIRQ 0x10000
+#define FC_CHECKIRQ    (0x08 | (0<<8) | (2<<12))
+#define FC_RECALIBRATE (0x07 | (1<<8) | (0<<12) | FCF_WAITIRQ)
+#define FC_READID      (0x4a | (1<<8) | (7<<12) | FCF_WAITIRQ)
+#define FC_READ        (0xe6 | (8<<8) | (7<<12) | FCF_WAITIRQ)
+#define FC_WRITE       (0xc5 | (8<<8) | (7<<12) | FCF_WAITIRQ)
+#define FC_FORMAT      (0x4d | (5<<8) | (7<<12) | FCF_WAITIRQ)
+
+// Send the specified command and it's parameters to the floppy controller.
 static int
-floppy_pio(struct floppy_pio_s *pio)
+floppy_pio(int command, u8 *param)
 {
-    // Send command to controller.
+    dprintf(9, "Floppy pio command %x\n", command);
+    // Send command and parameters to controller.
     u32 end = timer_calc(FLOPPY_PIO_TIMEOUT);
+    int send = (command >> 8) & 0xf;
     int i = 0;
     for (;;) {
         u8 sts = inb(PORT_FD_STATUS);
@@ -246,13 +251,16 @@ floppy_pio(struct floppy_pio_s *pio)
             floppy_disable_controller();
             return DISK_RET_ECONTROLLER;
         }
-        outb(pio->data[i++], PORT_FD_DATA);
-        if (i >= pio->cmdlen)
+        if (i == 0)
+            outb(command & 0xff, PORT_FD_DATA);
+        else
+            outb(param[i-1], PORT_FD_DATA);
+        if (i++ >= send)
             break;
     }
 
     // Wait for command to complete.
-    if (pio->waitirq) {
+    if (command & FCF_WAITIRQ) {
         int ret = floppy_wait_irq();
         if (ret)
             return ret;
@@ -260,6 +268,7 @@ floppy_pio(struct floppy_pio_s *pio)
 
     // Read response from controller.
     end = timer_calc(FLOPPY_PIO_TIMEOUT);
+    int receive = (command >> 12) & 0xf;
     i = 0;
     for (;;) {
         u8 sts = inb(PORT_FD_STATUS);
@@ -271,13 +280,18 @@ floppy_pio(struct floppy_pio_s *pio)
             }
             continue;
         }
-        if (i >= pio->resplen)
+        if (i >= receive) {
+            if (sts & 0x40) {
+                floppy_disable_controller();
+                return DISK_RET_ECONTROLLER;
+            }
             break;
+        }
         if (!(sts & 0x40)) {
             floppy_disable_controller();
             return DISK_RET_ECONTROLLER;
         }
-        pio->data[i++] = inb(PORT_FD_DATA);
+        param[i++] = inb(PORT_FD_DATA);
     }
 
     return DISK_RET_SUCCESS;
@@ -287,26 +301,21 @@ static int
 floppy_enable_controller(void)
 {
     dprintf(2, "Floppy_enable_controller\n");
+    SET_BDA(floppy_motor_counter, FLOPPY_MOTOR_TICKS);
     floppy_dor_write(0x00);
     floppy_dor_write(0x0c);
     int ret = floppy_wait_irq();
     if (ret)
         return ret;
 
-    struct floppy_pio_s pio;
-    pio.cmdlen = 1;
-    pio.resplen = 2;
-    pio.waitirq = 0;
-    pio.data[0] = 0x08;  // 08: Check Interrupt Status
-    return floppy_pio(&pio);
+    u8 param[2];
+    return floppy_pio(FC_CHECKIRQ, param);
 }
 
+// Activate a drive and send a command to it.
 static int
-floppy_select_drive(u8 floppyid)
+floppy_drive_pio(u8 floppyid, int command, u8 *param)
 {
-    // reset the disk motor timeout value of INT 08
-    SET_BDA(floppy_motor_counter, FLOPPY_MOTOR_TICKS);
-
     // Enable controller if it isn't running.
     if (!(GET_LOW(FloppyDOR) & 0x04)) {
         int ret = floppy_enable_controller();
@@ -314,9 +323,20 @@ floppy_select_drive(u8 floppyid)
             return ret;
     }
 
+    // reset the disk motor timeout value of INT 08
+    SET_BDA(floppy_motor_counter, FLOPPY_MOTOR_TICKS);
+
     // Turn on motor of selected drive, DMA & int enabled, normal operation
     floppy_dor_write((floppyid ? 0x20 : 0x10) | 0x0c | floppyid);
 
+    // Send command.
+    int ret = floppy_pio(command, param);
+    if (ret)
+        return ret;
+
+    // Check IRQ command is needed after irq commands with no results
+    if ((command & FCF_WAITIRQ) && ((command >> 12) & 0xf) == 0)
+        return floppy_pio(FC_CHECKIRQ, param);
     return DISK_RET_SUCCESS;
 }
 
@@ -335,26 +355,10 @@ static int
 floppy_drive_recal(u8 floppyid)
 {
     dprintf(2, "Floppy_drive_recal %d\n", floppyid);
-    int ret = floppy_select_drive(floppyid);
-    if (ret)
-        return ret;
-
-    // send Recalibrate command (2 bytes) to controller
-    struct floppy_pio_s pio;
-    pio.cmdlen = 2;
-    pio.resplen = 0;
-    pio.waitirq = 1;
-    pio.data[0] = 0x07;  // 07: Recalibrate
-    pio.data[1] = floppyid; // 0=drive0, 1=drive1
-    ret = floppy_pio(&pio);
-    if (ret)
-        return ret;
-
-    pio.cmdlen = 1;
-    pio.resplen = 2;
-    pio.waitirq = 0;
-    pio.data[0] = 0x08;  // 08: Check Interrupt Status
-    ret = floppy_pio(&pio);
+    // send Recalibrate command to controller
+    u8 param[2];
+    param[0] = floppyid;
+    int ret = floppy_drive_pio(floppyid, FC_RECALIBRATE, param);
     if (ret)
         return ret;
 
@@ -367,24 +371,16 @@ floppy_drive_recal(u8 floppyid)
 static int
 floppy_drive_readid(u8 floppyid, u8 data_rate, u8 head)
 {
-    int ret = floppy_select_drive(floppyid);
-    if (ret)
-        return ret;
-
     // Set data rate.
     outb(data_rate, PORT_FD_DIR);
 
     // send Read Sector Id command
-    struct floppy_pio_s pio;
-    pio.cmdlen = 2;
-    pio.resplen = 7;
-    pio.waitirq = 1;
-    pio.data[0] = 0x4a;  // 0a: Read Sector Id
-    pio.data[1] = (head << 2) | floppyid; // HD DR1 DR2
-    ret = floppy_pio(&pio);
+    u8 param[7];
+    param[0] = (head << 2) | floppyid; // HD DR1 DR2
+    int ret = floppy_drive_pio(floppyid, FC_READID, param);
     if (ret)
         return ret;
-    if (pio.data[0] & 0xc0)
+    if (param[0] & 0xc0)
         return -1;
     return 0;
 }
@@ -458,44 +454,39 @@ check_recal_drive(struct drive_s *drive_gf)
 
 // Perform a floppy transfer command (setup DMA and issue PIO).
 static int
-floppy_cmd(struct disk_op_s *op, int blocksize, struct floppy_pio_s *pio)
+floppy_cmd(struct disk_op_s *op, int blocksize, int command, u8 *param)
 {
     int ret = check_recal_drive(op->drive_gf);
     if (ret)
         return ret;
 
     // Setup DMA controller
-    int isWrite = pio->data[0] != 0xe6;
+    int isWrite = command != FC_READ;
     ret = dma_floppy((u32)op->buf_fl, op->count * blocksize, isWrite);
     if (ret)
         return DISK_RET_EBOUNDARY;
 
     // Invoke floppy controller
-    ret = floppy_select_drive(pio->data[1] & 1);
-    if (ret)
-        return ret;
-    pio->resplen = 7;
-    pio->waitirq = 1;
-    ret = floppy_pio(pio);
+    ret = floppy_drive_pio(param[0] & 1, command, param);
     if (ret)
         return ret;
 
     // Populate floppy_return_status in BDA
     int i;
     for (i=0; i<7; i++)
-        SET_BDA(floppy_return_status[i], pio->data[i]);
+        SET_BDA(floppy_return_status[i], param[i]);
 
-    if (pio->data[0] & 0xc0) {
-        if (pio->data[1] & 0x02)
+    if (param[0] & 0xc0) {
+        if (param[1] & 0x02)
             return DISK_RET_EWRITEPROTECT;
         dprintf(1, "floppy error: %02x %02x %02x %02x %02x %02x %02x\n"
-                , pio->data[0], pio->data[1], pio->data[2], pio->data[3]
-                , pio->data[4], pio->data[5], pio->data[6]);
+                , param[0], param[1], param[2], param[3]
+                , param[4], param[5], param[6]);
         return DISK_RET_ECONTROLLER;
     }
 
-    u8 track = (pio->cmdlen == 9 ? pio->data[3] : 0);
-    set_diskette_current_cyl(pio->data[0] & 1, track);
+    u8 track = (command != FC_FORMAT ? param[3] : 0);
+    set_diskette_current_cyl(param[0] & 1, track);
 
     return DISK_RET_SUCCESS;
 }
@@ -528,7 +519,6 @@ lba2chs(struct disk_op_s *op)
 static int
 floppy_reset(struct disk_op_s *op)
 {
-    u8 floppyid = GET_GLOBALFLAT(op->drive_gf->cntl_id);
     SET_BDA(floppy_recalibration_status, 0);
     SET_BDA(floppy_media_state[0], 0);
     SET_BDA(floppy_media_state[1], 0);
@@ -536,7 +526,7 @@ floppy_reset(struct disk_op_s *op)
     SET_BDA(floppy_track[1], 0);
     SET_BDA(floppy_last_data_rate, 0);
     floppy_disable_controller();
-    return floppy_select_drive(floppyid);
+    return floppy_enable_controller();
 }
 
 // Read Diskette Sectors
@@ -545,21 +535,18 @@ floppy_read(struct disk_op_s *op)
 {
     struct chs_s chs = lba2chs(op);
 
-    // send read-normal-data command (9 bytes) to controller
+    // send read-normal-data command to controller
     u8 floppyid = GET_GLOBALFLAT(op->drive_gf->cntl_id);
-    struct floppy_pio_s pio;
-    pio.cmdlen = 9;
-    pio.data[0] = 0xe6; // e6: read normal data
-    pio.data[1] = (chs.head << 2) | floppyid; // HD DR1 DR2
-    pio.data[2] = chs.cylinder;
-    pio.data[3] = chs.head;
-    pio.data[4] = chs.sector;
-    pio.data[5] = FLOPPY_SIZE_CODE;
-    pio.data[6] = chs.sector + op->count - 1; // last sector to read on track
-    pio.data[7] = FLOPPY_GAPLEN;
-    pio.data[8] = FLOPPY_DATALEN;
-
-    int res = floppy_cmd(op, DISK_SECTOR_SIZE, &pio);
+    u8 param[8];
+    param[0] = (chs.head << 2) | floppyid; // HD DR1 DR2
+    param[1] = chs.cylinder;
+    param[2] = chs.head;
+    param[3] = chs.sector;
+    param[4] = FLOPPY_SIZE_CODE;
+    param[5] = chs.sector + op->count - 1; // last sector to read on track
+    param[6] = FLOPPY_GAPLEN;
+    param[7] = FLOPPY_DATALEN;
+    int res = floppy_cmd(op, DISK_SECTOR_SIZE, FC_READ, param);
     if (res)
         goto fail;
     return DISK_RET_SUCCESS;
@@ -574,21 +561,18 @@ floppy_write(struct disk_op_s *op)
 {
     struct chs_s chs = lba2chs(op);
 
-    // send write-normal-data command (9 bytes) to controller
+    // send write-normal-data command to controller
     u8 floppyid = GET_GLOBALFLAT(op->drive_gf->cntl_id);
-    struct floppy_pio_s pio;
-    pio.cmdlen = 9;
-    pio.data[0] = 0xc5; // c5: write normal data
-    pio.data[1] = (chs.head << 2) | floppyid; // HD DR1 DR2
-    pio.data[2] = chs.cylinder;
-    pio.data[3] = chs.head;
-    pio.data[4] = chs.sector;
-    pio.data[5] = FLOPPY_SIZE_CODE;
-    pio.data[6] = chs.sector + op->count - 1; // last sector to write on track
-    pio.data[7] = FLOPPY_GAPLEN;
-    pio.data[8] = FLOPPY_DATALEN;
-
-    int res = floppy_cmd(op, DISK_SECTOR_SIZE, &pio);
+    u8 param[8];
+    param[0] = (chs.head << 2) | floppyid; // HD DR1 DR2
+    param[1] = chs.cylinder;
+    param[2] = chs.head;
+    param[3] = chs.sector;
+    param[4] = FLOPPY_SIZE_CODE;
+    param[5] = chs.sector + op->count - 1; // last sector to write on track
+    param[6] = FLOPPY_GAPLEN;
+    param[7] = FLOPPY_DATALEN;
+    int res = floppy_cmd(op, DISK_SECTOR_SIZE, FC_WRITE, param);
     if (res)
         goto fail;
     return DISK_RET_SUCCESS;
@@ -622,18 +606,15 @@ floppy_format(struct disk_op_s *op)
 {
     u8 head = op->lba;
 
-    // send format-track command (6 bytes) to controller
+    // send format-track command to controller
     u8 floppyid = GET_GLOBALFLAT(op->drive_gf->cntl_id);
-    struct floppy_pio_s pio;
-    pio.cmdlen = 6;
-    pio.data[0] = 0x4d; // 4d: format track
-    pio.data[1] = (head << 2) | floppyid; // HD DR1 DR2
-    pio.data[2] = FLOPPY_SIZE_CODE;
-    pio.data[3] = op->count; // number of sectors per track
-    pio.data[4] = FLOPPY_FORMAT_GAPLEN;
-    pio.data[5] = FLOPPY_FILLBYTE;
-
-    return floppy_cmd(op, 4, &pio);
+    u8 param[7];
+    param[0] = (head << 2) | floppyid; // HD DR1 DR2
+    param[1] = FLOPPY_SIZE_CODE;
+    param[2] = op->count; // number of sectors per track
+    param[3] = FLOPPY_FORMAT_GAPLEN;
+    param[4] = FLOPPY_FILLBYTE;
+    return floppy_cmd(op, 4, FC_FORMAT, param);
 }
 
 int
-- 
1.8.3.1




More information about the SeaBIOS mailing list