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,6 +2,7 @@
2 * QEMU Floppy disk emulator (Intel 82078) 2 * QEMU Floppy disk emulator (Intel 82078)
3 * 3 *
4 * Copyright (c) 2003, 2007 Jocelyn Mayer 4 * Copyright (c) 2003, 2007 Jocelyn Mayer
  5 + * Copyright (c) 2008 Hervé Poussineau
5 * 6 *
6 * Permission is hereby granted, free of charge, to any person obtaining a copy 7 * Permission is hereby granted, free of charge, to any person obtaining a copy
7 * of this software and associated documentation files (the "Software"), to deal 8 * of this software and associated documentation files (the "Software"), to deal
@@ -372,35 +373,34 @@ enum { @@ -372,35 +373,34 @@ enum {
372 }; 373 };
373 374
374 enum { 375 enum {
  376 + FD_CMD_READ_TRACK = 0x02,
375 FD_CMD_SPECIFY = 0x03, 377 FD_CMD_SPECIFY = 0x03,
376 FD_CMD_SENSE_DRIVE_STATUS = 0x04, 378 FD_CMD_SENSE_DRIVE_STATUS = 0x04,
  379 + FD_CMD_WRITE = 0x05,
  380 + FD_CMD_READ = 0x06,
377 FD_CMD_RECALIBRATE = 0x07, 381 FD_CMD_RECALIBRATE = 0x07,
378 FD_CMD_SENSE_INTERRUPT_STATUS = 0x08, 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 FD_CMD_DUMPREG = 0x0e, 387 FD_CMD_DUMPREG = 0x0e,
380 FD_CMD_SEEK = 0x0f, 388 FD_CMD_SEEK = 0x0f,
381 FD_CMD_VERSION = 0x10, 389 FD_CMD_VERSION = 0x10,
  390 + FD_CMD_SCAN_EQUAL = 0x11,
382 FD_CMD_PERPENDICULAR_MODE = 0x12, 391 FD_CMD_PERPENDICULAR_MODE = 0x12,
383 FD_CMD_CONFIGURE = 0x13, 392 FD_CMD_CONFIGURE = 0x13,
384 - FD_CMD_UNLOCK = 0x14, 393 + FD_CMD_LOCK = 0x14,
  394 + FD_CMD_VERIFY = 0x16,
385 FD_CMD_POWERDOWN_MODE = 0x17, 395 FD_CMD_POWERDOWN_MODE = 0x17,
386 FD_CMD_PART_ID = 0x18, 396 FD_CMD_PART_ID = 0x18,
  397 + FD_CMD_SCAN_LOW_OR_EQUAL = 0x19,
  398 + FD_CMD_SCAN_HIGH_OR_EQUAL = 0x1d,
387 FD_CMD_SAVE = 0x2c, 399 FD_CMD_SAVE = 0x2c,
388 FD_CMD_OPTION = 0x33, 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 FD_CMD_RESTORE = 0x4c, 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 FD_CMD_DRIVE_SPECIFICATION_COMMAND = 0x8e, 402 FD_CMD_DRIVE_SPECIFICATION_COMMAND = 0x8e,
402 FD_CMD_RELATIVE_SEEK_OUT = 0x8f, 403 FD_CMD_RELATIVE_SEEK_OUT = 0x8f,
403 - FD_CMD_LOCK = 0x94,  
404 FD_CMD_FORMAT_AND_WRITE = 0xcd, 404 FD_CMD_FORMAT_AND_WRITE = 0xcd,
405 FD_CMD_RELATIVE_SEEK_IN = 0xcf, 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,7 +1047,7 @@ static void fdctrl_set_fifo (fdctrl_t *fdctrl, int fifo_len, int do_irq)
1047 } 1047 }
1048 1048
1049 /* Set an error: unimplemented/unknown command */ 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 #if 0 1052 #if 0
1053 fdrive_t *cur_drv; 1053 fdrive_t *cur_drv;
@@ -1441,9 +1441,339 @@ static void fdctrl_format_sector (fdctrl_t *fdctrl) @@ -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 static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value) 1733 static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
1445 { 1734 {
1446 fdrive_t *cur_drv; 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 cur_drv = get_cur_drv(fdctrl); 1778 cur_drv = get_cur_drv(fdctrl);
1449 /* Reset mode */ 1779 /* Reset mode */
@@ -1473,258 +1803,18 @@ static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value) @@ -1473,258 +1803,18 @@ static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
1473 } 1803 }
1474 if (fdctrl->data_pos == 0) { 1804 if (fdctrl->data_pos == 0) {
1475 /* Command */ 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 enqueue: 1819 enqueue:
1730 FLOPPY_DPRINTF("%s: %02x\n", __func__, value); 1820 FLOPPY_DPRINTF("%s: %02x\n", __func__, value);
@@ -1737,259 +1827,13 @@ static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value) @@ -1737,259 +1827,13 @@ static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
1737 fdctrl_format_sector(fdctrl); 1827 fdctrl_format_sector(fdctrl);
1738 return; 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 }