[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[Qemu-devel] [PATCH] FDC: simplify code [v2]
|
From: |
Hervé Poussineau |
|
Subject: |
[Qemu-devel] [PATCH] FDC: simplify code [v2] |
|
Date: |
Sun, 06 Apr 2008 15:17:14 +0200 |
|
User-agent: |
Thunderbird 2.0.0.12 (Windows/20080213) |
Hi,
Attached patch prevents duplication of quite similar code in fdc.c, and
so, removes 300 lines and 8KB of code.
It first extracts implementation of FDC commands to separate methods,
and then uses a table to know which method to execute instead of a big
switch.
This patch takes cares of Blue Swirl's comments, and replaces linear
search by a direct table lookup.
Hervé
Index: fdc.c
===================================================================
--- fdc.c (revision 4161)
+++ fdc.c (working copy)
@@ -2,6 +2,7 @@
* QEMU Floppy disk emulator (Intel 82078)
*
* Copyright (c) 2003, 2007 Jocelyn Mayer
+ * Copyright (c) 2008 Hervé Poussineau
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to
deal
@@ -372,40 +373,6 @@
};
enum {
- FD_CMD_SPECIFY = 0x03,
- FD_CMD_SENSE_DRIVE_STATUS = 0x04,
- FD_CMD_RECALIBRATE = 0x07,
- FD_CMD_SENSE_INTERRUPT_STATUS = 0x08,
- FD_CMD_DUMPREG = 0x0e,
- FD_CMD_SEEK = 0x0f,
- FD_CMD_VERSION = 0x10,
- FD_CMD_PERPENDICULAR_MODE = 0x12,
- FD_CMD_CONFIGURE = 0x13,
- FD_CMD_UNLOCK = 0x14,
- FD_CMD_POWERDOWN_MODE = 0x17,
- FD_CMD_PART_ID = 0x18,
- FD_CMD_SAVE = 0x2c,
- FD_CMD_OPTION = 0x33,
- FD_CMD_READ_TRACK = 0x42,
- FD_CMD_WRITE = 0x45,
- FD_CMD_READ = 0x46,
- FD_CMD_WRITE_DELETED = 0x49,
- FD_CMD_READ_ID = 0x4a,
- FD_CMD_READ_DELETED = 0x4c,
- FD_CMD_RESTORE = 0x4c,
- FD_CMD_FORMAT_TRACK = 0x4d,
- FD_CMD_SCAN_EQUAL = 0x50,
- FD_CMD_VERIFY = 0x56,
- FD_CMD_SCAN_LOW_OR_EQUAL = 0x59,
- FD_CMD_SCAN_HIGH_OR_EQUAL = 0x5d,
- FD_CMD_DRIVE_SPECIFICATION_COMMAND = 0x8e,
- FD_CMD_RELATIVE_SEEK_OUT = 0x8f,
- FD_CMD_LOCK = 0x94,
- FD_CMD_FORMAT_AND_WRITE = 0xcd,
- FD_CMD_RELATIVE_SEEK_IN = 0xcf,
-};
-
-enum {
FD_CONFIG_PRETRK = 0xff, /* Pre-compensation set to track 0 */
FD_CONFIG_FIFOTHR = 0x0f, /* FIFO threshold set to 1 byte */
FD_CONFIG_POLL = 0x10, /* Poll enabled */
@@ -1046,8 +1013,8 @@
fdctrl_raise_irq(fdctrl, 0x00);
}
-/* Set an error: unimplemented/unknown command */
-static void fdctrl_unimplemented (fdctrl_t *fdctrl)
+/* Set an error: unimplemented command */
+static void fdctrl_unimplemented (fdctrl_t *fdctrl, int direction)
{
#if 0
fdrive_t *cur_drv;
@@ -1064,6 +1031,13 @@
#endif
}
+/* Set an error: unknown command */
+static void fdctrl_unknown (fdctrl_t *fdctrl, int direction)
+{
+ FLOPPY_ERROR("unknown command 0x%02x\n", fdctrl->fifo[0] & 0xff);
+ fdctrl_unimplemented(fdctrl, direction);
+}
+
/* Callback for transfer end (stop or abort) */
static void fdctrl_stop_transfer (fdctrl_t *fdctrl, uint8_t status0,
uint8_t status1, uint8_t status2)
@@ -1441,11 +1415,366 @@
}
}
-static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
+static void fdctrl_handle_lock (fdctrl_t *fdctrl, int direction)
{
+ fdctrl->lock = 1;
+ fdctrl->fifo[0] = 0x10;
+ fdctrl_set_fifo(fdctrl, 1, 1);
+}
+
+static void fdctrl_handle_unlock (fdctrl_t *fdctrl, int direction)
+{
+ fdctrl->lock = 0;
+ fdctrl->fifo[0] = 0;
+ fdctrl_set_fifo(fdctrl, 1, 0);
+}
+
+static void fdctrl_handle_dumpreg (fdctrl_t *fdctrl, int direction)
+{
+ fdrive_t *cur_drv = get_cur_drv(fdctrl);
+
+ /* Drives position */
+ fdctrl->fifo[0] = drv0(fdctrl)->track;
+ fdctrl->fifo[1] = drv1(fdctrl)->track;
+ fdctrl->fifo[2] = 0;
+ fdctrl->fifo[3] = 0;
+ /* timers */
+ fdctrl->fifo[4] = fdctrl->timer0;
+ fdctrl->fifo[5] = (fdctrl->timer1 << 1) | fdctrl->dma_en;
+ fdctrl->fifo[6] = cur_drv->last_sect;
+ fdctrl->fifo[7] = (fdctrl->lock << 7) |
+ (cur_drv->perpendicular << 2);
+ fdctrl->fifo[8] = fdctrl->config;
+ fdctrl->fifo[9] = fdctrl->precomp_trk;
+ fdctrl_set_fifo(fdctrl, 10, 0);
+}
+
+static void fdctrl_handle_version (fdctrl_t *fdctrl, int direction)
+{
+ /* Controller's version */
+ fdctrl->fifo[0] = fdctrl->version;
+ fdctrl_set_fifo(fdctrl, 1, 1);
+}
+
+static void fdctrl_handle_partid (fdctrl_t *fdctrl, int direction)
+{
+ fdctrl->fifo[0] = 0x41; /* Stepping 1 */
+ fdctrl_set_fifo(fdctrl, 1, 0);
+}
+
+static void fdctrl_handle_restore (fdctrl_t *fdctrl, int direction)
+{
+ fdrive_t *cur_drv = get_cur_drv(fdctrl);
+
+ /* Drives position */
+ drv0(fdctrl)->track = fdctrl->fifo[3];
+ drv1(fdctrl)->track = fdctrl->fifo[4];
+ /* timers */
+ fdctrl->timer0 = fdctrl->fifo[7];
+ fdctrl->timer1 = fdctrl->fifo[8];
+ cur_drv->last_sect = fdctrl->fifo[9];
+ fdctrl->lock = fdctrl->fifo[10] >> 7;
+ cur_drv->perpendicular = (fdctrl->fifo[10] >> 2) & 0xF;
+ fdctrl->config = fdctrl->fifo[11];
+ fdctrl->precomp_trk = fdctrl->fifo[12];
+ fdctrl->pwrd = fdctrl->fifo[13];
+ fdctrl_reset_fifo(fdctrl);
+}
+
+static void fdctrl_handle_save (fdctrl_t *fdctrl, int direction)
+{
+ fdrive_t *cur_drv = get_cur_drv(fdctrl);
+
+ fdctrl->fifo[0] = 0;
+ fdctrl->fifo[1] = 0;
+ /* Drives position */
+ fdctrl->fifo[2] = drv0(fdctrl)->track;
+ fdctrl->fifo[3] = drv1(fdctrl)->track;
+ fdctrl->fifo[4] = 0;
+ fdctrl->fifo[5] = 0;
+ /* timers */
+ fdctrl->fifo[6] = fdctrl->timer0;
+ fdctrl->fifo[7] = fdctrl->timer1;
+ fdctrl->fifo[8] = cur_drv->last_sect;
+ fdctrl->fifo[9] = (fdctrl->lock << 7) |
+ (cur_drv->perpendicular << 2);
+ fdctrl->fifo[10] = fdctrl->config;
+ fdctrl->fifo[11] = fdctrl->precomp_trk;
+ fdctrl->fifo[12] = fdctrl->pwrd;
+ fdctrl->fifo[13] = 0;
+ fdctrl->fifo[14] = 0;
+ fdctrl_set_fifo(fdctrl, 15, 1);
+}
+
+static void fdctrl_handle_readid (fdctrl_t *fdctrl, int direction)
+{
+ fdrive_t *cur_drv = get_cur_drv(fdctrl);
+
+ /* XXX: should set main status register to busy */
+ cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
+ qemu_mod_timer(fdctrl->result_timer,
+ qemu_get_clock(vm_clock) + (ticks_per_sec / 50));
+}
+
+static void fdctrl_handle_format_track (fdctrl_t *fdctrl, int direction)
+{
fdrive_t *cur_drv;
+ fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
cur_drv = get_cur_drv(fdctrl);
+ fdctrl->data_state |= FD_STATE_FORMAT;
+ if (fdctrl->fifo[0] & 0x80)
+ 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
+ cur_drv->last_sect =
+ cur_drv->flags & FDISK_DBL_SIDES ? fdctrl->fifo[3] :
+ fdctrl->fifo[3] / 2;
+#else
+ cur_drv->last_sect = fdctrl->fifo[3];
+#endif
+ /* TODO: implement format using DMA expected by the Bochs BIOS
+ * and Linux fdformat (read 3 bytes per sector via DMA and fill
+ * the sector with the specified fill byte
+ */
+ fdctrl->data_state &= ~FD_STATE_FORMAT;
+ fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
+}
+
+static void fdctrl_handle_specify (fdctrl_t *fdctrl, int direction)
+{
+ fdctrl->timer0 = (fdctrl->fifo[1] >> 4) & 0xF;
+ fdctrl->timer1 = fdctrl->fifo[2] >> 1;
+ fdctrl->dma_en = 1 - (fdctrl->fifo[2] & 1) ;
+ /* No result back */
+ fdctrl_reset_fifo(fdctrl);
+}
+
+static void fdctrl_handle_sense_drive_status (fdctrl_t *fdctrl, int direction)
+{
+ fdrive_t *cur_drv;
+
+ fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+ cur_drv = get_cur_drv(fdctrl);
+ cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
+ /* 1 Byte status back */
+ fdctrl->fifo[0] = (cur_drv->ro << 6) |
+ (cur_drv->track == 0 ? 0x10 : 0x00) |
+ (cur_drv->head << 2) |
+ fdctrl->cur_drv |
+ 0x28;
+ fdctrl_set_fifo(fdctrl, 1, 0);
+}
+
+static void fdctrl_handle_recalibrate (fdctrl_t *fdctrl, int direction)
+{
+ fdrive_t *cur_drv;
+
+ fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+ cur_drv = get_cur_drv(fdctrl);
+ fd_recalibrate(cur_drv);
+ fdctrl_reset_fifo(fdctrl);
+ /* Raise Interrupt */
+ fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
+}
+
+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->fifo[1] = cur_drv->track;
+ fdctrl_set_fifo(fdctrl, 2, 0);
+ fdctrl_reset_irq(fdctrl);
+ fdctrl->int_status = FD_SR0_RDYCHG;
+}
+
+static void fdctrl_handle_seek (fdctrl_t *fdctrl, int direction)
+{
+ fdrive_t *cur_drv;
+
+ fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+ cur_drv = get_cur_drv(fdctrl);
+ fd_start(cur_drv);
+ if (fdctrl->fifo[2] <= cur_drv->track)
+ cur_drv->dir = 1;
+ 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 {
+ cur_drv->track = fdctrl->fifo[2];
+ /* Raise Interrupt */
+ fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
+ }
+}
+
+static void fdctrl_handle_perpendicular_mode (fdctrl_t *fdctrl, int direction)
+{
+ fdrive_t *cur_drv = get_cur_drv(fdctrl);
+
+ if (fdctrl->fifo[1] & 0x80)
+ cur_drv->perpendicular = fdctrl->fifo[1] & 0x7;
+ /* No result back */
+ fdctrl_reset_fifo(fdctrl);
+}
+
+static void fdctrl_handle_configure (fdctrl_t *fdctrl, int direction)
+{
+ fdctrl->config = fdctrl->fifo[2];
+ fdctrl->precomp_trk = fdctrl->fifo[3];
+ /* No result back */
+ fdctrl_reset_fifo(fdctrl);
+}
+
+static void fdctrl_handle_powerdown_mode (fdctrl_t *fdctrl, int direction)
+{
+ fdctrl->pwrd = fdctrl->fifo[1];
+ fdctrl->fifo[0] = fdctrl->fifo[1];
+ fdctrl_set_fifo(fdctrl, 1, 1);
+}
+
+static void fdctrl_handle_option (fdctrl_t *fdctrl, int direction)
+{
+ /* No result back */
+ fdctrl_reset_fifo(fdctrl);
+}
+
+static void fdctrl_handle_drive_specification_command (fdctrl_t *fdctrl, int
direction)
+{
+ fdrive_t *cur_drv = get_cur_drv(fdctrl);
+
+ if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x80) {
+ /* Command parameters done */
+ if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x40) {
+ fdctrl->fifo[0] = fdctrl->fifo[1];
+ fdctrl->fifo[2] = 0;
+ fdctrl->fifo[3] = 0;
+ fdctrl_set_fifo(fdctrl, 4, 1);
+ } else {
+ fdctrl_reset_fifo(fdctrl);
+ }
+ } else if (fdctrl->data_len > 7) {
+ /* ERROR */
+ fdctrl->fifo[0] = 0x80 |
+ (cur_drv->head << 2) | fdctrl->cur_drv;
+ fdctrl_set_fifo(fdctrl, 1, 1);
+ }
+}
+
+static void fdctrl_handle_relative_seek_out (fdctrl_t *fdctrl, int direction)
+{
+ fdrive_t *cur_drv = get_cur_drv(fdctrl);
+
+ fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+ cur_drv = get_cur_drv(fdctrl);
+ fd_start(cur_drv);
+ cur_drv->dir = 0;
+ if (fdctrl->fifo[2] + cur_drv->track >= cur_drv->max_track) {
+ cur_drv->track = cur_drv->max_track - 1;
+ } else {
+ cur_drv->track += fdctrl->fifo[2];
+ }
+ fdctrl_reset_fifo(fdctrl);
+ fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
+}
+
+static void fdctrl_handle_relative_seek_in (fdctrl_t *fdctrl, int direction)
+{
+ fdrive_t *cur_drv = get_cur_drv(fdctrl);
+
+ fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
+ cur_drv = get_cur_drv(fdctrl);
+ fd_start(cur_drv);
+ cur_drv->dir = 1;
+ if (fdctrl->fifo[2] > cur_drv->track) {
+ cur_drv->track = 0;
+ } else {
+ cur_drv->track -= fdctrl->fifo[2];
+ }
+ fdctrl_reset_fifo(fdctrl);
+ /* Raise Interrupt */
+ fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
+}
+
+static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
+{
+ fdrive_t *cur_drv;
+ int pos;
+ static const struct {
+ const char* name;
+ int parameters;
+ void (*handler)(fdctrl_t *fdctrl, int direction);
+ int parameter;
+ } handlers[] = {
+ /* 00 */ { "RESTORE", 17, fdctrl_handle_restore },
+ /* 01 */ { "SAVE", 0, fdctrl_handle_save },
+ /* 02 */ { "READ TRACK", 8, fdctrl_start_transfer, FD_DIR_READ },
+ /* 03 */ { "SPECIFY", 2, fdctrl_handle_specify },
+ /* 04 */ { "SENSE DRIVE STATUS", 1, fdctrl_handle_sense_drive_status },
+ /* 05 */ { "WRITE", 8, fdctrl_start_transfer, FD_DIR_WRITE },
+ /* 06 */ { "READ", 8, fdctrl_start_transfer, FD_DIR_READ },
+ /* 07 */ { "RECALIBRATE", 1, fdctrl_handle_recalibrate },
+ /* 08 */ { "SENSE INTERRUPT STATUS", 0,
fdctrl_handle_sense_interrupt_status },
+ /* 09 */ { "WRITE DELETED DATA", 8, fdctrl_start_transfer_del,
FD_DIR_WRITE },
+ /* 0a */ { "READ ID", 1, fdctrl_handle_readid },
+ /* 0b */ { "DRIVE SPECIFICATION COMMAND", 5,
fdctrl_handle_drive_specification_command },
+ /* 0c */ { "READ DELETED DATA", 8, fdctrl_start_transfer_del,
FD_DIR_READ },
+ /* 0d */ { "FORMAT TRACK", 5, fdctrl_handle_format_track },
+ /* 0e */ { "DUMPREG", 0, fdctrl_handle_dumpreg },
+ /* 0f */ { "SEEK", 2, fdctrl_handle_seek },
+ /* 10 */ { "VERSION", 0, fdctrl_handle_version },
+ /* 11 */ { "SCAN EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANE },
+ /* 12 */ { "PERPENDICULAR MODE", 1, fdctrl_handle_perpendicular_mode },
+ /* 13 */ { "CONFIGURE", 3, fdctrl_handle_configure },
+ /* 14 */ { "UNLOCK", 0, fdctrl_handle_unlock },
+ /* 15 */ { "RELATIVE SEEK OUT", 2, fdctrl_handle_relative_seek_out },
+ /* 16 */ { "VERIFY", 8, fdctrl_unimplemented },
+ /* 17 */ { "POWERDOWN MODE", 2, fdctrl_handle_powerdown_mode },
+ /* 18 */ { "PART ID", 0, fdctrl_handle_partid },
+ /* 19 */ { "SCAN LOW OR EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANL
},
+ /* 1a */ { "FORMAT AND WRITE", 10, fdctrl_unimplemented },
+ /* 1b */ { "RELATIVE SEEK IN", 2, fdctrl_handle_relative_seek_in },
+ /* 1c */ { "LOCK", 0, fdctrl_handle_lock },
+ /* 1d */ { "SCAN HIGH OR EQUAL", 8, fdctrl_start_transfer,
FD_DIR_SCANH },
+ /* 1e */ { "OPTION", 1, fdctrl_handle_option },
+ /* 1f */ { "unknown", 0, fdctrl_unknown },
+ };
+ /* Associate command to an index in the 'handlers' array */
+ static const uint8_t command_to_index[] = {
+ /* .0 .1 .2 .3 .4 .5 .6 .7 .8 .9 .a .b .c .d
.e .f */
+
0x1f,0x1f,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0a,0x1f,0x0c,0x0d,0x0e,0x0f,
/* 0. */
+
0x10,0x11,0x12,0x13,0x14,0x1f,0x16,0x17,0x18,0x19,0x1f,0x1f,0x1f,0x1d,0x1f,0x1f,
/* 1. */
+
0x1f,0x1f,0x1f,0x1f,0x1f,0x05,0x06,0x1f,0x1f,0x1f,0x1f,0x1f,0x01,0x1f,0x1f,0x1f,
/* 2. */
+
0x1f,0x11,0x1f,0x1e,0x1f,0x1f,0x16,0x1f,0x1f,0x19,0x1f,0x1f,0x1f,0x1d,0x1f,0x1f,
/* 3. */
+
0x1f,0x1f,0x02,0x1f,0x1f,0x05,0x06,0x1f,0x1f,0x09,0x0a,0x1f,0x00,0x0d,0x1f,0x1f,
/* 4. */
+
0x1f,0x11,0x1f,0x1f,0x1f,0x1f,0x16,0x1f,0x1f,0x19,0x1f,0x1f,0x1f,0x1d,0x1f,0x1f,
/* 5. */
+
0x1f,0x1f,0x1f,0x1f,0x1f,0x05,0x06,0x1f,0x1f,0x1f,0x1f,0x1f,0x0c,0x1f,0x1f,0x1f,
/* 6. */
+
0x1f,0x11,0x1f,0x1f,0x1f,0x1f,0x16,0x1f,0x1f,0x19,0x1f,0x1f,0x1f,0x1d,0x1f,0x1f,
/* 7. */
+
0x1f,0x1f,0x1f,0x1f,0x1f,0x05,0x06,0x1f,0x1f,0x09,0x1f,0x1f,0x0c,0x1f,0x0b,0x15,
/* 8. */
+
0x1f,0x11,0x1f,0x1f,0x1c,0x1f,0x16,0x1f,0x1f,0x19,0x1f,0x1f,0x1f,0x1d,0x1f,0x1f,
/* 9. */
+
0x1f,0x1f,0x1f,0x1f,0x1f,0x05,0x06,0x1f,0x1f,0x1f,0x1f,0x1f,0x0c,0x1f,0x1f,0x1f,
/* a. */
+
0x1f,0x11,0x1f,0x1f,0x1f,0x1f,0x16,0x1f,0x1f,0x19,0x1f,0x1f,0x1f,0x1d,0x1f,0x1f,
/* b. */
+
0x1f,0x1f,0x1f,0x1f,0x1f,0x05,0x06,0x1f,0x1f,0x09,0x1f,0x1f,0x0c,0x1a,0x1f,0x1b,
/* c. */
+
0x1f,0x11,0x1f,0x1f,0x1f,0x1f,0x16,0x1f,0x1f,0x19,0x1f,0x1f,0x1f,0x1d,0x1f,0x1f,
/* d. */
+
0x1f,0x1f,0x1f,0x1f,0x1f,0x05,0x06,0x1f,0x1f,0x1f,0x1f,0x1f,0x0c,0x1f,0x1f,0x1f,
/* e. */
+
0x1f,0x11,0x1f,0x1f,0x1f,0x1f,0x16,0x1f,0x1f,0x19,0x1f,0x1f,0x1f,0x1d,0x1f,0x1f,
/* f. */
+ };
+
/* Reset mode */
if (fdctrl->state & FD_CTRL_RESET) {
FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
@@ -1462,6 +1791,7 @@
fdctrl->fifo[fdctrl->data_pos++] = value;
if (fdctrl->data_pos % FD_SECTOR_LEN == (FD_SECTOR_LEN - 1) ||
fdctrl->data_pos == fdctrl->data_len) {
+ cur_drv = get_cur_drv(fdctrl);
bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1);
}
/* Switch from transfer mode to status mode
@@ -1473,260 +1803,11 @@
}
if (fdctrl->data_pos == 0) {
/* Command */
- switch (value & 0x5F) {
- case FD_CMD_READ:
- /* READ variants */
- FLOPPY_DPRINTF("READ command\n");
- /* 8 parameters cmd */
- fdctrl->data_len = 9;
- goto enqueue;
- case FD_CMD_READ_DELETED:
- /* READ_DELETED variants */
- FLOPPY_DPRINTF("READ_DELETED command\n");
- /* 8 parameters cmd */
- fdctrl->data_len = 9;
- goto enqueue;
- case FD_CMD_SCAN_EQUAL:
- /* SCAN_EQUAL variants */
- FLOPPY_DPRINTF("SCAN_EQUAL command\n");
- /* 8 parameters cmd */
- fdctrl->data_len = 9;
- goto enqueue;
- case FD_CMD_VERIFY:
- /* VERIFY variants */
- FLOPPY_DPRINTF("VERIFY command\n");
- /* 8 parameters cmd */
- fdctrl->data_len = 9;
- goto enqueue;
- case FD_CMD_SCAN_LOW_OR_EQUAL:
- /* SCAN_LOW_OR_EQUAL variants */
- FLOPPY_DPRINTF("SCAN_LOW_OR_EQUAL command\n");
- /* 8 parameters cmd */
- fdctrl->data_len = 9;
- goto enqueue;
- case FD_CMD_SCAN_HIGH_OR_EQUAL:
- /* SCAN_HIGH_OR_EQUAL variants */
- FLOPPY_DPRINTF("SCAN_HIGH_OR_EQUAL command\n");
- /* 8 parameters cmd */
- fdctrl->data_len = 9;
- goto enqueue;
- default:
- break;
- }
- switch (value & 0x7F) {
- case FD_CMD_WRITE:
- /* WRITE variants */
- FLOPPY_DPRINTF("WRITE command\n");
- /* 8 parameters cmd */
- fdctrl->data_len = 9;
- goto enqueue;
- case FD_CMD_WRITE_DELETED:
- /* WRITE_DELETED variants */
- FLOPPY_DPRINTF("WRITE_DELETED command\n");
- /* 8 parameters cmd */
- fdctrl->data_len = 9;
- goto enqueue;
- default:
- break;
- }
- switch (value) {
- case FD_CMD_SPECIFY:
- /* SPECIFY */
- FLOPPY_DPRINTF("SPECIFY command\n");
- /* 1 parameter cmd */
- fdctrl->data_len = 3;
- goto enqueue;
- case FD_CMD_SENSE_DRIVE_STATUS:
- /* SENSE_DRIVE_STATUS */
- FLOPPY_DPRINTF("SENSE_DRIVE_STATUS command\n");
- /* 1 parameter cmd */
- fdctrl->data_len = 2;
- goto enqueue;
- case FD_CMD_RECALIBRATE:
- /* RECALIBRATE */
- FLOPPY_DPRINTF("RECALIBRATE command\n");
- /* 1 parameter cmd */
- fdctrl->data_len = 2;
- goto enqueue;
- case FD_CMD_SENSE_INTERRUPT_STATUS:
- /* SENSE_INTERRUPT_STATUS */
- FLOPPY_DPRINTF("SENSE_INTERRUPT_STATUS command (%02x)\n",
- fdctrl->int_status);
- /* No parameters cmd: returns status if no interrupt */
-#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->fifo[1] = cur_drv->track;
- fdctrl_set_fifo(fdctrl, 2, 0);
- fdctrl_reset_irq(fdctrl);
- fdctrl->int_status = FD_SR0_RDYCHG;
- return;
- case FD_CMD_DUMPREG:
- /* DUMPREG */
- FLOPPY_DPRINTF("DUMPREG command\n");
- /* Drives position */
- fdctrl->fifo[0] = drv0(fdctrl)->track;
- fdctrl->fifo[1] = drv1(fdctrl)->track;
- fdctrl->fifo[2] = 0;
- fdctrl->fifo[3] = 0;
- /* timers */
- fdctrl->fifo[4] = fdctrl->timer0;
- fdctrl->fifo[5] = (fdctrl->timer1 << 1) | fdctrl->dma_en;
- fdctrl->fifo[6] = cur_drv->last_sect;
- fdctrl->fifo[7] = (fdctrl->lock << 7) |
- (cur_drv->perpendicular << 2);
- fdctrl->fifo[8] = fdctrl->config;
- fdctrl->fifo[9] = fdctrl->precomp_trk;
- fdctrl_set_fifo(fdctrl, 10, 0);
- return;
- case FD_CMD_SEEK:
- /* SEEK */
- FLOPPY_DPRINTF("SEEK command\n");
- /* 2 parameters cmd */
- fdctrl->data_len = 3;
- goto enqueue;
- case FD_CMD_VERSION:
- /* VERSION */
- FLOPPY_DPRINTF("VERSION command\n");
- /* No parameters cmd */
- /* Controller's version */
- fdctrl->fifo[0] = fdctrl->version;
- fdctrl_set_fifo(fdctrl, 1, 1);
- return;
- case FD_CMD_PERPENDICULAR_MODE:
- /* PERPENDICULAR_MODE */
- FLOPPY_DPRINTF("PERPENDICULAR_MODE command\n");
- /* 1 parameter cmd */
- fdctrl->data_len = 2;
- goto enqueue;
- case FD_CMD_CONFIGURE:
- /* CONFIGURE */
- FLOPPY_DPRINTF("CONFIGURE command\n");
- /* 3 parameters cmd */
- fdctrl->data_len = 4;
- goto enqueue;
- case FD_CMD_UNLOCK:
- /* UNLOCK */
- FLOPPY_DPRINTF("UNLOCK command\n");
- /* No parameters cmd */
- fdctrl->lock = 0;
- fdctrl->fifo[0] = 0;
- fdctrl_set_fifo(fdctrl, 1, 0);
- return;
- case FD_CMD_POWERDOWN_MODE:
- /* POWERDOWN_MODE */
- FLOPPY_DPRINTF("POWERDOWN_MODE command\n");
- /* 2 parameters cmd */
- fdctrl->data_len = 3;
- goto enqueue;
- case FD_CMD_PART_ID:
- /* PART_ID */
- FLOPPY_DPRINTF("PART_ID command\n");
- /* No parameters cmd */
- fdctrl->fifo[0] = 0x41; /* Stepping 1 */
- fdctrl_set_fifo(fdctrl, 1, 0);
- return;
- case FD_CMD_SAVE:
- /* SAVE */
- FLOPPY_DPRINTF("SAVE command\n");
- /* No parameters cmd */
- fdctrl->fifo[0] = 0;
- fdctrl->fifo[1] = 0;
- /* Drives position */
- fdctrl->fifo[2] = drv0(fdctrl)->track;
- fdctrl->fifo[3] = drv1(fdctrl)->track;
- fdctrl->fifo[4] = 0;
- fdctrl->fifo[5] = 0;
- /* timers */
- fdctrl->fifo[6] = fdctrl->timer0;
- fdctrl->fifo[7] = fdctrl->timer1;
- fdctrl->fifo[8] = cur_drv->last_sect;
- fdctrl->fifo[9] = (fdctrl->lock << 7) |
- (cur_drv->perpendicular << 2);
- fdctrl->fifo[10] = fdctrl->config;
- fdctrl->fifo[11] = fdctrl->precomp_trk;
- fdctrl->fifo[12] = fdctrl->pwrd;
- fdctrl->fifo[13] = 0;
- fdctrl->fifo[14] = 0;
- fdctrl_set_fifo(fdctrl, 15, 1);
- return;
- case FD_CMD_OPTION:
- /* OPTION */
- FLOPPY_DPRINTF("OPTION command\n");
- /* 1 parameter cmd */
- fdctrl->data_len = 2;
- goto enqueue;
- case FD_CMD_READ_TRACK:
- /* READ_TRACK */
- FLOPPY_DPRINTF("READ_TRACK command\n");
- /* 8 parameters cmd */
- fdctrl->data_len = 9;
- goto enqueue;
- case FD_CMD_READ_ID:
- /* READ_ID */
- FLOPPY_DPRINTF("READ_ID command\n");
- /* 1 parameter cmd */
- fdctrl->data_len = 2;
- goto enqueue;
- case FD_CMD_RESTORE:
- /* RESTORE */
- FLOPPY_DPRINTF("RESTORE command\n");
- /* 17 parameters cmd */
- fdctrl->data_len = 18;
- goto enqueue;
- case FD_CMD_FORMAT_TRACK:
- /* FORMAT_TRACK */
- FLOPPY_DPRINTF("FORMAT_TRACK command\n");
- /* 5 parameters cmd */
- fdctrl->data_len = 6;
- goto enqueue;
- case FD_CMD_DRIVE_SPECIFICATION_COMMAND:
- /* DRIVE_SPECIFICATION_COMMAND */
- FLOPPY_DPRINTF("DRIVE_SPECIFICATION_COMMAND command\n");
- /* 5 parameters cmd */
- fdctrl->data_len = 6;
- goto enqueue;
- case FD_CMD_RELATIVE_SEEK_OUT:
- /* RELATIVE_SEEK_OUT */
- FLOPPY_DPRINTF("RELATIVE_SEEK_OUT command\n");
- /* 2 parameters cmd */
- fdctrl->data_len = 3;
- goto enqueue;
- case FD_CMD_LOCK:
- /* LOCK */
- FLOPPY_DPRINTF("LOCK command\n");
- /* No parameters cmd */
- fdctrl->lock = 1;
- fdctrl->fifo[0] = 0x10;
- fdctrl_set_fifo(fdctrl, 1, 1);
- return;
- case FD_CMD_FORMAT_AND_WRITE:
- /* FORMAT_AND_WRITE */
- FLOPPY_DPRINTF("FORMAT_AND_WRITE command\n");
- /* 10 parameters cmd */
- fdctrl->data_len = 11;
- goto enqueue;
- case FD_CMD_RELATIVE_SEEK_IN:
- /* RELATIVE_SEEK_IN */
- FLOPPY_DPRINTF("RELATIVE_SEEK_IN command\n");
- /* 2 parameters cmd */
- fdctrl->data_len = 3;
- goto enqueue;
- default:
- /* Unknown command */
- FLOPPY_ERROR("unknown command: 0x%02x\n", value);
- fdctrl_unimplemented(fdctrl);
- return;
- }
+ pos = command_to_index[value & 0xff];
+ FLOPPY_DPRINTF("%s command\n", handlers[pos].name);
+ fdctrl->data_len = handlers[pos].parameters + 1;
}
- enqueue:
+
FLOPPY_DPRINTF("%s: %02x\n", __func__, value);
fdctrl->fifo[fdctrl->data_pos] = value;
if (++fdctrl->data_pos == fdctrl->data_len) {
@@ -1737,260 +1818,10 @@
fdctrl_format_sector(fdctrl);
return;
}
- switch (fdctrl->fifo[0] & 0x1F) {
- case FD_CMD_READ & 0x1F:
- {
- /* READ variants */
- FLOPPY_DPRINTF("treat READ command\n");
- fdctrl_start_transfer(fdctrl, FD_DIR_READ);
- return;
- }
- case FD_CMD_READ_DELETED & 0x1F:
- /* READ_DELETED variants */
-// FLOPPY_DPRINTF("treat READ_DELETED command\n");
- FLOPPY_ERROR("treat READ_DELETED command\n");
- fdctrl_start_transfer_del(fdctrl, FD_DIR_READ);
- return;
- case FD_CMD_VERIFY & 0x1F:
- /* VERIFY variants */
-// FLOPPY_DPRINTF("treat VERIFY command\n");
- FLOPPY_ERROR("treat VERIFY command\n");
- fdctrl_stop_transfer(fdctrl, FD_SR0_SEEK, 0x00, 0x00);
- return;
- case FD_CMD_SCAN_EQUAL & 0x1F:
- /* SCAN_EQUAL variants */
-// FLOPPY_DPRINTF("treat SCAN_EQUAL command\n");
- FLOPPY_ERROR("treat SCAN_EQUAL command\n");
- fdctrl_start_transfer(fdctrl, FD_DIR_SCANE);
- return;
- case FD_CMD_SCAN_LOW_OR_EQUAL & 0x1F:
- /* SCAN_LOW_OR_EQUAL variants */
-// FLOPPY_DPRINTF("treat SCAN_LOW_OR_EQUAL command\n");
- FLOPPY_ERROR("treat SCAN_LOW_OR_EQUAL command\n");
- fdctrl_start_transfer(fdctrl, FD_DIR_SCANL);
- return;
- case FD_CMD_SCAN_HIGH_OR_EQUAL & 0x1F:
- /* SCAN_HIGH_OR_EQUAL variants */
-// FLOPPY_DPRINTF("treat SCAN_HIGH_OR_EQUAL command\n");
- FLOPPY_ERROR("treat SCAN_HIGH_OR_EQUAL command\n");
- fdctrl_start_transfer(fdctrl, FD_DIR_SCANH);
- return;
- default:
- break;
- }
- switch (fdctrl->fifo[0] & 0x3F) {
- case FD_CMD_WRITE & 0x3F:
- /* WRITE variants */
- FLOPPY_DPRINTF("treat WRITE command (%02x)\n", fdctrl->fifo[0]);
- fdctrl_start_transfer(fdctrl, FD_DIR_WRITE);
- return;
- case FD_CMD_WRITE_DELETED & 0x3F:
- /* WRITE_DELETED variants */
-// FLOPPY_DPRINTF("treat WRITE_DELETED command\n");
- FLOPPY_ERROR("treat WRITE_DELETED command\n");
- fdctrl_start_transfer_del(fdctrl, FD_DIR_WRITE);
- return;
- default:
- break;
- }
- switch (fdctrl->fifo[0]) {
- case FD_CMD_SPECIFY:
- /* SPECIFY */
- FLOPPY_DPRINTF("treat SPECIFY command\n");
- fdctrl->timer0 = (fdctrl->fifo[1] >> 4) & 0xF;
- fdctrl->timer1 = fdctrl->fifo[2] >> 1;
- fdctrl->dma_en = 1 - (fdctrl->fifo[2] & 1) ;
- /* No result back */
- fdctrl_reset_fifo(fdctrl);
- break;
- case FD_CMD_SENSE_DRIVE_STATUS:
- /* SENSE_DRIVE_STATUS */
- FLOPPY_DPRINTF("treat SENSE_DRIVE_STATUS command\n");
- fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
- cur_drv = get_cur_drv(fdctrl);
- cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
- /* 1 Byte status back */
- fdctrl->fifo[0] = (cur_drv->ro << 6) |
- (cur_drv->track == 0 ? 0x10 : 0x00) |
- (cur_drv->head << 2) |
- fdctrl->cur_drv |
- 0x28;
- fdctrl_set_fifo(fdctrl, 1, 0);
- break;
- case FD_CMD_RECALIBRATE:
- /* RECALIBRATE */
- FLOPPY_DPRINTF("treat RECALIBRATE command\n");
- fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
- cur_drv = get_cur_drv(fdctrl);
- fd_recalibrate(cur_drv);
- fdctrl_reset_fifo(fdctrl);
- /* Raise Interrupt */
- fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
- break;
- case FD_CMD_SEEK:
- /* SEEK */
- FLOPPY_DPRINTF("treat SEEK command\n");
- fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
- cur_drv = get_cur_drv(fdctrl);
- fd_start(cur_drv);
- if (fdctrl->fifo[2] <= cur_drv->track)
- cur_drv->dir = 1;
- 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 {
- cur_drv->track = fdctrl->fifo[2];
- /* Raise Interrupt */
- fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
- }
- break;
- case FD_CMD_PERPENDICULAR_MODE:
- /* PERPENDICULAR_MODE */
- FLOPPY_DPRINTF("treat PERPENDICULAR_MODE command\n");
- if (fdctrl->fifo[1] & 0x80)
- cur_drv->perpendicular = fdctrl->fifo[1] & 0x7;
- /* No result back */
- fdctrl_reset_fifo(fdctrl);
- break;
- case FD_CMD_CONFIGURE:
- /* CONFIGURE */
- FLOPPY_DPRINTF("treat CONFIGURE command\n");
- fdctrl->config = fdctrl->fifo[2];
- fdctrl->precomp_trk = fdctrl->fifo[3];
- /* No result back */
- fdctrl_reset_fifo(fdctrl);
- break;
- case FD_CMD_POWERDOWN_MODE:
- /* POWERDOWN_MODE */
- FLOPPY_DPRINTF("treat POWERDOWN_MODE command\n");
- fdctrl->pwrd = fdctrl->fifo[1];
- fdctrl->fifo[0] = fdctrl->fifo[1];
- fdctrl_set_fifo(fdctrl, 1, 1);
- break;
- case FD_CMD_OPTION:
- /* OPTION */
- FLOPPY_DPRINTF("treat OPTION command\n");
- /* No result back */
- fdctrl_reset_fifo(fdctrl);
- break;
- case FD_CMD_READ_TRACK:
- /* READ_TRACK */
- FLOPPY_DPRINTF("treat READ_TRACK command\n");
- FLOPPY_ERROR("treat READ_TRACK command\n");
- fdctrl_start_transfer(fdctrl, FD_DIR_READ);
- break;
- case FD_CMD_READ_ID:
- /* READ_ID */
- FLOPPY_DPRINTF("treat READ_ID command\n");
- /* XXX: should set main status register to busy */
- cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
- qemu_mod_timer(fdctrl->result_timer,
- qemu_get_clock(vm_clock) + (ticks_per_sec / 50));
- break;
- case FD_CMD_RESTORE:
- /* RESTORE */
- FLOPPY_DPRINTF("treat RESTORE command\n");
- /* Drives position */
- drv0(fdctrl)->track = fdctrl->fifo[3];
- drv1(fdctrl)->track = fdctrl->fifo[4];
- /* timers */
- fdctrl->timer0 = fdctrl->fifo[7];
- fdctrl->timer1 = fdctrl->fifo[8];
- cur_drv->last_sect = fdctrl->fifo[9];
- fdctrl->lock = fdctrl->fifo[10] >> 7;
- cur_drv->perpendicular = (fdctrl->fifo[10] >> 2) & 0xF;
- fdctrl->config = fdctrl->fifo[11];
- fdctrl->precomp_trk = fdctrl->fifo[12];
- fdctrl->pwrd = fdctrl->fifo[13];
- fdctrl_reset_fifo(fdctrl);
- break;
- case FD_CMD_FORMAT_TRACK:
- /* FORMAT_TRACK */
- FLOPPY_DPRINTF("treat FORMAT_TRACK command\n");
- fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
- cur_drv = get_cur_drv(fdctrl);
- fdctrl->data_state |= FD_STATE_FORMAT;
- if (fdctrl->fifo[0] & 0x80)
- 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
- cur_drv->last_sect =
- cur_drv->flags & FDISK_DBL_SIDES ? fdctrl->fifo[3] :
- fdctrl->fifo[3] / 2;
-#else
- cur_drv->last_sect = fdctrl->fifo[3];
-#endif
- /* TODO: implement format using DMA expected by the Bochs BIOS
- * and Linux fdformat (read 3 bytes per sector via DMA and fill
- * the sector with the specified fill byte
- */
- fdctrl->data_state &= ~FD_STATE_FORMAT;
- fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
- break;
- case FD_CMD_DRIVE_SPECIFICATION_COMMAND:
- /* DRIVE_SPECIFICATION_COMMAND */
- FLOPPY_DPRINTF("treat DRIVE_SPECIFICATION_COMMAND command\n");
- if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x80) {
- /* Command parameters done */
- if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x40) {
- fdctrl->fifo[0] = fdctrl->fifo[1];
- fdctrl->fifo[2] = 0;
- fdctrl->fifo[3] = 0;
- fdctrl_set_fifo(fdctrl, 4, 1);
- } else {
- fdctrl_reset_fifo(fdctrl);
- }
- } else if (fdctrl->data_len > 7) {
- /* ERROR */
- fdctrl->fifo[0] = 0x80 |
- (cur_drv->head << 2) | fdctrl->cur_drv;
- fdctrl_set_fifo(fdctrl, 1, 1);
- }
- break;
- case FD_CMD_RELATIVE_SEEK_OUT:
- /* RELATIVE_SEEK_OUT */
- FLOPPY_DPRINTF("treat RELATIVE_SEEK_OUT command\n");
- fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
- cur_drv = get_cur_drv(fdctrl);
- fd_start(cur_drv);
- cur_drv->dir = 0;
- if (fdctrl->fifo[2] + cur_drv->track >= cur_drv->max_track) {
- cur_drv->track = cur_drv->max_track - 1;
- } else {
- cur_drv->track += fdctrl->fifo[2];
- }
- fdctrl_reset_fifo(fdctrl);
- fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
- break;
- case FD_CMD_FORMAT_AND_WRITE:
- /* FORMAT_AND_WRITE */
- FLOPPY_DPRINTF("treat FORMAT_AND_WRITE command\n");
- FLOPPY_ERROR("treat FORMAT_AND_WRITE command\n");
- fdctrl_unimplemented(fdctrl);
- break;
- case FD_CMD_RELATIVE_SEEK_IN:
- /* RELATIVE_SEEK_IN */
- FLOPPY_DPRINTF("treat RELATIVE_SEEK_IN command\n");
- fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
- cur_drv = get_cur_drv(fdctrl);
- fd_start(cur_drv);
- cur_drv->dir = 1;
- if (fdctrl->fifo[2] > cur_drv->track) {
- cur_drv->track = 0;
- } else {
- cur_drv->track -= fdctrl->fifo[2];
- }
- fdctrl_reset_fifo(fdctrl);
- /* Raise Interrupt */
- fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
- break;
- }
+
+ pos = command_to_index[fdctrl->fifo[0] & 0xff];
+ FLOPPY_DPRINTF("treat %s command\n", handlers[pos].name);
+ (*handlers[pos].handler)(fdctrl, handlers[pos].parameter);
}
}
- [Qemu-devel] [PATCH] FDC: simplify code [v2],
Hervé Poussineau <=