From mboxrd@z Thu Jan 1 00:00:00 1970 Received: from mailman by lists.gnu.org with tmda-scanned (Exim 4.43) id 1JmDXx-00073z-L4 for qemu-devel@nongnu.org; Wed, 16 Apr 2008 15:43:57 -0400 Received: from exim by lists.gnu.org with spam-scanned (Exim 4.43) id 1JmDXp-00073W-4C for qemu-devel@nongnu.org; Wed, 16 Apr 2008 15:43:56 -0400 Received: from [199.232.76.173] (helo=monty-python.gnu.org) by lists.gnu.org with esmtp (Exim 4.43) id 1JmDXp-00073T-1b for qemu-devel@nongnu.org; Wed, 16 Apr 2008 15:43:49 -0400 Received: from smtp3-g19.free.fr ([212.27.42.29]) by monty-python.gnu.org with esmtp (Exim 4.60) (envelope-from ) id 1JmDXn-0003YP-GB for qemu-devel@nongnu.org; Wed, 16 Apr 2008 15:43:48 -0400 Received: from smtp3-g19.free.fr (localhost.localdomain [127.0.0.1]) by smtp3-g19.free.fr (Postfix) with ESMTP id B2F7017B5B0 for ; Wed, 16 Apr 2008 21:43:44 +0200 (CEST) Received: from [127.0.0.1] (rob92-10-88-171-126-33.fbx.proxad.net [88.171.126.33]) by smtp3-g19.free.fr (Postfix) with ESMTP id C427A17B542 for ; Wed, 16 Apr 2008 21:43:41 +0200 (CEST) Message-ID: <480656E7.7000707@reactos.org> Date: Wed, 16 Apr 2008 21:43:35 +0200 From: =?ISO-8859-1?Q?Herv=E9_Poussineau?= MIME-Version: 1.0 Content-Type: multipart/mixed; boundary="------------080908020200070308000501" Subject: [Qemu-devel] [12 patches] FDC: improve emulation Reply-To: qemu-devel@nongnu.org List-Id: qemu-devel.nongnu.org List-Unsubscribe: , List-Archive: List-Post: List-Help: List-Subscribe: , To: qemu-devel@nongnu.org This is a multi-part message in MIME format. --------------080908020200070308000501 Content-Type: text/plain; charset=ISO-8859-1; format=flowed Content-Transfer-Encoding: quoted-printable Hi, I've done big improvements on floppy drive controller. For easier reviewing, I've tried to split them up in differents parts.=20 Sorry if some parts are still big. Each patch applies on top of the previous one. If only some of them are accepted, I can provide a global patch for only=20 these ones. Patches which needs more work will be resubmitted later. List of attached patches: fdc_01_lookuptable.diff - Adds a command lookup table, as suggested by Fabrice at=20 http://lists.gnu.org/archive/html/qemu-devel/2008-04/msg00143.html - This also moves initialization functions at the bottom of the file to=20 prevent multiple forward declarations. fdc_02_seek_next_sector.diff - Extract seeking to next sector handling in a function. Add a sector=20 seek in PIO read and write modes fdc_03_status_ab.diff - Fixes status A and status B registers. It removes one Sun4m mutation.=20 Also removes the internal FD_CTRL_INTR flag. fdc_04_status_012.diff - Fixes status0, status1 and status2 handling, which were broken in=20 read/write commands and in some subtle ways. Status are now calculated=20 during command execution and not at the very end. - This allows removing of one hack in=20 fdctrl_handle_sense_interrupt_status(). fdc_05_dmaen.diff - Handles correctly FD_MSR_NONDMA/FD_DOR_NONDMA flags, and uses them=20 when possible. Fixes a problem with SPECIFY command. This allows=20 removing of last Sun4m mutation. fdc_06_dor_reg.diff - Better handling of DOR register. DOR register drives external motors,=20 but it not limited to existing drives. - Use FD_DOR_nRESET flag instead of internal FD_CTRL_RESET flag. - Support writing to DOR register even in reset mode (as said in=20 specification) fdc_07_msr_reg.diff - Stores controller state in MSR register instead of internal state=20 field. This simplifies the fdctrl_read_main_status() function, which may=20 be called in some tight loops. fdc_08_cleanup.diff - Removes useless fields in fdrive_t structure. - Adds a message when bdrv_read/bdrv_write calls fail. fdc_09_tdr_reg.diff - Replaces bootsel field by the whole tdr register. It may be easier if=20 we want to later add support for tapes. fdc_10_4_fds.diff - Supports up to 4 floppy drives if MAX_FD is set to 4. fdc_11_cur_drv.diff - Replaces access to cur_drv field by macros. fdc_12_pio_write.diff - Fixes transfer data length. - Fixes buffer overflow in PIO mode write. Floppy controller works better than before in Sun4m. It now gets READ=20 commands and correctly detects if floppy is write-protected or not.=20 Unfortunately, no valid filesystem seems to be found on the floppy. Please comment. Herv=E9 --------------080908020200070308000501 Content-Type: text/plain; name="fdc_12_pio_write.diff" Content-Transfer-Encoding: 7bit Content-Disposition: inline; filename="fdc_12_pio_write.diff" Index: hw/fdc.c =================================================================== --- hw/fdc.c (revision 4215) +++ hw/fdc.c (working copy) @@ -1117,13 +1117,13 @@ else fdctrl->data_state &= ~FD_STATE_MULTI; if (fdctrl->fifo[5] == 00) { - fdctrl->data_len = fdctrl->fifo[8]; + fdctrl->data_len = fdctrl->fifo[7]; } else { int tmp; fdctrl->data_len = 128 << (fdctrl->fifo[5] > 7 ? 7 : fdctrl->fifo[5]); - tmp = (cur_drv->last_sect - ks + 1); + tmp = (fdctrl->fifo[6] - ks + 1); if (fdctrl->fifo[0] & 0x80) - tmp += cur_drv->last_sect; + tmp += fdctrl->fifo[6]; fdctrl->data_len *= tmp; } fdctrl->eot = fdctrl->fifo[6]; @@ -1335,7 +1335,7 @@ kh = fdctrl->fifo[7]; ks = fdctrl->fifo[8]; FLOPPY_DPRINTF("format sector at %d %d %02x %02x (%d)\n", - GET_CUR_DRV(fdctrl->dor), kh, kt, ks, + GET_CUR_DRV(fdctrl), kh, kt, ks, _fd_sector(kh, kt, ks, cur_drv->last_sect)); switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & FD_CONFIG_EIS)) { case 2: @@ -1643,7 +1643,7 @@ } else if (fdctrl->data_len > 7) { /* ERROR */ fdctrl->fifo[0] = 0x80 | - (cur_drv->head << 2) | GET_CUR_DRV(fdctrl->dor); + (cur_drv->head << 2) | GET_CUR_DRV(fdctrl); fdctrl_set_fifo(fdctrl, 1, 1); } } @@ -1743,8 +1743,10 @@ /* Is it write command time ? */ if (fdctrl->msr & FD_MSR_NONDMA) { /* FIFO data write */ - fdctrl->fifo[fdctrl->data_pos++] = value; - if (fdctrl->data_pos % FD_SECTOR_LEN == (FD_SECTOR_LEN - 1) || + pos = fdctrl->data_pos; + pos %= FD_SECTOR_LEN; + fdctrl->fifo[pos] = value; + if (pos == FD_SECTOR_LEN - 1 || fdctrl->data_pos == fdctrl->data_len) { cur_drv = get_cur_drv(fdctrl); if (bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1) < 0) { --------------080908020200070308000501 Content-Type: text/plain; name="fdc_01_lookuptable.diff" Content-Transfer-Encoding: 7bit Content-Disposition: inline; filename="fdc_01_lookuptable.diff" Index: hw/fdc.c =================================================================== --- hw/fdc.c (revision 4215) +++ hw/fdc.c (working copy) @@ -322,7 +322,6 @@ static int fdctrl_transfer_handler (void *opaque, int nchan, int dma_pos, int dma_len); static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status); -static void fdctrl_result_timer(void *opaque); static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl); static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl); @@ -694,78 +693,6 @@ fdctrl_reset(s, 0); } -static fdctrl_t *fdctrl_init_common (qemu_irq irq, int dma_chann, - target_phys_addr_t io_base, - BlockDriverState **fds) -{ - fdctrl_t *fdctrl; - int i; - - FLOPPY_DPRINTF("init controller\n"); - fdctrl = qemu_mallocz(sizeof(fdctrl_t)); - if (!fdctrl) - return NULL; - fdctrl->fifo = qemu_memalign(512, FD_SECTOR_LEN); - if (fdctrl->fifo == NULL) { - qemu_free(fdctrl); - return NULL; - } - fdctrl->result_timer = qemu_new_timer(vm_clock, - fdctrl_result_timer, fdctrl); - - fdctrl->version = 0x90; /* Intel 82078 controller */ - fdctrl->irq = irq; - fdctrl->dma_chann = dma_chann; - fdctrl->io_base = io_base; - fdctrl->config = FD_CONFIG_EIS | FD_CONFIG_EFIFO; /* Implicit seek, polling & FIFO enabled */ - if (fdctrl->dma_chann != -1) { - fdctrl->dma_en = 1; - DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl); - } else { - fdctrl->dma_en = 0; - } - for (i = 0; i < MAX_FD; i++) { - fd_init(&fdctrl->drives[i], fds[i]); - } - fdctrl_reset(fdctrl, 0); - fdctrl->state = FD_CTRL_ACTIVE; - register_savevm("fdc", io_base, 1, fdc_save, fdc_load, fdctrl); - qemu_register_reset(fdctrl_external_reset, fdctrl); - for (i = 0; i < MAX_FD; i++) { - fd_revalidate(&fdctrl->drives[i]); - } - - return fdctrl; -} - -fdctrl_t *fdctrl_init (qemu_irq irq, int dma_chann, int mem_mapped, - target_phys_addr_t io_base, - BlockDriverState **fds) -{ - fdctrl_t *fdctrl; - int io_mem; - - fdctrl = fdctrl_init_common(irq, dma_chann, io_base, fds); - - fdctrl->sun4m = 0; - if (mem_mapped) { - io_mem = cpu_register_io_memory(0, fdctrl_mem_read, fdctrl_mem_write, - fdctrl); - cpu_register_physical_memory(io_base, 0x08, io_mem); - } else { - register_ioport_read((uint32_t)io_base + 0x01, 5, 1, &fdctrl_read, - fdctrl); - register_ioport_read((uint32_t)io_base + 0x07, 1, 1, &fdctrl_read, - fdctrl); - register_ioport_write((uint32_t)io_base + 0x01, 5, 1, &fdctrl_write, - fdctrl); - register_ioport_write((uint32_t)io_base + 0x07, 1, 1, &fdctrl_write, - fdctrl); - } - - return fdctrl; -} - static void fdctrl_handle_tc(void *opaque, int irq, int level) { //fdctrl_t *s = opaque; @@ -776,23 +703,6 @@ } } -fdctrl_t *sun4m_fdctrl_init (qemu_irq irq, target_phys_addr_t io_base, - BlockDriverState **fds, qemu_irq *fdc_tc) -{ - fdctrl_t *fdctrl; - int io_mem; - - fdctrl = fdctrl_init_common(irq, 0, io_base, fds); - fdctrl->sun4m = 1; - io_mem = cpu_register_io_memory(0, fdctrl_mem_read_strict, - fdctrl_mem_write_strict, - fdctrl); - cpu_register_physical_memory(io_base, 0x08, io_mem); - *fdc_tc = *qemu_allocate_irqs(fdctrl_handle_tc, fdctrl, 1); - - return fdctrl; -} - /* XXX: may change if moved to bdrv */ int fdctrl_get_drive_type(fdctrl_t *fdctrl, int drive_num) { @@ -1730,50 +1640,54 @@ fdctrl_raise_irq(fdctrl, FD_SR0_SEEK); } +static const struct { + uint8_t value; + uint8_t mask; + const char* name; + int parameters; + void (*handler)(fdctrl_t *fdctrl, int direction); + int parameter; +} handlers[] = { + { FD_CMD_READ, 0x1f, "READ", 8, fdctrl_start_transfer, FD_DIR_READ }, + { FD_CMD_WRITE, 0x3f, "WRITE", 8, fdctrl_start_transfer, FD_DIR_WRITE }, + { FD_CMD_SEEK, 0xff, "SEEK", 2, fdctrl_handle_seek }, + { FD_CMD_SENSE_INTERRUPT_STATUS, 0xff, "SENSE INTERRUPT STATUS", 0, fdctrl_handle_sense_interrupt_status }, + { FD_CMD_RECALIBRATE, 0xff, "RECALIBRATE", 1, fdctrl_handle_recalibrate }, + { 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_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_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_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_SPECIFY, 0xff, "SPECIFY", 2, fdctrl_handle_specify }, + { FD_CMD_SENSE_DRIVE_STATUS, 0xff, "SENSE DRIVE STATUS", 1, fdctrl_handle_sense_drive_status }, + { 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 */ + { 0, 0, "unknown", 0, fdctrl_unimplemented }, /* default handler */ +}; +/* Associate command to an index in the 'handlers' array */ +static uint8_t command_to_handler[0xff]; + static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value) { fdrive_t *cur_drv; int pos; - static const struct { - uint8_t value; - uint8_t mask; - const char* name; - int parameters; - void (*handler)(fdctrl_t *fdctrl, int direction); - int parameter; - } commands[] = { - { FD_CMD_READ, 0x1f, "READ", 8, fdctrl_start_transfer, FD_DIR_READ }, - { FD_CMD_WRITE, 0x3f, "WRITE", 8, fdctrl_start_transfer, FD_DIR_WRITE }, - { FD_CMD_SEEK, 0xff, "SEEK", 2, fdctrl_handle_seek }, - { FD_CMD_SENSE_INTERRUPT_STATUS, 0xff, "SENSE INTERRUPT STATUS", 0, fdctrl_handle_sense_interrupt_status }, - { FD_CMD_RECALIBRATE, 0xff, "RECALIBRATE", 1, fdctrl_handle_recalibrate }, - { 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_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_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_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_SPECIFY, 0xff, "SPECIFY", 2, fdctrl_handle_specify }, - { FD_CMD_SENSE_DRIVE_STATUS, 0xff, "SENSE DRIVE STATUS", 1, fdctrl_handle_sense_drive_status }, - { 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 */ @@ -1803,20 +1717,11 @@ } if (fdctrl->data_pos == 0) { /* Command */ - 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); - fdctrl->data_len = commands[pos].parameters + 1; - goto enqueue; - } + pos = command_to_handler[value & 0xff]; + FLOPPY_DPRINTF("%s command\n", handlers[pos].name); + fdctrl->data_len = handlers[pos].parameters + 1; } - /* Unknown command */ - FLOPPY_ERROR("unknown command: 0x%02x\n", value); - fdctrl_unimplemented(fdctrl, 0); - return; - } - enqueue: FLOPPY_DPRINTF("%s: %02x\n", __func__, value); fdctrl->fifo[fdctrl->data_pos] = value; if (++fdctrl->data_pos == fdctrl->data_len) { @@ -1828,13 +1733,9 @@ 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; - } - } + pos = command_to_handler[fdctrl->fifo[0] & 0xff]; + FLOPPY_DPRINTF("treat %s command\n", handlers[pos].name); + (*handlers[pos].handler)(fdctrl, handlers[pos].parameter); } } @@ -1851,4 +1752,102 @@ cur_drv->sect = (cur_drv->sect % cur_drv->last_sect) + 1; } fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00); +} + +/* Init functions */ +static fdctrl_t *fdctrl_init_common (qemu_irq irq, int dma_chann, + target_phys_addr_t io_base, + BlockDriverState **fds) +{ + fdctrl_t *fdctrl; + int i, j; + + /* Fill 'command_to_handler' lookup table */ + for (i = sizeof(handlers)/sizeof(handlers[0]) - 1; i >= 0; i--) { + for (j = 0; j < sizeof(command_to_handler); j++) { + if ((j & handlers[i].mask) == handlers[i].value) + command_to_handler[j] = i; + } + } + + FLOPPY_DPRINTF("init controller\n"); + fdctrl = qemu_mallocz(sizeof(fdctrl_t)); + if (!fdctrl) + return NULL; + fdctrl->fifo = qemu_memalign(512, FD_SECTOR_LEN); + if (fdctrl->fifo == NULL) { + qemu_free(fdctrl); + return NULL; + } + fdctrl->result_timer = qemu_new_timer(vm_clock, + fdctrl_result_timer, fdctrl); + + fdctrl->version = 0x90; /* Intel 82078 controller */ + fdctrl->irq = irq; + fdctrl->dma_chann = dma_chann; + fdctrl->io_base = io_base; + fdctrl->config = FD_CONFIG_EIS | FD_CONFIG_EFIFO; /* Implicit seek, polling & FIFO enabled */ + if (fdctrl->dma_chann != -1) { + fdctrl->dma_en = 1; + DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl); + } else { + fdctrl->dma_en = 0; + } + for (i = 0; i < MAX_FD; i++) { + fd_init(&fdctrl->drives[i], fds[i]); + } + fdctrl_reset(fdctrl, 0); + fdctrl->state = FD_CTRL_ACTIVE; + register_savevm("fdc", io_base, 1, fdc_save, fdc_load, fdctrl); + qemu_register_reset(fdctrl_external_reset, fdctrl); + for (i = 0; i < MAX_FD; i++) { + fd_revalidate(&fdctrl->drives[i]); + } + + return fdctrl; +} + +fdctrl_t *fdctrl_init (qemu_irq irq, int dma_chann, int mem_mapped, + target_phys_addr_t io_base, + BlockDriverState **fds) +{ + fdctrl_t *fdctrl; + int io_mem; + + fdctrl = fdctrl_init_common(irq, dma_chann, io_base, fds); + + fdctrl->sun4m = 0; + if (mem_mapped) { + io_mem = cpu_register_io_memory(0, fdctrl_mem_read, fdctrl_mem_write, + fdctrl); + cpu_register_physical_memory(io_base, 0x08, io_mem); + } else { + register_ioport_read((uint32_t)io_base + 0x01, 5, 1, &fdctrl_read, + fdctrl); + register_ioport_read((uint32_t)io_base + 0x07, 1, 1, &fdctrl_read, + fdctrl); + register_ioport_write((uint32_t)io_base + 0x01, 5, 1, &fdctrl_write, + fdctrl); + register_ioport_write((uint32_t)io_base + 0x07, 1, 1, &fdctrl_write, + fdctrl); + } + + return fdctrl; +} + +fdctrl_t *sun4m_fdctrl_init (qemu_irq irq, target_phys_addr_t io_base, + BlockDriverState **fds, qemu_irq *fdc_tc) +{ + fdctrl_t *fdctrl; + int io_mem; + + fdctrl = fdctrl_init_common(irq, 0, io_base, fds); + fdctrl->sun4m = 1; + io_mem = cpu_register_io_memory(0, fdctrl_mem_read_strict, + fdctrl_mem_write_strict, + fdctrl); + cpu_register_physical_memory(io_base, 0x08, io_mem); + *fdc_tc = *qemu_allocate_irqs(fdctrl_handle_tc, fdctrl, 1); + + return fdctrl; } --------------080908020200070308000501 Content-Type: text/plain; name="fdc_02_seek_next_sector.diff" Content-Transfer-Encoding: base64 Content-Disposition: inline; filename="fdc_02_seek_next_sector.diff" SW5kZXg6IGh3L2ZkYy5jCj09PT09PT09PT09PT09PT09PT09PT09PT09PT09PT09PT09PT09 PT09PT09PT09PT09PT09PT09PT09PT09PT09PT0KLS0tIGh3L2ZkYy5jCShyZXZpc2lvbiA0 MjE1KQorKysgaHcvZmRjLmMJKHdvcmtpbmcgY29weSkNCkBAIC05NzQsNiArOTc0LDQwIEBA DQogI2VuZGlmDQogfQ0KIA0KKy8qIFNlZWsgdG8gbmV4dCBzZWN0b3IgKi8NCitzdGF0aWMg aW50IGZkY3RybF9zZWVrX3RvX25leHRfc2VjdCAoZmRjdHJsX3QgKmZkY3RybCwgZmRyaXZl X3QgKmN1cl9kcnYpDQorew0KKyAgICBGTE9QUFlfRFBSSU5URigic2VlayB0byBuZXh0IHNl Y3RvciAoJWQgJTAyeCAlMDJ4ID0+ICVkKVxuIiwNCisgICAgICAgICAgICAgICAgICAgY3Vy X2Rydi0+aGVhZCwgY3VyX2Rydi0+dHJhY2ssIGN1cl9kcnYtPnNlY3QsDQorICAgICAgICAg ICAgICAgICAgIGZkX3NlY3RvcihjdXJfZHJ2KSk7DQorICAgIC8qIFhYWDogY3VyX2Rydi0+ c2VjdCA+PSBjdXJfZHJ2LT5sYXN0X3NlY3Qgc2hvdWxkIGJlIGFuDQorICAgICAgIGVycm9y IGluIGZhY3QgKi8NCisgICAgaWYgKGN1cl9kcnYtPnNlY3QgPj0gY3VyX2Rydi0+bGFzdF9z ZWN0IHx8DQorICAgICAgICBjdXJfZHJ2LT5zZWN0ID09IGZkY3RybC0+ZW90KSB7DQorICAg ICAgICBjdXJfZHJ2LT5zZWN0ID0gMTsNCisgICAgICAgIGlmIChGRF9NVUxUSV9UUkFDSyhm ZGN0cmwtPmRhdGFfc3RhdGUpKSB7DQorICAgICAgICAgICAgaWYgKGN1cl9kcnYtPmhlYWQg PT0gMCAmJg0KKyAgICAgICAgICAgICAgICAoY3VyX2Rydi0+ZmxhZ3MgJiBGRElTS19EQkxf U0lERVMpICE9IDApIHsNCisgICAgICAgICAgICAgICAgY3VyX2Rydi0+aGVhZCA9IDE7DQor ICAgICAgICAgICAgfSBlbHNlIHsNCisgICAgICAgICAgICAgICAgY3VyX2Rydi0+aGVhZCA9 IDA7DQorICAgICAgICAgICAgICAgIGN1cl9kcnYtPnRyYWNrKys7DQorICAgICAgICAgICAg ICAgIGlmICgoY3VyX2Rydi0+ZmxhZ3MgJiBGRElTS19EQkxfU0lERVMpID09IDApDQorICAg ICAgICAgICAgICAgICAgICByZXR1cm4gMDsNCisgICAgICAgICAgICB9DQorICAgICAgICB9 IGVsc2Ugew0KKyAgICAgICAgICAgIGN1cl9kcnYtPnRyYWNrKys7DQorICAgICAgICAgICAg cmV0dXJuIDE7DQorICAgICAgICB9DQorICAgICAgICBGTE9QUFlfRFBSSU5URigic2VlayB0 byBuZXh0IHRyYWNrICglZCAlMDJ4ICUwMnggPT4gJWQpXG4iLA0KKyAgICAgICAgICAgICAg ICAgICAgICAgY3VyX2Rydi0+aGVhZCwgY3VyX2Rydi0+dHJhY2ssDQorICAgICAgICAgICAg ICAgICAgICAgICBjdXJfZHJ2LT5zZWN0LCBmZF9zZWN0b3IoY3VyX2RydikpOw0KKyAgICB9 IGVsc2Ugew0KKyAgICAgICAgY3VyX2Rydi0+c2VjdCsrOw0KKyAgICB9DQorICAgIHJldHVy biAxOw0KK30NCisNCiAvKiBDYWxsYmFjayBmb3IgdHJhbnNmZXIgZW5kIChzdG9wIG9yIGFi b3J0KSAqLw0KIHN0YXRpYyB2b2lkIGZkY3RybF9zdG9wX3RyYW5zZmVyIChmZGN0cmxfdCAq ZmRjdHJsLCB1aW50OF90IHN0YXR1czAsDQogICAgICAgICAgICAgICAgICAgICAgICAgICAg ICAgICAgIHVpbnQ4X3Qgc3RhdHVzMSwgdWludDhfdCBzdGF0dXMyKQ0KQEAgLTExOTYsMzUg KzEyMzAsOCBAQA0KICAgICAgICAgcmVsX3BvcyA9IGZkY3RybC0+ZGF0YV9wb3MgJSBGRF9T RUNUT1JfTEVOOw0KICAgICAgICAgaWYgKHJlbF9wb3MgPT0gMCkgew0KICAgICAgICAgICAg IC8qIFNlZWsgdG8gbmV4dCBzZWN0b3IgKi8NCi0gICAgICAgICAgICBGTE9QUFlfRFBSSU5U Rigic2VlayB0byBuZXh0IHNlY3RvciAoJWQgJTAyeCAlMDJ4ID0+ICVkKSAoJWQpXG4iLA0K LSAgICAgICAgICAgICAgICAgICAgICAgICAgIGN1cl9kcnYtPmhlYWQsIGN1cl9kcnYtPnRy YWNrLCBjdXJfZHJ2LT5zZWN0LA0KLSAgICAgICAgICAgICAgICAgICAgICAgICAgIGZkX3Nl Y3RvcihjdXJfZHJ2KSwNCi0gICAgICAgICAgICAgICAgICAgICAgICAgICBmZGN0cmwtPmRh dGFfcG9zIC0gbGVuKTsNCi0gICAgICAgICAgICAvKiBYWFg6IGN1cl9kcnYtPnNlY3QgPj0g Y3VyX2Rydi0+bGFzdF9zZWN0IHNob3VsZCBiZSBhbg0KLSAgICAgICAgICAgICAgIGVycm9y IGluIGZhY3QgKi8NCi0gICAgICAgICAgICBpZiAoY3VyX2Rydi0+c2VjdCA+PSBjdXJfZHJ2 LT5sYXN0X3NlY3QgfHwNCi0gICAgICAgICAgICAgICAgY3VyX2Rydi0+c2VjdCA9PSBmZGN0 cmwtPmVvdCkgew0KLSAgICAgICAgICAgICAgICBjdXJfZHJ2LT5zZWN0ID0gMTsNCi0gICAg ICAgICAgICAgICAgaWYgKEZEX01VTFRJX1RSQUNLKGZkY3RybC0+ZGF0YV9zdGF0ZSkpIHsN Ci0gICAgICAgICAgICAgICAgICAgIGlmIChjdXJfZHJ2LT5oZWFkID09IDAgJiYNCi0gICAg ICAgICAgICAgICAgICAgICAgICAoY3VyX2Rydi0+ZmxhZ3MgJiBGRElTS19EQkxfU0lERVMp ICE9IDApIHsNCi0gICAgICAgICAgICAgICAgICAgICAgICBjdXJfZHJ2LT5oZWFkID0gMTsN Ci0gICAgICAgICAgICAgICAgICAgIH0gZWxzZSB7DQotICAgICAgICAgICAgICAgICAgICAg ICAgY3VyX2Rydi0+aGVhZCA9IDA7DQotICAgICAgICAgICAgICAgICAgICAgICAgY3VyX2Ry di0+dHJhY2srKzsNCi0gICAgICAgICAgICAgICAgICAgICAgICBpZiAoKGN1cl9kcnYtPmZs YWdzICYgRkRJU0tfREJMX1NJREVTKSA9PSAwKQ0KLSAgICAgICAgICAgICAgICAgICAgICAg ICAgICBicmVhazsNCi0gICAgICAgICAgICAgICAgICAgIH0NCi0gICAgICAgICAgICAgICAg fSBlbHNlIHsNCi0gICAgICAgICAgICAgICAgICAgIGN1cl9kcnYtPnRyYWNrKys7DQotICAg ICAgICAgICAgICAgICAgICBicmVhazsNCi0gICAgICAgICAgICAgICAgfQ0KLSAgICAgICAg ICAgICAgICBGTE9QUFlfRFBSSU5URigic2VlayB0byBuZXh0IHRyYWNrICglZCAlMDJ4ICUw MnggPT4gJWQpXG4iLA0KLSAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICBjdXJfZHJ2 LT5oZWFkLCBjdXJfZHJ2LT50cmFjaywNCi0gICAgICAgICAgICAgICAgICAgICAgICAgICAg ICAgY3VyX2Rydi0+c2VjdCwgZmRfc2VjdG9yKGN1cl9kcnYpKTsNCi0gICAgICAgICAgICB9 IGVsc2Ugew0KLSAgICAgICAgICAgICAgICBjdXJfZHJ2LT5zZWN0Kys7DQotICAgICAgICAg ICAgfQ0KKyAgICAgICAgICAgIGlmICghZmRjdHJsX3NlZWtfdG9fbmV4dF9zZWN0KGZkY3Ry bCwgY3VyX2RydikpDQ0KKyAgICAgICAgICAgICAgICBicmVhazsNCiAgICAgICAgIH0NCiAg ICAgfQ0KICBlbmRfdHJhbnNmZXI6DQpAQCAtMTI1MCw3ICsxMjU3LDcgQEANCiB7DQogICAg IGZkcml2ZV90ICpjdXJfZHJ2Ow0KICAgICB1aW50MzJfdCByZXR2YWwgPSAwOw0KLSAgICBp bnQgcG9zLCBsZW47DQorICAgIGludCBwb3M7DQogDQogICAgIGN1cl9kcnYgPSBnZXRfY3Vy X2RydihmZGN0cmwpOw0KICAgICBmZGN0cmwtPnN0YXRlICY9IH5GRF9DVFJMX1NMRUVQOw0K QEAgLTEyNjIsOSArMTI2OSwxMiBAQA0KICAgICBpZiAoRkRfU1RBVEUoZmRjdHJsLT5kYXRh X3N0YXRlKSA9PSBGRF9TVEFURV9EQVRBKSB7DQogICAgICAgICBwb3MgJT0gRkRfU0VDVE9S X0xFTjsNCiAgICAgICAgIGlmIChwb3MgPT0gMCkgew0KLSAgICAgICAgICAgIGxlbiA9IGZk Y3RybC0+ZGF0YV9sZW4gLSBmZGN0cmwtPmRhdGFfcG9zOw0KLSAgICAgICAgICAgIGlmIChs ZW4gPiBGRF9TRUNUT1JfTEVOKQ0KLSAgICAgICAgICAgICAgICBsZW4gPSBGRF9TRUNUT1Jf TEVOOw0KKyAgICAgICAgICAgIGlmIChmZGN0cmwtPmRhdGFfcG9zICE9IDApDQorICAgICAg ICAgICAgICAgIGlmICghZmRjdHJsX3NlZWtfdG9fbmV4dF9zZWN0KGZkY3RybCwgY3VyX2Ry dikpIHsNCisgICAgICAgICAgICAgICAgICAgIEZMT1BQWV9EUFJJTlRGKCJlcnJvciBzZWVr aW5nIHRvIG5leHQgc2VjdG9yICVkXG4iLA0KKyAgICAgICAgICAgICAgICAgICAgICAgICAg ICAgICAgICAgZmRfc2VjdG9yKGN1cl9kcnYpKTsNCisgICAgICAgICAgICAgICAgICAgIHJl dHVybiAwOw0KKyAgICAgICAgICAgICAgICB9DQogICAgICAgICAgICAgYmRydl9yZWFkKGN1 cl9kcnYtPmJzLCBmZF9zZWN0b3IoY3VyX2RydiksIGZkY3RybC0+ZmlmbywgMSk7DQogICAg ICAgICB9DQogICAgIH0NCkBAIC0xNzA3LDYgKzE3MTcsMTEgQEANCiAgICAgICAgIGlmIChm ZGN0cmwtPmRhdGFfcG9zICUgRkRfU0VDVE9SX0xFTiA9PSAoRkRfU0VDVE9SX0xFTiAtIDEp IHx8DQogICAgICAgICAgICAgZmRjdHJsLT5kYXRhX3BvcyA9PSBmZGN0cmwtPmRhdGFfbGVu KSB7DQogICAgICAgICAgICAgYmRydl93cml0ZShjdXJfZHJ2LT5icywgZmRfc2VjdG9yKGN1 cl9kcnYpLCBmZGN0cmwtPmZpZm8sIDEpOw0KKyAgICAgICAgICAgIGlmICghZmRjdHJsX3Nl ZWtfdG9fbmV4dF9zZWN0KGZkY3RybCwgY3VyX2RydikpIHsNCisgICAgICAgICAgICAgICAg RkxPUFBZX0RQUklOVEYoImVycm9yIHNlZWtpbmcgdG8gbmV4dCBzZWN0b3IgJWRcbiIsDQor ICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgIGZkX3NlY3RvcihjdXJfZHJ2KSk7DQor ICAgICAgICAgICAgICAgIHJldHVybjsNCisgICAgICAgICAgICB9DQogICAgICAgICB9DQog ICAgICAgICAvKiBTd2l0Y2ggZnJvbSB0cmFuc2ZlciBtb2RlIHRvIHN0YXR1cyBtb2RlDQog ICAgICAgICAgKiB0aGVuIGZyb20gc3RhdHVzIG1vZGUgdG8gY29tbWFuZCBtb2RlDQo= --------------080908020200070308000501 Content-Type: text/plain; name="fdc_03_status_ab.diff" Content-Transfer-Encoding: 7bit Content-Disposition: inline; filename="fdc_03_status_ab.diff" Index: hw/fdc.c =================================================================== --- hw/fdc.c (revision 4215) +++ hw/fdc.c (working copy) @@ -323,6 +323,7 @@ int dma_pos, int dma_len); static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status); +static uint32_t fdctrl_read_statusA (fdctrl_t *fdctrl); static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl); static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl); static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value); @@ -339,7 +340,6 @@ FD_CTRL_RESET = 0x02, FD_CTRL_SLEEP = 0x04, /* XXX: suppress that */ FD_CTRL_BUSY = 0x08, /* dma transfer in progress */ - FD_CTRL_INTR = 0x10, }; enum { @@ -361,8 +361,8 @@ }; enum { - FD_REG_0 = 0x00, - FD_REG_STATUSB = 0x01, + FD_REG_SRA = 0x00, + FD_REG_SRB = 0x01, FD_REG_DOR = 0x02, FD_REG_TDR = 0x03, FD_REG_MSR = 0x04, @@ -420,6 +420,26 @@ FD_SR0_RDYCHG = 0xc0, }; +enum { + FD_SRA_DIR = 0x01, + FD_SRA_nWP = 0x02, + FD_SRA_nINDX = 0x04, + FD_SRA_HDSEL = 0x08, + FD_SRA_nTRK0 = 0x10, + FD_SRA_STEP = 0x20, + FD_SRA_nDRV2 = 0x40, + FD_SRA_INTPEND = 0x80, +}; + +enum { + FD_SRB_MTR0 = 0x01, + FD_SRB_MTR1 = 0x02, + FD_SRB_WGATE = 0x04, + FD_SRB_RDATA = 0x08, + FD_SRB_WDATA = 0x10, + FD_SRB_DR0 = 0x20, +}; + enum { FD_DOR_SELMASK = 0x01, FD_DOR_nRESET = 0x04, @@ -472,6 +492,8 @@ target_phys_addr_t io_base; /* Controller state */ QEMUTimer *result_timer; + uint8_t sra; + uint8_t srb; uint8_t state; uint8_t dma_en; uint8_t cur_drv; @@ -506,15 +528,10 @@ uint32_t retval; switch (reg & 0x07) { - case FD_REG_0: - if (fdctrl->sun4m) { - // Identify to Linux as S82078B - retval = fdctrl_read_statusB(fdctrl); - } else { - retval = (uint32_t)(-1); - } + case FD_REG_SRA: + retval = fdctrl_read_statusA(fdctrl); break; - case FD_REG_STATUSB: + case FD_REG_SRB: retval = fdctrl_read_statusB(fdctrl); break; case FD_REG_DOR: @@ -617,6 +634,9 @@ { fdctrl_t *s = opaque; + /* Controller state */ + qemu_put_8s(f, &s->sra); + qemu_put_8s(f, &s->srb); qemu_put_8s(f, &s->state); qemu_put_8s(f, &s->dma_en); qemu_put_8s(f, &s->cur_drv); @@ -661,6 +681,9 @@ if (version_id != 1) return -EINVAL; + /* Controller state */ + qemu_get_8s(f, &s->sra); + qemu_get_8s(f, &s->srb); qemu_get_8s(f, &s->state); qemu_get_8s(f, &s->dma_en); qemu_get_8s(f, &s->cur_drv); @@ -712,9 +735,11 @@ /* Change IRQ state */ static void fdctrl_reset_irq (fdctrl_t *fdctrl) { + if (!(fdctrl->sra & FD_SRA_INTPEND)) + return; FLOPPY_DPRINTF("Reset interrupt\n"); qemu_set_irq(fdctrl->irq, 0); - fdctrl->state &= ~FD_CTRL_INTR; + fdctrl->sra &= ~FD_SRA_INTPEND; } static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status) @@ -725,9 +750,9 @@ fdctrl->int_status = status; return; } - if (~(fdctrl->state & FD_CTRL_INTR)) { + if (!(fdctrl->sra & FD_SRA_INTPEND)) { qemu_set_irq(fdctrl->irq, 1); - fdctrl->state |= FD_CTRL_INTR; + fdctrl->sra |= FD_SRA_INTPEND; } FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", status); fdctrl->int_status = status; @@ -741,6 +766,10 @@ FLOPPY_DPRINTF("reset controller\n"); fdctrl_reset_irq(fdctrl); /* Initialise controller */ + fdctrl->sra = 0; + fdctrl->srb = 0xc0; + if (!fdctrl->drives[1].bs) + fdctrl->sra |= FD_SRA_nDRV2; fdctrl->cur_drv = 0; /* FIFO state */ fdctrl->data_pos = 0; @@ -769,11 +798,24 @@ return fdctrl->cur_drv == 0 ? drv0(fdctrl) : drv1(fdctrl); } +/* Status A register : 0x00 (read-only) */ +static uint32_t fdctrl_read_statusA (fdctrl_t *fdctrl) +{ + uint32_t retval = fdctrl->sra; + + FLOPPY_DPRINTF("status register A: 0x%02x\n", retval); + + return retval; +} + /* Status B register : 0x01 (read-only) */ static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl) { - FLOPPY_DPRINTF("status register: 0x00\n"); - return 0; + uint32_t retval = fdctrl->srb; + + FLOPPY_DPRINTF("status register B: 0x%02x\n", retval); + + return retval; } /* Digital output register : 0x02 */ @@ -809,6 +851,23 @@ } } FLOPPY_DPRINTF("digital output register set to 0x%02x\n", value); + + /* Motors */ + if (value & FD_DOR_MOTEN0) + fdctrl->srb |= FD_SRB_MTR0; + else + fdctrl->srb &= ~FD_SRB_MTR0; + if (value & FD_DOR_MOTEN1) + fdctrl->srb |= FD_SRB_MTR1; + else + fdctrl->srb &= ~FD_SRB_MTR1; + + /* Drive */ + if (value & 1) + fdctrl->srb |= FD_SRB_DR0; + else + fdctrl->srb &= ~FD_SRB_DR0; + /* Drive motors state indicators */ if (value & FD_DOR_MOTEN1) fd_start(drv1(fdctrl)); --------------080908020200070308000501 Content-Type: text/plain; name="fdc_04_status_012.diff" Content-Transfer-Encoding: 7bit Content-Disposition: inline; filename="fdc_04_status_012.diff" Index: hw/fdc.c =================================================================== --- hw/fdc.c (revision 4215) +++ hw/fdc.c (working copy) @@ -321,7 +321,7 @@ static void fdctrl_reset_fifo (fdctrl_t *fdctrl); static int fdctrl_transfer_handler (void *opaque, int nchan, int dma_pos, int dma_len); -static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status); +static void fdctrl_raise_irq (fdctrl_t *fdctrl); static uint32_t fdctrl_read_statusA (fdctrl_t *fdctrl); static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl); @@ -356,7 +356,6 @@ FD_STATE_DATA = 0x02, FD_STATE_STATE = 0x03, FD_STATE_MULTI = 0x10, - FD_STATE_SEEK = 0x20, FD_STATE_FORMAT = 0x40, }; @@ -421,6 +420,15 @@ }; enum { + FD_SR1_EC = 0x80, /* End of cylinder */ +}; + +enum { + FD_SR2_SNS = 0x04, /* Scan not satisfied */ + FD_SR2_SEH = 0x08, /* Scan equal hit */ +}; + +enum { FD_SRA_DIR = 0x01, FD_SRA_nWP = 0x02, FD_SRA_nINDX = 0x04, @@ -479,7 +487,6 @@ #define FD_SET_STATE(state, new_state) \ do { (state) = ((state) & ~FD_STATE_STATE) | (new_state); } while (0) #define FD_MULTI_TRACK(state) ((state) & FD_STATE_MULTI) -#define FD_DID_SEEK(state) ((state) & FD_STATE_SEEK) #define FD_FORMAT_CMD(state) ((state) & FD_STATE_FORMAT) struct fdctrl_t { @@ -498,13 +505,15 @@ uint8_t dma_en; uint8_t cur_drv; uint8_t bootsel; + uint8_t status0; + uint8_t status1; + uint8_t status2; /* Command FIFO */ uint8_t *fifo; uint32_t data_pos; uint32_t data_len; uint8_t data_state; uint8_t data_dir; - uint8_t int_status; uint8_t eot; /* last wanted sector */ /* States kept only to be returned back */ /* Timers state */ @@ -641,13 +650,17 @@ qemu_put_8s(f, &s->dma_en); qemu_put_8s(f, &s->cur_drv); qemu_put_8s(f, &s->bootsel); + qemu_put_8s(f, &s->status0); + qemu_put_8s(f, &s->status1); + qemu_put_8s(f, &s->status2); + /* Command FIFO */ qemu_put_buffer(f, s->fifo, FD_SECTOR_LEN); qemu_put_be32s(f, &s->data_pos); qemu_put_be32s(f, &s->data_len); qemu_put_8s(f, &s->data_state); qemu_put_8s(f, &s->data_dir); - qemu_put_8s(f, &s->int_status); qemu_put_8s(f, &s->eot); + /* States kept only to be returned back */ qemu_put_8s(f, &s->timer0); qemu_put_8s(f, &s->timer1); qemu_put_8s(f, &s->precomp_trk); @@ -688,13 +701,17 @@ qemu_get_8s(f, &s->dma_en); qemu_get_8s(f, &s->cur_drv); qemu_get_8s(f, &s->bootsel); + qemu_get_8s(f, &s->status0); + qemu_get_8s(f, &s->status1); + qemu_get_8s(f, &s->status2); + /* Command FIFO */ qemu_get_buffer(f, s->fifo, FD_SECTOR_LEN); qemu_get_be32s(f, &s->data_pos); qemu_get_be32s(f, &s->data_len); qemu_get_8s(f, &s->data_state); qemu_get_8s(f, &s->data_dir); - qemu_get_8s(f, &s->int_status); qemu_get_8s(f, &s->eot); + /* States kept only to be returned back */ qemu_get_8s(f, &s->timer0); qemu_get_8s(f, &s->timer1); qemu_get_8s(f, &s->precomp_trk); @@ -742,20 +759,18 @@ fdctrl->sra &= ~FD_SRA_INTPEND; } -static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status) +static void fdctrl_raise_irq (fdctrl_t *fdctrl) { // Sparc mutation if (fdctrl->sun4m && !fdctrl->dma_en) { fdctrl->state &= ~FD_CTRL_BUSY; - fdctrl->int_status = status; return; } if (!(fdctrl->sra & FD_SRA_INTPEND)) { qemu_set_irq(fdctrl->irq, 1); fdctrl->sra |= FD_SRA_INTPEND; } - FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", status); - fdctrl->int_status = status; + FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", fdctrl->status0); } /* Reset controller */ @@ -779,8 +794,10 @@ for (i = 0; i < MAX_FD; i++) fd_reset(&fdctrl->drives[i]); fdctrl_reset_fifo(fdctrl); - if (do_irq) - fdctrl_raise_irq(fdctrl, FD_SR0_RDYCHG); + if (do_irq) { + fdctrl->status0 |= FD_SR0_RDYCHG; + fdctrl_raise_irq(fdctrl); + } } static inline fdrive_t *drv0 (fdctrl_t *fdctrl) @@ -1012,7 +1029,7 @@ fdctrl->data_pos = 0; FD_SET_STATE(fdctrl->data_state, FD_STATE_STATUS); if (do_irq) - fdctrl_raise_irq(fdctrl, 0x00); + fdctrl_raise_irq(fdctrl); } /* Set an error: unimplemented/unknown command */ @@ -1064,22 +1081,26 @@ } else { cur_drv->sect++; } + fdctrl->status0 |= FD_SR0_SEEK; return 1; } /* Callback for transfer end (stop or abort) */ -static void fdctrl_stop_transfer (fdctrl_t *fdctrl, uint8_t status0, - uint8_t status1, uint8_t status2) +static void fdctrl_stop_transfer (fdctrl_t *fdctrl) { fdrive_t *cur_drv; + uint8_t status0 = fdctrl->status0; cur_drv = get_cur_drv(fdctrl); + /* Add head and drive to status0 */ + status0 |= (cur_drv->head << 2) | (fdctrl->cur_drv); + FLOPPY_DPRINTF("transfer status: %02x %02x %02x (%02x)\n", - status0, status1, status2, - status0 | (cur_drv->head << 2) | fdctrl->cur_drv); - fdctrl->fifo[0] = status0 | (cur_drv->head << 2) | fdctrl->cur_drv; - fdctrl->fifo[1] = status1; - fdctrl->fifo[2] = status2; + fdctrl->status0, fdctrl->status1, fdctrl->status2, + status0); + fdctrl->fifo[0] = status0; + fdctrl->fifo[1] = fdctrl->status1; + fdctrl->fifo[2] = fdctrl->status2; fdctrl->fifo[3] = cur_drv->track; fdctrl->fifo[4] = cur_drv->head; fdctrl->fifo[5] = cur_drv->sect; @@ -1097,7 +1118,6 @@ { fdrive_t *cur_drv; uint8_t kh, kt, ks; - int did_seek; fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK; cur_drv = get_cur_drv(fdctrl); @@ -1107,31 +1127,35 @@ FLOPPY_DPRINTF("Start transfer at %d %d %02x %02x (%d)\n", fdctrl->cur_drv, kh, kt, ks, _fd_sector(kh, kt, ks, cur_drv->last_sect)); - did_seek = 0; - switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & 0x40)) { + switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & FD_CONFIG_EIS)) { case 2: /* sect too big */ - fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00); + fdctrl->status0 |= FD_SR0_ABNTERM; + fdctrl_stop_transfer(fdctrl); fdctrl->fifo[3] = kt; fdctrl->fifo[4] = kh; fdctrl->fifo[5] = ks; return; case 3: /* track too big */ - fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x80, 0x00); + fdctrl->status0 |= FD_SR0_ABNTERM; + fdctrl->status1 |= FD_SR1_EC; + fdctrl_stop_transfer(fdctrl); fdctrl->fifo[3] = kt; fdctrl->fifo[4] = kh; fdctrl->fifo[5] = ks; return; case 4: /* No seek enabled */ - fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00); + fdctrl->status0 |= FD_SR0_ABNTERM; + fdctrl_stop_transfer(fdctrl); fdctrl->fifo[3] = kt; fdctrl->fifo[4] = kh; fdctrl->fifo[5] = ks; return; + case 0: case 1: - did_seek = 1; + fdctrl->status0 |= FD_SR0_SEEK; break; default: break; @@ -1144,10 +1168,6 @@ fdctrl->data_state |= FD_STATE_MULTI; else fdctrl->data_state &= ~FD_STATE_MULTI; - if (did_seek) - fdctrl->data_state |= FD_STATE_SEEK; - else - fdctrl->data_state &= ~FD_STATE_SEEK; if (fdctrl->fifo[5] == 00) { fdctrl->data_len = fdctrl->fifo[8]; } else { @@ -1186,7 +1206,7 @@ } FLOPPY_DPRINTF("start non-DMA transfer\n"); /* IO based transfer: calculate len */ - fdctrl_raise_irq(fdctrl, 0x00); + fdctrl_raise_irq(fdctrl); return; } @@ -1194,10 +1214,13 @@ /* Prepare a transfer of deleted data */ static void fdctrl_start_transfer_del (fdctrl_t *fdctrl, int direction) { + FLOPPY_ERROR("fdctrl_start_transfer_del() unimplemented\n"); + /* We don't handle deleted data, * so we don't return *ANYTHING* */ - fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00); + fdctrl->status0 |= FD_SR0_ABNTERM; + fdctrl_stop_transfer(fdctrl); } /* handlers for DMA transfers */ @@ -1207,7 +1230,6 @@ fdctrl_t *fdctrl; fdrive_t *cur_drv; int len, start_pos, rel_pos; - uint8_t status0 = 0x00, status1 = 0x00, status2 = 0x00; fdctrl = opaque; if (!(fdctrl->state & FD_CTRL_BUSY)) { @@ -1215,16 +1237,11 @@ return 0; } cur_drv = get_cur_drv(fdctrl); - if (fdctrl->data_dir == FD_DIR_SCANE || fdctrl->data_dir == FD_DIR_SCANL || - fdctrl->data_dir == FD_DIR_SCANH) - status2 = 0x04; if (dma_len > fdctrl->data_len) dma_len = fdctrl->data_len; if (cur_drv->bs == NULL) { - if (fdctrl->data_dir == FD_DIR_WRITE) - fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00); - else - fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00); + fdctrl->status0 |= FD_SR0_ABNTERM; + fdctrl_stop_transfer(fdctrl); len = 0; goto transfer_error; } @@ -1261,8 +1278,9 @@ fdctrl->data_pos, len); if (bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1) < 0) { - FLOPPY_ERROR("writting sector %d\n", fd_sector(cur_drv)); - fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00); + FLOPPY_ERROR("writing sector %d\n", fd_sector(cur_drv)); + fdctrl->status0 |= FD_SR0_ABNTERM; + fdctrl_stop_transfer(fdctrl); goto transfer_error; } break; @@ -1274,14 +1292,16 @@ DMA_read_memory (nchan, tmpbuf, fdctrl->data_pos, len); ret = memcmp(tmpbuf, fdctrl->fifo + rel_pos, len); if (ret == 0) { - status2 = 0x08; + fdctrl->status2 |= FD_SR2_SEH; + fdctrl->status2 &= ~FD_SR2_SNS; goto end_transfer; } if ((ret < 0 && fdctrl->data_dir == FD_DIR_SCANL) || (ret > 0 && fdctrl->data_dir == FD_DIR_SCANH)) { - status2 = 0x00; + fdctrl->status2 &= ~FD_SR2_SNS; goto end_transfer; } + fdctrl->status2 |= FD_SR2_SNS; } break; } @@ -1297,15 +1317,8 @@ len = fdctrl->data_pos - start_pos; FLOPPY_DPRINTF("end transfer %d %d %d\n", fdctrl->data_pos, len, fdctrl->data_len); - if (fdctrl->data_dir == FD_DIR_SCANE || - fdctrl->data_dir == FD_DIR_SCANL || - fdctrl->data_dir == FD_DIR_SCANH) - status2 = 0x08; - if (FD_DID_SEEK(fdctrl->data_state)) - status0 |= FD_SR0_SEEK; fdctrl->data_len -= len; - // if (fdctrl->data_len == 0) - fdctrl_stop_transfer(fdctrl, status0, status1, status2); + fdctrl_stop_transfer(fdctrl); transfer_error: return len; @@ -1344,7 +1357,7 @@ * then from status mode to command mode */ if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) { - fdctrl_stop_transfer(fdctrl, FD_SR0_SEEK, 0x00, 0x00); + fdctrl_stop_transfer(fdctrl); } else { fdctrl_reset_fifo(fdctrl); fdctrl_reset_irq(fdctrl); @@ -1359,7 +1372,6 @@ { fdrive_t *cur_drv; uint8_t kh, kt, ks; - int did_seek; fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK; cur_drv = get_cur_drv(fdctrl); @@ -1369,32 +1381,35 @@ FLOPPY_DPRINTF("format sector at %d %d %02x %02x (%d)\n", fdctrl->cur_drv, kh, kt, ks, _fd_sector(kh, kt, ks, cur_drv->last_sect)); - did_seek = 0; switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & FD_CONFIG_EIS)) { case 2: /* sect too big */ - fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00); + fdctrl->status0 |= FD_SR0_ABNTERM; + fdctrl_stop_transfer(fdctrl); fdctrl->fifo[3] = kt; fdctrl->fifo[4] = kh; fdctrl->fifo[5] = ks; return; case 3: /* track too big */ - fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x80, 0x00); + fdctrl->status0 |= FD_SR0_ABNTERM; + fdctrl->status1 |= FD_SR1_EC; + fdctrl_stop_transfer(fdctrl); fdctrl->fifo[3] = kt; fdctrl->fifo[4] = kh; fdctrl->fifo[5] = ks; return; case 4: /* No seek enabled */ - fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00); + fdctrl->status0 |= FD_SR0_ABNTERM; + fdctrl_stop_transfer(fdctrl); fdctrl->fifo[3] = kt; fdctrl->fifo[4] = kh; fdctrl->fifo[5] = ks; return; + case 0: case 1: - did_seek = 1; - fdctrl->data_state |= FD_STATE_SEEK; + fdctrl->status0 |= FD_SR0_SEEK; break; default: break; @@ -1403,15 +1418,13 @@ if (cur_drv->bs == NULL || bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1) < 0) { FLOPPY_ERROR("formatting sector %d\n", fd_sector(cur_drv)); - fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00); + fdctrl->status0 |= FD_SR0_ABNTERM; + fdctrl_stop_transfer(fdctrl); } else { if (cur_drv->sect == cur_drv->last_sect) { fdctrl->data_state &= ~FD_STATE_FORMAT; /* Last sector done */ - if (FD_DID_SEEK(fdctrl->data_state)) - fdctrl_stop_transfer(fdctrl, FD_SR0_SEEK, 0x00, 0x00); - else - fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00); + fdctrl_stop_transfer(fdctrl); } else { /* More to do */ fdctrl->data_pos = 0; @@ -1525,7 +1538,6 @@ fdctrl->data_state |= FD_STATE_MULTI; else fdctrl->data_state &= ~FD_STATE_MULTI; - fdctrl->data_state &= ~FD_STATE_SEEK; cur_drv->bps = fdctrl->fifo[2] > 7 ? 16384 : 128 << fdctrl->fifo[2]; #if 0 @@ -1540,7 +1552,7 @@ * the sector with the specified fill byte */ fdctrl->data_state &= ~FD_STATE_FORMAT; - fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00); + fdctrl_stop_transfer(fdctrl); } static void fdctrl_handle_specify (fdctrl_t *fdctrl, int direction) @@ -1577,27 +1589,20 @@ fd_recalibrate(cur_drv); fdctrl_reset_fifo(fdctrl); /* Raise Interrupt */ - fdctrl_raise_irq(fdctrl, FD_SR0_SEEK); + fdctrl->status0 |= FD_SR0_SEEK; + fdctrl_raise_irq(fdctrl); } static void fdctrl_handle_sense_interrupt_status (fdctrl_t *fdctrl, int direction) { fdrive_t *cur_drv = get_cur_drv(fdctrl); -#if 0 - fdctrl->fifo[0] = - fdctrl->int_status | (cur_drv->head << 2) | fdctrl->cur_drv; -#else - /* XXX: int_status handling is broken for read/write - commands, so we do this hack. It should be suppressed - ASAP */ fdctrl->fifo[0] = - 0x20 | (cur_drv->head << 2) | fdctrl->cur_drv; -#endif + fdctrl->status0 | (cur_drv->head << 2) | fdctrl->cur_drv; fdctrl->fifo[1] = cur_drv->track; fdctrl_set_fifo(fdctrl, 2, 0); fdctrl_reset_irq(fdctrl); - fdctrl->int_status = FD_SR0_RDYCHG; + fdctrl->status0 |= FD_SR0_RDYCHG; } static void fdctrl_handle_seek (fdctrl_t *fdctrl, int direction) @@ -1612,13 +1617,13 @@ else cur_drv->dir = 0; fdctrl_reset_fifo(fdctrl); - if (fdctrl->fifo[2] > cur_drv->max_track) { - fdctrl_raise_irq(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK); - } else { + fdctrl->status0 |= FD_SR0_SEEK; + if (fdctrl->fifo[2] > cur_drv->max_track) + fdctrl->status0 |= FD_SR0_ABNTERM; + else cur_drv->track = fdctrl->fifo[2]; - /* Raise Interrupt */ - fdctrl_raise_irq(fdctrl, FD_SR0_SEEK); - } + /* Raise Interrupt */ + fdctrl_raise_irq(fdctrl); } static void fdctrl_handle_perpendicular_mode (fdctrl_t *fdctrl, int direction) @@ -1688,7 +1693,8 @@ cur_drv->track += fdctrl->fifo[2]; } fdctrl_reset_fifo(fdctrl); - fdctrl_raise_irq(fdctrl, FD_SR0_SEEK); + fdctrl->status0 |= FD_SR0_SEEK; + fdctrl_raise_irq(fdctrl); } static void fdctrl_handle_relative_seek_in (fdctrl_t *fdctrl, int direction) @@ -1706,7 +1712,8 @@ } fdctrl_reset_fifo(fdctrl); /* Raise Interrupt */ - fdctrl_raise_irq(fdctrl, FD_SR0_SEEK); + fdctrl->status0 |= FD_SR0_SEEK; + fdctrl_raise_irq(fdctrl); } static const struct { @@ -1786,7 +1793,7 @@ * then from status mode to command mode */ if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) - fdctrl_stop_transfer(fdctrl, FD_SR0_SEEK, 0x00, 0x00); + fdctrl_stop_transfer(fdctrl); return; } if (fdctrl->data_pos == 0) { @@ -1807,6 +1814,15 @@ return; } + if (fdctrl->fifo[0] != FD_CMD_SENSE_INTERRUPT_STATUS) + { + /* Reset status0, except for SENSE_INTERRUPT_STATUS + * which returns it */ + fdctrl->status0 = 0; + } + fdctrl->status1 = 0; + fdctrl->status2 = 0; + pos = command_to_handler[fdctrl->fifo[0] & 0xff]; FLOPPY_DPRINTF("treat %s command\n", handlers[pos].name); (*handlers[pos].handler)(fdctrl, handlers[pos].parameter); @@ -1825,7 +1841,7 @@ if (cur_drv->last_sect != 0) { cur_drv->sect = (cur_drv->sect % cur_drv->last_sect) + 1; } - fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00); + fdctrl_stop_transfer(fdctrl); } /* Init functions */ --------------080908020200070308000501 Content-Type: text/plain; name="fdc_05_dmaen.diff" Content-Transfer-Encoding: 7bit Content-Disposition: inline; filename="fdc_05_dmaen.diff" Index: hw/fdc.c =================================================================== --- hw/fdc.c (revision 4215) +++ hw/fdc.c (working copy) @@ -22,10 +22,7 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ -/* - * The controller is used in Sun4m systems in a slightly different - * way. There are changes in DOR register and DMA is not available. - */ + #include "hw.h" #include "fdc.h" #include "block.h" @@ -501,8 +498,9 @@ QEMUTimer *result_timer; uint8_t sra; uint8_t srb; + uint8_t dor; + uint8_t msr; uint8_t state; - uint8_t dma_en; uint8_t cur_drv; uint8_t bootsel; uint8_t status0; @@ -525,8 +523,6 @@ uint8_t lock; /* Power down config (also with status regB access mode */ uint8_t pwrd; - /* Sun4m quirks? */ - int sun4m; /* Floppy drives */ fdrive_t drives[2]; }; @@ -646,8 +642,9 @@ /* Controller state */ qemu_put_8s(f, &s->sra); qemu_put_8s(f, &s->srb); + qemu_put_8s(f, &s->dor); + qemu_put_8s(f, &s->msr); qemu_put_8s(f, &s->state); - qemu_put_8s(f, &s->dma_en); qemu_put_8s(f, &s->cur_drv); qemu_put_8s(f, &s->bootsel); qemu_put_8s(f, &s->status0); @@ -697,8 +694,9 @@ /* Controller state */ qemu_get_8s(f, &s->sra); qemu_get_8s(f, &s->srb); + qemu_get_8s(f, &s->dor); + qemu_get_8s(f, &s->msr); qemu_get_8s(f, &s->state); - qemu_get_8s(f, &s->dma_en); qemu_get_8s(f, &s->cur_drv); qemu_get_8s(f, &s->bootsel); qemu_get_8s(f, &s->status0); @@ -761,11 +759,6 @@ static void fdctrl_raise_irq (fdctrl_t *fdctrl) { - // Sparc mutation - if (fdctrl->sun4m && !fdctrl->dma_en) { - fdctrl->state &= ~FD_CTRL_BUSY; - return; - } if (!(fdctrl->sra & FD_SRA_INTPEND)) { qemu_set_irq(fdctrl->irq, 1); fdctrl->sra |= FD_SRA_INTPEND; @@ -786,6 +779,8 @@ if (!fdctrl->drives[1].bs) fdctrl->sra |= FD_SRA_nDRV2; fdctrl->cur_drv = 0; + fdctrl->dor = 0; + fdctrl->dor |= (fdctrl->dma_chann != -1) ? FD_DOR_DMAEN : 0; /* FIFO state */ fdctrl->data_pos = 0; fdctrl->data_len = 0; @@ -846,7 +841,7 @@ if (drv1(fdctrl)->drflags & FDRIVE_MOTOR_ON) retval |= FD_DOR_MOTEN1; /* DMA enable */ - if (fdctrl->dma_en) + if (fdctrl->dor & FD_DOR_DMAEN) retval |= FD_DOR_DMAEN; /* Reset indicator */ if (!(fdctrl->state & FD_CTRL_RESET)) @@ -894,11 +889,6 @@ fd_start(drv0(fdctrl)); else fd_stop(drv0(fdctrl)); - /* DMA enable */ -#if 0 - if (fdctrl->dma_chann != -1) - fdctrl->dma_en = value & FD_DOR_DMAEN ? 1 : 0; -#endif /* Reset */ if (!(value & FD_DOR_nRESET)) { if (!(fdctrl->state & FD_CTRL_RESET)) { @@ -914,6 +904,8 @@ } /* Selected drive */ fdctrl->cur_drv = value & FD_DOR_SELMASK; + + fdctrl->dor = value; } /* Tape drive register : 0x03 */ @@ -954,8 +946,10 @@ /* Data transfer direction indicator */ if (fdctrl->data_dir == FD_DIR_READ) retval |= FD_MSR_DIO; - } - /* Should handle FD_MSR_NONDMA for SPECIFY command */ + } + /* Non DMA indicator */ + if (fdctrl->msr & FD_MSR_NONDMA) + retval |= FD_MSR_NONDMA; /* Command busy indicator */ if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA || FD_STATE(fdctrl->data_state) == FD_STATE_STATUS) @@ -1106,10 +1100,11 @@ fdctrl->fifo[5] = cur_drv->sect; fdctrl->fifo[6] = FD_SECTOR_SC; fdctrl->data_dir = FD_DIR_READ; - if (fdctrl->state & FD_CTRL_BUSY) { + if (!(fdctrl->msr & FD_MSR_NONDMA)) { DMA_release_DREQ(fdctrl->dma_chann); fdctrl->state &= ~FD_CTRL_BUSY; } + fdctrl->msr &= ~FD_MSR_NONDMA; fdctrl_set_fifo(fdctrl, 7, 1); } @@ -1179,7 +1174,7 @@ fdctrl->data_len *= tmp; } fdctrl->eot = fdctrl->fifo[6]; - if (fdctrl->dma_en) { + if (fdctrl->dor & FD_DOR_DMAEN) { int dma_mode; /* DMA transfer are enabled. Check if DMA channel is well programmed */ dma_mode = DMA_get_channel_mode(fdctrl->dma_chann); @@ -1205,6 +1200,7 @@ } } FLOPPY_DPRINTF("start non-DMA transfer\n"); + fdctrl->msr |= FD_MSR_NONDMA; /* IO based transfer: calculate len */ fdctrl_raise_irq(fdctrl); @@ -1338,7 +1334,7 @@ return 0; } pos = fdctrl->data_pos; - if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) { + if (fdctrl->msr & FD_MSR_NONDMA) { pos %= FD_SECTOR_LEN; if (pos == 0) { if (fdctrl->data_pos != 0) @@ -1356,7 +1352,7 @@ /* Switch from transfer mode to status mode * then from status mode to command mode */ - if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) { + if (fdctrl->msr & FD_MSR_NONDMA) { fdctrl_stop_transfer(fdctrl); } else { fdctrl_reset_fifo(fdctrl); @@ -1451,7 +1447,7 @@ fdctrl->fifo[3] = 0; /* timers */ fdctrl->fifo[4] = fdctrl->timer0; - fdctrl->fifo[5] = (fdctrl->timer1 << 1) | fdctrl->dma_en; + fdctrl->fifo[5] = (fdctrl->timer1 << 1) | (fdctrl->dor & FD_DOR_DMAEN ? 1 : 0); fdctrl->fifo[6] = cur_drv->last_sect; fdctrl->fifo[7] = (fdctrl->lock << 7) | (cur_drv->perpendicular << 2); @@ -1559,7 +1555,10 @@ { fdctrl->timer0 = (fdctrl->fifo[1] >> 4) & 0xF; fdctrl->timer1 = fdctrl->fifo[2] >> 1; - fdctrl->dma_en = 1 - (fdctrl->fifo[2] & 1) ; + if (fdctrl->fifo[2] & 1) + fdctrl->dor &= ~FD_DOR_DMAEN; + else + fdctrl->dor |= FD_DOR_DMAEN; /* No result back */ fdctrl_reset_fifo(fdctrl); } @@ -1633,7 +1632,7 @@ if (fdctrl->fifo[1] & 0x80) cur_drv->perpendicular = fdctrl->fifo[1] & 0x7; /* No result back */ - fdctrl_reset_fifo(fdctrl); + fdctrl_reset_fifo(fdctrl); } static void fdctrl_handle_configure (fdctrl_t *fdctrl, int direction) @@ -1777,7 +1776,7 @@ return; } /* Is it write command time ? */ - if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) { + if (fdctrl->msr & FD_MSR_NONDMA) { /* FIFO data write */ fdctrl->fifo[fdctrl->data_pos++] = value; if (fdctrl->data_pos % FD_SECTOR_LEN == (FD_SECTOR_LEN - 1) || @@ -1878,10 +1877,7 @@ fdctrl->io_base = io_base; fdctrl->config = FD_CONFIG_EIS | FD_CONFIG_EFIFO; /* Implicit seek, polling & FIFO enabled */ if (fdctrl->dma_chann != -1) { - fdctrl->dma_en = 1; DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl); - } else { - fdctrl->dma_en = 0; } for (i = 0; i < MAX_FD; i++) { fd_init(&fdctrl->drives[i], fds[i]); @@ -1906,7 +1902,6 @@ fdctrl = fdctrl_init_common(irq, dma_chann, io_base, fds); - fdctrl->sun4m = 0; if (mem_mapped) { io_mem = cpu_register_io_memory(0, fdctrl_mem_read, fdctrl_mem_write, fdctrl); @@ -1931,8 +1926,7 @@ fdctrl_t *fdctrl; int io_mem; - fdctrl = fdctrl_init_common(irq, 0, io_base, fds); - fdctrl->sun4m = 1; + fdctrl = fdctrl_init_common(irq, -1, io_base, fds); io_mem = cpu_register_io_memory(0, fdctrl_mem_read_strict, fdctrl_mem_write_strict, fdctrl); --------------080908020200070308000501 Content-Type: text/plain; name="fdc_06_dor_reg.diff" Content-Transfer-Encoding: 7bit Content-Disposition: inline; filename="fdc_06_dor_reg.diff" Index: hw/fdc.c =================================================================== --- hw/fdc.c (revision 4215) +++ hw/fdc.c (working copy) @@ -66,10 +66,6 @@ FDRIVE_DRV_NONE = 0x03, /* No drive connected */ } fdrive_type_t; -typedef enum fdrive_flags_t { - FDRIVE_MOTOR_ON = 0x01, /* motor on/off */ -} fdrive_flags_t; - typedef enum fdisk_flags_t { FDISK_DBL_SIDES = 0x01, } fdisk_flags_t; @@ -78,7 +74,6 @@ BlockDriverState *bs; /* Drive status */ fdrive_type_t drive; - fdrive_flags_t drflags; uint8_t perpendicular; /* 2.88 MB access mode */ /* Position */ uint8_t head; @@ -100,7 +95,6 @@ /* Drive */ drv->bs = bs; drv->drive = FDRIVE_DRV_NONE; - drv->drflags = 0; drv->perpendicular = 0; /* Disk */ drv->last_sect = 0; @@ -293,24 +287,6 @@ } } -/* Motor control */ -static void fd_start (fdrive_t *drv) -{ - drv->drflags |= FDRIVE_MOTOR_ON; -} - -static void fd_stop (fdrive_t *drv) -{ - drv->drflags &= ~FDRIVE_MOTOR_ON; -} - -/* Re-initialise a drives (motor off, repositioned) */ -static void fd_reset (fdrive_t *drv) -{ - fd_stop(drv); - fd_recalibrate(drv); -} - /********************************************************/ /* Intel 82078 floppy disk controller emulation */ @@ -334,7 +310,6 @@ enum { FD_CTRL_ACTIVE = 0x01, /* XXX: suppress that */ - FD_CTRL_RESET = 0x02, FD_CTRL_SLEEP = 0x04, /* XXX: suppress that */ FD_CTRL_BUSY = 0x08, /* dma transfer in progress */ }; @@ -624,10 +599,6 @@ static void fd_save (QEMUFile *f, fdrive_t *fd) { - uint8_t tmp; - - tmp = fd->drflags; - qemu_put_8s(f, &tmp); qemu_put_8s(f, &fd->head); qemu_put_8s(f, &fd->track); qemu_put_8s(f, &fd->sect); @@ -670,10 +641,6 @@ static int fd_load (QEMUFile *f, fdrive_t *fd) { - uint8_t tmp; - - qemu_get_8s(f, &tmp); - fd->drflags = tmp; qemu_get_8s(f, &fd->head); qemu_get_8s(f, &fd->track); qemu_get_8s(f, &fd->sect); @@ -779,7 +746,7 @@ if (!fdctrl->drives[1].bs) fdctrl->sra |= FD_SRA_nDRV2; fdctrl->cur_drv = 0; - fdctrl->dor = 0; + fdctrl->dor = FD_DOR_nRESET; fdctrl->dor |= (fdctrl->dma_chann != -1) ? FD_DOR_DMAEN : 0; /* FIFO state */ fdctrl->data_pos = 0; @@ -787,7 +754,7 @@ fdctrl->data_state = FD_STATE_CMD; fdctrl->data_dir = FD_DIR_WRITE; for (i = 0; i < MAX_FD; i++) - fd_reset(&fdctrl->drives[i]); + fd_recalibrate(&fdctrl->drives[i]); fdctrl_reset_fifo(fdctrl); if (do_irq) { fdctrl->status0 |= FD_SR0_RDYCHG; @@ -833,19 +800,8 @@ /* Digital output register : 0x02 */ static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl) { - uint32_t retval = 0; + uint32_t retval = fdctrl->dor; - /* Drive motors state indicators */ - if (drv0(fdctrl)->drflags & FDRIVE_MOTOR_ON) - retval |= FD_DOR_MOTEN0; - if (drv1(fdctrl)->drflags & FDRIVE_MOTOR_ON) - retval |= FD_DOR_MOTEN1; - /* DMA enable */ - if (fdctrl->dor & FD_DOR_DMAEN) - retval |= FD_DOR_DMAEN; - /* Reset indicator */ - if (!(fdctrl->state & FD_CTRL_RESET)) - retval |= FD_DOR_nRESET; /* Selected drive */ retval |= fdctrl->cur_drv; FLOPPY_DPRINTF("digital output register: 0x%02x\n", retval); @@ -855,13 +811,6 @@ static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value) { - /* Reset mode */ - if (fdctrl->state & FD_CTRL_RESET) { - if (!(value & FD_DOR_nRESET)) { - FLOPPY_DPRINTF("Floppy controller in RESET state !\n"); - return; - } - } FLOPPY_DPRINTF("digital output register set to 0x%02x\n", value); /* Motors */ @@ -880,26 +829,16 @@ else fdctrl->srb &= ~FD_SRB_DR0; - /* Drive motors state indicators */ - if (value & FD_DOR_MOTEN1) - fd_start(drv1(fdctrl)); - else - fd_stop(drv1(fdctrl)); - if (value & FD_DOR_MOTEN0) - fd_start(drv0(fdctrl)); - else - fd_stop(drv0(fdctrl)); /* Reset */ if (!(value & FD_DOR_nRESET)) { - if (!(fdctrl->state & FD_CTRL_RESET)) { + if (fdctrl->dor & FD_DOR_nRESET) { FLOPPY_DPRINTF("controller enter RESET state\n"); - fdctrl->state |= FD_CTRL_RESET; } } else { - if (fdctrl->state & FD_CTRL_RESET) { + if (!(fdctrl->dor & FD_DOR_nRESET)) { FLOPPY_DPRINTF("controller out of RESET state\n"); fdctrl_reset(fdctrl, 1); - fdctrl->state &= ~(FD_CTRL_RESET | FD_CTRL_SLEEP); + fdctrl->state &= ~FD_CTRL_SLEEP; } } /* Selected drive */ @@ -924,7 +863,7 @@ static void fdctrl_write_tape (fdctrl_t *fdctrl, uint32_t value) { /* Reset mode */ - if (fdctrl->state & FD_CTRL_RESET) { + if (!(fdctrl->dor & FD_DOR_nRESET)) { FLOPPY_DPRINTF("Floppy controller in RESET state !\n"); return; } @@ -939,7 +878,8 @@ { uint32_t retval = 0; - fdctrl->state &= ~(FD_CTRL_SLEEP | FD_CTRL_RESET); + fdctrl->dor |= FD_DOR_nRESET; + fdctrl->state &= ~FD_CTRL_SLEEP; if (!(fdctrl->state & FD_CTRL_BUSY)) { /* Data transfer allowed */ retval |= FD_MSR_RQM; @@ -963,16 +903,16 @@ static void fdctrl_write_rate (fdctrl_t *fdctrl, uint32_t value) { /* Reset mode */ - if (fdctrl->state & FD_CTRL_RESET) { + if (!(fdctrl->dor & FD_DOR_nRESET)) { FLOPPY_DPRINTF("Floppy controller in RESET state !\n"); return; } FLOPPY_DPRINTF("select rate register set to 0x%02x\n", value); /* Reset: autoclear */ if (value & FD_DSR_SWRESET) { - fdctrl->state |= FD_CTRL_RESET; + fdctrl->dor &= ~FD_DOR_nRESET; fdctrl_reset(fdctrl, 1); - fdctrl->state &= ~FD_CTRL_RESET; + fdctrl->dor |= FD_DOR_nRESET; } if (value & FD_DSR_PWRDOWN) { fdctrl->state |= FD_CTRL_SLEEP; @@ -1610,7 +1550,6 @@ fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK; cur_drv = get_cur_drv(fdctrl); - fd_start(cur_drv); if (fdctrl->fifo[2] <= cur_drv->track) cur_drv->dir = 1; else @@ -1684,7 +1623,6 @@ fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK; cur_drv = get_cur_drv(fdctrl); - fd_start(cur_drv); cur_drv->dir = 0; if (fdctrl->fifo[2] + cur_drv->track >= cur_drv->max_track) { cur_drv->track = cur_drv->max_track - 1; @@ -1702,7 +1640,6 @@ fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK; cur_drv = get_cur_drv(fdctrl); - fd_start(cur_drv); cur_drv->dir = 1; if (fdctrl->fifo[2] > cur_drv->track) { cur_drv->track = 0; @@ -1766,7 +1703,7 @@ cur_drv = get_cur_drv(fdctrl); /* Reset mode */ - if (fdctrl->state & FD_CTRL_RESET) { + if (!(fdctrl->dor & FD_DOR_nRESET)) { FLOPPY_DPRINTF("Floppy controller in RESET state !\n"); return; } --------------080908020200070308000501 Content-Type: text/plain; name="fdc_07_msr_reg.diff" Content-Transfer-Encoding: 7bit Content-Disposition: inline; filename="fdc_07_msr_reg.diff" Index: hw/fdc.c =================================================================== --- hw/fdc.c (revision 4215) +++ hw/fdc.c (working copy) @@ -309,12 +309,6 @@ static uint32_t fdctrl_read_dir (fdctrl_t *fdctrl); enum { - FD_CTRL_ACTIVE = 0x01, /* XXX: suppress that */ - FD_CTRL_SLEEP = 0x04, /* XXX: suppress that */ - FD_CTRL_BUSY = 0x08, /* dma transfer in progress */ -}; - -enum { FD_DIR_WRITE = 0, FD_DIR_READ = 1, FD_DIR_SCANE = 2, @@ -323,12 +317,8 @@ }; enum { - FD_STATE_CMD = 0x00, - FD_STATE_STATUS = 0x01, - FD_STATE_DATA = 0x02, - FD_STATE_STATE = 0x03, - FD_STATE_MULTI = 0x10, - FD_STATE_FORMAT = 0x40, + FD_STATE_MULTI = 0x01, /* multi track flag */ + FD_STATE_FORMAT = 0x02, /* format flag */ }; enum { @@ -455,9 +445,6 @@ FD_DIR_DSKCHG = 0x80, }; -#define FD_STATE(state) ((state) & FD_STATE_STATE) -#define FD_SET_STATE(state, new_state) \ -do { (state) = ((state) & ~FD_STATE_STATE) | (new_state); } while (0) #define FD_MULTI_TRACK(state) ((state) & FD_STATE_MULTI) #define FD_FORMAT_CMD(state) ((state) & FD_STATE_FORMAT) @@ -474,6 +461,7 @@ uint8_t sra; uint8_t srb; uint8_t dor; + uint8_t dsr; uint8_t msr; uint8_t state; uint8_t cur_drv; @@ -614,8 +602,8 @@ qemu_put_8s(f, &s->sra); qemu_put_8s(f, &s->srb); qemu_put_8s(f, &s->dor); + qemu_put_8s(f, &s->dsr); qemu_put_8s(f, &s->msr); - qemu_put_8s(f, &s->state); qemu_put_8s(f, &s->cur_drv); qemu_put_8s(f, &s->bootsel); qemu_put_8s(f, &s->status0); @@ -662,8 +650,8 @@ qemu_get_8s(f, &s->sra); qemu_get_8s(f, &s->srb); qemu_get_8s(f, &s->dor); + qemu_get_8s(f, &s->dsr); qemu_get_8s(f, &s->msr); - qemu_get_8s(f, &s->state); qemu_get_8s(f, &s->cur_drv); qemu_get_8s(f, &s->bootsel); qemu_get_8s(f, &s->status0); @@ -748,10 +736,11 @@ fdctrl->cur_drv = 0; fdctrl->dor = FD_DOR_nRESET; fdctrl->dor |= (fdctrl->dma_chann != -1) ? FD_DOR_DMAEN : 0; + fdctrl->msr = FD_MSR_RQM; /* FIFO state */ fdctrl->data_pos = 0; fdctrl->data_len = 0; - fdctrl->data_state = FD_STATE_CMD; + fdctrl->data_state = 0; fdctrl->data_dir = FD_DIR_WRITE; for (i = 0; i < MAX_FD; i++) fd_recalibrate(&fdctrl->drives[i]); @@ -838,7 +827,7 @@ if (!(fdctrl->dor & FD_DOR_nRESET)) { FLOPPY_DPRINTF("controller out of RESET state\n"); fdctrl_reset(fdctrl, 1); - fdctrl->state &= ~FD_CTRL_SLEEP; + fdctrl->dsr &= ~FD_DSR_PWRDOWN; } } /* Selected drive */ @@ -876,27 +865,12 @@ /* Main status register : 0x04 (read) */ static uint32_t fdctrl_read_main_status (fdctrl_t *fdctrl) { - uint32_t retval = 0; - + fdctrl->dsr &= ~FD_DSR_PWRDOWN; fdctrl->dor |= FD_DOR_nRESET; - fdctrl->state &= ~FD_CTRL_SLEEP; - if (!(fdctrl->state & FD_CTRL_BUSY)) { - /* Data transfer allowed */ - retval |= FD_MSR_RQM; - /* Data transfer direction indicator */ - if (fdctrl->data_dir == FD_DIR_READ) - retval |= FD_MSR_DIO; - } - /* Non DMA indicator */ - if (fdctrl->msr & FD_MSR_NONDMA) - retval |= FD_MSR_NONDMA; - /* Command busy indicator */ - if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA || - FD_STATE(fdctrl->data_state) == FD_STATE_STATUS) - retval |= FD_MSR_CMDBUSY; - FLOPPY_DPRINTF("main status register: 0x%02x\n", retval); - return retval; + FLOPPY_DPRINTF("main status register: 0x%02x\n", fdctrl->msr); + + return fdctrl->msr; } /* Data select rate register : 0x04 (write) */ @@ -915,9 +889,9 @@ fdctrl->dor |= FD_DOR_nRESET; } if (value & FD_DSR_PWRDOWN) { - fdctrl->state |= FD_CTRL_SLEEP; fdctrl_reset(fdctrl, 1); } + fdctrl->dsr = value; } static int fdctrl_media_changed(fdrive_t *drv) @@ -952,7 +926,7 @@ { fdctrl->data_dir = FD_DIR_WRITE; fdctrl->data_pos = 0; - FD_SET_STATE(fdctrl->data_state, FD_STATE_CMD); + fdctrl->msr &= ~(FD_MSR_CMDBUSY | FD_MSR_DIO); } /* Set FIFO status for the host to read */ @@ -961,7 +935,7 @@ fdctrl->data_dir = FD_DIR_READ; fdctrl->data_len = fifo_len; fdctrl->data_pos = 0; - FD_SET_STATE(fdctrl->data_state, FD_STATE_STATUS); + fdctrl->msr |= FD_MSR_CMDBUSY | FD_MSR_RQM | FD_MSR_DIO; if (do_irq) fdctrl_raise_irq(fdctrl); } @@ -1042,8 +1016,8 @@ fdctrl->data_dir = FD_DIR_READ; if (!(fdctrl->msr & FD_MSR_NONDMA)) { DMA_release_DREQ(fdctrl->dma_chann); - fdctrl->state &= ~FD_CTRL_BUSY; } + fdctrl->msr |= FD_MSR_RQM | FD_MSR_DIO; fdctrl->msr &= ~FD_MSR_NONDMA; fdctrl_set_fifo(fdctrl, 7, 1); } @@ -1095,10 +1069,11 @@ default: break; } + /* Set the FIFO state */ fdctrl->data_dir = direction; fdctrl->data_pos = 0; - FD_SET_STATE(fdctrl->data_state, FD_STATE_DATA); /* FIFO ready for data */ + fdctrl->msr |= FD_MSR_CMDBUSY; if (fdctrl->fifo[0] & 0x80) fdctrl->data_state |= FD_STATE_MULTI; else @@ -1128,7 +1103,7 @@ (direction == FD_DIR_WRITE && dma_mode == 2) || (direction == FD_DIR_READ && dma_mode == 1)) { /* No access is allowed until DMA transfer has completed */ - fdctrl->state |= FD_CTRL_BUSY; + fdctrl->msr &= ~FD_MSR_RQM; /* Now, we just have to wait for the DMA controller to * recall us... */ @@ -1141,6 +1116,8 @@ } FLOPPY_DPRINTF("start non-DMA transfer\n"); fdctrl->msr |= FD_MSR_NONDMA; + if (direction != FD_DIR_WRITE) + fdctrl->msr |= FD_MSR_DIO; /* IO based transfer: calculate len */ fdctrl_raise_irq(fdctrl); @@ -1168,7 +1145,7 @@ int len, start_pos, rel_pos; fdctrl = opaque; - if (!(fdctrl->state & FD_CTRL_BUSY)) { + if (fdctrl->msr & FD_MSR_RQM) { FLOPPY_DPRINTF("Not in DMA transfer mode !\n"); return 0; } @@ -1268,9 +1245,9 @@ int pos; cur_drv = get_cur_drv(fdctrl); - fdctrl->state &= ~FD_CTRL_SLEEP; - if (FD_STATE(fdctrl->data_state) == FD_STATE_CMD) { - FLOPPY_ERROR("can't read data in CMD state\n"); + fdctrl->dsr &= ~FD_DSR_PWRDOWN; + if (!(fdctrl->msr & FD_MSR_RQM) || !(fdctrl->msr & FD_MSR_DIO)) { + FLOPPY_ERROR("controller not ready for reading\n"); return 0; } pos = fdctrl->data_pos; @@ -1457,7 +1434,9 @@ { fdrive_t *cur_drv = get_cur_drv(fdctrl); - /* XXX: should set main status register to busy */ + /* Set main status register to busy */ + fdctrl->msr &= ~FD_MSR_RQM; + cur_drv->head = (fdctrl->fifo[1] >> 2) & 1; qemu_mod_timer(fdctrl->result_timer, qemu_get_clock(vm_clock) + (ticks_per_sec / 50)); @@ -1707,11 +1686,11 @@ FLOPPY_DPRINTF("Floppy controller in RESET state !\n"); return; } - fdctrl->state &= ~FD_CTRL_SLEEP; - if (FD_STATE(fdctrl->data_state) == FD_STATE_STATUS) { - FLOPPY_ERROR("can't write data in status mode\n"); + if (!(fdctrl->msr & FD_MSR_RQM) || (fdctrl->msr & FD_MSR_DIO)) { + FLOPPY_ERROR("controller not ready for writing\n"); return; } + fdctrl->dsr &= ~FD_DSR_PWRDOWN; /* Is it write command time ? */ if (fdctrl->msr & FD_MSR_NONDMA) { /* FIFO data write */ @@ -1728,7 +1707,7 @@ /* Switch from transfer mode to status mode * then from status mode to command mode */ - if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) + if (++fdctrl->data_pos == fdctrl->data_len) fdctrl_stop_transfer(fdctrl); return; } @@ -1737,7 +1716,7 @@ pos = command_to_handler[value & 0xff]; FLOPPY_DPRINTF("%s command\n", handlers[pos].name); fdctrl->data_len = handlers[pos].parameters + 1; - } + } FLOPPY_DPRINTF("%s: %02x\n", __func__, value); fdctrl->fifo[fdctrl->data_pos] = value; @@ -1820,7 +1799,6 @@ fd_init(&fdctrl->drives[i], fds[i]); } fdctrl_reset(fdctrl, 0); - fdctrl->state = FD_CTRL_ACTIVE; register_savevm("fdc", io_base, 1, fdc_save, fdc_load, fdctrl); qemu_register_reset(fdctrl_external_reset, fdctrl); for (i = 0; i < MAX_FD; i++) { --------------080908020200070308000501 Content-Type: text/plain; name="fdc_08_cleanup.diff" Content-Transfer-Encoding: 7bit Content-Disposition: inline; filename="fdc_08_cleanup.diff" Index: hw/fdc.c =================================================================== --- hw/fdc.c (revision 4215) +++ hw/fdc.c (working copy) @@ -79,9 +79,6 @@ uint8_t head; uint8_t track; uint8_t sect; - /* Last operation status */ - uint8_t dir; /* Direction */ - uint8_t rw; /* Read/write */ /* Media */ fdisk_flags_t flags; uint8_t last_sect; /* Nb sector per track */ @@ -113,6 +110,13 @@ return _fd_sector(drv->head, drv->track, drv->sect, drv->last_sect); } +/* Seek to a new position: + * returns 0 if already on right track + * returns 1 if track changed + * returns 2 if track is invalid + * returns 3 if sector is invalid + * returns 4 if seek is disabled + */ static int fd_seek (fdrive_t *drv, uint8_t head, uint8_t track, uint8_t sect, int enable_seek) { @@ -161,8 +165,6 @@ drv->head = 0; drv->track = 0; drv->sect = 1; - drv->dir = 1; - drv->rw = 0; } /* Recognize floppy formats */ @@ -449,9 +451,6 @@ #define FD_FORMAT_CMD(state) ((state) & FD_STATE_FORMAT) struct fdctrl_t { - fdctrl_t *fdctrl; - /* Controller's identification */ - uint8_t version; /* HW */ qemu_irq irq; int dma_chann; @@ -463,7 +462,6 @@ uint8_t dor; uint8_t dsr; uint8_t msr; - uint8_t state; uint8_t cur_drv; uint8_t bootsel; uint8_t status0; @@ -590,8 +588,6 @@ qemu_put_8s(f, &fd->head); qemu_put_8s(f, &fd->track); qemu_put_8s(f, &fd->sect); - qemu_put_8s(f, &fd->dir); - qemu_put_8s(f, &fd->rw); } static void fdc_save (QEMUFile *f, void *opaque) @@ -632,8 +628,6 @@ qemu_get_8s(f, &fd->head); qemu_get_8s(f, &fd->track); qemu_get_8s(f, &fd->sect); - qemu_get_8s(f, &fd->dir); - qemu_get_8s(f, &fd->rw); return 0; } @@ -943,19 +937,9 @@ /* Set an error: unimplemented/unknown command */ static void fdctrl_unimplemented (fdctrl_t *fdctrl, int direction) { -#if 0 - fdrive_t *cur_drv; - - cur_drv = get_cur_drv(fdctrl); - fdctrl->fifo[0] = FD_SR0_ABNTERM | FD_SR0_SEEK | (cur_drv->head << 2) | fdctrl->cur_drv; - fdctrl->fifo[1] = 0x00; - fdctrl->fifo[2] = 0x00; - fdctrl_set_fifo(fdctrl, 3, 1); -#else - // fdctrl_reset_fifo(fdctrl); + FLOPPY_ERROR("unimplemented command 0x%02x\n", fdctrl->fifo[0]); fdctrl->fifo[0] = FD_SR0_INVCMD; fdctrl_set_fifo(fdctrl, 1, 0); -#endif } /* Seek to next sector */ @@ -1260,7 +1244,12 @@ fd_sector(cur_drv)); return 0; } - bdrv_read(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1); + if (bdrv_read(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1) < 0) { + FLOPPY_DPRINTF("error getting sector %d\n", + fd_sector(cur_drv)); + /* Sure, image size is too small... */ + memset(fdctrl->fifo, 0, FD_SECTOR_LEN); + } } } retval = fdctrl->fifo[pos]; @@ -1375,8 +1364,7 @@ static void fdctrl_handle_version (fdctrl_t *fdctrl, int direction) { - /* Controller's version */ - fdctrl->fifo[0] = fdctrl->version; + fdctrl->fifo[0] = 0x90; /* Intel 82078 controller */ fdctrl_set_fifo(fdctrl, 1, 1); } @@ -1436,7 +1424,7 @@ /* Set main status register to busy */ fdctrl->msr &= ~FD_MSR_RQM; - + cur_drv->head = (fdctrl->fifo[1] >> 2) & 1; qemu_mod_timer(fdctrl->result_timer, qemu_get_clock(vm_clock) + (ticks_per_sec / 50)); @@ -1529,10 +1517,6 @@ fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK; cur_drv = get_cur_drv(fdctrl); - if (fdctrl->fifo[2] <= cur_drv->track) - cur_drv->dir = 1; - else - cur_drv->dir = 0; fdctrl_reset_fifo(fdctrl); fdctrl->status0 |= FD_SR0_SEEK; if (fdctrl->fifo[2] > cur_drv->max_track) @@ -1602,7 +1586,6 @@ fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK; cur_drv = get_cur_drv(fdctrl); - cur_drv->dir = 0; if (fdctrl->fifo[2] + cur_drv->track >= cur_drv->max_track) { cur_drv->track = cur_drv->max_track - 1; } else { @@ -1619,7 +1602,6 @@ fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK; cur_drv = get_cur_drv(fdctrl); - cur_drv->dir = 1; if (fdctrl->fifo[2] > cur_drv->track) { cur_drv->track = 0; } else { @@ -1680,7 +1662,6 @@ fdrive_t *cur_drv; int pos; - cur_drv = get_cur_drv(fdctrl); /* Reset mode */ if (!(fdctrl->dor & FD_DOR_nRESET)) { FLOPPY_DPRINTF("Floppy controller in RESET state !\n"); @@ -1697,7 +1678,11 @@ fdctrl->fifo[fdctrl->data_pos++] = value; if (fdctrl->data_pos % FD_SECTOR_LEN == (FD_SECTOR_LEN - 1) || fdctrl->data_pos == fdctrl->data_len) { - bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1); + cur_drv = get_cur_drv(fdctrl); + if (bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1) < 0) { + FLOPPY_ERROR("writing sector %d\n", fd_sector(cur_drv)); + return; + } if (!fdctrl_seek_to_next_sect(fdctrl, cur_drv)) { FLOPPY_DPRINTF("error seeking to next sector %d\n", fd_sector(cur_drv)); @@ -1787,7 +1772,6 @@ fdctrl->result_timer = qemu_new_timer(vm_clock, fdctrl_result_timer, fdctrl); - fdctrl->version = 0x90; /* Intel 82078 controller */ fdctrl->irq = irq; fdctrl->dma_chann = dma_chann; fdctrl->io_base = io_base; @@ -1798,7 +1782,7 @@ for (i = 0; i < MAX_FD; i++) { fd_init(&fdctrl->drives[i], fds[i]); } - fdctrl_reset(fdctrl, 0); + fdctrl_external_reset(fdctrl); register_savevm("fdc", io_base, 1, fdc_save, fdc_load, fdctrl); qemu_register_reset(fdctrl_external_reset, fdctrl); for (i = 0; i < MAX_FD; i++) { --------------080908020200070308000501 Content-Type: text/plain; name="fdc_09_tdr_reg.diff" Content-Transfer-Encoding: 7bit Content-Disposition: inline; filename="fdc_09_tdr_reg.diff" Index: hw/fdc.c =================================================================== --- hw/fdc.c (revision 4215) +++ hw/fdc.c (working copy) @@ -460,10 +460,10 @@ uint8_t sra; uint8_t srb; uint8_t dor; + uint8_t tdr; uint8_t dsr; uint8_t msr; uint8_t cur_drv; - uint8_t bootsel; uint8_t status0; uint8_t status1; uint8_t status2; @@ -598,10 +598,10 @@ qemu_put_8s(f, &s->sra); qemu_put_8s(f, &s->srb); qemu_put_8s(f, &s->dor); + qemu_put_8s(f, &s->tdr); qemu_put_8s(f, &s->dsr); qemu_put_8s(f, &s->msr); qemu_put_8s(f, &s->cur_drv); - qemu_put_8s(f, &s->bootsel); qemu_put_8s(f, &s->status0); qemu_put_8s(f, &s->status1); qemu_put_8s(f, &s->status2); @@ -644,10 +644,10 @@ qemu_get_8s(f, &s->sra); qemu_get_8s(f, &s->srb); qemu_get_8s(f, &s->dor); + qemu_get_8s(f, &s->tdr); qemu_get_8s(f, &s->dsr); qemu_get_8s(f, &s->msr); qemu_get_8s(f, &s->cur_drv); - qemu_get_8s(f, &s->bootsel); qemu_get_8s(f, &s->status0); qemu_get_8s(f, &s->status1); qemu_get_8s(f, &s->status2); @@ -747,12 +747,12 @@ static inline fdrive_t *drv0 (fdctrl_t *fdctrl) { - return &fdctrl->drives[fdctrl->bootsel]; + return &fdctrl->drives[(fdctrl->tdr & FD_TDR_BOOTSEL) >> 2]; } static inline fdrive_t *drv1 (fdctrl_t *fdctrl) { - return &fdctrl->drives[1 - fdctrl->bootsel]; + return &fdctrl->drives[1 - ((fdctrl->tdr & FD_TDR_BOOTSEL) >> 2)]; } static fdrive_t *get_cur_drv (fdctrl_t *fdctrl) @@ -833,11 +833,8 @@ /* Tape drive register : 0x03 */ static uint32_t fdctrl_read_tape (fdctrl_t *fdctrl) { - uint32_t retval = 0; + uint32_t retval = fdctrl->tdr; - /* Disk boot selection indicator */ - retval |= fdctrl->bootsel << 2; - /* Tape indicators: never allowed */ FLOPPY_DPRINTF("tape drive register: 0x%02x\n", retval); return retval; @@ -852,7 +849,7 @@ } FLOPPY_DPRINTF("tape drive register set to 0x%02x\n", value); /* Disk boot selection indicator */ - fdctrl->bootsel = (value & FD_TDR_BOOTSEL) >> 2; + fdctrl->tdr = value & FD_TDR_BOOTSEL; /* Tape indicators: never allow */ } --------------080908020200070308000501 Content-Type: text/plain; name="fdc_10_4_fds.diff" Content-Transfer-Encoding: 7bit Content-Disposition: inline; filename="fdc_10_4_fds.diff" Index: hw/fdc.c =================================================================== --- hw/fdc.c (revision 4215) +++ hw/fdc.c (working copy) @@ -413,7 +413,11 @@ }; enum { +#if MAX_FD == 4 + FD_DOR_SELMASK = 0x03, +#else FD_DOR_SELMASK = 0x01, +#endif FD_DOR_nRESET = 0x04, FD_DOR_DMAEN = 0x08, FD_DOR_MOTEN0 = 0x10, @@ -423,7 +427,11 @@ }; enum { +#if MAX_FD == 4 FD_TDR_BOOTSEL = 0x0c, +#else + FD_TDR_BOOTSEL = 0x04, +#endif }; enum { @@ -485,7 +493,7 @@ /* Power down config (also with status regB access mode */ uint8_t pwrd; /* Floppy drives */ - fdrive_t drives[2]; + fdrive_t drives[MAX_FD]; }; static uint32_t fdctrl_read (void *opaque, uint32_t reg) @@ -593,6 +601,8 @@ static void fdc_save (QEMUFile *f, void *opaque) { fdctrl_t *s = opaque; + uint8_t tmp; + int i; /* Controller state */ qemu_put_8s(f, &s->sra); @@ -619,8 +629,11 @@ qemu_put_8s(f, &s->config); qemu_put_8s(f, &s->lock); qemu_put_8s(f, &s->pwrd); - fd_save(f, &s->drives[0]); - fd_save(f, &s->drives[1]); + + tmp = MAX_FD; + qemu_put_8s(f, &tmp); + for (i = 0; i < MAX_FD; i++) + fd_save(f, &s->drives[i]); } static int fd_load (QEMUFile *f, fdrive_t *fd) @@ -635,9 +648,10 @@ static int fdc_load (QEMUFile *f, void *opaque, int version_id) { fdctrl_t *s = opaque; - int ret; + int i, ret = 0; + uint8_t n; - if (version_id != 1) + if (version_id != 2) return -EINVAL; /* Controller state */ @@ -665,10 +679,16 @@ qemu_get_8s(f, &s->config); qemu_get_8s(f, &s->lock); qemu_get_8s(f, &s->pwrd); + qemu_get_8s(f, &n); - ret = fd_load(f, &s->drives[0]); - if (ret == 0) - ret = fd_load(f, &s->drives[1]); + if (n > MAX_FD) + return -EINVAL; + + for (i = 0; i < n; i++) { + ret = fd_load(f, &s->drives[i]); + if (ret != 0) + break; + } return ret; } @@ -752,12 +772,41 @@ static inline fdrive_t *drv1 (fdctrl_t *fdctrl) { - return &fdctrl->drives[1 - ((fdctrl->tdr & FD_TDR_BOOTSEL) >> 2)]; + if ((fdctrl->tdr & FD_TDR_BOOTSEL) < (1 << 2)) + return &fdctrl->drives[1]; + else + return &fdctrl->drives[0]; +} + +#if MAX_FD == 4 +static inline fdrive_t *drv2 (fdctrl_t *fdctrl) +{ + if ((fdctrl->tdr & FD_TDR_BOOTSEL) < (2 << 2)) + return &fdctrl->drives[2]; + else + return &fdctrl->drives[1]; +} + +static inline fdrive_t *drv3 (fdctrl_t *fdctrl) +{ + if ((fdctrl->tdr & FD_TDR_BOOTSEL) < (3 << 2)) + return &fdctrl->drives[3]; + else + return &fdctrl->drives[2]; } +#endif static fdrive_t *get_cur_drv (fdctrl_t *fdctrl) { - return fdctrl->cur_drv == 0 ? drv0(fdctrl) : drv1(fdctrl); + switch (fdctrl->dor & FD_DOR_SELMASK) { + case 0: return drv0(fdctrl); + case 1: return drv1(fdctrl); +#if MAX_FD == 4 + case 2: return drv2(fdctrl); + case 3: return drv3(fdctrl); +#endif + default: return NULL; + } } /* Status A register : 0x00 (read-only) */ @@ -903,8 +952,13 @@ { uint32_t retval = 0; - if (fdctrl_media_changed(drv0(fdctrl)) || - fdctrl_media_changed(drv1(fdctrl))) + if (fdctrl_media_changed(drv0(fdctrl)) + || fdctrl_media_changed(drv1(fdctrl)) +#if MAX_FD == 4 + || fdctrl_media_changed(drv2(fdctrl)) + || fdctrl_media_changed(drv3(fdctrl)) +#endif + ) retval |= FD_DIR_DSKCHG; if (retval != 0) FLOPPY_DPRINTF("Floppy digital input register: 0x%02x\n", retval); @@ -1346,8 +1400,13 @@ /* Drives position */ fdctrl->fifo[0] = drv0(fdctrl)->track; fdctrl->fifo[1] = drv1(fdctrl)->track; +#if MAX_FD == 4 + fdctrl->fifo[2] = drv2(fdctrl)->track; + fdctrl->fifo[3] = drv3(fdctrl)->track; +#else fdctrl->fifo[2] = 0; fdctrl->fifo[3] = 0; +#endif /* timers */ fdctrl->fifo[4] = fdctrl->timer0; fdctrl->fifo[5] = (fdctrl->timer1 << 1) | (fdctrl->dor & FD_DOR_DMAEN ? 1 : 0); @@ -1378,6 +1437,10 @@ /* Drives position */ drv0(fdctrl)->track = fdctrl->fifo[3]; drv1(fdctrl)->track = fdctrl->fifo[4]; +#if MAX_FD == 4 + drv2(fdctrl)->track = fdctrl->fifo[5]; + drv3(fdctrl)->track = fdctrl->fifo[6]; +#endif /* timers */ fdctrl->timer0 = fdctrl->fifo[7]; fdctrl->timer1 = fdctrl->fifo[8]; @@ -1399,8 +1462,13 @@ /* Drives position */ fdctrl->fifo[2] = drv0(fdctrl)->track; fdctrl->fifo[3] = drv1(fdctrl)->track; +#if MAX_FD == 4 + fdctrl->fifo[4] = drv2(fdctrl)->track; + fdctrl->fifo[5] = drv3(fdctrl)->track; +#else fdctrl->fifo[4] = 0; fdctrl->fifo[5] = 0; +#endif /* timers */ fdctrl->fifo[6] = fdctrl->timer0; fdctrl->fifo[7] = fdctrl->timer1; @@ -1780,7 +1848,7 @@ fd_init(&fdctrl->drives[i], fds[i]); } fdctrl_external_reset(fdctrl); - register_savevm("fdc", io_base, 1, fdc_save, fdc_load, fdctrl); + register_savevm("fdc", io_base, 2, fdc_save, fdc_load, fdctrl); qemu_register_reset(fdctrl_external_reset, fdctrl); for (i = 0; i < MAX_FD; i++) { fd_revalidate(&fdctrl->drives[i]); --------------080908020200070308000501 Content-Type: text/plain; name="fdc_11_cur_drv.diff" Content-Transfer-Encoding: 7bit Content-Disposition: inline; filename="fdc_11_cur_drv.diff" Index: hw/fdc.c =================================================================== --- hw/fdc.c (revision 4215) +++ hw/fdc.c (working copy) @@ -46,6 +46,9 @@ /********************************************************/ /* Floppy drive emulation */ +#define GET_CUR_DRV(fdctrl) ((fdctrl)->cur_drv) +#define SET_CUR_DRV(fdctrl, drive) ((fdctrl)->cur_drv = (drive)) + /* Will always be a fixed parameter for us */ #define FD_SECTOR_LEN 512 #define FD_SECTOR_SC 2 /* Sector size code */ @@ -706,7 +709,7 @@ if (level) { // XXX - FLOPPY_DPRINTF("TC pulsed\n"); + FLOPPY_DPRINTF("Terminal Count Line pulsed\n"); } } @@ -798,7 +801,7 @@ static fdrive_t *get_cur_drv (fdctrl_t *fdctrl) { - switch (fdctrl->dor & FD_DOR_SELMASK) { + switch (GET_CUR_DRV(fdctrl)) { case 0: return drv0(fdctrl); case 1: return drv1(fdctrl); #if MAX_FD == 4 @@ -1036,7 +1039,7 @@ cur_drv = get_cur_drv(fdctrl); /* Add head and drive to status0 */ - status0 |= (cur_drv->head << 2) | (fdctrl->cur_drv); + status0 |= (cur_drv->head << 2) | GET_CUR_DRV(fdctrl); FLOPPY_DPRINTF("transfer status: %02x %02x %02x (%02x)\n", fdctrl->status0, fdctrl->status1, fdctrl->status2, @@ -1063,13 +1066,13 @@ fdrive_t *cur_drv; uint8_t kh, kt, ks; - fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK; + SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK); cur_drv = get_cur_drv(fdctrl); kt = fdctrl->fifo[2]; kh = fdctrl->fifo[3]; ks = fdctrl->fifo[4]; FLOPPY_DPRINTF("Start transfer at %d %d %02x %02x (%d)\n", - fdctrl->cur_drv, kh, kt, ks, + GET_CUR_DRV(fdctrl), kh, kt, ks, _fd_sector(kh, kt, ks, cur_drv->last_sect)); switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & FD_CONFIG_EIS)) { case 2: @@ -1200,7 +1203,7 @@ len = FD_SECTOR_LEN - rel_pos; FLOPPY_DPRINTF("copy %d bytes (%d %d %d) %d pos %d %02x " "(%d-0x%08x 0x%08x)\n", len, dma_len, fdctrl->data_pos, - fdctrl->data_len, fdctrl->cur_drv, cur_drv->head, + fdctrl->data_len, GET_CUR_DRV(fdctrl), cur_drv->head, cur_drv->track, cur_drv->sect, fd_sector(cur_drv), fd_sector(cur_drv) * FD_SECTOR_LEN); if (fdctrl->data_dir != FD_DIR_WRITE || @@ -1326,13 +1329,13 @@ fdrive_t *cur_drv; uint8_t kh, kt, ks; - fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK; + SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK); cur_drv = get_cur_drv(fdctrl); kt = fdctrl->fifo[6]; kh = fdctrl->fifo[7]; ks = fdctrl->fifo[8]; FLOPPY_DPRINTF("format sector at %d %d %02x %02x (%d)\n", - fdctrl->cur_drv, kh, kt, ks, + GET_CUR_DRV(fdctrl->dor), kh, kt, ks, _fd_sector(kh, kt, ks, cur_drv->last_sect)); switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & FD_CONFIG_EIS)) { case 2: @@ -1499,7 +1502,7 @@ { fdrive_t *cur_drv; - fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK; + SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK); cur_drv = get_cur_drv(fdctrl); fdctrl->data_state |= FD_STATE_FORMAT; if (fdctrl->fifo[0] & 0x80) @@ -1539,14 +1542,14 @@ { fdrive_t *cur_drv; - fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK; + SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK); cur_drv = get_cur_drv(fdctrl); cur_drv->head = (fdctrl->fifo[1] >> 2) & 1; /* 1 Byte status back */ fdctrl->fifo[0] = (cur_drv->ro << 6) | (cur_drv->track == 0 ? 0x10 : 0x00) | (cur_drv->head << 2) | - fdctrl->cur_drv | + GET_CUR_DRV(fdctrl) | 0x28; fdctrl_set_fifo(fdctrl, 1, 0); } @@ -1555,7 +1558,7 @@ { fdrive_t *cur_drv; - fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK; + SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK); cur_drv = get_cur_drv(fdctrl); fd_recalibrate(cur_drv); fdctrl_reset_fifo(fdctrl); @@ -1569,7 +1572,7 @@ fdrive_t *cur_drv = get_cur_drv(fdctrl); fdctrl->fifo[0] = - fdctrl->status0 | (cur_drv->head << 2) | fdctrl->cur_drv; + fdctrl->status0 | (cur_drv->head << 2) | GET_CUR_DRV(fdctrl); fdctrl->fifo[1] = cur_drv->track; fdctrl_set_fifo(fdctrl, 2, 0); fdctrl_reset_irq(fdctrl); @@ -1580,7 +1583,7 @@ { fdrive_t *cur_drv; - fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK; + SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK); cur_drv = get_cur_drv(fdctrl); fdctrl_reset_fifo(fdctrl); fdctrl->status0 |= FD_SR0_SEEK; @@ -1640,16 +1643,16 @@ } else if (fdctrl->data_len > 7) { /* ERROR */ fdctrl->fifo[0] = 0x80 | - (cur_drv->head << 2) | fdctrl->cur_drv; + (cur_drv->head << 2) | GET_CUR_DRV(fdctrl->dor); fdctrl_set_fifo(fdctrl, 1, 1); } } static void fdctrl_handle_relative_seek_out (fdctrl_t *fdctrl, int direction) { - fdrive_t *cur_drv = get_cur_drv(fdctrl); + fdrive_t *cur_drv; - fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK; + SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK); cur_drv = get_cur_drv(fdctrl); if (fdctrl->fifo[2] + cur_drv->track >= cur_drv->max_track) { cur_drv->track = cur_drv->max_track - 1; @@ -1663,9 +1666,9 @@ static void fdctrl_handle_relative_seek_in (fdctrl_t *fdctrl, int direction) { - fdrive_t *cur_drv = get_cur_drv(fdctrl); + fdrive_t *cur_drv; - fdctrl->cur_drv = fdctrl->fifo[1] & FD_DOR_SELMASK; + SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK); cur_drv = get_cur_drv(fdctrl); if (fdctrl->fifo[2] > cur_drv->track) { cur_drv->track = 0; --------------080908020200070308000501--