Commit message: hw/block/fdc: Prevent end-of-track overrun (CVE-2021-3507)

Per the 82078 datasheet, if the end-of-track (EOT byte in
the FIFO) is more than the number of sectors per side, the
command is terminated unsuccessfully:

* 5.2.5 DATA TRANSFER TERMINATION

  The 82078 supports terminal count explicitly through
  the TC pin and implicitly through the underrun/over-
  run and end-of-track (EOT) functions. For full sector
  transfers, the EOT parameter can define the last
  sector to be transferred in a single or multisector
  transfer. If the last sector to be transferred is a par-
  tial sector, the host can stop transferring the data in
  mid-sector, and the 82078 will continue to complete
  the sector as if a hardware TC was received. The
  only difference between these implicit functions and
  TC is that they return "abnormal termination" result
  status. Such status indications can be ignored if they
  were expected.

* 6.1.3 READ TRACK

  This command terminates when the EOT specified
  number of sectors have been read. If the 82078
  does not find an I D Address Mark on the diskette
  after the second· occurrence of a pulse on the
  INDX# pin, then it sets the IC code in Status Regis-
  ter 0 to "01" (Abnormal termination), sets the MA bit
  in Status Register 1 to "1", and terminates the com-
  mand.

* 6.1.6 VERIFY

  Refer to Table 6-6 and Table 6-7 for information
  concerning the values of MT and EC versus SC and
  EOT value.

* Table 6·6. Result Phase Table

* Table 6-7. Verify Command Result Phase Table

Fix by aborting the transfer when EOT > # Sectors Per Side.

Cc: qemu-stable@nongnu.org
Cc: Hervé Poussineau <hpoussin@reactos.org>
Fixes: baca51faff0 ("floppy driver: disk geometry auto detect")
Reported-by: Alexander Bulekov <alxndr@bu.edu>
Resolves: https://gitlab.com/qemu-project/qemu/-/issues/339
Signed-off-by: Philippe Mathieu-Daudé <philmd@redhat.com>
Message-Id: <20211118115733.4038610-2-philmd@redhat.com>
Reviewed-by: Hanna Reitz <hreitz@redhat.com>
Signed-off-by: Kevin Wolf <kwolf@redhat.com>
CVE: CVE-2021-3507
CWE: CWE-787
Vulnerability description: A heap buffer overflow was found in the floppy disk emulator of QEMU up to 6.0.0 (including). It could occur in fdctrl_transfer_handler() in hw/block/fdc.c while processing DMA read data transfers from the floppy drive to the guest system. A privileged guest user could use this flaw to crash the QEMU process on the host resulting in DoS scenario, or potential information leakage from the host memory.

Project: qemu

static void floppy_drive_realize(DeviceState *qdev, Error **errp)
{
    FloppyDrive *dev = FLOPPY_DRIVE(qdev);
    FloppyBus *bus = FLOPPY_BUS(qdev->parent_bus);
    FDrive *drive;
    bool read_only;
    int ret;

    if (dev->unit == -1) {
        for (dev->unit = 0; dev->unit < MAX_FD; dev->unit++) {
            drive = get_drv(bus->fdc, dev->unit);
            if (!drive->blk) {
                break;
            }
        }
    }

    if (dev->unit >= MAX_FD) {
        error_setg(errp, "Can't create floppy unit %d, bus supports "
                   "only %d units", dev->unit, MAX_FD);
        return;
    }

    drive = get_drv(bus->fdc, dev->unit);
    if (drive->blk) {
        error_setg(errp, "Floppy unit %d is in use", dev->unit);
        return;
    }

    if (!dev->conf.blk) {
        dev->conf.blk = blk_create_empty_drive();
        ret = blk_attach_dev(dev->conf.blk, qdev);
        assert(ret == 0);

        /* Don't take write permissions on an empty drive to allow attaching a
         * read-only node later */
        read_only = true;
    } else {
        read_only = !blk_bs(dev->conf.blk) ||
                    !blk_supports_write_perm(dev->conf.blk);
    }

    if (!blkconf_blocksizes(&dev->conf, errp)) {
        return;
    }

    if (dev->conf.logical_block_size != 512 ||
        dev->conf.physical_block_size != 512)
    {
        error_setg(errp, "Physical and logical block size must "
                   "be 512 for floppy");
        return;
    }

    /* rerror/werror aren't supported by fdc and therefore not even registered
     * with qdev. So set the defaults manually before they are used in
     * blkconf_apply_backend_options(). */
    dev->conf.rerror = BLOCKDEV_ON_ERROR_AUTO;
    dev->conf.werror = BLOCKDEV_ON_ERROR_AUTO;

    if (!blkconf_apply_backend_options(&dev->conf, read_only, false, errp)) {
        return;
    }

    /* 'enospc' is the default for -drive, 'report' is what blk_new() gives us
     * for empty drives. */
    if (blk_get_on_error(dev->conf.blk, 0) != BLOCKDEV_ON_ERROR_ENOSPC &&
        blk_get_on_error(dev->conf.blk, 0) != BLOCKDEV_ON_ERROR_REPORT) {
        error_setg(errp, "fdc doesn't support drive option werror");
        return;
    }
    if (blk_get_on_error(dev->conf.blk, 1) != BLOCKDEV_ON_ERROR_REPORT) {
        error_setg(errp, "fdc doesn't support drive option rerror");
        return;
    }

    drive->conf = &dev->conf;
    drive->blk = dev->conf.blk;
    drive->fdctrl = bus->fdc;

    fd_init(drive);
    blk_set_dev_ops(drive->blk, &fd_block_ops, drive);

    /* Keep 'type' qdev property and FDrive->drive in sync */
    drive->drive = dev->type;
    pick_drive_type(drive);
    dev->type = drive->drive;

    fd_revalidate(drive);
}

Project: qemu

static void fdctrl_handle_dumpreg(FDCtrl *fdctrl, int direction)
{
    FDrive *cur_drv = get_cur_drv(fdctrl);

    /* 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);
    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_to_result_phase(fdctrl, 10);
}

Project: qemu

static void fdctrl_handle_seek(FDCtrl *fdctrl, int direction)
{
    FDrive *cur_drv;

    SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
    cur_drv = get_cur_drv(fdctrl);
    fdctrl_to_command_phase(fdctrl);
    /* The seek command just sends step pulses to the drive and doesn't care if
     * there is a medium inserted of if it's banging the head against the drive.
     */
    fd_seek(cur_drv, cur_drv->head, fdctrl->fifo[2], cur_drv->sect, 1);
    /* Raise Interrupt */
    fdctrl->status0 |= FD_SR0_SEEK;
    fdctrl_raise_irq(fdctrl);
}

Project: qemu

static void fdctrl_handle_partid(FDCtrl *fdctrl, int direction)
{
    fdctrl->fifo[0] = 0x41; /* Stepping 1 */
    fdctrl_to_result_phase(fdctrl, 1);
}

Project: qemu

static int reconstruct_phase(FDCtrl *fdctrl)
{
    if (fdctrl->msr & FD_MSR_NONDMA) {
        return FD_PHASE_EXECUTION;
    } else if ((fdctrl->msr & FD_MSR_RQM) == 0) {
        /* qemu 2.3 disabled RQM only during DMA transfers */
        return FD_PHASE_EXECUTION;
    } else if (fdctrl->msr & FD_MSR_DIO) {
        return FD_PHASE_RESULT;
    } else {
        return FD_PHASE_COMMAND;
    }
}

Project: qemu

static void fdctrl_to_result_phase(FDCtrl *fdctrl, int fifo_len)
{
    fdctrl->phase = FD_PHASE_RESULT;
    fdctrl->data_dir = FD_DIR_READ;
    fdctrl->data_len = fifo_len;
    fdctrl->data_pos = 0;
    fdctrl->msr |= FD_MSR_CMDBUSY | FD_MSR_RQM | FD_MSR_DIO;
}

Project: qemu

static void fdctrl_handle_recalibrate(FDCtrl *fdctrl, int direction)
{
    FDrive *cur_drv;

    SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
    cur_drv = get_cur_drv(fdctrl);
    fd_recalibrate(cur_drv);
    fdctrl_to_command_phase(fdctrl);
    /* Raise Interrupt */
    fdctrl->status0 |= FD_SR0_SEEK;
    fdctrl_raise_irq(fdctrl);
}

Project: qemu

static void fdctrl_handle_powerdown_mode(FDCtrl *fdctrl, int direction)
{
    fdctrl->pwrd = fdctrl->fifo[1];
    fdctrl->fifo[0] = fdctrl->fifo[1];
    fdctrl_to_result_phase(fdctrl, 1);
}

Project: qemu

static void fdctrl_handle_sense_interrupt_status(FDCtrl *fdctrl, int direction)
{
    FDrive *cur_drv = get_cur_drv(fdctrl);

    if (fdctrl->reset_sensei > 0) {
        fdctrl->fifo[0] =
            FD_SR0_RDYCHG + FD_RESET_SENSEI_COUNT - fdctrl->reset_sensei;
        fdctrl->reset_sensei--;
    } else if (!(fdctrl->sra & FD_SRA_INTPEND)) {
        fdctrl->fifo[0] = FD_SR0_INVCMD;
        fdctrl_to_result_phase(fdctrl, 1);
        return;
    } else {
        fdctrl->fifo[0] =
                (fdctrl->status0 & ~(FD_SR0_HEAD | FD_SR0_DS1 | FD_SR0_DS0))
                | GET_CUR_DRV(fdctrl);
    }

    fdctrl->fifo[1] = cur_drv->track;
    fdctrl_to_result_phase(fdctrl, 2);
    fdctrl_reset_irq(fdctrl);
    fdctrl->status0 = FD_SR0_RDYCHG;
}

Project: qemu

static const FDCtrlCommand *get_command(uint8_t cmd)
{
    int idx;

    idx = command_to_handler[cmd];
    FLOPPY_DPRINTF("%s command\n", handlers[idx].name);
    return &handlers[idx];
}

Project: qemu

static int fd_offset(FDrive *drv)
{
    g_assert(fd_sector(drv) < INT_MAX >> BDRV_SECTOR_BITS);
    return fd_sector(drv) << BDRV_SECTOR_BITS;
}

Project: qemu

static void fdctrl_handle_specify(FDCtrl *fdctrl, int direction)
{
    fdctrl->timer0 = (fdctrl->fifo[1] >> 4) & 0xF;
    fdctrl->timer1 = fdctrl->fifo[2] >> 1;
    if (fdctrl->fifo[2] & 1)
        fdctrl->dor &= ~FD_DOR_DMAEN;
    else
        fdctrl->dor |= FD_DOR_DMAEN;
    /* No result back */
    fdctrl_to_command_phase(fdctrl);
}

Project: qemu

void fdctrl_reset(FDCtrl *fdctrl, int do_irq)
{
    int i;

    FLOPPY_DPRINTF("reset controller\n");
    fdctrl_reset_irq(fdctrl);
    /* Initialise controller */
    fdctrl->sra = 0;
    fdctrl->srb = 0xc0;
    if (!fdctrl->drives[1].blk) {
        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;
    fdctrl->reset_sensei = 0;
    timer_del(fdctrl->result_timer);
    /* FIFO state */
    fdctrl->data_pos = 0;
    fdctrl->data_len = 0;
    fdctrl->data_state = 0;
    fdctrl->data_dir = FD_DIR_WRITE;
    for (i = 0; i < MAX_FD; i++)
        fd_recalibrate(&fdctrl->drives[i]);
    fdctrl_to_command_phase(fdctrl);
    if (do_irq) {
        fdctrl->status0 |= FD_SR0_RDYCHG;
        fdctrl_raise_irq(fdctrl);
        fdctrl->reset_sensei = FD_RESET_SENSEI_COUNT;
    }
}

Project: qemu

void fdctrl_write(void *opaque, uint32_t reg, uint32_t value)
{
    FDCtrl *fdctrl = opaque;

    reg &= 7;
    trace_fdc_ioport_write(reg, value);
    switch (reg) {
    case FD_REG_DOR:
        fdctrl_write_dor(fdctrl, value);
        break;
    case FD_REG_TDR:
        fdctrl_write_tape(fdctrl, value);
        break;
    case FD_REG_DSR:
        fdctrl_write_rate(fdctrl, value);
        break;
    case FD_REG_FIFO:
        fdctrl_write_data(fdctrl, value);
        break;
    case FD_REG_CCR:
        fdctrl_write_ccr(fdctrl, value);
        break;
    default:
        break;
    }
}

Project: qemu

static void fdctrl_to_command_phase(FDCtrl *fdctrl)
{
    fdctrl->phase = FD_PHASE_COMMAND;
    fdctrl->data_dir = FD_DIR_WRITE;
    fdctrl->data_pos = 0;
    fdctrl->data_len = 1; /* Accept command byte, adjust for params later */
    fdctrl->msr &= ~(FD_MSR_CMDBUSY | FD_MSR_DIO);
    fdctrl->msr |= FD_MSR_RQM;
}

Project: qemu

static void fd_revalidate(FDrive *drv)
{
    int rc;

    FLOPPY_DPRINTF("revalidate\n");
    if (drv->blk != NULL) {
        drv->ro = !blk_is_writable(drv->blk);
        if (!blk_is_inserted(drv->blk)) {
            FLOPPY_DPRINTF("No disk in drive\n");
            drv->disk = FLOPPY_DRIVE_TYPE_NONE;
            fd_empty_seek_hack(drv);
        } else if (!drv->media_validated) {
            rc = pick_geometry(drv);
            if (rc) {
                FLOPPY_DPRINTF("Could not validate floppy drive media");
            } else {
                drv->media_validated = true;
                FLOPPY_DPRINTF("Floppy disk (%d h %d t %d s) %s\n",
                               (drv->flags & FDISK_DBL_SIDES) ? 2 : 1,
                               drv->max_track, drv->last_sect,
                               drv->ro ? "ro" : "rw");
            }
        }
    } else {
        FLOPPY_DPRINTF("No drive connected\n");
        drv->last_sect = 0;
        drv->max_track = 0;
        drv->flags &= ~FDISK_DBL_SIDES;
        drv->drive = FLOPPY_DRIVE_TYPE_NONE;
        drv->disk = FLOPPY_DRIVE_TYPE_NONE;
    }
}

Project: qemu

static void fdctrl_handle_relative_seek_in(FDCtrl *fdctrl, int direction)
{
    FDrive *cur_drv;

    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) {
        fd_seek(cur_drv, cur_drv->head, cur_drv->max_track - 1,
                cur_drv->sect, 1);
    } else {
        fd_seek(cur_drv, cur_drv->head,
                cur_drv->track + fdctrl->fifo[2], cur_drv->sect, 1);
    }
    fdctrl_to_command_phase(fdctrl);
    /* Raise Interrupt */
    fdctrl->status0 |= FD_SR0_SEEK;
    fdctrl_raise_irq(fdctrl);
}

Project: qemu

uint32_t fdctrl_read(void *opaque, uint32_t reg)
{
    FDCtrl *fdctrl = opaque;
    uint32_t retval;

    reg &= 7;
    switch (reg) {
    case FD_REG_SRA:
        retval = fdctrl_read_statusA(fdctrl);
        break;
    case FD_REG_SRB:
        retval = fdctrl_read_statusB(fdctrl);
        break;
    case FD_REG_DOR:
        retval = fdctrl_read_dor(fdctrl);
        break;
    case FD_REG_TDR:
        retval = fdctrl_read_tape(fdctrl);
        break;
    case FD_REG_MSR:
        retval = fdctrl_read_main_status(fdctrl);
        break;
    case FD_REG_FIFO:
        retval = fdctrl_read_data(fdctrl);
        break;
    case FD_REG_DIR:
        retval = fdctrl_read_dir(fdctrl);
        break;
    default:
        retval = (uint32_t)(-1);
        break;
    }
    trace_fdc_ioport_read(reg, retval);

    return retval;
}

Project: qemu

static void fdctrl_handle_configure(FDCtrl *fdctrl, int direction)
{
    fdctrl->config = fdctrl->fifo[2];
    fdctrl->precomp_trk =  fdctrl->fifo[3];
    /* No result back */
    fdctrl_to_command_phase(fdctrl);
}

Project: qemu

static void fdctrl_handle_lock(FDCtrl *fdctrl, int direction)
{
    fdctrl->lock = (fdctrl->fifo[0] & 0x80) ? 1 : 0;
    fdctrl->fifo[0] = fdctrl->lock << 4;
    fdctrl_to_result_phase(fdctrl, 1);
}

Project: qemu

static void fdctrl_write_data(FDCtrl *fdctrl, uint32_t value)
{
    FDrive *cur_drv;
    const FDCtrlCommand *cmd;
    uint32_t pos;

    /* Reset mode */
    if (!(fdctrl->dor & FD_DOR_nRESET)) {
        FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
        return;
    }
    if (!(fdctrl->msr & FD_MSR_RQM) || (fdctrl->msr & FD_MSR_DIO)) {
        FLOPPY_DPRINTF("error: controller not ready for writing\n");
        return;
    }
    fdctrl->dsr &= ~FD_DSR_PWRDOWN;

    FLOPPY_DPRINTF("%s: %02x\n", __func__, value);

    /* If data_len spans multiple sectors, the current position in the FIFO
     * wraps around while fdctrl->data_pos is the real position in the whole
     * request. */
    pos = fdctrl->data_pos++;
    pos %= FD_SECTOR_LEN;
    fdctrl->fifo[pos] = value;

    if (fdctrl->data_pos == fdctrl->data_len) {
        fdctrl->msr &= ~FD_MSR_RQM;
    }

    switch (fdctrl->phase) {
    case FD_PHASE_EXECUTION:
        /* For DMA requests, RQM should be cleared during execution phase, so
         * we would have errored out above. */
        assert(fdctrl->msr & FD_MSR_NONDMA);

        /* FIFO data write */
        if (pos == FD_SECTOR_LEN - 1 ||
            fdctrl->data_pos == fdctrl->data_len) {
            cur_drv = get_cur_drv(fdctrl);
            if (blk_pwrite(cur_drv->blk, fd_offset(cur_drv), fdctrl->fifo,
                           BDRV_SECTOR_SIZE, 0) < 0) {
                FLOPPY_DPRINTF("error writing sector %d\n",
                               fd_sector(cur_drv));
                break;
            }
            if (!fdctrl_seek_to_next_sect(fdctrl, cur_drv)) {
                FLOPPY_DPRINTF("error seeking to next sector %d\n",
                               fd_sector(cur_drv));
                break;
            }
        }

        /* Switch to result phase when done with the transfer */
        if (fdctrl->data_pos == fdctrl->data_len) {
            fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
        }
        break;

    case FD_PHASE_COMMAND:
        assert(!(fdctrl->msr & FD_MSR_NONDMA));
        assert(fdctrl->data_pos < FD_SECTOR_LEN);

        if (pos == 0) {
            /* The first byte specifies the command. Now we start reading
             * as many parameters as this command requires. */
            cmd = get_command(value);
            fdctrl->data_len = cmd->parameters + 1;
            if (cmd->parameters) {
                fdctrl->msr |= FD_MSR_RQM;
            }
            fdctrl->msr |= FD_MSR_CMDBUSY;
        }

        if (fdctrl->data_pos == fdctrl->data_len) {
            /* We have all parameters now, execute the command */
            fdctrl->phase = FD_PHASE_EXECUTION;

            if (fdctrl->data_state & FD_STATE_FORMAT) {
                fdctrl_format_sector(fdctrl);
                break;
            }

            cmd = get_command(fdctrl->fifo[0]);
            FLOPPY_DPRINTF("Calling handler for '%s'\n", cmd->name);
            cmd->handler(fdctrl, cmd->direction);
        }
        break;

    case FD_PHASE_RESULT:
    default:
        abort();
    }
}

Project: qemu

static void fdc_register_types(void)
{
    type_register_static(&floppy_bus_info);
    type_register_static(&floppy_drive_info);
}

Project: qemu

static void fdctrl_handle_sense_drive_status(FDCtrl *fdctrl, int direction)
{
    FDrive *cur_drv;

    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) |
        GET_CUR_DRV(fdctrl) |
        0x28;
    fdctrl_to_result_phase(fdctrl, 1);
}

Project: qemu

static void floppy_bus_create(FDCtrl *fdc, FloppyBus *bus, DeviceState *dev)
{
    qbus_init(bus, sizeof(FloppyBus), TYPE_FLOPPY_BUS, dev, NULL);
    bus->fdc = fdc;
}

Project: qemu

static void floppy_drive_class_init(ObjectClass *klass, void *data)
{
    DeviceClass *k = DEVICE_CLASS(klass);
    k->realize = floppy_drive_realize;
    set_bit(DEVICE_CATEGORY_STORAGE, k->categories);
    k->bus_type = TYPE_FLOPPY_BUS;
    device_class_set_props(k, floppy_drive_properties);
    k->desc = "virtual floppy drive";
}

Project: qemu

static void fdctrl_handle_version(FDCtrl *fdctrl, int direction)
{
    /* Controller's version */
    fdctrl->fifo[0] = fdctrl->version;
    fdctrl_to_result_phase(fdctrl, 1);
}

Project: qemu

static void fdctrl_handle_perpendicular_mode(FDCtrl *fdctrl, int direction)
{
    FDrive *cur_drv = get_cur_drv(fdctrl);

    if (fdctrl->fifo[1] & 0x80)
        cur_drv->perpendicular = fdctrl->fifo[1] & 0x7;
    /* No result back */
    fdctrl_to_command_phase(fdctrl);
}

Project: qemu

static void fdctrl_stop_transfer(FDCtrl *fdctrl, uint8_t status0,
                                 uint8_t status1, uint8_t status2)
{
    FDrive *cur_drv;
    cur_drv = get_cur_drv(fdctrl);

    fdctrl->status0 &= ~(FD_SR0_DS0 | FD_SR0_DS1 | FD_SR0_HEAD);
    fdctrl->status0 |= GET_CUR_DRV(fdctrl);
    if (cur_drv->head) {
        fdctrl->status0 |= FD_SR0_HEAD;
    }
    fdctrl->status0 |= status0;

    FLOPPY_DPRINTF("transfer status: %02x %02x %02x (%02x)\n",
                   status0, status1, status2, fdctrl->status0);
    fdctrl->fifo[0] = fdctrl->status0;
    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->dma_chann != -1 && !(fdctrl->msr & FD_MSR_NONDMA)) {
        IsaDmaClass *k = ISADMA_GET_CLASS(fdctrl->dma);
        k->release_DREQ(fdctrl->dma, fdctrl->dma_chann);
    }
    fdctrl->msr |= FD_MSR_RQM | FD_MSR_DIO;
    fdctrl->msr &= ~FD_MSR_NONDMA;

    fdctrl_to_result_phase(fdctrl, 7);
    fdctrl_raise_irq(fdctrl);
}

Project: qemu

static void fdctrl_unimplemented(FDCtrl *fdctrl, int direction)
{
    qemu_log_mask(LOG_UNIMP, "fdc: unimplemented command 0x%02x\n",
                  fdctrl->fifo[0]);
    fdctrl->fifo[0] = FD_SR0_INVCMD;
    fdctrl_to_result_phase(fdctrl, 1);
}

Project: qemu

void fdctrl_init_drives(FloppyBus *bus, DriveInfo **fds)
{
    DeviceState *dev;
    int i;

    for (i = 0; i < MAX_FD; i++) {
        if (fds[i]) {
            dev = qdev_new("floppy");
            qdev_prop_set_uint32(dev, "unit", i);
            qdev_prop_set_enum(dev, "drive-type", FLOPPY_DRIVE_TYPE_AUTO);
            qdev_prop_set_drive_err(dev, "drive", blk_by_legacy_dinfo(fds[i]),
                                    &error_fatal);
            qdev_realize_and_unref(dev, &bus->bus, &error_fatal);
        }
    }
}

Project: qemu

static void fdctrl_result_timer(void *opaque)
{
    FDCtrl *fdctrl = opaque;
    FDrive *cur_drv = get_cur_drv(fdctrl);

    /* Pretend we are spinning.
     * This is needed for Coherent, which uses READ ID to check for
     * sector interleaving.
     */
    if (cur_drv->last_sect != 0) {
        cur_drv->sect = (cur_drv->sect % cur_drv->last_sect) + 1;
    }
    /* READ_ID can't automatically succeed! */
    if ((fdctrl->dsr & FD_DSR_DRATEMASK) != cur_drv->media_rate) {
        FLOPPY_DPRINTF("read id rate mismatch (fdc=%d, media=%d)\n",
                       fdctrl->dsr & FD_DSR_DRATEMASK, cur_drv->media_rate);
        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, FD_SR1_MA, 0x00);
    } else {
        fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
    }
}

Project: qemu

static void fdctrl_handle_drive_specification_command(FDCtrl *fdctrl, int direction)
{
    FDrive *cur_drv = get_cur_drv(fdctrl);
    uint32_t pos;

    pos = fdctrl->data_pos - 1;
    pos %= FD_SECTOR_LEN;
    if (fdctrl->fifo[pos] & 0x80) {
        /* Command parameters done */
        if (fdctrl->fifo[pos] & 0x40) {
            fdctrl->fifo[0] = fdctrl->fifo[1];
            fdctrl->fifo[2] = 0;
            fdctrl->fifo[3] = 0;
            fdctrl_to_result_phase(fdctrl, 4);
        } else {
            fdctrl_to_command_phase(fdctrl);
        }
    } else if (fdctrl->data_len > 7) {
        /* ERROR */
        fdctrl->fifo[0] = 0x80 |
            (cur_drv->head << 2) | GET_CUR_DRV(fdctrl);
        fdctrl_to_result_phase(fdctrl, 1);
    }
}

Project: qemu

static FDrive *get_drv(FDCtrl *fdctrl, int unit)
{
    switch (unit) {
        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;
    }
}

Project: qemu

static void fd_empty_seek_hack(FDrive *drv)
{
    drv->last_sect = 0xFF;
    drv->max_track = 0xFF;
}

Project: qemu

static void fdctrl_handle_option(FDCtrl *fdctrl, int direction)
{
    /* No result back */
    fdctrl_to_command_phase(fdctrl);
}

Project: qemu

static void fdctrl_handle_readid(FDCtrl *fdctrl, int direction)
{
    FDrive *cur_drv = get_cur_drv(fdctrl);

    cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
    timer_mod(fdctrl->result_timer, qemu_clock_get_ns(QEMU_CLOCK_VIRTUAL) +
             (NANOSECONDS_PER_SECOND / 50));
}

Project: qemu

static int fdc_post_load(void *opaque, int version_id)
{
    FDCtrl *s = opaque;

    SET_CUR_DRV(s, s->dor_vmstate & FD_DOR_SELMASK);
    s->dor = s->dor_vmstate & ~FD_DOR_SELMASK;

    if (s->phase == FD_PHASE_RECONSTRUCT) {
        s->phase = reconstruct_phase(s);
    }

    return 0;
}

Project: qemu

int fdctrl_transfer_handler(void *opaque, int nchan, int dma_pos, int dma_len)
{
    FDCtrl *fdctrl;
    FDrive *cur_drv;
    int len, start_pos, rel_pos;
    uint8_t status0 = 0x00, status1 = 0x00, status2 = 0x00;
    IsaDmaClass *k;

    fdctrl = opaque;
    if (fdctrl->msr & FD_MSR_RQM) {
        FLOPPY_DPRINTF("Not in DMA transfer mode !\n");
        return 0;
    }
    k = ISADMA_GET_CLASS(fdctrl->dma);
    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 = FD_SR2_SNS;
    if (dma_len > fdctrl->data_len)
        dma_len = fdctrl->data_len;
    if (cur_drv->blk == 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);
        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, 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 ||
            len < FD_SECTOR_LEN || rel_pos != 0) {
            /* READ & SCAN commands and realign to a sector for WRITE */
            if (blk_pread(cur_drv->blk, fd_offset(cur_drv),
                          fdctrl->fifo, BDRV_SECTOR_SIZE) < 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 */
            k->write_memory(fdctrl->dma, nchan, fdctrl->fifo + rel_pos,
                            fdctrl->data_pos, len);
            break;
        case FD_DIR_WRITE:
            /* WRITE commands */
            if (cur_drv->ro) {
                /* Handle readonly medium early, no need to do DMA, touch the
                 * LED or attempt any writes. A real floppy doesn't attempt
                 * to write to readonly media either. */
                fdctrl_stop_transfer(fdctrl,
                                     FD_SR0_ABNTERM | FD_SR0_SEEK, FD_SR1_NW,
                                     0x00);
                goto transfer_error;
            }

            k->read_memory(fdctrl->dma, nchan, fdctrl->fifo + rel_pos,
                           fdctrl->data_pos, len);
            if (blk_pwrite(cur_drv->blk, fd_offset(cur_drv),
                           fdctrl->fifo, BDRV_SECTOR_SIZE, 0) < 0) {
                FLOPPY_DPRINTF("error writing sector %d\n",
                               fd_sector(cur_drv));
                fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00);
                goto transfer_error;
            }
            break;
        case FD_DIR_VERIFY:
            /* VERIFY commands */
            break;
        default:
            /* SCAN commands */
            {
                uint8_t tmpbuf[FD_SECTOR_LEN];
                int ret;
                k->read_memory(fdctrl->dma, nchan, tmpbuf, fdctrl->data_pos,
                               len);
                ret = memcmp(tmpbuf, fdctrl->fifo + rel_pos, len);
                if (ret == 0) {
                    status2 = FD_SR2_SEH;
                    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 */
            if (!fdctrl_seek_to_next_sect(fdctrl, cur_drv))
                break;
        }
    }
 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 = FD_SR2_SEH;
    fdctrl->data_len -= len;
    fdctrl_stop_transfer(fdctrl, status0, status1, status2);
 transfer_error:

    return len;
}

Project: qemu

static int pick_geometry(FDrive *drv)
{
    BlockBackend *blk = drv->blk;
    const FDFormat *parse;
    uint64_t nb_sectors, size;
    int i;
    int match, size_match, type_match;
    bool magic = drv->drive == FLOPPY_DRIVE_TYPE_AUTO;

    /* We can only pick a geometry if we have a diskette. */
    if (!drv->blk || !blk_is_inserted(drv->blk) ||
        drv->drive == FLOPPY_DRIVE_TYPE_NONE)
    {
        return -1;
    }

    /* We need to determine the likely geometry of the inserted medium.
     * In order of preference, we look for:
     * (1) The same drive type and number of sectors,
     * (2) The same diskette size and number of sectors,
     * (3) The same drive type.
     *
     * In all cases, matches that occur higher in the drive table will take
     * precedence over matches that occur later in the table.
     */
    blk_get_geometry(blk, &nb_sectors);
    match = size_match = type_match = -1;
    for (i = 0; ; i++) {
        parse = &fd_formats[i];
        if (parse->drive == FLOPPY_DRIVE_TYPE_NONE) {
            break;
        }
        size = (parse->max_head + 1) * parse->max_track * parse->last_sect;
        if (nb_sectors == size) {
            if (magic || parse->drive == drv->drive) {
                /* (1) perfect match -- nb_sectors and drive type */
                goto out;
            } else if (drive_size(parse->drive) == drive_size(drv->drive)) {
                /* (2) size match -- nb_sectors and physical medium size */
                match = (match == -1) ? i : match;
            } else {
                /* This is suspicious -- Did the user misconfigure? */
                size_match = (size_match == -1) ? i : size_match;
            }
        } else if (type_match == -1) {
            if ((parse->drive == drv->drive) ||
                (magic && (parse->drive == get_fallback_drive_type(drv)))) {
                /* (3) type match -- nb_sectors mismatch, but matches the type
                 *     specified explicitly by the user, or matches the fallback
                 *     default type when using the drive autodetect mechanism */
                type_match = i;
            }
        }
    }

    /* No exact match found */
    if (match == -1) {
        if (size_match != -1) {
            parse = &fd_formats[size_match];
            FLOPPY_DPRINTF("User requested floppy drive type '%s', "
                           "but inserted medium appears to be a "
                           "%"PRId64" sector '%s' type\n",
                           FloppyDriveType_str(drv->drive),
                           nb_sectors,
                           FloppyDriveType_str(parse->drive));
        }
        assert(type_match != -1 && "misconfigured fd_format");
        match = type_match;
    }
    parse = &(fd_formats[match]);

 out:
    if (parse->max_head == 0) {
        drv->flags &= ~FDISK_DBL_SIDES;
    } else {
        drv->flags |= FDISK_DBL_SIDES;
    }
    drv->max_track = parse->max_track;
    drv->last_sect = parse->last_sect;
    drv->disk = parse->drive;
    drv->media_rate = parse->rate;
    return 0;
}

Project: qemu

static void fdctrl_format_sector(FDCtrl *fdctrl)
{
    FDrive *cur_drv;
    uint8_t kh, kt, ks;

    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",
                   GET_CUR_DRV(fdctrl), kh, kt, ks,
                   fd_sector_calc(kh, kt, ks, cur_drv->last_sect,
                                  NUM_SIDES(cur_drv)));
    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->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
        return;
    case 3:
        /* track too big */
        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, FD_SR1_EC, 0x00);
        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->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
        return;
    case 1:
        fdctrl->status0 |= FD_SR0_SEEK;
        break;
    default:
        break;
    }
    memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
    if (cur_drv->blk == NULL ||
        blk_pwrite(cur_drv->blk, fd_offset(cur_drv), fdctrl->fifo,
                   BDRV_SECTOR_SIZE, 0) < 0) {
        FLOPPY_DPRINTF("error formatting sector %d\n", fd_sector(cur_drv));
        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00);
    } else {
        if (cur_drv->sect == cur_drv->last_sect) {
            fdctrl->data_state &= ~FD_STATE_FORMAT;
            /* Last sector done */
            fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
        } else {
            /* More to do */
            fdctrl->data_pos = 0;
            fdctrl->data_len = 4;
        }
    }
}

Project: qemu

static void pick_drive_type(FDrive *drv)
{
    if (drv->drive != FLOPPY_DRIVE_TYPE_AUTO) {
        return;
    }

    if (pick_geometry(drv) == 0) {
        drv->drive = drv->disk;
    } else {
        drv->drive = get_fallback_drive_type(drv);
    }

    g_assert(drv->drive != FLOPPY_DRIVE_TYPE_AUTO);
}

Project: qemu

static bool fdc_phase_needed(void *opaque)
{
    FDCtrl *fdctrl = opaque;

    return reconstruct_phase(fdctrl) != fdctrl->phase;
}

Project: qemu

static int fdc_pre_save(void *opaque)
{
    FDCtrl *s = opaque;

    s->dor_vmstate = s->dor | GET_CUR_DRV(s);

    return 0;
}

Project: qemu

static void fdctrl_handle_relative_seek_out(FDCtrl *fdctrl, int direction)
{
    FDrive *cur_drv;

    SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
    cur_drv = get_cur_drv(fdctrl);
    if (fdctrl->fifo[2] > cur_drv->track) {
        fd_seek(cur_drv, cur_drv->head, 0, cur_drv->sect, 1);
    } else {
        fd_seek(cur_drv, cur_drv->head,
                cur_drv->track - fdctrl->fifo[2], cur_drv->sect, 1);
    }
    fdctrl_to_command_phase(fdctrl);
    /* Raise Interrupt */
    fdctrl->status0 |= FD_SR0_SEEK;
    fdctrl_raise_irq(fdctrl);
}

Project: qemu

static int fdc_pre_load(void *opaque)
{
    FDCtrl *s = opaque;
    s->phase = FD_PHASE_RECONSTRUCT;
    return 0;
}

Project: qemu

static FloppyDriveType get_fallback_drive_type(FDrive *drv)
{
    return drv->fdctrl->fallback;
}

Project: qemu

static void fd_change_cb(void *opaque, bool load, Error **errp)
{
    FDrive *drive = opaque;

    if (!load) {
        blk_set_perm(drive->blk, 0, BLK_PERM_ALL, &error_abort);
    } else {
        if (!blkconf_apply_backend_options(drive->conf,
                                           !blk_supports_write_perm(drive->blk),
                                           false, errp)) {
            return;
        }
    }

    drive->media_changed = 1;
    drive->media_validated = false;
    fd_revalidate(drive);
}

Project: qemu

static FDrive *get_cur_drv(FDCtrl *fdctrl)
{
    FDrive *cur_drv = get_drv(fdctrl, fdctrl->cur_drv);

    if (!cur_drv->blk) {
        /*
         * Kludge: empty drive line selected. Create an anonymous
         * BlockBackend to avoid NULL deref with various BlockBackend
         * API calls within this model (CVE-2021-20196).
         * Due to the controller QOM model limitations, we don't
         * attach the created to the controller device.
         */
        cur_drv->blk = blk_create_empty_drive();
    }
    return cur_drv;
}

Project: qemu

static FDriveSize drive_size(FloppyDriveType drive)
{
    switch (drive) {
    case FLOPPY_DRIVE_TYPE_120:
        return FDRIVE_SIZE_525;
    case FLOPPY_DRIVE_TYPE_144:
    case FLOPPY_DRIVE_TYPE_288:
        return FDRIVE_SIZE_350;
    default:
        return FDRIVE_SIZE_UNKNOWN;
    }
}

Project: qemu

static void fdctrl_handle_restore(FDCtrl *fdctrl, int direction)
{
    FDrive *cur_drv = get_cur_drv(fdctrl);

    /* 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];
    cur_drv->last_sect = fdctrl->fifo[9];
    fdctrl->lock = fdctrl->fifo[10] >> 7;
    cur_drv->perpendicular = (fdctrl->fifo[10] >> 2) & 0xF;
    fdctrl->config = fdctrl->fifo[11];
    fdctrl->precomp_trk = fdctrl->fifo[12];
    fdctrl->pwrd = fdctrl->fifo[13];
    fdctrl_to_command_phase(fdctrl);
}

Project: qemu

static void fdctrl_start_transfer(FDCtrl *fdctrl, int direction)
{
    FDrive *cur_drv;
    uint8_t kh, kt, ks;

    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",
                   GET_CUR_DRV(fdctrl), kh, kt, ks,
                   fd_sector_calc(kh, kt, ks, cur_drv->last_sect,
                                  NUM_SIDES(cur_drv)));
    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->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
        return;
    case 3:
        /* track too big */
        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, FD_SR1_EC, 0x00);
        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->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
        return;
    case 1:
        fdctrl->status0 |= FD_SR0_SEEK;
        break;
    default:
        break;
    }

    /* Check the data rate. If the programmed data rate does not match
     * the currently inserted medium, the operation has to fail. */
    if ((fdctrl->dsr & FD_DSR_DRATEMASK) != cur_drv->media_rate) {
        FLOPPY_DPRINTF("data rate mismatch (fdc=%d, media=%d)\n",
                       fdctrl->dsr & FD_DSR_DRATEMASK, cur_drv->media_rate);
        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, FD_SR1_MA, 0x00);
        fdctrl->fifo[3] = kt;
        fdctrl->fifo[4] = kh;
        fdctrl->fifo[5] = ks;
        return;
    }

    /* Set the FIFO state */
    fdctrl->data_dir = direction;
    fdctrl->data_pos = 0;
    assert(fdctrl->msr & FD_MSR_CMDBUSY);
    if (fdctrl->fifo[0] & 0x80)
        fdctrl->data_state |= FD_STATE_MULTI;
    else
        fdctrl->data_state &= ~FD_STATE_MULTI;
    if (fdctrl->fifo[5] == 0) {
        fdctrl->data_len = fdctrl->fifo[8];
    } else {
        int tmp;
        fdctrl->data_len = 128 << (fdctrl->fifo[5] > 7 ? 7 : fdctrl->fifo[5]);
        tmp = (fdctrl->fifo[6] - ks + 1);
        if (tmp < 0) {
            FLOPPY_DPRINTF("invalid EOT: %d\n", tmp);
            fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, FD_SR1_MA, 0x00);
            fdctrl->fifo[3] = kt;
            fdctrl->fifo[4] = kh;
            fdctrl->fifo[5] = ks;
            return;
        }
        if (fdctrl->fifo[0] & 0x80)
            tmp += fdctrl->fifo[6];
        fdctrl->data_len *= tmp;
    }
    fdctrl->eot = fdctrl->fifo[6];
    if (fdctrl->dor & FD_DOR_DMAEN) {
        /* DMA transfer is enabled. */
        IsaDmaClass *k = ISADMA_GET_CLASS(fdctrl->dma);

        FLOPPY_DPRINTF("direction=%d (%d - %d)\n",
                       direction, (128 << fdctrl->fifo[5]) *
                       (cur_drv->last_sect - ks + 1), fdctrl->data_len);

        /* No access is allowed until DMA transfer has completed */
        fdctrl->msr &= ~FD_MSR_RQM;
        if (direction != FD_DIR_VERIFY) {
            /*
             * Now, we just have to wait for the DMA controller to
             * recall us...
             */
            k->hold_DREQ(fdctrl->dma, fdctrl->dma_chann);
            k->schedule(fdctrl->dma);
        } else {
            /* Start transfer */
            fdctrl_transfer_handler(fdctrl, fdctrl->dma_chann, 0,
                    fdctrl->data_len);
        }
        return;
    }
    FLOPPY_DPRINTF("start non-DMA transfer\n");
    fdctrl->msr |= FD_MSR_NONDMA | FD_MSR_RQM;
    if (direction != FD_DIR_WRITE)
        fdctrl->msr |= FD_MSR_DIO;
    /* IO based transfer: calculate len */
    fdctrl_raise_irq(fdctrl);
}

Project: qemu

static void fd_init(FDrive *drv)
{
    /* Drive */
    drv->perpendicular = 0;
    /* Disk */
    drv->disk = FLOPPY_DRIVE_TYPE_NONE;
    drv->last_sect = 0;
    drv->max_track = 0;
    drv->ro = true;
    drv->media_changed = 1;
}

Project: qemu

void fdctrl_realize_common(DeviceState *dev, FDCtrl *fdctrl, Error **errp)
{
    int i, j;
    FDrive *drive;
    static int command_tables_inited = 0;

    if (fdctrl->fallback == FLOPPY_DRIVE_TYPE_AUTO) {
        error_setg(errp, "Cannot choose a fallback FDrive type of 'auto'");
        return;
    }

    /* Fill 'command_to_handler' lookup table */
    if (!command_tables_inited) {
        command_tables_inited = 1;
        for (i = ARRAY_SIZE(handlers) - 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->fifo = qemu_memalign(512, FD_SECTOR_LEN);
    memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
    fdctrl->fifo_size = 512;
    fdctrl->result_timer = timer_new_ns(QEMU_CLOCK_VIRTUAL,
                                             fdctrl_result_timer, fdctrl);

    fdctrl->version = 0x90; /* Intel 82078 controller */
    fdctrl->config = FD_CONFIG_EIS | FD_CONFIG_EFIFO; /* Implicit seek, polling & FIFO enabled */
    fdctrl->num_floppies = MAX_FD;

    floppy_bus_create(fdctrl, &fdctrl->bus, dev);

    for (i = 0; i < MAX_FD; i++) {
        drive = &fdctrl->drives[i];
        drive->fdctrl = fdctrl;
        fd_init(drive);
        fd_revalidate(drive);
    }
}

Project: qemu

static uint32_t fdctrl_read_data(FDCtrl *fdctrl)
{
    FDrive *cur_drv;
    uint32_t retval = 0;
    uint32_t pos;

    cur_drv = get_cur_drv(fdctrl);
    fdctrl->dsr &= ~FD_DSR_PWRDOWN;
    if (!(fdctrl->msr & FD_MSR_RQM) || !(fdctrl->msr & FD_MSR_DIO)) {
        FLOPPY_DPRINTF("error: controller not ready for reading\n");
        return 0;
    }

    /* If data_len spans multiple sectors, the current position in the FIFO
     * wraps around while fdctrl->data_pos is the real position in the whole
     * request. */
    pos = fdctrl->data_pos;
    pos %= FD_SECTOR_LEN;

    switch (fdctrl->phase) {
    case FD_PHASE_EXECUTION:
        assert(fdctrl->msr & FD_MSR_NONDMA);
        if (pos == 0) {
            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 (blk_pread(cur_drv->blk, fd_offset(cur_drv), fdctrl->fifo,
                          BDRV_SECTOR_SIZE)
                < 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);
            }
        }

        if (++fdctrl->data_pos == fdctrl->data_len) {
            fdctrl->msr &= ~FD_MSR_RQM;
            fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
        }
        break;

    case FD_PHASE_RESULT:
        assert(!(fdctrl->msr & FD_MSR_NONDMA));
        if (++fdctrl->data_pos == fdctrl->data_len) {
            fdctrl->msr &= ~FD_MSR_RQM;
            fdctrl_to_command_phase(fdctrl);
            fdctrl_reset_irq(fdctrl);
        }
        break;

    case FD_PHASE_COMMAND:
    default:
        abort();
    }

    retval = fdctrl->fifo[pos];
    FLOPPY_DPRINTF("data register: 0x%02x\n", retval);

    return retval;
}

Project: qemu

static void fdctrl_handle_save(FDCtrl *fdctrl, int direction)
{
    FDrive *cur_drv = get_cur_drv(fdctrl);

    fdctrl->fifo[0] = 0;
    fdctrl->fifo[1] = 0;
    /* 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;
    fdctrl->fifo[8] = cur_drv->last_sect;
    fdctrl->fifo[9] = (fdctrl->lock << 7) |
        (cur_drv->perpendicular << 2);
    fdctrl->fifo[10] = fdctrl->config;
    fdctrl->fifo[11] = fdctrl->precomp_trk;
    fdctrl->fifo[12] = fdctrl->pwrd;
    fdctrl->fifo[13] = 0;
    fdctrl->fifo[14] = 0;
    fdctrl_to_result_phase(fdctrl, 15);
}

Project: qemu

static BlockBackend *blk_create_empty_drive(void)
{
    return blk_new(qemu_get_aio_context(), 0, BLK_PERM_ALL);
}