Index: hw/fdc.c =================================================================== --- hw/fdc.c (revision 4220) +++ hw/fdc.c (working copy) @@ -49,6 +49,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 */ @@ -69,10 +72,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; @@ -81,15 +80,11 @@ BlockDriverState *bs; /* Drive status */ fdrive_type_t drive; - fdrive_flags_t drflags; uint8_t perpendicular; /* 2.88 MB access mode */ /* Position */ 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 */ @@ -103,7 +98,6 @@ /* Drive */ drv->bs = bs; drv->drive = FDRIVE_DRV_NONE; - drv->drflags = 0; drv->perpendicular = 0; /* Disk */ drv->last_sect = 0; @@ -122,6 +116,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) { @@ -170,8 +171,6 @@ drv->head = 0; drv->track = 0; drv->sect = 1; - drv->dir = 1; - drv->rw = 0; } /* Recognize floppy formats */ @@ -296,24 +295,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 */ @@ -321,9 +302,9 @@ 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_result_timer(void *opaque); +static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status0); +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); @@ -336,14 +317,6 @@ static uint32_t fdctrl_read_dir (fdctrl_t *fdctrl); 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 */ - FD_CTRL_INTR = 0x10, -}; - -enum { FD_DIR_WRITE = 0, FD_DIR_READ = 1, FD_DIR_SCANE = 2, @@ -352,18 +325,14 @@ }; enum { - FD_STATE_CMD = 0x00, - FD_STATE_STATUS = 0x01, - FD_STATE_DATA = 0x02, - FD_STATE_STATE = 0x03, - FD_STATE_MULTI = 0x10, - FD_STATE_SEEK = 0x20, - FD_STATE_FORMAT = 0x40, + FD_STATE_MULTI = 0x01, /* multi track flag */ + FD_STATE_FORMAT = 0x02, /* format flag */ + FD_STATE_SEEK = 0x04, /* seek flag */ }; 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, @@ -422,7 +391,40 @@ }; 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, + 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 { +#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, @@ -432,7 +434,11 @@ }; enum { +#if MAX_FD == 4 FD_TDR_BOOTSEL = 0x0c, +#else + FD_TDR_BOOTSEL = 0x04, +#endif }; enum { @@ -456,15 +462,11 @@ 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_DID_SEEK(state) ((state) & FD_STATE_SEEK) #define FD_FORMAT_CMD(state) ((state) & FD_STATE_FORMAT) struct fdctrl_t { - fdctrl_t *fdctrl; /* Controller's identification */ uint8_t version; /* HW */ @@ -473,17 +475,22 @@ target_phys_addr_t io_base; /* Controller state */ QEMUTimer *result_timer; - uint8_t state; - uint8_t dma_en; + 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; /* 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 */ @@ -498,7 +505,7 @@ /* Sun4m quirks? */ int sun4m; /* Floppy drives */ - fdrive_t drives[2]; + fdrive_t drives[MAX_FD]; }; static uint32_t fdctrl_read (void *opaque, uint32_t reg) @@ -507,15 +514,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: @@ -603,53 +605,54 @@ 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); - qemu_put_8s(f, &fd->dir); - qemu_put_8s(f, &fd->rw); } static void fdc_save (QEMUFile *f, void *opaque) { fdctrl_t *s = opaque; + uint8_t tmp; + int i; + uint8_t dor = s->dor | GET_CUR_DRV(s); - 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); + /* Controller state */ + qemu_put_8s(f, &s->sra); + qemu_put_8s(f, &s->srb); + qemu_put_8s(f, &dor); + qemu_put_8s(f, &s->tdr); + qemu_put_8s(f, &s->dsr); + qemu_put_8s(f, &s->msr); + 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); 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) { - 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); - qemu_get_8s(f, &fd->dir); - qemu_get_8s(f, &fd->rw); return 0; } @@ -657,33 +660,49 @@ 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; - 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); + /* Controller state */ + qemu_get_8s(f, &s->sra); + qemu_get_8s(f, &s->srb); + qemu_get_8s(f, &s->dor); + SET_CUR_DRV(s, s->dor & FD_DOR_SELMASK); + s->dor &= ~FD_DOR_SELMASK; + qemu_get_8s(f, &s->tdr); + qemu_get_8s(f, &s->dsr); + qemu_get_8s(f, &s->msr); + 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); 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; } @@ -694,78 +713,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 +723,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) { @@ -802,25 +732,28 @@ /* 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) +static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status0) { - // Sparc mutation - if (fdctrl->sun4m && !fdctrl->dma_en) { - fdctrl->state &= ~FD_CTRL_BUSY; - fdctrl->int_status = status; + /* Sparc mutation */ + if (fdctrl->sun4m && (fdctrl->msr & FD_MSR_CMDBUSY)) { + fdctrl->msr &= ~FD_MSR_CMDBUSY; + fdctrl->msr |= FD_MSR_RQM | FD_MSR_DIO; /* XXX: not sure */ + fdctrl->status0 = status0; 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; + fdctrl->status0 = status0; + FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", fdctrl->status0); } /* Reset controller */ @@ -831,57 +764,96 @@ 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; + 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_reset(&fdctrl->drives[i]); + fd_recalibrate(&fdctrl->drives[i]); fdctrl_reset_fifo(fdctrl); - if (do_irq) + if (do_irq) { fdctrl_raise_irq(fdctrl, FD_SR0_RDYCHG); + } } 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]; + 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 (GET_CUR_DRV(fdctrl)) { + 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) */ +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 */ 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->dma_en) - 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); @@ -891,53 +863,47 @@ 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); - /* Drive motors state indicators */ + + /* Motors */ + if (value & FD_DOR_MOTEN0) + fdctrl->srb |= FD_SRB_MTR0; + else + fdctrl->srb &= ~FD_SRB_MTR0; if (value & FD_DOR_MOTEN1) - fd_start(drv1(fdctrl)); + fdctrl->srb |= FD_SRB_MTR1; else - fd_stop(drv1(fdctrl)); - if (value & FD_DOR_MOTEN0) - fd_start(drv0(fdctrl)); + fdctrl->srb &= ~FD_SRB_MTR1; + + /* Drive */ + if (value & 1) + fdctrl->srb |= FD_SRB_DR0; else - fd_stop(drv0(fdctrl)); - /* DMA enable */ -#if 0 - if (fdctrl->dma_chann != -1) - fdctrl->dma_en = value & FD_DOR_DMAEN ? 1 : 0; -#endif + fdctrl->srb &= ~FD_SRB_DR0; + /* 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->dsr &= ~FD_DSR_PWRDOWN; } } /* Selected drive */ fdctrl->cur_drv = value & FD_DOR_SELMASK; + + fdctrl->dor = value; } /* 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; @@ -946,34 +912,24 @@ 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; } 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 */ } /* Main status register : 0x04 (read) */ static uint32_t fdctrl_read_main_status (fdctrl_t *fdctrl) { - uint32_t retval = 0; + uint32_t retval = fdctrl->msr; - fdctrl->state &= ~(FD_CTRL_SLEEP | FD_CTRL_RESET); - 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; - } - /* Should handle FD_MSR_NONDMA for SPECIFY command */ - /* Command busy indicator */ - if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA || - FD_STATE(fdctrl->data_state) == FD_STATE_STATUS) - retval |= FD_MSR_CMDBUSY; + fdctrl->dsr &= ~FD_DSR_PWRDOWN; + fdctrl->dor |= FD_DOR_nRESET; + FLOPPY_DPRINTF("main status register: 0x%02x\n", retval); return retval; @@ -983,21 +939,21 @@ 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; fdctrl_reset(fdctrl, 1); } + fdctrl->dsr = value; } static int fdctrl_media_changed(fdrive_t *drv) @@ -1018,8 +974,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); @@ -1032,7 +993,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 */ @@ -1041,7 +1002,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, 0x00); } @@ -1049,21 +1010,45 @@ /* 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 */ +static int fdctrl_seek_to_next_sect (fdctrl_t *fdctrl, fdrive_t *cur_drv) +{ + FLOPPY_DPRINTF("seek to next sector (%d %02x %02x => %d)\n", + cur_drv->head, cur_drv->track, cur_drv->sect, + fd_sector(cur_drv)); + /* XXX: cur_drv->sect >= cur_drv->last_sect should be an + error in fact */ + if (cur_drv->sect >= cur_drv->last_sect || + cur_drv->sect == fdctrl->eot) { + cur_drv->sect = 1; + if (FD_MULTI_TRACK(fdctrl->data_state)) { + if (cur_drv->head == 0 && + (cur_drv->flags & FDISK_DBL_SIDES) != 0) { + cur_drv->head = 1; + } else { + cur_drv->head = 0; + cur_drv->track++; + if ((cur_drv->flags & FDISK_DBL_SIDES) == 0) + return 0; + } + } else { + cur_drv->track++; + return 1; + } + FLOPPY_DPRINTF("seek to next track (%d %02x %02x => %d)\n", + cur_drv->head, cur_drv->track, + cur_drv->sect, fd_sector(cur_drv)); + } else { + cur_drv->sect++; + } + 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) @@ -1073,8 +1058,8 @@ cur_drv = get_cur_drv(fdctrl); 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; + status0 | (cur_drv->head << 2) | GET_CUR_DRV(fdctrl)); + fdctrl->fifo[0] = status0 | (cur_drv->head << 2) | GET_CUR_DRV(fdctrl); fdctrl->fifo[1] = status1; fdctrl->fifo[2] = status2; fdctrl->fifo[3] = cur_drv->track; @@ -1082,10 +1067,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_RQM | FD_MSR_DIO; + fdctrl->msr &= ~FD_MSR_NONDMA; fdctrl_set_fifo(fdctrl, 7, 1); } @@ -1094,18 +1080,17 @@ { fdrive_t *cur_drv; uint8_t kh, kt, ks; - int did_seek; + int did_seek = 0; - 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)); - 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); @@ -1115,7 +1100,7 @@ return; case 3: /* track too big */ - fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x80, 0x00); + fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, FD_SR1_EC, 0x00); fdctrl->fifo[3] = kt; fdctrl->fifo[4] = kh; fdctrl->fifo[5] = ks; @@ -1133,10 +1118,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 @@ -1156,7 +1142,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); @@ -1170,7 +1156,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... */ @@ -1182,6 +1168,9 @@ } } 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, 0x00); @@ -1191,6 +1180,8 @@ /* 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* */ @@ -1207,14 +1198,14 @@ uint8_t status0 = 0x00, status1 = 0x00, status2 = 0x00; fdctrl = opaque; - if (!(fdctrl->state & FD_CTRL_BUSY)) { + if (fdctrl->msr & FD_MSR_RQM) { FLOPPY_DPRINTF("Not in DMA transfer mode !\n"); 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; + status2 = FD_SR2_SNS; if (dma_len > fdctrl->data_len) dma_len = fdctrl->data_len; if (cur_drv->bs == NULL) { @@ -1232,7 +1223,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 || @@ -1258,7 +1249,7 @@ 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)); + FLOPPY_ERROR("writing sector %d\n", fd_sector(cur_drv)); fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00); goto transfer_error; } @@ -1271,7 +1262,7 @@ DMA_read_memory (nchan, tmpbuf, fdctrl->data_pos, len); ret = memcmp(tmpbuf, fdctrl->fifo + rel_pos, len); if (ret == 0) { - status2 = 0x08; + status2 = FD_SR2_SEH; goto end_transfer; } if ((ret < 0 && fdctrl->data_dir == FD_DIR_SCANL) || @@ -1286,35 +1277,8 @@ rel_pos = fdctrl->data_pos % FD_SECTOR_LEN; if (rel_pos == 0) { /* Seek to next sector */ - FLOPPY_DPRINTF("seek to next sector (%d %02x %02x => %d) (%d)\n", - cur_drv->head, cur_drv->track, cur_drv->sect, - fd_sector(cur_drv), - fdctrl->data_pos - len); - /* XXX: cur_drv->sect >= cur_drv->last_sect should be an - error in fact */ - if (cur_drv->sect >= cur_drv->last_sect || - cur_drv->sect == fdctrl->eot) { - cur_drv->sect = 1; - if (FD_MULTI_TRACK(fdctrl->data_state)) { - if (cur_drv->head == 0 && - (cur_drv->flags & FDISK_DBL_SIDES) != 0) { - cur_drv->head = 1; - } else { - cur_drv->head = 0; - cur_drv->track++; - if ((cur_drv->flags & FDISK_DBL_SIDES) == 0) - break; - } - } else { - cur_drv->track++; - break; - } - FLOPPY_DPRINTF("seek to next track (%d %02x %02x => %d)\n", - cur_drv->head, cur_drv->track, - cur_drv->sect, fd_sector(cur_drv)); - } else { - cur_drv->sect++; - } + if (!fdctrl_seek_to_next_sect(fdctrl, cur_drv)) + break; } } end_transfer: @@ -1324,11 +1288,10 @@ if (fdctrl->data_dir == FD_DIR_SCANE || fdctrl->data_dir == FD_DIR_SCANL || fdctrl->data_dir == FD_DIR_SCANH) - status2 = 0x08; + status2 = FD_SR2_SEH; 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); transfer_error: @@ -1340,22 +1303,30 @@ { fdrive_t *cur_drv; uint32_t retval = 0; - int pos, len; + 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; - if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) { + if (fdctrl->msr & FD_MSR_NONDMA) { pos %= FD_SECTOR_LEN; if (pos == 0) { - len = fdctrl->data_len - fdctrl->data_pos; - if (len > FD_SECTOR_LEN) - len = FD_SECTOR_LEN; - bdrv_read(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1); + if (fdctrl->data_pos != 0) + if (!fdctrl_seek_to_next_sect(fdctrl, cur_drv)) { + FLOPPY_DPRINTF("error seeking to next sector %d\n", + fd_sector(cur_drv)); + return 0; + } + 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]; @@ -1364,7 +1335,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, FD_SR0_SEEK, 0x00, 0x00); } else { fdctrl_reset_fifo(fdctrl); @@ -1380,17 +1351,15 @@ { fdrive_t *cur_drv; uint8_t kh, kt, ks; - int did_seek; - 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), 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 */ @@ -1401,7 +1370,7 @@ return; case 3: /* track too big */ - fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x80, 0x00); + fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, FD_SR1_EC, 0x00); fdctrl->fifo[3] = kt; fdctrl->fifo[4] = kh; fdctrl->fifo[5] = ks; @@ -1414,7 +1383,6 @@ fdctrl->fifo[5] = ks; return; case 1: - did_seek = 1; fdctrl->data_state |= FD_STATE_SEEK; break; default: @@ -1455,11 +1423,16 @@ /* 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->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); @@ -1470,7 +1443,6 @@ static void fdctrl_handle_version (fdctrl_t *fdctrl, int direction) { - /* Controller's version */ fdctrl->fifo[0] = fdctrl->version; fdctrl_set_fifo(fdctrl, 1, 1); } @@ -1488,6 +1460,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]; @@ -1509,8 +1485,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; @@ -1529,7 +1510,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)); @@ -1539,14 +1522,13 @@ { 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) 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 @@ -1568,7 +1550,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); } @@ -1577,14 +1562,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); } @@ -1593,7 +1578,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); @@ -1607,31 +1592,26 @@ #if 0 fdctrl->fifo[0] = - fdctrl->int_status | (cur_drv->head << 2) | fdctrl->cur_drv; + fdctrl->status0 | (cur_drv->head << 2) | GET_CUR_DRV(fdctrl); #else - /* XXX: int_status handling is broken for read/write + /* XXX: status0 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; + FD_SR0_SEEK | (cur_drv->head << 2) | GET_CUR_DRV(fdctrl); #endif 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) { 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_start(cur_drv); - if (fdctrl->fifo[2] <= cur_drv->track) - cur_drv->dir = 1; - 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); @@ -1649,7 +1629,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) @@ -1690,36 +1670,33 @@ } 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); 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); - 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; } else { cur_drv->track += fdctrl->fifo[2]; } fdctrl_reset_fifo(fdctrl); + /* Raise Interrupt */ fdctrl_raise_irq(fdctrl, FD_SR0_SEEK); } 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); - fd_start(cur_drv); - cur_drv->dir = 1; if (fdctrl->fifo[2] > cur_drv->track) { cur_drv->track = 0; } else { @@ -1730,96 +1707,99 @@ 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 direction; +} 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 */ - if (fdctrl->state & FD_CTRL_RESET) { + if (!(fdctrl->dor & FD_DOR_nRESET)) { 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 (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) || 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)); + return; + } } /* 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, FD_SR0_SEEK, 0x00, 0x00); return; } 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) { + fdctrl->fifo[fdctrl->data_pos++] = value; + if (fdctrl->data_pos == fdctrl->data_len) { /* We now have all parameters * and will be able to treat the command */ @@ -1828,13 +1808,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].direction); } } @@ -1852,3 +1828,97 @@ } 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) { + DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl); + } + for (i = 0; i < MAX_FD; i++) { + fd_init(&fdctrl->drives[i], fds[i]); + } + fdctrl_external_reset(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]); + } + + 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, -1, 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; +}