gwj / at91sam9263 · Commits

Commit 65cef780e2a637b0ae08ad759665241b9c1a8b0f

Authored by blueswir1
1 parent 2e4d9fb1

Simplify FDC code (Hervé Poussineau) 

git-svn-id: svn://svn.savannah.nongnu.org/qemu/trunk@4174 c046a42c-6fe2-441c-8c8c-71466251a162
Inline Side-by-side
Showing 1 changed file with 361 additions and 517 deletions
hw/fdc.c
  View file @65cef78
... ... @@ -2,6 +2,7 @@
2 2 * QEMU Floppy disk emulator (Intel 82078)
3 3 *
4 4 * Copyright (c) 2003, 2007 Jocelyn Mayer
  5 + * Copyright (c) 2008 Hervé Poussineau
5 6 *
6 7 * Permission is hereby granted, free of charge, to any person obtaining a copy
7 8 * of this software and associated documentation files (the "Software"), to deal
... ... @@ -372,35 +373,34 @@ enum {
372 373 };
373 374  
374 375 enum {
  376 + FD_CMD_READ_TRACK = 0x02,
375 377 FD_CMD_SPECIFY = 0x03,
376 378 FD_CMD_SENSE_DRIVE_STATUS = 0x04,
  379 + FD_CMD_WRITE = 0x05,
  380 + FD_CMD_READ = 0x06,
377 381 FD_CMD_RECALIBRATE = 0x07,
378 382 FD_CMD_SENSE_INTERRUPT_STATUS = 0x08,
  383 + FD_CMD_WRITE_DELETED = 0x09,
  384 + FD_CMD_READ_ID = 0x0a,
  385 + FD_CMD_READ_DELETED = 0x0c,
  386 + FD_CMD_FORMAT_TRACK = 0x0d,
379 387 FD_CMD_DUMPREG = 0x0e,
380 388 FD_CMD_SEEK = 0x0f,
381 389 FD_CMD_VERSION = 0x10,
  390 + FD_CMD_SCAN_EQUAL = 0x11,
382 391 FD_CMD_PERPENDICULAR_MODE = 0x12,
383 392 FD_CMD_CONFIGURE = 0x13,
384   - FD_CMD_UNLOCK = 0x14,
  393 + FD_CMD_LOCK = 0x14,
  394 + FD_CMD_VERIFY = 0x16,
385 395 FD_CMD_POWERDOWN_MODE = 0x17,
386 396 FD_CMD_PART_ID = 0x18,
  397 + FD_CMD_SCAN_LOW_OR_EQUAL = 0x19,
  398 + FD_CMD_SCAN_HIGH_OR_EQUAL = 0x1d,
387 399 FD_CMD_SAVE = 0x2c,
388 400 FD_CMD_OPTION = 0x33,
389   - FD_CMD_READ_TRACK = 0x42,
390   - FD_CMD_WRITE = 0x45,
391   - FD_CMD_READ = 0x46,
392   - FD_CMD_WRITE_DELETED = 0x49,
393   - FD_CMD_READ_ID = 0x4a,
394   - FD_CMD_READ_DELETED = 0x4c,
395 401 FD_CMD_RESTORE = 0x4c,
396   - FD_CMD_FORMAT_TRACK = 0x4d,
397   - FD_CMD_SCAN_EQUAL = 0x50,
398   - FD_CMD_VERIFY = 0x56,
399   - FD_CMD_SCAN_LOW_OR_EQUAL = 0x59,
400   - FD_CMD_SCAN_HIGH_OR_EQUAL = 0x5d,
401 402 FD_CMD_DRIVE_SPECIFICATION_COMMAND = 0x8e,
402 403 FD_CMD_RELATIVE_SEEK_OUT = 0x8f,
403   - FD_CMD_LOCK = 0x94,
404 404 FD_CMD_FORMAT_AND_WRITE = 0xcd,
405 405 FD_CMD_RELATIVE_SEEK_IN = 0xcf,
406 406 };
... ... @@ -1047,7 +1047,7 @@ static void fdctrl_set_fifo (fdctrl_t *fdctrl, int fifo_len, int do_irq)
1047 1047 }
1048 1048  
1049 1049 /* Set an error: unimplemented/unknown command */
1050   -static void fdctrl_unimplemented (fdctrl_t *fdctrl)
  1050 +static void fdctrl_unimplemented (fdctrl_t *fdctrl, int direction)
1051 1051 {
1052 1052 #if 0
1053 1053 fdrive_t *cur_drv;
... ... @@ -1441,9 +1441,339 @@ static void fdctrl_format_sector (fdctrl_t *fdctrl)
1441 1441 }
1442 1442 }
1443 1443  
  1444 +static void fdctrl_handle_lock (fdctrl_t *fdctrl, int direction)
  1445 +{
  1446 + fdctrl->lock = (fdctrl->fifo[0] & 0x80) ? 1 : 0;
  1447 + fdctrl->fifo[0] = fdctrl->lock << 4;
  1448 + fdctrl_set_fifo(fdctrl, 1, fdctrl->lock);
  1449 +}
  1450 +
  1451 +static void fdctrl_handle_dumpreg (fdctrl_t *fdctrl, int direction)
  1452 +{
  1453 + fdrive_t *cur_drv = get_cur_drv(fdctrl);
  1454 +
  1455 + /* Drives position */
  1456 + fdctrl->fifo[0] = drv0(fdctrl)->track;
  1457 + fdctrl->fifo[1] = drv1(fdctrl)->track;
  1458 + fdctrl->fifo[2] = 0;
  1459 + fdctrl->fifo[3] = 0;
  1460 + /* timers */
  1461 + fdctrl->fifo[4] = fdctrl->timer0;
  1462 + fdctrl->fifo[5] = (fdctrl->timer1 << 1) | fdctrl->dma_en;
  1463 + fdctrl->fifo[6] = cur_drv->last_sect;
  1464 + fdctrl->fifo[7] = (fdctrl->lock << 7) |
  1465 + (cur_drv->perpendicular << 2);
  1466 + fdctrl->fifo[8] = fdctrl->config;
  1467 + fdctrl->fifo[9] = fdctrl->precomp_trk;
  1468 + fdctrl_set_fifo(fdctrl, 10, 0);
  1469 +}
  1470 +
  1471 +static void fdctrl_handle_version (fdctrl_t *fdctrl, int direction)
  1472 +{
  1473 + /* Controller's version */
  1474 + fdctrl->fifo[0] = fdctrl->version;
  1475 + fdctrl_set_fifo(fdctrl, 1, 1);
  1476 +}
  1477 +
  1478 +static void fdctrl_handle_partid (fdctrl_t *fdctrl, int direction)
  1479 +{
  1480 + fdctrl->fifo[0] = 0x41; /* Stepping 1 */
  1481 + fdctrl_set_fifo(fdctrl, 1, 0);
  1482 +}
  1483 +
  1484 +static void fdctrl_handle_restore (fdctrl_t *fdctrl, int direction)
  1485 +{
  1486 + fdrive_t *cur_drv = get_cur_drv(fdctrl);
  1487 +
  1488 + /* Drives position */
  1489 + drv0(fdctrl)->track = fdctrl->fifo[3];
  1490 + drv1(fdctrl)->track = fdctrl->fifo[4];
  1491 + /* timers */
  1492 + fdctrl->timer0 = fdctrl->fifo[7];
  1493 + fdctrl->timer1 = fdctrl->fifo[8];
  1494 + cur_drv->last_sect = fdctrl->fifo[9];
  1495 + fdctrl->lock = fdctrl->fifo[10] >> 7;
  1496 + cur_drv->perpendicular = (fdctrl->fifo[10] >> 2) & 0xF;
  1497 + fdctrl->config = fdctrl->fifo[11];
  1498 + fdctrl->precomp_trk = fdctrl->fifo[12];
  1499 + fdctrl->pwrd = fdctrl->fifo[13];
  1500 + fdctrl_reset_fifo(fdctrl);
  1501 +}
  1502 +
  1503 +static void fdctrl_handle_save (fdctrl_t *fdctrl, int direction)
  1504 +{
  1505 + fdrive_t *cur_drv = get_cur_drv(fdctrl);
  1506 +
  1507 + fdctrl->fifo[0] = 0;
  1508 + fdctrl->fifo[1] = 0;
  1509 + /* Drives position */
  1510 + fdctrl->fifo[2] = drv0(fdctrl)->track;
  1511 + fdctrl->fifo[3] = drv1(fdctrl)->track;
  1512 + fdctrl->fifo[4] = 0;
  1513 + fdctrl->fifo[5] = 0;
  1514 + /* timers */
  1515 + fdctrl->fifo[6] = fdctrl->timer0;
  1516 + fdctrl->fifo[7] = fdctrl->timer1;
  1517 + fdctrl->fifo[8] = cur_drv->last_sect;
  1518 + fdctrl->fifo[9] = (fdctrl->lock << 7) |
  1519 + (cur_drv->perpendicular << 2);
  1520 + fdctrl->fifo[10] = fdctrl->config;
  1521 + fdctrl->fifo[11] = fdctrl->precomp_trk;
  1522 + fdctrl->fifo[12] = fdctrl->pwrd;
  1523 + fdctrl->fifo[13] = 0;
  1524 + fdctrl->fifo[14] = 0;
  1525 + fdctrl_set_fifo(fdctrl, 15, 1);
  1526 +}
  1527 +
  1528 +static void fdctrl_handle_readid (fdctrl_t *fdctrl, int direction)
  1529 +{
  1530 + fdrive_t *cur_drv = get_cur_drv(fdctrl);
  1531 +
  1532 + /* XXX: should set main status register to busy */
  1533 + cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
  1534 + qemu_mod_timer(fdctrl->result_timer,
  1535 + qemu_get_clock(vm_clock) + (ticks_per_sec / 50));
  1536 +}
  1537 +
  1538 +static void fdctrl_handle_format_track (fdctrl_t *fdctrl, int direction)
  1539 +{
  1540 + fdrive_t *cur_drv;
  1541 +
  1542 + fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
  1543 + cur_drv = get_cur_drv(fdctrl);
  1544 + fdctrl->data_state |= FD_STATE_FORMAT;
  1545 + if (fdctrl->fifo[0] & 0x80)
  1546 + fdctrl->data_state |= FD_STATE_MULTI;
  1547 + else
  1548 + fdctrl->data_state &= ~FD_STATE_MULTI;
  1549 + fdctrl->data_state &= ~FD_STATE_SEEK;
  1550 + cur_drv->bps =
  1551 + fdctrl->fifo[2] > 7 ? 16384 : 128 << fdctrl->fifo[2];
  1552 +#if 0
  1553 + cur_drv->last_sect =
  1554 + cur_drv->flags & FDISK_DBL_SIDES ? fdctrl->fifo[3] :
  1555 + fdctrl->fifo[3] / 2;
  1556 +#else
  1557 + cur_drv->last_sect = fdctrl->fifo[3];
  1558 +#endif
  1559 + /* TODO: implement format using DMA expected by the Bochs BIOS
  1560 + * and Linux fdformat (read 3 bytes per sector via DMA and fill
  1561 + * the sector with the specified fill byte
  1562 + */
  1563 + fdctrl->data_state &= ~FD_STATE_FORMAT;
  1564 + fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
  1565 +}
  1566 +
  1567 +static void fdctrl_handle_specify (fdctrl_t *fdctrl, int direction)
  1568 +{
  1569 + fdctrl->timer0 = (fdctrl->fifo[1] >> 4) & 0xF;
  1570 + fdctrl->timer1 = fdctrl->fifo[2] >> 1;
  1571 + fdctrl->dma_en = 1 - (fdctrl->fifo[2] & 1) ;
  1572 + /* No result back */
  1573 + fdctrl_reset_fifo(fdctrl);
  1574 +}
  1575 +
  1576 +static void fdctrl_handle_sense_drive_status (fdctrl_t *fdctrl, int direction)
  1577 +{
  1578 + fdrive_t *cur_drv;
  1579 +
  1580 + fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
  1581 + cur_drv = get_cur_drv(fdctrl);
  1582 + cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
  1583 + /* 1 Byte status back */
  1584 + fdctrl->fifo[0] = (cur_drv->ro << 6) |
  1585 + (cur_drv->track == 0 ? 0x10 : 0x00) |
  1586 + (cur_drv->head << 2) |
  1587 + fdctrl->cur_drv |
  1588 + 0x28;
  1589 + fdctrl_set_fifo(fdctrl, 1, 0);
  1590 +}
  1591 +
  1592 +static void fdctrl_handle_recalibrate (fdctrl_t *fdctrl, int direction)
  1593 +{
  1594 + fdrive_t *cur_drv;
  1595 +
  1596 + fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
  1597 + cur_drv = get_cur_drv(fdctrl);
  1598 + fd_recalibrate(cur_drv);
  1599 + fdctrl_reset_fifo(fdctrl);
  1600 + /* Raise Interrupt */
  1601 + fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
  1602 +}
  1603 +
  1604 +static void fdctrl_handle_sense_interrupt_status (fdctrl_t *fdctrl, int direction)
  1605 +{
  1606 + fdrive_t *cur_drv = get_cur_drv(fdctrl);
  1607 +
  1608 +#if 0
  1609 + fdctrl->fifo[0] =
  1610 + fdctrl->int_status | (cur_drv->head << 2) | fdctrl->cur_drv;
  1611 +#else
  1612 + /* XXX: int_status handling is broken for read/write
  1613 + commands, so we do this hack. It should be suppressed
  1614 + ASAP */
  1615 + fdctrl->fifo[0] =
  1616 + 0x20 | (cur_drv->head << 2) | fdctrl->cur_drv;
  1617 +#endif
  1618 + fdctrl->fifo[1] = cur_drv->track;
  1619 + fdctrl_set_fifo(fdctrl, 2, 0);
  1620 + fdctrl_reset_irq(fdctrl);
  1621 + fdctrl->int_status = FD_SR0_RDYCHG;
  1622 +}
  1623 +
  1624 +static void fdctrl_handle_seek (fdctrl_t *fdctrl, int direction)
  1625 +{
  1626 + fdrive_t *cur_drv;
  1627 +
  1628 + fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
  1629 + cur_drv = get_cur_drv(fdctrl);
  1630 + fd_start(cur_drv);
  1631 + if (fdctrl->fifo[2] <= cur_drv->track)
  1632 + cur_drv->dir = 1;
  1633 + else
  1634 + cur_drv->dir = 0;
  1635 + fdctrl_reset_fifo(fdctrl);
  1636 + if (fdctrl->fifo[2] > cur_drv->max_track) {
  1637 + fdctrl_raise_irq(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK);
  1638 + } else {
  1639 + cur_drv->track = fdctrl->fifo[2];
  1640 + /* Raise Interrupt */
  1641 + fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
  1642 + }
  1643 +}
  1644 +
  1645 +static void fdctrl_handle_perpendicular_mode (fdctrl_t *fdctrl, int direction)
  1646 +{
  1647 + fdrive_t *cur_drv = get_cur_drv(fdctrl);
  1648 +
  1649 + if (fdctrl->fifo[1] & 0x80)
  1650 + cur_drv->perpendicular = fdctrl->fifo[1] & 0x7;
  1651 + /* No result back */
  1652 + fdctrl_reset_fifo(fdctrl);
  1653 +}
  1654 +
  1655 +static void fdctrl_handle_configure (fdctrl_t *fdctrl, int direction)
  1656 +{
  1657 + fdctrl->config = fdctrl->fifo[2];
  1658 + fdctrl->precomp_trk = fdctrl->fifo[3];
  1659 + /* No result back */
  1660 + fdctrl_reset_fifo(fdctrl);
  1661 +}
  1662 +
  1663 +static void fdctrl_handle_powerdown_mode (fdctrl_t *fdctrl, int direction)
  1664 +{
  1665 + fdctrl->pwrd = fdctrl->fifo[1];
  1666 + fdctrl->fifo[0] = fdctrl->fifo[1];
  1667 + fdctrl_set_fifo(fdctrl, 1, 1);
  1668 +}
  1669 +
  1670 +static void fdctrl_handle_option (fdctrl_t *fdctrl, int direction)
  1671 +{
  1672 + /* No result back */
  1673 + fdctrl_reset_fifo(fdctrl);
  1674 +}
  1675 +
  1676 +static void fdctrl_handle_drive_specification_command (fdctrl_t *fdctrl, int direction)
  1677 +{
  1678 + fdrive_t *cur_drv = get_cur_drv(fdctrl);
  1679 +
  1680 + if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x80) {
  1681 + /* Command parameters done */
  1682 + if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x40) {
  1683 + fdctrl->fifo[0] = fdctrl->fifo[1];
  1684 + fdctrl->fifo[2] = 0;
  1685 + fdctrl->fifo[3] = 0;
  1686 + fdctrl_set_fifo(fdctrl, 4, 1);
  1687 + } else {
  1688 + fdctrl_reset_fifo(fdctrl);
  1689 + }
  1690 + } else if (fdctrl->data_len > 7) {
  1691 + /* ERROR */
  1692 + fdctrl->fifo[0] = 0x80 |
  1693 + (cur_drv->head << 2) | fdctrl->cur_drv;
  1694 + fdctrl_set_fifo(fdctrl, 1, 1);
  1695 + }
  1696 +}
  1697 +
  1698 +static void fdctrl_handle_relative_seek_out (fdctrl_t *fdctrl, int direction)
  1699 +{
  1700 + fdrive_t *cur_drv = get_cur_drv(fdctrl);
  1701 +
  1702 + fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
  1703 + cur_drv = get_cur_drv(fdctrl);
  1704 + fd_start(cur_drv);
  1705 + cur_drv->dir = 0;
  1706 + if (fdctrl->fifo[2] + cur_drv->track >= cur_drv->max_track) {
  1707 + cur_drv->track = cur_drv->max_track - 1;
  1708 + } else {
  1709 + cur_drv->track += fdctrl->fifo[2];
  1710 + }
  1711 + fdctrl_reset_fifo(fdctrl);
  1712 + fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
  1713 +}
  1714 +
  1715 +static void fdctrl_handle_relative_seek_in (fdctrl_t *fdctrl, int direction)
  1716 +{
  1717 + fdrive_t *cur_drv = get_cur_drv(fdctrl);
  1718 +
  1719 + fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
  1720 + cur_drv = get_cur_drv(fdctrl);
  1721 + fd_start(cur_drv);
  1722 + cur_drv->dir = 1;
  1723 + if (fdctrl->fifo[2] > cur_drv->track) {
  1724 + cur_drv->track = 0;
  1725 + } else {
  1726 + cur_drv->track -= fdctrl->fifo[2];
  1727 + }
  1728 + fdctrl_reset_fifo(fdctrl);
  1729 + /* Raise Interrupt */
  1730 + fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
  1731 +}
  1732 +
1444 1733 static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
1445 1734 {
1446 1735 fdrive_t *cur_drv;
  1736 + int pos;
  1737 + static const struct {
  1738 + uint8_t value;
  1739 + uint8_t mask;
  1740 + const char* name;
  1741 + int parameters;
  1742 + void (*handler)(fdctrl_t *fdctrl, int direction);
  1743 + int parameter;
  1744 + } commands[] = {
  1745 + { FD_CMD_READ, 0x1f, "READ", 8, fdctrl_start_transfer, FD_DIR_READ },
  1746 + { FD_CMD_WRITE, 0x3f, "WRITE", 8, fdctrl_start_transfer, FD_DIR_WRITE },
  1747 + { FD_CMD_SEEK, 0xff, "SEEK", 2, fdctrl_handle_seek },
  1748 + { FD_CMD_SENSE_INTERRUPT_STATUS, 0xff, "SENSE INTERRUPT STATUS", 0, fdctrl_handle_sense_interrupt_status },
  1749 + { FD_CMD_RECALIBRATE, 0xff, "RECALIBRATE", 1, fdctrl_handle_recalibrate },
  1750 + { FD_CMD_FORMAT_TRACK, 0xbf, "FORMAT TRACK", 5, fdctrl_handle_format_track },
  1751 + { FD_CMD_READ_TRACK, 0xbf, "READ TRACK", 8, fdctrl_start_transfer, FD_DIR_READ },
  1752 + { FD_CMD_RESTORE, 0xff, "RESTORE", 17, fdctrl_handle_restore }, /* part of READ DELETED DATA */
  1753 + { FD_CMD_SAVE, 0xff, "SAVE", 0, fdctrl_handle_save }, /* part of READ DELETED DATA */
  1754 + { FD_CMD_READ_DELETED, 0x1f, "READ DELETED DATA", 8, fdctrl_start_transfer_del, FD_DIR_READ },
  1755 + { FD_CMD_SCAN_EQUAL, 0x1f, "SCAN EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANE },
  1756 + { FD_CMD_VERIFY, 0x1f, "VERIFY", 8, fdctrl_unimplemented },
  1757 + { FD_CMD_SCAN_LOW_OR_EQUAL, 0x1f, "SCAN LOW OR EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANL },
  1758 + { FD_CMD_SCAN_HIGH_OR_EQUAL, 0x1f, "SCAN HIGH OR EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANH },
  1759 + { FD_CMD_WRITE_DELETED, 0x3f, "WRITE DELETED DATA", 8, fdctrl_start_transfer_del, FD_DIR_WRITE },
  1760 + { FD_CMD_READ_ID, 0xbf, "READ ID", 1, fdctrl_handle_readid },
  1761 + { FD_CMD_SPECIFY, 0xff, "SPECIFY", 2, fdctrl_handle_specify },
  1762 + { FD_CMD_SENSE_DRIVE_STATUS, 0xff, "SENSE DRIVE STATUS", 1, fdctrl_handle_sense_drive_status },
  1763 + { FD_CMD_PERPENDICULAR_MODE, 0xff, "PERPENDICULAR MODE", 1, fdctrl_handle_perpendicular_mode },
  1764 + { FD_CMD_CONFIGURE, 0xff, "CONFIGURE", 3, fdctrl_handle_configure },
  1765 + { FD_CMD_POWERDOWN_MODE, 0xff, "POWERDOWN MODE", 2, fdctrl_handle_powerdown_mode },
  1766 + { FD_CMD_OPTION, 0xff, "OPTION", 1, fdctrl_handle_option },
  1767 + { FD_CMD_DRIVE_SPECIFICATION_COMMAND, 0xff, "DRIVE SPECIFICATION COMMAND", 5, fdctrl_handle_drive_specification_command },
  1768 + { FD_CMD_RELATIVE_SEEK_OUT, 0xff, "RELATIVE SEEK OUT", 2, fdctrl_handle_relative_seek_out },
  1769 + { FD_CMD_FORMAT_AND_WRITE, 0xff, "FORMAT AND WRITE", 10, fdctrl_unimplemented },
  1770 + { FD_CMD_RELATIVE_SEEK_IN, 0xff, "RELATIVE SEEK IN", 2, fdctrl_handle_relative_seek_in },
  1771 + { FD_CMD_LOCK, 0x7f, "LOCK", 0, fdctrl_handle_lock },
  1772 + { FD_CMD_DUMPREG, 0xff, "DUMPREG", 0, fdctrl_handle_dumpreg },
  1773 + { FD_CMD_VERSION, 0xff, "VERSION", 0, fdctrl_handle_version },
  1774 + { FD_CMD_PART_ID, 0xff, "PART ID", 0, fdctrl_handle_partid },
  1775 + { FD_CMD_WRITE, 0x1f, "WRITE (BeOS)", 8, fdctrl_start_transfer, FD_DIR_WRITE }, /* not in specification ; BeOS 4.5 bug */
  1776 + };
1447 1777  
1448 1778 cur_drv = get_cur_drv(fdctrl);
1449 1779 /* Reset mode */
... ... @@ -1473,258 +1803,18 @@ static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
1473 1803 }
1474 1804 if (fdctrl->data_pos == 0) {
1475 1805 /* Command */
1476   - switch (value & 0x5F) {
1477   - case FD_CMD_READ:
1478   - /* READ variants */
1479   - FLOPPY_DPRINTF("READ command\n");
1480   - /* 8 parameters cmd */
1481   - fdctrl->data_len = 9;
1482   - goto enqueue;
1483   - case FD_CMD_READ_DELETED:
1484   - /* READ_DELETED variants */
1485   - FLOPPY_DPRINTF("READ_DELETED command\n");
1486   - /* 8 parameters cmd */
1487   - fdctrl->data_len = 9;
1488   - goto enqueue;
1489   - case FD_CMD_SCAN_EQUAL:
1490   - /* SCAN_EQUAL variants */
1491   - FLOPPY_DPRINTF("SCAN_EQUAL command\n");
1492   - /* 8 parameters cmd */
1493   - fdctrl->data_len = 9;
1494   - goto enqueue;
1495   - case FD_CMD_VERIFY:
1496   - /* VERIFY variants */
1497   - FLOPPY_DPRINTF("VERIFY command\n");
1498   - /* 8 parameters cmd */
1499   - fdctrl->data_len = 9;
1500   - goto enqueue;
1501   - case FD_CMD_SCAN_LOW_OR_EQUAL:
1502   - /* SCAN_LOW_OR_EQUAL variants */
1503   - FLOPPY_DPRINTF("SCAN_LOW_OR_EQUAL command\n");
1504   - /* 8 parameters cmd */
1505   - fdctrl->data_len = 9;
1506   - goto enqueue;
1507   - case FD_CMD_SCAN_HIGH_OR_EQUAL:
1508   - /* SCAN_HIGH_OR_EQUAL variants */
1509   - FLOPPY_DPRINTF("SCAN_HIGH_OR_EQUAL command\n");
1510   - /* 8 parameters cmd */
1511   - fdctrl->data_len = 9;
1512   - goto enqueue;
1513   - default:
1514   - break;
1515   - }
1516   - switch (value & 0x7F) {
1517   - case FD_CMD_WRITE:
1518   - /* WRITE variants */
1519   - FLOPPY_DPRINTF("WRITE command\n");
1520   - /* 8 parameters cmd */
1521   - fdctrl->data_len = 9;
1522   - goto enqueue;
1523   - case FD_CMD_WRITE_DELETED:
1524   - /* WRITE_DELETED variants */
1525   - FLOPPY_DPRINTF("WRITE_DELETED command\n");
1526   - /* 8 parameters cmd */
1527   - fdctrl->data_len = 9;
1528   - goto enqueue;
1529   - default:
1530   - break;
1531   - }
1532   - switch (value) {
1533   - case FD_CMD_SPECIFY:
1534   - /* SPECIFY */
1535   - FLOPPY_DPRINTF("SPECIFY command\n");
1536   - /* 1 parameter cmd */
1537   - fdctrl->data_len = 3;
1538   - goto enqueue;
1539   - case FD_CMD_SENSE_DRIVE_STATUS:
1540   - /* SENSE_DRIVE_STATUS */
1541   - FLOPPY_DPRINTF("SENSE_DRIVE_STATUS command\n");
1542   - /* 1 parameter cmd */
1543   - fdctrl->data_len = 2;
1544   - goto enqueue;
1545   - case FD_CMD_RECALIBRATE:
1546   - /* RECALIBRATE */
1547   - FLOPPY_DPRINTF("RECALIBRATE command\n");
1548   - /* 1 parameter cmd */
1549   - fdctrl->data_len = 2;
1550   - goto enqueue;
1551   - case FD_CMD_SENSE_INTERRUPT_STATUS:
1552   - /* SENSE_INTERRUPT_STATUS */
1553   - FLOPPY_DPRINTF("SENSE_INTERRUPT_STATUS command (%02x)\n",
1554   - fdctrl->int_status);
1555   - /* No parameters cmd: returns status if no interrupt */
1556   -#if 0
1557   - fdctrl->fifo[0] =
1558   - fdctrl->int_status | (cur_drv->head << 2) | fdctrl->cur_drv;
1559   -#else
1560   - /* XXX: int_status handling is broken for read/write
1561   - commands, so we do this hack. It should be suppressed
1562   - ASAP */
1563   - fdctrl->fifo[0] =
1564   - 0x20 | (cur_drv->head << 2) | fdctrl->cur_drv;
1565   -#endif
1566   - fdctrl->fifo[1] = cur_drv->track;
1567   - fdctrl_set_fifo(fdctrl, 2, 0);
1568   - fdctrl_reset_irq(fdctrl);
1569   - fdctrl->int_status = FD_SR0_RDYCHG;
1570   - return;
1571   - case FD_CMD_DUMPREG:
1572   - /* DUMPREG */
1573   - FLOPPY_DPRINTF("DUMPREG command\n");
1574   - /* Drives position */
1575   - fdctrl->fifo[0] = drv0(fdctrl)->track;
1576   - fdctrl->fifo[1] = drv1(fdctrl)->track;
1577   - fdctrl->fifo[2] = 0;
1578   - fdctrl->fifo[3] = 0;
1579   - /* timers */
1580   - fdctrl->fifo[4] = fdctrl->timer0;
1581   - fdctrl->fifo[5] = (fdctrl->timer1 << 1) | fdctrl->dma_en;
1582   - fdctrl->fifo[6] = cur_drv->last_sect;
1583   - fdctrl->fifo[7] = (fdctrl->lock << 7) |
1584   - (cur_drv->perpendicular << 2);
1585   - fdctrl->fifo[8] = fdctrl->config;
1586   - fdctrl->fifo[9] = fdctrl->precomp_trk;
1587   - fdctrl_set_fifo(fdctrl, 10, 0);
1588   - return;
1589   - case FD_CMD_SEEK:
1590   - /* SEEK */
1591   - FLOPPY_DPRINTF("SEEK command\n");
1592   - /* 2 parameters cmd */
1593   - fdctrl->data_len = 3;
1594   - goto enqueue;
1595   - case FD_CMD_VERSION:
1596   - /* VERSION */
1597   - FLOPPY_DPRINTF("VERSION command\n");
1598   - /* No parameters cmd */
1599   - /* Controller's version */
1600   - fdctrl->fifo[0] = fdctrl->version;
1601   - fdctrl_set_fifo(fdctrl, 1, 1);
1602   - return;
1603   - case FD_CMD_PERPENDICULAR_MODE:
1604   - /* PERPENDICULAR_MODE */
1605   - FLOPPY_DPRINTF("PERPENDICULAR_MODE command\n");
1606   - /* 1 parameter cmd */
1607   - fdctrl->data_len = 2;
1608   - goto enqueue;
1609   - case FD_CMD_CONFIGURE:
1610   - /* CONFIGURE */
1611   - FLOPPY_DPRINTF("CONFIGURE command\n");
1612   - /* 3 parameters cmd */
1613   - fdctrl->data_len = 4;
1614   - goto enqueue;
1615   - case FD_CMD_UNLOCK:
1616   - /* UNLOCK */
1617   - FLOPPY_DPRINTF("UNLOCK command\n");
1618   - /* No parameters cmd */
1619   - fdctrl->lock = 0;
1620   - fdctrl->fifo[0] = 0;
1621   - fdctrl_set_fifo(fdctrl, 1, 0);
1622   - return;
1623   - case FD_CMD_POWERDOWN_MODE:
1624   - /* POWERDOWN_MODE */
1625   - FLOPPY_DPRINTF("POWERDOWN_MODE command\n");
1626   - /* 2 parameters cmd */
1627   - fdctrl->data_len = 3;
1628   - goto enqueue;
1629   - case FD_CMD_PART_ID:
1630   - /* PART_ID */
1631   - FLOPPY_DPRINTF("PART_ID command\n");
1632   - /* No parameters cmd */
1633   - fdctrl->fifo[0] = 0x41; /* Stepping 1 */
1634   - fdctrl_set_fifo(fdctrl, 1, 0);
1635   - return;
1636   - case FD_CMD_SAVE:
1637   - /* SAVE */
1638   - FLOPPY_DPRINTF("SAVE command\n");
1639   - /* No parameters cmd */
1640   - fdctrl->fifo[0] = 0;
1641   - fdctrl->fifo[1] = 0;
1642   - /* Drives position */
1643   - fdctrl->fifo[2] = drv0(fdctrl)->track;
1644   - fdctrl->fifo[3] = drv1(fdctrl)->track;
1645   - fdctrl->fifo[4] = 0;
1646   - fdctrl->fifo[5] = 0;
1647   - /* timers */
1648   - fdctrl->fifo[6] = fdctrl->timer0;
1649   - fdctrl->fifo[7] = fdctrl->timer1;
1650   - fdctrl->fifo[8] = cur_drv->last_sect;
1651   - fdctrl->fifo[9] = (fdctrl->lock << 7) |
1652   - (cur_drv->perpendicular << 2);
1653   - fdctrl->fifo[10] = fdctrl->config;
1654   - fdctrl->fifo[11] = fdctrl->precomp_trk;
1655   - fdctrl->fifo[12] = fdctrl->pwrd;
1656   - fdctrl->fifo[13] = 0;
1657   - fdctrl->fifo[14] = 0;
1658   - fdctrl_set_fifo(fdctrl, 15, 1);
1659   - return;
1660   - case FD_CMD_OPTION:
1661   - /* OPTION */
1662   - FLOPPY_DPRINTF("OPTION command\n");
1663   - /* 1 parameter cmd */
1664   - fdctrl->data_len = 2;
1665   - goto enqueue;
1666   - case FD_CMD_READ_TRACK:
1667   - /* READ_TRACK */
1668   - FLOPPY_DPRINTF("READ_TRACK command\n");
1669   - /* 8 parameters cmd */
1670   - fdctrl->data_len = 9;
1671   - goto enqueue;
1672   - case FD_CMD_READ_ID:
1673   - /* READ_ID */
1674   - FLOPPY_DPRINTF("READ_ID command\n");
1675   - /* 1 parameter cmd */
1676   - fdctrl->data_len = 2;
1677   - goto enqueue;
1678   - case FD_CMD_RESTORE:
1679   - /* RESTORE */
1680   - FLOPPY_DPRINTF("RESTORE command\n");
1681   - /* 17 parameters cmd */
1682   - fdctrl->data_len = 18;
1683   - goto enqueue;
1684   - case FD_CMD_FORMAT_TRACK:
1685   - /* FORMAT_TRACK */
1686   - FLOPPY_DPRINTF("FORMAT_TRACK command\n");
1687   - /* 5 parameters cmd */
1688   - fdctrl->data_len = 6;
1689   - goto enqueue;
1690   - case FD_CMD_DRIVE_SPECIFICATION_COMMAND:
1691   - /* DRIVE_SPECIFICATION_COMMAND */
1692   - FLOPPY_DPRINTF("DRIVE_SPECIFICATION_COMMAND command\n");
1693   - /* 5 parameters cmd */
1694   - fdctrl->data_len = 6;
1695   - goto enqueue;
1696   - case FD_CMD_RELATIVE_SEEK_OUT:
1697   - /* RELATIVE_SEEK_OUT */
1698   - FLOPPY_DPRINTF("RELATIVE_SEEK_OUT command\n");
1699   - /* 2 parameters cmd */
1700   - fdctrl->data_len = 3;
1701   - goto enqueue;
1702   - case FD_CMD_LOCK:
1703   - /* LOCK */
1704   - FLOPPY_DPRINTF("LOCK command\n");
1705   - /* No parameters cmd */
1706   - fdctrl->lock = 1;
1707   - fdctrl->fifo[0] = 0x10;
1708   - fdctrl_set_fifo(fdctrl, 1, 1);
1709   - return;
1710   - case FD_CMD_FORMAT_AND_WRITE:
1711   - /* FORMAT_AND_WRITE */
1712   - FLOPPY_DPRINTF("FORMAT_AND_WRITE command\n");
1713   - /* 10 parameters cmd */
1714   - fdctrl->data_len = 11;
1715   - goto enqueue;
1716   - case FD_CMD_RELATIVE_SEEK_IN:
1717   - /* RELATIVE_SEEK_IN */
1718   - FLOPPY_DPRINTF("RELATIVE_SEEK_IN command\n");
1719   - /* 2 parameters cmd */
1720   - fdctrl->data_len = 3;
1721   - goto enqueue;
1722   - default:
1723   - /* Unknown command */
1724   - FLOPPY_ERROR("unknown command: 0x%02x\n", value);
1725   - fdctrl_unimplemented(fdctrl);
1726   - return;
  1806 + for (pos = 0; pos < sizeof(commands)/sizeof(commands[0]); pos++) {
  1807 + if ((value & commands[pos].mask) == commands[pos].value) {
  1808 + FLOPPY_DPRINTF("%s command\n", commands[pos].name);
  1809 + fdctrl->data_len = commands[pos].parameters + 1;
  1810 + goto enqueue;
  1811 + }
1727 1812 }
  1813 +
  1814 + /* Unknown command */
  1815 + FLOPPY_ERROR("unknown command: 0x%02x\n", value);
  1816 + fdctrl_unimplemented(fdctrl, 0);
  1817 + return;
1728 1818 }
1729 1819 enqueue:
1730 1820 FLOPPY_DPRINTF("%s: %02x\n", __func__, value);
... ... @@ -1737,259 +1827,13 @@ static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
1737 1827 fdctrl_format_sector(fdctrl);
1738 1828 return;
1739 1829 }
1740   - switch (fdctrl->fifo[0] & 0x1F) {
1741   - case FD_CMD_READ & 0x1F:
1742   - {
1743   - /* READ variants */
1744   - FLOPPY_DPRINTF("treat READ command\n");
1745   - fdctrl_start_transfer(fdctrl, FD_DIR_READ);
1746   - return;
1747   - }
1748   - case FD_CMD_READ_DELETED & 0x1F:
1749   - /* READ_DELETED variants */
1750   -// FLOPPY_DPRINTF("treat READ_DELETED command\n");
1751   - FLOPPY_ERROR("treat READ_DELETED command\n");
1752   - fdctrl_start_transfer_del(fdctrl, FD_DIR_READ);
1753   - return;
1754   - case FD_CMD_VERIFY & 0x1F:
1755   - /* VERIFY variants */
1756   -// FLOPPY_DPRINTF("treat VERIFY command\n");
1757   - FLOPPY_ERROR("treat VERIFY command\n");
1758   - fdctrl_stop_transfer(fdctrl, FD_SR0_SEEK, 0x00, 0x00);
1759   - return;
1760   - case FD_CMD_SCAN_EQUAL & 0x1F:
1761   - /* SCAN_EQUAL variants */
1762   -// FLOPPY_DPRINTF("treat SCAN_EQUAL command\n");
1763   - FLOPPY_ERROR("treat SCAN_EQUAL command\n");
1764   - fdctrl_start_transfer(fdctrl, FD_DIR_SCANE);
1765   - return;
1766   - case FD_CMD_SCAN_LOW_OR_EQUAL & 0x1F:
1767   - /* SCAN_LOW_OR_EQUAL variants */
1768   -// FLOPPY_DPRINTF("treat SCAN_LOW_OR_EQUAL command\n");
1769   - FLOPPY_ERROR("treat SCAN_LOW_OR_EQUAL command\n");
1770   - fdctrl_start_transfer(fdctrl, FD_DIR_SCANL);
1771   - return;
1772   - case FD_CMD_SCAN_HIGH_OR_EQUAL & 0x1F:
1773   - /* SCAN_HIGH_OR_EQUAL variants */
1774   -// FLOPPY_DPRINTF("treat SCAN_HIGH_OR_EQUAL command\n");
1775   - FLOPPY_ERROR("treat SCAN_HIGH_OR_EQUAL command\n");
1776   - fdctrl_start_transfer(fdctrl, FD_DIR_SCANH);
1777   - return;
1778   - default:
1779   - break;
1780   - }
1781   - switch (fdctrl->fifo[0] & 0x3F) {
1782   - case FD_CMD_WRITE & 0x3F:
1783   - /* WRITE variants */
1784   - FLOPPY_DPRINTF("treat WRITE command (%02x)\n", fdctrl->fifo[0]);
1785   - fdctrl_start_transfer(fdctrl, FD_DIR_WRITE);
1786   - return;
1787   - case FD_CMD_WRITE_DELETED & 0x3F:
1788   - /* WRITE_DELETED variants */
1789   -// FLOPPY_DPRINTF("treat WRITE_DELETED command\n");
1790   - FLOPPY_ERROR("treat WRITE_DELETED command\n");
1791   - fdctrl_start_transfer_del(fdctrl, FD_DIR_WRITE);
1792   - return;
1793   - default:
1794   - break;
1795   - }
1796   - switch (fdctrl->fifo[0]) {
1797   - case FD_CMD_SPECIFY:
1798   - /* SPECIFY */
1799   - FLOPPY_DPRINTF("treat SPECIFY command\n");
1800   - fdctrl->timer0 = (fdctrl->fifo[1] >> 4) & 0xF;
1801   - fdctrl->timer1 = fdctrl->fifo[2] >> 1;
1802   - fdctrl->dma_en = 1 - (fdctrl->fifo[2] & 1) ;
1803   - /* No result back */
1804   - fdctrl_reset_fifo(fdctrl);
1805   - break;
1806   - case FD_CMD_SENSE_DRIVE_STATUS:
1807   - /* SENSE_DRIVE_STATUS */
1808   - FLOPPY_DPRINTF("treat SENSE_DRIVE_STATUS command\n");
1809   - fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
1810   - cur_drv = get_cur_drv(fdctrl);
1811   - cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
1812   - /* 1 Byte status back */
1813   - fdctrl->fifo[0] = (cur_drv->ro << 6) |
1814   - (cur_drv->track == 0 ? 0x10 : 0x00) |
1815   - (cur_drv->head << 2) |
1816   - fdctrl->cur_drv |
1817   - 0x28;
1818   - fdctrl_set_fifo(fdctrl, 1, 0);
1819   - break;
1820   - case FD_CMD_RECALIBRATE:
1821   - /* RECALIBRATE */
1822   - FLOPPY_DPRINTF("treat RECALIBRATE command\n");
1823   - fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
1824   - cur_drv = get_cur_drv(fdctrl);
1825   - fd_recalibrate(cur_drv);
1826   - fdctrl_reset_fifo(fdctrl);
1827   - /* Raise Interrupt */
1828   - fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
1829   - break;
1830   - case FD_CMD_SEEK:
1831   - /* SEEK */
1832   - FLOPPY_DPRINTF("treat SEEK command\n");
1833   - fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
1834   - cur_drv = get_cur_drv(fdctrl);
1835   - fd_start(cur_drv);
1836   - if (fdctrl->fifo[2] <= cur_drv->track)
1837   - cur_drv->dir = 1;
1838   - else
1839   - cur_drv->dir = 0;
1840   - fdctrl_reset_fifo(fdctrl);
1841   - if (fdctrl->fifo[2] > cur_drv->max_track) {
1842   - fdctrl_raise_irq(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK);
1843   - } else {
1844   - cur_drv->track = fdctrl->fifo[2];
1845   - /* Raise Interrupt */
1846   - fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
1847   - }
1848   - break;
1849   - case FD_CMD_PERPENDICULAR_MODE:
1850   - /* PERPENDICULAR_MODE */
1851   - FLOPPY_DPRINTF("treat PERPENDICULAR_MODE command\n");
1852   - if (fdctrl->fifo[1] & 0x80)
1853   - cur_drv->perpendicular = fdctrl->fifo[1] & 0x7;
1854   - /* No result back */
1855   - fdctrl_reset_fifo(fdctrl);
1856   - break;
1857   - case FD_CMD_CONFIGURE:
1858   - /* CONFIGURE */
1859   - FLOPPY_DPRINTF("treat CONFIGURE command\n");
1860   - fdctrl->config = fdctrl->fifo[2];
1861   - fdctrl->precomp_trk = fdctrl->fifo[3];
1862   - /* No result back */
1863   - fdctrl_reset_fifo(fdctrl);
1864   - break;
1865   - case FD_CMD_POWERDOWN_MODE:
1866   - /* POWERDOWN_MODE */
1867   - FLOPPY_DPRINTF("treat POWERDOWN_MODE command\n");
1868   - fdctrl->pwrd = fdctrl->fifo[1];
1869   - fdctrl->fifo[0] = fdctrl->fifo[1];
1870   - fdctrl_set_fifo(fdctrl, 1, 1);
1871   - break;
1872   - case FD_CMD_OPTION:
1873   - /* OPTION */
1874   - FLOPPY_DPRINTF("treat OPTION command\n");
1875   - /* No result back */
1876   - fdctrl_reset_fifo(fdctrl);
1877   - break;
1878   - case FD_CMD_READ_TRACK:
1879   - /* READ_TRACK */
1880   - FLOPPY_DPRINTF("treat READ_TRACK command\n");
1881   - FLOPPY_ERROR("treat READ_TRACK command\n");
1882   - fdctrl_start_transfer(fdctrl, FD_DIR_READ);
1883   - break;
1884   - case FD_CMD_READ_ID:
1885   - /* READ_ID */
1886   - FLOPPY_DPRINTF("treat READ_ID command\n");
1887   - /* XXX: should set main status register to busy */
1888   - cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
1889   - qemu_mod_timer(fdctrl->result_timer,
1890   - qemu_get_clock(vm_clock) + (ticks_per_sec / 50));
1891   - break;
1892   - case FD_CMD_RESTORE:
1893   - /* RESTORE */
1894   - FLOPPY_DPRINTF("treat RESTORE command\n");
1895   - /* Drives position */
1896   - drv0(fdctrl)->track = fdctrl->fifo[3];
1897   - drv1(fdctrl)->track = fdctrl->fifo[4];
1898   - /* timers */
1899   - fdctrl->timer0 = fdctrl->fifo[7];
1900   - fdctrl->timer1 = fdctrl->fifo[8];
1901   - cur_drv->last_sect = fdctrl->fifo[9];
1902   - fdctrl->lock = fdctrl->fifo[10] >> 7;
1903   - cur_drv->perpendicular = (fdctrl->fifo[10] >> 2) & 0xF;
1904   - fdctrl->config = fdctrl->fifo[11];
1905   - fdctrl->precomp_trk = fdctrl->fifo[12];
1906   - fdctrl->pwrd = fdctrl->fifo[13];
1907   - fdctrl_reset_fifo(fdctrl);
1908   - break;
1909   - case FD_CMD_FORMAT_TRACK:
1910   - /* FORMAT_TRACK */
1911   - FLOPPY_DPRINTF("treat FORMAT_TRACK command\n");
1912   - fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
1913   - cur_drv = get_cur_drv(fdctrl);
1914   - fdctrl->data_state |= FD_STATE_FORMAT;
1915   - if (fdctrl->fifo[0] & 0x80)
1916   - fdctrl->data_state |= FD_STATE_MULTI;
1917   - else
1918   - fdctrl->data_state &= ~FD_STATE_MULTI;
1919   - fdctrl->data_state &= ~FD_STATE_SEEK;
1920   - cur_drv->bps =
1921   - fdctrl->fifo[2] > 7 ? 16384 : 128 << fdctrl->fifo[2];
1922   -#if 0
1923   - cur_drv->last_sect =
1924   - cur_drv->flags & FDISK_DBL_SIDES ? fdctrl->fifo[3] :
1925   - fdctrl->fifo[3] / 2;
1926   -#else
1927   - cur_drv->last_sect = fdctrl->fifo[3];
1928   -#endif
1929   - /* TODO: implement format using DMA expected by the Bochs BIOS
1930   - * and Linux fdformat (read 3 bytes per sector via DMA and fill
1931   - * the sector with the specified fill byte
1932   - */
1933   - fdctrl->data_state &= ~FD_STATE_FORMAT;
1934   - fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
1935   - break;
1936   - case FD_CMD_DRIVE_SPECIFICATION_COMMAND:
1937   - /* DRIVE_SPECIFICATION_COMMAND */
1938   - FLOPPY_DPRINTF("treat DRIVE_SPECIFICATION_COMMAND command\n");
1939   - if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x80) {
1940   - /* Command parameters done */
1941   - if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x40) {
1942   - fdctrl->fifo[0] = fdctrl->fifo[1];
1943   - fdctrl->fifo[2] = 0;
1944   - fdctrl->fifo[3] = 0;
1945   - fdctrl_set_fifo(fdctrl, 4, 1);
1946   - } else {
1947   - fdctrl_reset_fifo(fdctrl);
1948   - }
1949   - } else if (fdctrl->data_len > 7) {
1950   - /* ERROR */
1951   - fdctrl->fifo[0] = 0x80 |
1952   - (cur_drv->head << 2) | fdctrl->cur_drv;
1953   - fdctrl_set_fifo(fdctrl, 1, 1);
1954   - }
1955   - break;
1956   - case FD_CMD_RELATIVE_SEEK_OUT:
1957   - /* RELATIVE_SEEK_OUT */
1958   - FLOPPY_DPRINTF("treat RELATIVE_SEEK_OUT command\n");
1959   - fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
1960   - cur_drv = get_cur_drv(fdctrl);
1961   - fd_start(cur_drv);
1962   - cur_drv->dir = 0;
1963   - if (fdctrl->fifo[2] + cur_drv->track >= cur_drv->max_track) {
1964   - cur_drv->track = cur_drv->max_track - 1;
1965   - } else {
1966   - cur_drv->track += fdctrl->fifo[2];
1967   - }
1968   - fdctrl_reset_fifo(fdctrl);
1969   - fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
1970   - break;
1971   - case FD_CMD_FORMAT_AND_WRITE:
1972   - /* FORMAT_AND_WRITE */
1973   - FLOPPY_DPRINTF("treat FORMAT_AND_WRITE command\n");
1974   - FLOPPY_ERROR("treat FORMAT_AND_WRITE command\n");
1975   - fdctrl_unimplemented(fdctrl);
1976   - break;
1977   - case FD_CMD_RELATIVE_SEEK_IN:
1978   - /* RELATIVE_SEEK_IN */
1979   - FLOPPY_DPRINTF("treat RELATIVE_SEEK_IN command\n");
1980   - fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK;
1981   - cur_drv = get_cur_drv(fdctrl);
1982   - fd_start(cur_drv);
1983   - cur_drv->dir = 1;
1984   - if (fdctrl->fifo[2] > cur_drv->track) {
1985   - cur_drv->track = 0;
1986   - } else {
1987   - cur_drv->track -= fdctrl->fifo[2];
  1830 +
  1831 + for (pos = 0; pos < sizeof(commands)/sizeof(commands[0]); pos++) {
  1832 + if ((fdctrl->fifo[0] & commands[pos].mask) == commands[pos].value) {
  1833 + FLOPPY_DPRINTF("treat %s command\n", commands[pos].name);
  1834 + (*commands[pos].handler)(fdctrl, commands[pos].parameter);
  1835 + break;
1988 1836 }
1989   - fdctrl_reset_fifo(fdctrl);
1990   - /* Raise Interrupt */
1991   - fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
1992   - break;
1993 1837 }
1994 1838 }
1995 1839 }
... ...