gwj / at91sam9263 · Commits

Commit 678803abe6d74efcf5e46a5d616849cedae247dd

Authored by blueswir1
1 parent b1fa7164

FDC fix 1/12 (Hervé Poussineau): 

- Adds a command lookup table, as suggested by Fabrice at http://lists.gnu.org/archive/html/qemu-devel/2008-04/msg00143.html
- This also moves initialization functions at the bottom of the file to prevent multiple forward declarations.


git-svn-id: svn://svn.savannah.nongnu.org/qemu/trunk@4281 c046a42c-6fe2-441c-8c8c-71466251a162
Inline Side-by-side
Showing 1 changed file with 149 additions and 150 deletions
hw/fdc.c
  View file @678803a
... ... @@ -322,7 +322,6 @@ static void fdctrl_reset_fifo (fdctrl_t *fdctrl);
322 322 static int fdctrl_transfer_handler (void *opaque, int nchan,
323 323 int dma_pos, int dma_len);
324 324 static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status);
325   -static void fdctrl_result_timer(void *opaque);
326 325  
327 326 static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl);
328 327 static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl);
... ... @@ -694,78 +693,6 @@ static void fdctrl_external_reset(void *opaque)
694 693 fdctrl_reset(s, 0);
695 694 }
696 695  
697   -static fdctrl_t *fdctrl_init_common (qemu_irq irq, int dma_chann,
698   - target_phys_addr_t io_base,
699   - BlockDriverState **fds)
700   -{
701   - fdctrl_t *fdctrl;
702   - int i;
703   -
704   - FLOPPY_DPRINTF("init controller\n");
705   - fdctrl = qemu_mallocz(sizeof(fdctrl_t));
706   - if (!fdctrl)
707   - return NULL;
708   - fdctrl->fifo = qemu_memalign(512, FD_SECTOR_LEN);
709   - if (fdctrl->fifo == NULL) {
710   - qemu_free(fdctrl);
711   - return NULL;
712   - }
713   - fdctrl->result_timer = qemu_new_timer(vm_clock,
714   - fdctrl_result_timer, fdctrl);
715   -
716   - fdctrl->version = 0x90; /* Intel 82078 controller */
717   - fdctrl->irq = irq;
718   - fdctrl->dma_chann = dma_chann;
719   - fdctrl->io_base = io_base;
720   - fdctrl->config = FD_CONFIG_EIS | FD_CONFIG_EFIFO; /* Implicit seek, polling & FIFO enabled */
721   - if (fdctrl->dma_chann != -1) {
722   - fdctrl->dma_en = 1;
723   - DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl);
724   - } else {
725   - fdctrl->dma_en = 0;
726   - }
727   - for (i = 0; i < MAX_FD; i++) {
728   - fd_init(&fdctrl->drives[i], fds[i]);
729   - }
730   - fdctrl_reset(fdctrl, 0);
731   - fdctrl->state = FD_CTRL_ACTIVE;
732   - register_savevm("fdc", io_base, 1, fdc_save, fdc_load, fdctrl);
733   - qemu_register_reset(fdctrl_external_reset, fdctrl);
734   - for (i = 0; i < MAX_FD; i++) {
735   - fd_revalidate(&fdctrl->drives[i]);
736   - }
737   -
738   - return fdctrl;
739   -}
740   -
741   -fdctrl_t *fdctrl_init (qemu_irq irq, int dma_chann, int mem_mapped,
742   - target_phys_addr_t io_base,
743   - BlockDriverState **fds)
744   -{
745   - fdctrl_t *fdctrl;
746   - int io_mem;
747   -
748   - fdctrl = fdctrl_init_common(irq, dma_chann, io_base, fds);
749   -
750   - fdctrl->sun4m = 0;
751   - if (mem_mapped) {
752   - io_mem = cpu_register_io_memory(0, fdctrl_mem_read, fdctrl_mem_write,
753   - fdctrl);
754   - cpu_register_physical_memory(io_base, 0x08, io_mem);
755   - } else {
756   - register_ioport_read((uint32_t)io_base + 0x01, 5, 1, &fdctrl_read,
757   - fdctrl);
758   - register_ioport_read((uint32_t)io_base + 0x07, 1, 1, &fdctrl_read,
759   - fdctrl);
760   - register_ioport_write((uint32_t)io_base + 0x01, 5, 1, &fdctrl_write,
761   - fdctrl);
762   - register_ioport_write((uint32_t)io_base + 0x07, 1, 1, &fdctrl_write,
763   - fdctrl);
764   - }
765   -
766   - return fdctrl;
767   -}
768   -
769 696 static void fdctrl_handle_tc(void *opaque, int irq, int level)
770 697 {
771 698 //fdctrl_t *s = opaque;
... ... @@ -776,23 +703,6 @@ static void fdctrl_handle_tc(void *opaque, int irq, int level)
776 703 }
777 704 }
778 705  
779   -fdctrl_t *sun4m_fdctrl_init (qemu_irq irq, target_phys_addr_t io_base,
780   - BlockDriverState **fds, qemu_irq *fdc_tc)
781   -{
782   - fdctrl_t *fdctrl;
783   - int io_mem;
784   -
785   - fdctrl = fdctrl_init_common(irq, 0, io_base, fds);
786   - fdctrl->sun4m = 1;
787   - io_mem = cpu_register_io_memory(0, fdctrl_mem_read_strict,
788   - fdctrl_mem_write_strict,
789   - fdctrl);
790   - cpu_register_physical_memory(io_base, 0x08, io_mem);
791   - *fdc_tc = *qemu_allocate_irqs(fdctrl_handle_tc, fdctrl, 1);
792   -
793   - return fdctrl;
794   -}
795   -
796 706 /* XXX: may change if moved to bdrv */
797 707 int fdctrl_get_drive_type(fdctrl_t *fdctrl, int drive_num)
798 708 {
... ... @@ -1730,50 +1640,54 @@ static void fdctrl_handle_relative_seek_in (fdctrl_t *fdctrl, int direction)
1730 1640 fdctrl_raise_irq(fdctrl, FD_SR0_SEEK);
1731 1641 }
1732 1642  
  1643 +static const struct {
  1644 + uint8_t value;
  1645 + uint8_t mask;
  1646 + const char* name;
  1647 + int parameters;
  1648 + void (*handler)(fdctrl_t *fdctrl, int direction);
  1649 + int direction;
  1650 +} handlers[] = {
  1651 + { FD_CMD_READ, 0x1f, "READ", 8, fdctrl_start_transfer, FD_DIR_READ },
  1652 + { FD_CMD_WRITE, 0x3f, "WRITE", 8, fdctrl_start_transfer, FD_DIR_WRITE },
  1653 + { FD_CMD_SEEK, 0xff, "SEEK", 2, fdctrl_handle_seek },
  1654 + { FD_CMD_SENSE_INTERRUPT_STATUS, 0xff, "SENSE INTERRUPT STATUS", 0, fdctrl_handle_sense_interrupt_status },
  1655 + { FD_CMD_RECALIBRATE, 0xff, "RECALIBRATE", 1, fdctrl_handle_recalibrate },
  1656 + { FD_CMD_FORMAT_TRACK, 0xbf, "FORMAT TRACK", 5, fdctrl_handle_format_track },
  1657 + { FD_CMD_READ_TRACK, 0xbf, "READ TRACK", 8, fdctrl_start_transfer, FD_DIR_READ },
  1658 + { FD_CMD_RESTORE, 0xff, "RESTORE", 17, fdctrl_handle_restore }, /* part of READ DELETED DATA */
  1659 + { FD_CMD_SAVE, 0xff, "SAVE", 0, fdctrl_handle_save }, /* part of READ DELETED DATA */
  1660 + { FD_CMD_READ_DELETED, 0x1f, "READ DELETED DATA", 8, fdctrl_start_transfer_del, FD_DIR_READ },
  1661 + { FD_CMD_SCAN_EQUAL, 0x1f, "SCAN EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANE },
  1662 + { FD_CMD_VERIFY, 0x1f, "VERIFY", 8, fdctrl_unimplemented },
  1663 + { FD_CMD_SCAN_LOW_OR_EQUAL, 0x1f, "SCAN LOW OR EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANL },
  1664 + { FD_CMD_SCAN_HIGH_OR_EQUAL, 0x1f, "SCAN HIGH OR EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANH },
  1665 + { FD_CMD_WRITE_DELETED, 0x3f, "WRITE DELETED DATA", 8, fdctrl_start_transfer_del, FD_DIR_WRITE },
  1666 + { FD_CMD_READ_ID, 0xbf, "READ ID", 1, fdctrl_handle_readid },
  1667 + { FD_CMD_SPECIFY, 0xff, "SPECIFY", 2, fdctrl_handle_specify },
  1668 + { FD_CMD_SENSE_DRIVE_STATUS, 0xff, "SENSE DRIVE STATUS", 1, fdctrl_handle_sense_drive_status },
  1669 + { FD_CMD_PERPENDICULAR_MODE, 0xff, "PERPENDICULAR MODE", 1, fdctrl_handle_perpendicular_mode },
  1670 + { FD_CMD_CONFIGURE, 0xff, "CONFIGURE", 3, fdctrl_handle_configure },
  1671 + { FD_CMD_POWERDOWN_MODE, 0xff, "POWERDOWN MODE", 2, fdctrl_handle_powerdown_mode },
  1672 + { FD_CMD_OPTION, 0xff, "OPTION", 1, fdctrl_handle_option },
  1673 + { FD_CMD_DRIVE_SPECIFICATION_COMMAND, 0xff, "DRIVE SPECIFICATION COMMAND", 5, fdctrl_handle_drive_specification_command },
  1674 + { FD_CMD_RELATIVE_SEEK_OUT, 0xff, "RELATIVE SEEK OUT", 2, fdctrl_handle_relative_seek_out },
  1675 + { FD_CMD_FORMAT_AND_WRITE, 0xff, "FORMAT AND WRITE", 10, fdctrl_unimplemented },
  1676 + { FD_CMD_RELATIVE_SEEK_IN, 0xff, "RELATIVE SEEK IN", 2, fdctrl_handle_relative_seek_in },
  1677 + { FD_CMD_LOCK, 0x7f, "LOCK", 0, fdctrl_handle_lock },
  1678 + { FD_CMD_DUMPREG, 0xff, "DUMPREG", 0, fdctrl_handle_dumpreg },
  1679 + { FD_CMD_VERSION, 0xff, "VERSION", 0, fdctrl_handle_version },
  1680 + { FD_CMD_PART_ID, 0xff, "PART ID", 0, fdctrl_handle_partid },
  1681 + { FD_CMD_WRITE, 0x1f, "WRITE (BeOS)", 8, fdctrl_start_transfer, FD_DIR_WRITE }, /* not in specification ; BeOS 4.5 bug */
  1682 + { 0, 0, "unknown", 0, fdctrl_unimplemented }, /* default handler */
  1683 +};
  1684 +/* Associate command to an index in the 'handlers' array */
  1685 +static uint8_t command_to_handler[256];
  1686 +
1733 1687 static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
1734 1688 {
1735 1689 fdrive_t *cur_drv;
1736 1690 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   - };
1777 1691  
1778 1692 cur_drv = get_cur_drv(fdctrl);
1779 1693 /* Reset mode */
... ... @@ -1803,20 +1717,11 @@ static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
1803 1717 }
1804 1718 if (fdctrl->data_pos == 0) {
1805 1719 /* Command */
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   - }
1812   - }
1813   -
1814   - /* Unknown command */
1815   - FLOPPY_ERROR("unknown command: 0x%02x\n", value);
1816   - fdctrl_unimplemented(fdctrl, 0);
1817   - return;
  1720 + pos = command_to_handler[value & 0xff];
  1721 + FLOPPY_DPRINTF("%s command\n", handlers[pos].name);
  1722 + fdctrl->data_len = handlers[pos].parameters + 1;
1818 1723 }
1819   - enqueue:
  1724 +
1820 1725 FLOPPY_DPRINTF("%s: %02x\n", __func__, value);
1821 1726 fdctrl->fifo[fdctrl->data_pos] = value;
1822 1727 if (++fdctrl->data_pos == fdctrl->data_len) {
... ... @@ -1828,13 +1733,9 @@ static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
1828 1733 return;
1829 1734 }
1830 1735  
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;
1836   - }
1837   - }
  1736 + pos = command_to_handler[fdctrl->fifo[0] & 0xff];
  1737 + FLOPPY_DPRINTF("treat %s command\n", handlers[pos].name);
  1738 + (*handlers[pos].handler)(fdctrl, handlers[pos].direction);
1838 1739 }
1839 1740 }
1840 1741  
... ... @@ -1852,3 +1753,101 @@ static void fdctrl_result_timer(void *opaque)
1852 1753 }
1853 1754 fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
1854 1755 }
  1756 +
  1757 +/* Init functions */
  1758 +static fdctrl_t *fdctrl_init_common (qemu_irq irq, int dma_chann,
  1759 + target_phys_addr_t io_base,
  1760 + BlockDriverState **fds)
  1761 +{
  1762 + fdctrl_t *fdctrl;
  1763 + int i, j;
  1764 +
  1765 + /* Fill 'command_to_handler' lookup table */
  1766 + for (i = sizeof(handlers)/sizeof(handlers[0]) - 1; i >= 0; i--) {
  1767 + for (j = 0; j < sizeof(command_to_handler); j++) {
  1768 + if ((j & handlers[i].mask) == handlers[i].value)
  1769 + command_to_handler[j] = i;
  1770 + }
  1771 + }
  1772 +
  1773 + FLOPPY_DPRINTF("init controller\n");
  1774 + fdctrl = qemu_mallocz(sizeof(fdctrl_t));
  1775 + if (!fdctrl)
  1776 + return NULL;
  1777 + fdctrl->fifo = qemu_memalign(512, FD_SECTOR_LEN);
  1778 + if (fdctrl->fifo == NULL) {
  1779 + qemu_free(fdctrl);
  1780 + return NULL;
  1781 + }
  1782 + fdctrl->result_timer = qemu_new_timer(vm_clock,
  1783 + fdctrl_result_timer, fdctrl);
  1784 +
  1785 + fdctrl->version = 0x90; /* Intel 82078 controller */
  1786 + fdctrl->irq = irq;
  1787 + fdctrl->dma_chann = dma_chann;
  1788 + fdctrl->io_base = io_base;
  1789 + fdctrl->config = FD_CONFIG_EIS | FD_CONFIG_EFIFO; /* Implicit seek, polling & FIFO enabled */
  1790 + if (fdctrl->dma_chann != -1) {
  1791 + fdctrl->dma_en = 1;
  1792 + DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl);
  1793 + } else {
  1794 + fdctrl->dma_en = 0;
  1795 + }
  1796 + for (i = 0; i < MAX_FD; i++) {
  1797 + fd_init(&fdctrl->drives[i], fds[i]);
  1798 + }
  1799 + fdctrl_reset(fdctrl, 0);
  1800 + fdctrl->state = FD_CTRL_ACTIVE;
  1801 + register_savevm("fdc", io_base, 1, fdc_save, fdc_load, fdctrl);
  1802 + qemu_register_reset(fdctrl_external_reset, fdctrl);
  1803 + for (i = 0; i < MAX_FD; i++) {
  1804 + fd_revalidate(&fdctrl->drives[i]);
  1805 + }
  1806 +
  1807 + return fdctrl;
  1808 +}
  1809 +
  1810 +fdctrl_t *fdctrl_init (qemu_irq irq, int dma_chann, int mem_mapped,
  1811 + target_phys_addr_t io_base,
  1812 + BlockDriverState **fds)
  1813 +{
  1814 + fdctrl_t *fdctrl;
  1815 + int io_mem;
  1816 +
  1817 + fdctrl = fdctrl_init_common(irq, dma_chann, io_base, fds);
  1818 +
  1819 + fdctrl->sun4m = 0;
  1820 + if (mem_mapped) {
  1821 + io_mem = cpu_register_io_memory(0, fdctrl_mem_read, fdctrl_mem_write,
  1822 + fdctrl);
  1823 + cpu_register_physical_memory(io_base, 0x08, io_mem);
  1824 + } else {
  1825 + register_ioport_read((uint32_t)io_base + 0x01, 5, 1, &fdctrl_read,
  1826 + fdctrl);
  1827 + register_ioport_read((uint32_t)io_base + 0x07, 1, 1, &fdctrl_read,
  1828 + fdctrl);
  1829 + register_ioport_write((uint32_t)io_base + 0x01, 5, 1, &fdctrl_write,
  1830 + fdctrl);
  1831 + register_ioport_write((uint32_t)io_base + 0x07, 1, 1, &fdctrl_write,
  1832 + fdctrl);
  1833 + }
  1834 +
  1835 + return fdctrl;
  1836 +}
  1837 +
  1838 +fdctrl_t *sun4m_fdctrl_init (qemu_irq irq, target_phys_addr_t io_base,
  1839 + BlockDriverState **fds, qemu_irq *fdc_tc)
  1840 +{
  1841 + fdctrl_t *fdctrl;
  1842 + int io_mem;
  1843 +
  1844 + fdctrl = fdctrl_init_common(irq, 0, io_base, fds);
  1845 + fdctrl->sun4m = 1;
  1846 + io_mem = cpu_register_io_memory(0, fdctrl_mem_read_strict,
  1847 + fdctrl_mem_write_strict,
  1848 + fdctrl);
  1849 + cpu_register_physical_memory(io_base, 0x08, io_mem);
  1850 + *fdc_tc = *qemu_allocate_irqs(fdctrl_handle_tc, fdctrl, 1);
  1851 +
  1852 + return fdctrl;
  1853 +}
... ...