DMA_write_memory (nchan, fdctrl->fifo + rel_pos,                              fdctrl->data_pos, len);            break;        case FD_DIR_WRITE:            /* WRITE commands */            DMA_read_memory (nchan, fdctrl->fifo + rel_pos,                             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, 0x60, 0x00, 0x00);                goto transfer_error;            }            break;        default:            /* SCAN commands */            {                uint8_t tmpbuf[FD_SECTOR_LEN];                int ret;                DMA_read_memory (nchan, tmpbuf, fdctrl->data_pos, len);                ret = memcmp(tmpbuf, fdctrl->fifo + rel_pos, len);                if (ret == 0) {                    status2 = 0x08;                    goto end_transfer;                }                if ((ret < 0 && fdctrl->data_dir == FD_DIR_SCANL) ||                    (ret > 0 && fdctrl->data_dir == FD_DIR_SCANH)) {                    status2 = 0x00;                    goto end_transfer;                }            }            break;        }        fdctrl->data_pos += len;        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++;            }        }    } end_transfer:    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 |= 0x20;    fdctrl->data_len -= len;    //    if (fdctrl->data_len == 0)    fdctrl_stop_transfer(fdctrl, status0, status1, status2); transfer_error:    return len;}/* Data register : 0x05 */static uint32_t fdctrl_read_data (fdctrl_t *fdctrl){    fdrive_t *cur_drv;    uint32_t retval = 0;    int pos, len;    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");        return 0;    }    pos = fdctrl->data_pos;    if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {        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);        }    }    retval = fdctrl->fifo[pos];    if (++fdctrl->data_pos == fdctrl->data_len) {        fdctrl->data_pos = 0;        /* Switch from transfer mode to status mode         * then from status mode to command mode         */        if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {            fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);        } else {            fdctrl_reset_fifo(fdctrl);            fdctrl_reset_irq(fdctrl);        }    }    FLOPPY_DPRINTF("data register: 0x%02x\n", retval);    return retval;}static void fdctrl_format_sector (fdctrl_t *fdctrl){    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[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,                   _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;        fdctrl->data_state |= FD_STATE_SEEK;        break;    default:        break;    }    memset(fdctrl->fifo, 0, FD_SECTOR_LEN);    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, 0x60, 0x00, 0x00);    } 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, 0x20, 0x00, 0x00);            else                fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);        } else {            /* More to do */            fdctrl->data_pos = 0;            fdctrl->data_len = 4;        }    }}static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value){    fdrive_t *cur_drv;    cur_drv = get_cur_drv(fdctrl);    /* Reset mode */    if (fdctrl->state & FD_CTRL_RESET) {        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");        return;    }    /* Is it write command time ? */    if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {        /* 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);        }        /* Switch from transfer mode to status mode         * then from status mode to command mode         */        if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA)            fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);        return;    }    if (fdctrl->data_pos == 0) {        /* Command */        switch (value & 0x5F) {        case 0x46:            /* READ variants */            FLOPPY_DPRINTF("READ command\n");            /* 8 parameters cmd */            fdctrl->data_len = 9;            goto enqueue;        case 0x4C:            /* READ_DELETED variants */            FLOPPY_DPRINTF("READ_DELETED command\n");            /* 8 parameters cmd */            fdctrl->data_len = 9;            goto enqueue;        case 0x50:            /* SCAN_EQUAL variants */            FLOPPY_DPRINTF("SCAN_EQUAL command\n");            /* 8 parameters cmd */            fdctrl->data_len = 9;            goto enqueue;        case 0x56:            /* VERIFY variants */            FLOPPY_DPRINTF("VERIFY command\n");            /* 8 parameters cmd */            fdctrl->data_len = 9;            goto enqueue;        case 0x59:            /* SCAN_LOW_OR_EQUAL variants */            FLOPPY_DPRINTF("SCAN_LOW_OR_EQUAL command\n");            /* 8 parameters cmd */            fdctrl->data_len = 9;            goto enqueue;        case 0x5D:            /* SCAN_HIGH_OR_EQUAL variants */            FLOPPY_DPRINTF("SCAN_HIGH_OR_EQUAL command\n");            /* 8 parameters cmd */            fdctrl->data_len = 9;            goto enqueue;        default:            break;        }        switch (value & 0x7F) {        case 0x45:            /* WRITE variants */            FLOPPY_DPRINTF("WRITE command\n");            /* 8 parameters cmd */            fdctrl->data_len = 9;            goto enqueue;        case 0x49:            /* WRITE_DELETED variants */            FLOPPY_DPRINTF("WRITE_DELETED command\n");            /* 8 parameters cmd */            fdctrl->data_len = 9;            goto enqueue;        default:            break;        }        switch (value) {        case 0x03:            /* SPECIFY */            FLOPPY_DPRINTF("SPECIFY command\n");            /* 1 parameter cmd */            fdctrl->data_len = 3;            goto enqueue;        case 0x04:            /* SENSE_DRIVE_STATUS */            FLOPPY_DPRINTF("SENSE_DRIVE_STATUS command\n");            /* 1 parameter cmd */            fdctrl->data_len = 2;            goto enqueue;        case 0x07:            /* RECALIBRATE */            FLOPPY_DPRINTF("RECALIBRATE command\n");            /* 1 parameter cmd */            fdctrl->data_len = 2;            goto enqueue;        case 0x08:            /* SENSE_INTERRUPT_STATUS */            FLOPPY_DPRINTF("SENSE_INTERRUPT_STATUS command (%02x)\n",                           fdctrl->int_status);            /* No parameters cmd: returns status if no interrupt */#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->fifo[1] = cur_drv->track;            fdctrl_set_fifo(fdctrl, 2, 0);            fdctrl_reset_irq(fdctrl);            fdctrl->int_status = 0xC0;            return;        case 0x0E:            /* DUMPREG */            FLOPPY_DPRINTF("DUMPREG command\n");            /* Drives position */            fdctrl->fifo[0] = drv0(fdctrl)->track;            fdctrl->fifo[1] = drv1(fdctrl)->track;            fdctrl->fifo[2] = 0;            fdctrl->fifo[3] = 0;            /* timers */            fdctrl->fifo[4] = fdctrl->timer0;            fdctrl->fifo[5] = (fdctrl->timer1 << 1) | fdctrl->dma_en;            fdctrl->fifo[6] = cur_drv->last_sect;            fdctrl->fifo[7] = (fdctrl->lock << 7) |                (cur_drv->perpendicular << 2);            fdctrl->fifo[8] = fdctrl->config;            fdctrl->fifo[9] = fdctrl->precomp_trk;            fdctrl_set_fifo(fdctrl, 10, 0);            return;        case 0x0F:            /* SEEK */            FLOPPY_DPRINTF("SEEK command\n");            /* 2 parameters cmd */            fdctrl->data_len = 3;            goto enqueue;        case 0x10:            /* VERSION */            FLOPPY_DPRINTF("VERSION command\n");            /* No parameters cmd */            /* Controller's version */            fdctrl->fifo[0] = fdctrl->version;            fdctrl_set_fifo(fdctrl, 1, 1);            return;        case 0x12:            /* PERPENDICULAR_MODE */            FLOPPY_DPRINTF("PERPENDICULAR_MODE command\n");            /* 1 parameter cmd */            fdctrl->data_len = 2;            goto enqueue;        case 0x13:            /* CONFIGURE */            FLOPPY_DPRINTF("CONFIGURE command\n");            /* 3 parameters cmd */            fdctrl->data_len = 4;            goto enqueue;        case 0x14:            /* UNLOCK */            FLOPPY_DPRINTF("UNLOCK command\n");            /* No parameters cmd */            fdctrl->lock = 0;            fdctrl->fifo[0] = 0;            fdctrl_set_fifo(fdctrl, 1, 0);            return;        case 0x17:            /* POWERDOWN_MODE */            FLOPPY_DPRINTF("POWERDOWN_MODE command\n");            /* 2 parameters cmd */            fdctrl->data_len = 3;            goto enqueue;        case 0x18:            /* PART_ID */            FLOPPY_DPRINTF("PART_ID command\n");            /* No parameters cmd */