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().
Signed-off-by: Kevin O'Connor kevin@koconnor.net --- src/hw/floppy.c | 215 ++++++++++++++++++++++++++++---------------------------- 1 file changed, 107 insertions(+), 108 deletions(-)
diff --git a/src/hw/floppy.c b/src/hw/floppy.c index 48958e6..e2b494d 100644 --- a/src/hw/floppy.c +++ b/src/hw/floppy.c @@ -219,43 +219,68 @@ floppy_wait_irq(void) return DISK_RET_SUCCESS; }
-struct floppy_pio_s { - u8 cmdlen; - u8 resplen; - u8 waitirq; - u8 data[9]; -}; - static int -floppy_pio(struct floppy_pio_s *pio) +floppy_send(u8 data, u32 end) { - // Send command to controller. - u32 end = timer_calc(FLOPPY_PIO_TIMEOUT); - int i = 0; for (;;) { u8 sts = inb(PORT_FD_STATUS); - if (!(sts & 0x80)) { - if (timer_check(end)) { - warn_timeout(); - floppy_disable_controller(); - return DISK_RET_ETIMEOUT; - } - continue; - } + if (sts & 0x80) + break; if (sts & 0x40) { floppy_disable_controller(); return DISK_RET_ECONTROLLER; } - outb(pio->data[i++], PORT_FD_DATA); - if (i >= pio->cmdlen) - break; + if (timer_check(end)) { + warn_timeout(); + floppy_disable_controller(); + return DISK_RET_ETIMEOUT; + } + } + outb(data, PORT_FD_DATA); + return 0; +} + +// 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) + +static int +floppy_pio(int command, u8 *param) +{ + dprintf(9, "Floppy pio command %x\n", command); + // Send command (and parameters if any) to controller. + u32 end = timer_calc(FLOPPY_PIO_TIMEOUT); + int send = (command >> 8) & 0xf; + int ret = floppy_send(command & 0xff, end); + if (ret) + return ret; + int i; + for (i=0; i<send; i++) { + ret = floppy_send(param[i], end); + if (ret) + return ret; }
// Wait for command to complete. - if (pio->waitirq) { - int ret = floppy_wait_irq(); + int receive = (command >> 12) & 0xf; + if (command & FCF_WAITIRQ) { + ret = floppy_wait_irq(); if (ret) return ret; + + if (!receive) { + // Send Check IRQ Status command to clear irq and get status. + end = timer_calc(FLOPPY_PIO_TIMEOUT); + ret = floppy_send(FC_CHECKIRQ & 0xff, end); + if (ret) + return ret; + receive = (FC_CHECKIRQ >> 12) & 0xf; + } }
// Read response from controller. @@ -271,13 +296,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; @@ -293,12 +323,8 @@ floppy_enable_controller(void) 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); }
static int @@ -339,22 +365,10 @@ floppy_drive_recal(u8 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; + ret = floppy_pio(FC_RECALIBRATE, param); if (ret) return ret;
@@ -375,16 +389,12 @@ floppy_drive_readid(u8 floppyid, u8 data_rate, u8 head) 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 + ret = floppy_pio(FC_READID, param); if (ret) return ret; - if (pio.data[0] & 0xc0) + if (param[0] & 0xc0) return -1; return 0; } @@ -458,44 +468,42 @@ 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); + ret = floppy_select_drive(param[0] & 1); if (ret) return ret; - pio->resplen = 7; - pio->waitirq = 1; - ret = floppy_pio(pio); + ret = floppy_pio(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; } @@ -545,21 +553,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 +579,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 +624,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