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@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