Index: fdc.c
===================================================================
--- fdc.c	(revision 4154)
+++ 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
@@ -1047,7 +1048,7 @@
 }
 
 /* Set an error: unimplemented/unknown command */
-static void fdctrl_unimplemented (fdctrl_t *fdctrl)
+static void fdctrl_unimplemented (fdctrl_t *fdctrl, int direction)
 {
 #if 0
     fdrive_t *cur_drv;
@@ -1720,6 +1721,46 @@
 static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
 {
     fdrive_t *cur_drv;
+    int pos;
+    struct {
+        uint8_t value;
+        uint8_t mask;
+        const char* name;
+        int parameters;
+        void (*handler)(fdctrl_t *fdctrl, int direction);
+        int parameter;
+    } commands[] = {
+        { FD_CMD_RESTORE, 0xff, "RESTORE", 17, fdctrl_handle_restore }, /* part of READ DELETED DATA */
+        { FD_CMD_SAVE, 0xff, "SAVE", 0, fdctrl_handle_save }, /* part of READ DELETED DATA */
+        { FD_CMD_READ, 0x1f, "READ", 8, fdctrl_start_transfer, FD_DIR_READ },
+        { FD_CMD_READ_DELETED, 0x1f, "READ DELETED DATA", 8, fdctrl_start_transfer_del, FD_DIR_READ },
+        { FD_CMD_SCAN_EQUAL, 0x1f, "SCAN EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANE },
+        { FD_CMD_VERIFY, 0x1f, "VERIFY", 8, fdctrl_unimplemented },
+        { FD_CMD_SCAN_LOW_OR_EQUAL, 0x1f, "SCAN LOW OR EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANL },
+        { FD_CMD_SCAN_HIGH_OR_EQUAL, 0x1f, "SCAN HIGH OR EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANH },
+        { FD_CMD_WRITE, 0x3f, "WRITE", 8, fdctrl_start_transfer, FD_DIR_WRITE },
+        { FD_CMD_WRITE_DELETED, 0x3f, "WRITE DELETED DATA", 8, fdctrl_start_transfer_del, FD_DIR_WRITE },
+        { FD_CMD_READ_ID, 0xbf, "READ ID", 1, fdctrl_handle_readid },
+        { FD_CMD_FORMAT_TRACK, 0xbf, "FORMAT TRACK", 5, fdctrl_handle_format_track },
+        { FD_CMD_READ_TRACK, 0xbf, "READ TRACK", 8, fdctrl_start_transfer, FD_DIR_READ },
+        { FD_CMD_SPECIFY, 0xff, "SPECIFY", 2, fdctrl_handle_specify },
+        { FD_CMD_SENSE_DRIVE_STATUS, 0xff, "SENSE DRIVE STATUS", 1, fdctrl_handle_sense_drive_status },
+        { FD_CMD_RECALIBRATE, 0xff, "RECALIBRATE", 1, fdctrl_handle_recalibrate },
+        { FD_CMD_SEEK, 0xff, "SEEK", 2, fdctrl_handle_seek },
+        { FD_CMD_PERPENDICULAR_MODE, 0xff, "PERPENDICULAR MODE", 1, fdctrl_handle_perpendicular_mode },
+        { FD_CMD_CONFIGURE, 0xff, "CONFIGURE", 3, fdctrl_handle_configure },
+        { FD_CMD_POWERDOWN_MODE, 0xff, "POWERDOWN MODE", 2, fdctrl_handle_powerdown_mode },
+        { FD_CMD_OPTION, 0xff, "OPTION", 1, fdctrl_handle_option },
+        { FD_CMD_DRIVE_SPECIFICATION_COMMAND, 0xff, "DRIVE SPECIFICATION COMMAND", 5, fdctrl_handle_drive_specification_command },
+        { FD_CMD_RELATIVE_SEEK_OUT, 0xff, "RELATIVE SEEK OUT", 2, fdctrl_handle_relative_seek_out },
+        { FD_CMD_FORMAT_AND_WRITE, 0xff, "FORMAT AND WRITE", 10, fdctrl_unimplemented },
+        { FD_CMD_RELATIVE_SEEK_IN, 0xff, "RELATIVE SEEK IN", 2, fdctrl_handle_relative_seek_in },
+        { FD_CMD_LOCK, 0x7f, "LOCK", 0, fdctrl_handle_lock },
+        { FD_CMD_DUMPREG, 0xff, "DUMPREG", 0, fdctrl_handle_dumpreg },
+        { FD_CMD_VERSION, 0xff, "VERSION", 0, fdctrl_handle_version },
+        { FD_CMD_PART_ID, 0xff, "PART ID", 0, fdctrl_handle_partid },
+        { FD_CMD_WRITE, 0x1f, "WRITE (BeOS)", 8, fdctrl_start_transfer, FD_DIR_WRITE }, /* not in specification ; BeOS 4.5 bug */
+    };
 
     cur_drv = get_cur_drv(fdctrl);
     /* Reset mode */
@@ -1749,82 +1790,7 @@
     }
     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:
+        if (value == FD_CMD_SENSE_INTERRUPT_STATUS) {
             /* SENSE_INTERRUPT_STATUS */
             FLOPPY_DPRINTF("SENSE_INTERRUPT_STATUS command (%02x)\n",
                            fdctrl->int_status);
@@ -1844,120 +1810,24 @@
             fdctrl_reset_irq(fdctrl);
             fdctrl->int_status = FD_SR0_RDYCHG;
             return;
-        case FD_CMD_DUMPREG:
-            /* DUMPREG */
-            FLOPPY_DPRINTF("DUMPREG command\n");
-            fdctrl_handle_dumpreg(fdctrl, 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");
-            fdctrl_handle_version(fdctrl, 0);
-            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");
-            fdctrl_handle_unlock(fdctrl, 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");
-            fdctrl_handle_partid(fdctrl, 0);
-            return;
-        case FD_CMD_SAVE:
-            /* SAVE */
-            FLOPPY_DPRINTF("SAVE command\n");
-            fdctrl_handle_save(fdctrl, 0);
-            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");
-            fdctrl_handle_lock(fdctrl, 0);
-            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;
         }
+
+        for (pos = 0; pos < sizeof(commands)/sizeof(commands[0]); pos++) {
+            if ((value & commands[pos].mask) == commands[pos].value) {
+                FLOPPY_DPRINTF("%s command\n", commands[pos].name);
+                if (commands[pos].parameters == 0) {
+                    (*commands[pos].handler)(fdctrl, commands[pos].parameter);
+                    return;
+                }
+                fdctrl->data_len = commands[pos].parameters + 1;
+                goto enqueue;
+            }
+        }
+
+        /* Unknown command */
+        FLOPPY_ERROR("unknown command: 0x%02x\n", value);
+        fdctrl_unimplemented(fdctrl, 0);
+        return;
     }
  enqueue:
     FLOPPY_DPRINTF("%s: %02x\n", __func__, value);
@@ -1970,145 +1840,13 @@
             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;
+
+        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);
+                (*commands[pos].handler)(fdctrl, commands[pos].parameter);
+                break;
             }
-        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_handle_specify(fdctrl, 0);
-            break;
-        case FD_CMD_SENSE_DRIVE_STATUS:
-            /* SENSE_DRIVE_STATUS */
-            FLOPPY_DPRINTF("treat SENSE_DRIVE_STATUS command\n");
-            fdctrl_handle_sense_drive_status(fdctrl, 0);
-            break;
-        case FD_CMD_RECALIBRATE:
-            /* RECALIBRATE */
-            FLOPPY_DPRINTF("treat RECALIBRATE command\n");
-            fdctrl_handle_recalibrate(fdctrl, 0);            
-            break;
-        case FD_CMD_SEEK:
-            /* SEEK */
-            FLOPPY_DPRINTF("treat SEEK command\n");
-            fdctrl_handle_seek(fdctrl, 0);
-            break;
-        case FD_CMD_PERPENDICULAR_MODE:
-            /* PERPENDICULAR_MODE */
-            FLOPPY_DPRINTF("treat PERPENDICULAR_MODE command\n");
-            fdctrl_handle_perpendicular_mode(fdctrl, 0);
-            break;
-        case FD_CMD_CONFIGURE:
-            /* CONFIGURE */
-            FLOPPY_DPRINTF("treat CONFIGURE command\n");
-            fdctrl_handle_configure(fdctrl, 0);            
-            break;
-        case FD_CMD_POWERDOWN_MODE:
-            /* POWERDOWN_MODE */
-            FLOPPY_DPRINTF("treat POWERDOWN_MODE command\n");
-            fdctrl_handle_powerdown_mode(fdctrl, 0);            
-            break;
-        case FD_CMD_OPTION:
-            /* OPTION */
-            FLOPPY_DPRINTF("treat OPTION command\n");
-            fdctrl_handle_option(fdctrl, 0);
-            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");
-            fdctrl_handle_readid(fdctrl, 0);
-            break;
-        case FD_CMD_RESTORE:
-            /* RESTORE */
-            FLOPPY_DPRINTF("treat RESTORE command\n");
-            fdctrl_handle_restore(fdctrl, 0);
-            break;
-        case FD_CMD_FORMAT_TRACK:
-            /* FORMAT_TRACK */
-            FLOPPY_DPRINTF("treat FORMAT_TRACK command\n");
-            fdctrl_handle_format_track(fdctrl, 0);
-            break;
-        case FD_CMD_DRIVE_SPECIFICATION_COMMAND:
-            /* DRIVE_SPECIFICATION_COMMAND */
-            FLOPPY_DPRINTF("treat DRIVE_SPECIFICATION_COMMAND command\n");
-            fdctrl_handle_drive_specification_command(fdctrl, 0);
-            break;
-        case FD_CMD_RELATIVE_SEEK_OUT:
-            /* RELATIVE_SEEK_OUT */
-            FLOPPY_DPRINTF("treat RELATIVE_SEEK_OUT command\n");
-            fdctrl_handle_relative_seek_out(fdctrl, 0);
-            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_handle_relative_seek_in(fdctrl, 0);
-            break;
         }
     }
 }