static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl){    uint32_t retval = 0;    /* Drive motors state indicators */    if (drv0(fdctrl)->drflags & FDRIVE_MOTOR_ON)        retval |= 1 << 5;    if (drv1(fdctrl)->drflags & FDRIVE_MOTOR_ON)        retval |= 1 << 4;    /* DMA enable */    retval |= fdctrl->dma_en << 3;    /* Reset indicator */    retval |= (fdctrl->state & FD_CTRL_RESET) == 0 ? 0x04 : 0;    /* Selected drive */    retval |= fdctrl->cur_drv;    FLOPPY_DPRINTF("digital output register: 0x%02x\n", retval);    return retval;}static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value){    /* Reset mode */    if (fdctrl->state & FD_CTRL_RESET) {        if (!(value & 0x04)) {            FLOPPY_DPRINTF("Floppy controller in RESET state !\n");            return;        }    }    FLOPPY_DPRINTF("digital output register set to 0x%02x\n", value);    /* Drive motors state indicators */    if (value & 0x20)        fd_start(drv1(fdctrl));    else        fd_stop(drv1(fdctrl));    if (value & 0x10)        fd_start(drv0(fdctrl));    else        fd_stop(drv0(fdctrl));    /* DMA enable */#if 0    if (fdctrl->dma_chann != -1)        fdctrl->dma_en = 1 - ((value >> 3) & 1);#endif    /* Reset */    if (!(value & 0x04)) {        if (!(fdctrl->state & FD_CTRL_RESET)) {            FLOPPY_DPRINTF("controller enter RESET state\n");            fdctrl->state |= FD_CTRL_RESET;        }    } else {        if (fdctrl->state & FD_CTRL_RESET) {            FLOPPY_DPRINTF("controller out of RESET state\n");            fdctrl_reset(fdctrl, 1);            fdctrl->state &= ~(FD_CTRL_RESET | FD_CTRL_SLEEP);        }    }    /* Selected drive */    fdctrl->cur_drv = value & 1;}/* Tape drive register : 0x03 */static uint32_t fdctrl_read_tape (fdctrl_t *fdctrl){    uint32_t retval = 0;    /* Disk boot selection indicator */    retval |= fdctrl->bootsel << 2;    /* Tape indicators: never allowed */    FLOPPY_DPRINTF("tape drive register: 0x%02x\n", retval);    return retval;}static void fdctrl_write_tape (fdctrl_t *fdctrl, uint32_t value){    /* Reset mode */    if (fdctrl->state & FD_CTRL_RESET) {        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 >> 2) & 1;    /* Tape indicators: never allow */}/* Main status register : 0x04 (read) */static uint32_t fdctrl_read_main_status (fdctrl_t *fdctrl){    uint32_t retval = 0;    fdctrl->state &= ~(FD_CTRL_SLEEP | FD_CTRL_RESET);    if (!(fdctrl->state & FD_CTRL_BUSY)) {        /* Data transfer allowed */        retval |= 0x80;        /* Data transfer direction indicator */        if (fdctrl->data_dir == FD_DIR_READ)            retval |= 0x40;    }    /* Should handle 0x20 for SPECIFY command */    /* Command busy indicator */    if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA ||        FD_STATE(fdctrl->data_state) == FD_STATE_STATUS)        retval |= 0x10;    FLOPPY_DPRINTF("main status register: 0x%02x\n", retval);    return retval;}/* Data select rate register : 0x04 (write) */static void fdctrl_write_rate (fdctrl_t *fdctrl, uint32_t value){    /* Reset mode */    if (fdctrl->state & FD_CTRL_RESET) {        FLOPPY_DPRINTF("Floppy controller in RESET state !\n");        return;    }    FLOPPY_DPRINTF("select rate register set to 0x%02x\n", value);    /* Reset: autoclear */    if (value & 0x80) {        fdctrl->state |= FD_CTRL_RESET;        fdctrl_reset(fdctrl, 1);        fdctrl->state &= ~FD_CTRL_RESET;    }    if (value & 0x40) {        fdctrl->state |= FD_CTRL_SLEEP;        fdctrl_reset(fdctrl, 1);    }//        fdctrl.precomp = (value >> 2) & 0x07;}static int fdctrl_media_changed(fdrive_t *drv){    int ret;    if (!drv->bs)        return 0;    ret = bdrv_media_changed(drv->bs);    if (ret) {        fd_revalidate(drv);    }    return ret;}/* Digital input register : 0x07 (read-only) */static uint32_t fdctrl_read_dir (fdctrl_t *fdctrl){    uint32_t retval = 0;    if (fdctrl_media_changed(drv0(fdctrl)) ||        fdctrl_media_changed(drv1(fdctrl)))        retval |= 0x80;    if (retval != 0)        FLOPPY_DPRINTF("Floppy digital input register: 0x%02x\n", retval);    return retval;}/* FIFO state control */static void fdctrl_reset_fifo (fdctrl_t *fdctrl){    fdctrl->data_dir = FD_DIR_WRITE;    fdctrl->data_pos = 0;    FD_SET_STATE(fdctrl->data_state, FD_STATE_CMD);}/* Set FIFO status for the host to read */static void fdctrl_set_fifo (fdctrl_t *fdctrl, int fifo_len, int do_irq){    fdctrl->data_dir = FD_DIR_READ;    fdctrl->data_len = fifo_len;    fdctrl->data_pos = 0;    FD_SET_STATE(fdctrl->data_state, FD_STATE_STATUS);    if (do_irq)        fdctrl_raise_irq(fdctrl, 0x00);}/* Set an error: unimplemented/unknown command */static void fdctrl_unimplemented (fdctrl_t *fdctrl){#if 0    fdrive_t *cur_drv;    cur_drv = get_cur_drv(fdctrl);    fdctrl->fifo[0] = 0x60 | (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);    fdctrl->fifo[0] = 0x80;    fdctrl_set_fifo(fdctrl, 1, 0);#endif}/* Callback for transfer end (stop or abort) */static void fdctrl_stop_transfer (fdctrl_t *fdctrl, uint8_t status0,                                  uint8_t status1, uint8_t status2){    fdrive_t *cur_drv;    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;    fdctrl->fifo[1] = status1;    fdctrl->fifo[2] = status2;    fdctrl->fifo[3] = cur_drv->track;    fdctrl->fifo[4] = cur_drv->head;    fdctrl->fifo[5] = cur_drv->sect;    fdctrl->fifo[6] = FD_SECTOR_SC;    fdctrl->data_dir = FD_DIR_READ;    if (fdctrl->state & FD_CTRL_BUSY) {        DMA_release_DREQ(fdctrl->dma_chann);        fdctrl->state &= ~FD_CTRL_BUSY;    }    fdctrl_set_fifo(fdctrl, 7, 1);}/* Prepare a data transfer (either DMA or FIFO) */static void fdctrl_start_transfer (fdctrl_t *fdctrl, int direction){    fdrive_t *cur_drv;    uint8_t kh, kt, ks;    int did_seek;    fdctrl->cur_drv = fdctrl->fifo[1] & 1;    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,                   _fd_sector(kh, kt, ks, cur_drv->last_sect));    did_seek = 0;    switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & 0x40)) {    case 2:        /* sect too big */        fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);        fdctrl->fifo[3] = kt;        fdctrl->fifo[4] = kh;        fdctrl->fifo[5] = ks;        return;    case 3:        /* track too big */        fdctrl_stop_transfer(fdctrl, 0x40, 0x80, 0x00);        fdctrl->fifo[3] = kt;        fdctrl->fifo[4] = kh;        fdctrl->fifo[5] = ks;        return;    case 4:        /* No seek enabled */        fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);        fdctrl->fifo[3] = kt;        fdctrl->fifo[4] = kh;        fdctrl->fifo[5] = ks;        return;    case 1:        did_seek = 1;        break;    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 */    if (fdctrl->fifo[0] & 0x80)        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 {        int tmp;        fdctrl->data_len = 128 << (fdctrl->fifo[5] > 7 ? 7 : fdctrl->fifo[5]);        tmp = (cur_drv->last_sect - ks + 1);        if (fdctrl->fifo[0] & 0x80)            tmp += cur_drv->last_sect;        fdctrl->data_len *= tmp;    }    fdctrl->eot = fdctrl->fifo[6];    if (fdctrl->dma_en) {        int dma_mode;        /* DMA transfer are enabled. Check if DMA channel is well programmed */        dma_mode = DMA_get_channel_mode(fdctrl->dma_chann);        dma_mode = (dma_mode >> 2) & 3;        FLOPPY_DPRINTF("dma_mode=%d direction=%d (%d - %d)\n",                       dma_mode, direction,                       (128 << fdctrl->fifo[5]) *                       (cur_drv->last_sect - ks + 1), fdctrl->data_len);        if (((direction == FD_DIR_SCANE || direction == FD_DIR_SCANL ||              direction == FD_DIR_SCANH) && dma_mode == 0) ||            (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;            /* Now, we just have to wait for the DMA controller to             * recall us...             */            DMA_hold_DREQ(fdctrl->dma_chann);            DMA_schedule(fdctrl->dma_chann);            return;        } else {            FLOPPY_ERROR("dma_mode=%d direction=%d\n", dma_mode, direction);        }    }    FLOPPY_DPRINTF("start non-DMA transfer\n");    /* IO based transfer: calculate len */    fdctrl_raise_irq(fdctrl, 0x00);    return;}/* Prepare a transfer of deleted data */static void fdctrl_start_transfer_del (fdctrl_t *fdctrl, int direction){    /* We don't handle deleted data,     * so we don't return *ANYTHING*     */    fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);}/* handlers for DMA transfers */static int fdctrl_transfer_handler (void *opaque, int nchan,                                    int dma_pos, int dma_len){    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)) {        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;    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, 0x60, 0x00, 0x00);        else            fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);        len = 0;        goto transfer_error;    }    rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;    for (start_pos = fdctrl->data_pos; fdctrl->data_pos < dma_len;) {        len = dma_len - fdctrl->data_pos;        if (len + rel_pos > FD_SECTOR_LEN)            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,                       cur_drv->track, cur_drv->sect, fd_sector(cur_drv),                       fd_sector(cur_drv) * 512);        if (fdctrl->data_dir != FD_DIR_WRITE ||            len < FD_SECTOR_LEN || rel_pos != 0) {            /* READ & SCAN commands and realign to a sector for WRITE */            if (bdrv_read(cur_drv->bs, fd_sector(cur_drv),                          fdctrl->fifo, 1) < 0) {                FLOPPY_DPRINTF("Floppy: error getting sector %d\n",                               fd_sector(cur_drv));                /* Sure, image size is too small... */                memset(fdctrl->fifo, 0, FD_SECTOR_LEN);            }        }        switch (fdctrl->data_dir) {        case FD_DIR_READ:            /* READ commands */