qemu/hw/block/fdc.c
<<
>>
Prefs
   1/*
   2 * QEMU Floppy disk emulator (Intel 82078)
   3 *
   4 * Copyright (c) 2003, 2007 Jocelyn Mayer
   5 * Copyright (c) 2008 Hervé Poussineau
   6 *
   7 * Permission is hereby granted, free of charge, to any person obtaining a copy
   8 * of this software and associated documentation files (the "Software"), to deal
   9 * in the Software without restriction, including without limitation the rights
  10 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  11 * copies of the Software, and to permit persons to whom the Software is
  12 * furnished to do so, subject to the following conditions:
  13 *
  14 * The above copyright notice and this permission notice shall be included in
  15 * all copies or substantial portions of the Software.
  16 *
  17 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  18 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  19 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
  20 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  21 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  22 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  23 * THE SOFTWARE.
  24 */
  25/*
  26 * The controller is used in Sun4m systems in a slightly different
  27 * way. There are changes in DOR register and DMA is not available.
  28 */
  29
  30#include "qemu/osdep.h"
  31#include "hw/block/fdc.h"
  32#include "qapi/error.h"
  33#include "qemu/error-report.h"
  34#include "qemu/timer.h"
  35#include "hw/irq.h"
  36#include "hw/isa/isa.h"
  37#include "hw/qdev-properties.h"
  38#include "hw/qdev-properties-system.h"
  39#include "migration/vmstate.h"
  40#include "hw/block/block.h"
  41#include "sysemu/block-backend.h"
  42#include "sysemu/blockdev.h"
  43#include "sysemu/sysemu.h"
  44#include "qemu/log.h"
  45#include "qemu/main-loop.h"
  46#include "qemu/module.h"
  47#include "trace.h"
  48#include "qom/object.h"
  49#include "fdc-internal.h"
  50
  51/********************************************************/
  52/* debug Floppy devices */
  53
  54#define DEBUG_FLOPPY 0
  55
  56#define FLOPPY_DPRINTF(fmt, ...)                                \
  57    do {                                                        \
  58        if (DEBUG_FLOPPY) {                                     \
  59            fprintf(stderr, "FLOPPY: " fmt , ## __VA_ARGS__);   \
  60        }                                                       \
  61    } while (0)
  62
  63
  64/* Anonymous BlockBackend for empty drive */
  65static BlockBackend *blk_create_empty_drive(void)
  66{
  67    return blk_new(qemu_get_aio_context(), 0, BLK_PERM_ALL);
  68}
  69
  70/********************************************************/
  71/* qdev floppy bus                                      */
  72
  73#define TYPE_FLOPPY_BUS "floppy-bus"
  74OBJECT_DECLARE_SIMPLE_TYPE(FloppyBus, FLOPPY_BUS)
  75
  76static FDrive *get_drv(FDCtrl *fdctrl, int unit);
  77
  78static const TypeInfo floppy_bus_info = {
  79    .name = TYPE_FLOPPY_BUS,
  80    .parent = TYPE_BUS,
  81    .instance_size = sizeof(FloppyBus),
  82};
  83
  84static void floppy_bus_create(FDCtrl *fdc, FloppyBus *bus, DeviceState *dev)
  85{
  86    qbus_init(bus, sizeof(FloppyBus), TYPE_FLOPPY_BUS, dev, NULL);
  87    bus->fdc = fdc;
  88}
  89
  90
  91/********************************************************/
  92/* Floppy drive emulation                               */
  93
  94/* In many cases, the total sector size of a format is enough to uniquely
  95 * identify it. However, there are some total sector collisions between
  96 * formats of different physical size, and these are noted below by
  97 * highlighting the total sector size for entries with collisions. */
  98const FDFormat fd_formats[] = {
  99    /* First entry is default format */
 100    /* 1.44 MB 3"1/2 floppy disks */
 101    { FLOPPY_DRIVE_TYPE_144, 18, 80, 1, FDRIVE_RATE_500K, }, /* 3.5" 2880 */
 102    { FLOPPY_DRIVE_TYPE_144, 20, 80, 1, FDRIVE_RATE_500K, }, /* 3.5" 3200 */
 103    { FLOPPY_DRIVE_TYPE_144, 21, 80, 1, FDRIVE_RATE_500K, },
 104    { FLOPPY_DRIVE_TYPE_144, 21, 82, 1, FDRIVE_RATE_500K, },
 105    { FLOPPY_DRIVE_TYPE_144, 21, 83, 1, FDRIVE_RATE_500K, },
 106    { FLOPPY_DRIVE_TYPE_144, 22, 80, 1, FDRIVE_RATE_500K, },
 107    { FLOPPY_DRIVE_TYPE_144, 23, 80, 1, FDRIVE_RATE_500K, },
 108    { FLOPPY_DRIVE_TYPE_144, 24, 80, 1, FDRIVE_RATE_500K, },
 109    /* 2.88 MB 3"1/2 floppy disks */
 110    { FLOPPY_DRIVE_TYPE_288, 36, 80, 1, FDRIVE_RATE_1M, },
 111    { FLOPPY_DRIVE_TYPE_288, 39, 80, 1, FDRIVE_RATE_1M, },
 112    { FLOPPY_DRIVE_TYPE_288, 40, 80, 1, FDRIVE_RATE_1M, },
 113    { FLOPPY_DRIVE_TYPE_288, 44, 80, 1, FDRIVE_RATE_1M, },
 114    { FLOPPY_DRIVE_TYPE_288, 48, 80, 1, FDRIVE_RATE_1M, },
 115    /* 720 kB 3"1/2 floppy disks */
 116    { FLOPPY_DRIVE_TYPE_144,  9, 80, 1, FDRIVE_RATE_250K, }, /* 3.5" 1440 */
 117    { FLOPPY_DRIVE_TYPE_144, 10, 80, 1, FDRIVE_RATE_250K, },
 118    { FLOPPY_DRIVE_TYPE_144, 10, 82, 1, FDRIVE_RATE_250K, },
 119    { FLOPPY_DRIVE_TYPE_144, 10, 83, 1, FDRIVE_RATE_250K, },
 120    { FLOPPY_DRIVE_TYPE_144, 13, 80, 1, FDRIVE_RATE_250K, },
 121    { FLOPPY_DRIVE_TYPE_144, 14, 80, 1, FDRIVE_RATE_250K, },
 122    /* 1.2 MB 5"1/4 floppy disks */
 123    { FLOPPY_DRIVE_TYPE_120, 15, 80, 1, FDRIVE_RATE_500K, },
 124    { FLOPPY_DRIVE_TYPE_120, 18, 80, 1, FDRIVE_RATE_500K, }, /* 5.25" 2880 */
 125    { FLOPPY_DRIVE_TYPE_120, 18, 82, 1, FDRIVE_RATE_500K, },
 126    { FLOPPY_DRIVE_TYPE_120, 18, 83, 1, FDRIVE_RATE_500K, },
 127    { FLOPPY_DRIVE_TYPE_120, 20, 80, 1, FDRIVE_RATE_500K, }, /* 5.25" 3200 */
 128    /* 720 kB 5"1/4 floppy disks */
 129    { FLOPPY_DRIVE_TYPE_120,  9, 80, 1, FDRIVE_RATE_250K, }, /* 5.25" 1440 */
 130    { FLOPPY_DRIVE_TYPE_120, 11, 80, 1, FDRIVE_RATE_250K, },
 131    /* 360 kB 5"1/4 floppy disks */
 132    { FLOPPY_DRIVE_TYPE_120,  9, 40, 1, FDRIVE_RATE_300K, }, /* 5.25" 720 */
 133    { FLOPPY_DRIVE_TYPE_120,  9, 40, 0, FDRIVE_RATE_300K, },
 134    { FLOPPY_DRIVE_TYPE_120, 10, 41, 1, FDRIVE_RATE_300K, },
 135    { FLOPPY_DRIVE_TYPE_120, 10, 42, 1, FDRIVE_RATE_300K, },
 136    /* 320 kB 5"1/4 floppy disks */
 137    { FLOPPY_DRIVE_TYPE_120,  8, 40, 1, FDRIVE_RATE_250K, },
 138    { FLOPPY_DRIVE_TYPE_120,  8, 40, 0, FDRIVE_RATE_250K, },
 139    /* 360 kB must match 5"1/4 better than 3"1/2... */
 140    { FLOPPY_DRIVE_TYPE_144,  9, 80, 0, FDRIVE_RATE_250K, }, /* 3.5" 720 */
 141    /* end */
 142    { FLOPPY_DRIVE_TYPE_NONE, -1, -1, 0, 0, },
 143};
 144
 145static FDriveSize drive_size(FloppyDriveType drive)
 146{
 147    switch (drive) {
 148    case FLOPPY_DRIVE_TYPE_120:
 149        return FDRIVE_SIZE_525;
 150    case FLOPPY_DRIVE_TYPE_144:
 151    case FLOPPY_DRIVE_TYPE_288:
 152        return FDRIVE_SIZE_350;
 153    default:
 154        return FDRIVE_SIZE_UNKNOWN;
 155    }
 156}
 157
 158#define GET_CUR_DRV(fdctrl) ((fdctrl)->cur_drv)
 159#define SET_CUR_DRV(fdctrl, drive) ((fdctrl)->cur_drv = (drive))
 160
 161/* Will always be a fixed parameter for us */
 162#define FD_SECTOR_LEN          512
 163#define FD_SECTOR_SC           2   /* Sector size code */
 164#define FD_RESET_SENSEI_COUNT  4   /* Number of sense interrupts on RESET */
 165
 166
 167static FloppyDriveType get_fallback_drive_type(FDrive *drv);
 168
 169/* Hack: FD_SEEK is expected to work on empty drives. However, QEMU
 170 * currently goes through some pains to keep seeks within the bounds
 171 * established by last_sect and max_track. Correcting this is difficult,
 172 * as refactoring FDC code tends to expose nasty bugs in the Linux kernel.
 173 *
 174 * For now: allow empty drives to have large bounds so we can seek around,
 175 * with the understanding that when a diskette is inserted, the bounds will
 176 * properly tighten to match the geometry of that inserted medium.
 177 */
 178static void fd_empty_seek_hack(FDrive *drv)
 179{
 180    drv->last_sect = 0xFF;
 181    drv->max_track = 0xFF;
 182}
 183
 184static void fd_init(FDrive *drv)
 185{
 186    /* Drive */
 187    drv->perpendicular = 0;
 188    /* Disk */
 189    drv->disk = FLOPPY_DRIVE_TYPE_NONE;
 190    drv->last_sect = 0;
 191    drv->max_track = 0;
 192    drv->ro = true;
 193    drv->media_changed = 1;
 194}
 195
 196#define NUM_SIDES(drv) ((drv)->flags & FDISK_DBL_SIDES ? 2 : 1)
 197
 198static int fd_sector_calc(uint8_t head, uint8_t track, uint8_t sect,
 199                          uint8_t last_sect, uint8_t num_sides)
 200{
 201    return (((track * num_sides) + head) * last_sect) + sect - 1;
 202}
 203
 204/* Returns current position, in sectors, for given drive */
 205static int fd_sector(FDrive *drv)
 206{
 207    return fd_sector_calc(drv->head, drv->track, drv->sect, drv->last_sect,
 208                          NUM_SIDES(drv));
 209}
 210
 211/* Returns current position, in bytes, for given drive */
 212static int fd_offset(FDrive *drv)
 213{
 214    g_assert(fd_sector(drv) < INT_MAX >> BDRV_SECTOR_BITS);
 215    return fd_sector(drv) << BDRV_SECTOR_BITS;
 216}
 217
 218/* Seek to a new position:
 219 * returns 0 if already on right track
 220 * returns 1 if track changed
 221 * returns 2 if track is invalid
 222 * returns 3 if sector is invalid
 223 * returns 4 if seek is disabled
 224 */
 225static int fd_seek(FDrive *drv, uint8_t head, uint8_t track, uint8_t sect,
 226                   int enable_seek)
 227{
 228    uint32_t sector;
 229    int ret;
 230
 231    if (track > drv->max_track ||
 232        (head != 0 && (drv->flags & FDISK_DBL_SIDES) == 0)) {
 233        FLOPPY_DPRINTF("try to read %d %02x %02x (max=%d %d %02x %02x)\n",
 234                       head, track, sect, 1,
 235                       (drv->flags & FDISK_DBL_SIDES) == 0 ? 0 : 1,
 236                       drv->max_track, drv->last_sect);
 237        return 2;
 238    }
 239    if (sect > drv->last_sect) {
 240        FLOPPY_DPRINTF("try to read %d %02x %02x (max=%d %d %02x %02x)\n",
 241                       head, track, sect, 1,
 242                       (drv->flags & FDISK_DBL_SIDES) == 0 ? 0 : 1,
 243                       drv->max_track, drv->last_sect);
 244        return 3;
 245    }
 246    sector = fd_sector_calc(head, track, sect, drv->last_sect, NUM_SIDES(drv));
 247    ret = 0;
 248    if (sector != fd_sector(drv)) {
 249#if 0
 250        if (!enable_seek) {
 251            FLOPPY_DPRINTF("error: no implicit seek %d %02x %02x"
 252                           " (max=%d %02x %02x)\n",
 253                           head, track, sect, 1, drv->max_track,
 254                           drv->last_sect);
 255            return 4;
 256        }
 257#endif
 258        drv->head = head;
 259        if (drv->track != track) {
 260            if (drv->blk != NULL && blk_is_inserted(drv->blk)) {
 261                drv->media_changed = 0;
 262            }
 263            ret = 1;
 264        }
 265        drv->track = track;
 266        drv->sect = sect;
 267    }
 268
 269    if (drv->blk == NULL || !blk_is_inserted(drv->blk)) {
 270        ret = 2;
 271    }
 272
 273    return ret;
 274}
 275
 276/* Set drive back to track 0 */
 277static void fd_recalibrate(FDrive *drv)
 278{
 279    FLOPPY_DPRINTF("recalibrate\n");
 280    fd_seek(drv, 0, 0, 1, 1);
 281}
 282
 283/**
 284 * Determine geometry based on inserted diskette.
 285 * Will not operate on an empty drive.
 286 *
 287 * @return: 0 on success, -1 if the drive is empty.
 288 */
 289static int pick_geometry(FDrive *drv)
 290{
 291    BlockBackend *blk = drv->blk;
 292    const FDFormat *parse;
 293    uint64_t nb_sectors, size;
 294    int i;
 295    int match, size_match, type_match;
 296    bool magic = drv->drive == FLOPPY_DRIVE_TYPE_AUTO;
 297
 298    /* We can only pick a geometry if we have a diskette. */
 299    if (!drv->blk || !blk_is_inserted(drv->blk) ||
 300        drv->drive == FLOPPY_DRIVE_TYPE_NONE)
 301    {
 302        return -1;
 303    }
 304
 305    /* We need to determine the likely geometry of the inserted medium.
 306     * In order of preference, we look for:
 307     * (1) The same drive type and number of sectors,
 308     * (2) The same diskette size and number of sectors,
 309     * (3) The same drive type.
 310     *
 311     * In all cases, matches that occur higher in the drive table will take
 312     * precedence over matches that occur later in the table.
 313     */
 314    blk_get_geometry(blk, &nb_sectors);
 315    match = size_match = type_match = -1;
 316    for (i = 0; ; i++) {
 317        parse = &fd_formats[i];
 318        if (parse->drive == FLOPPY_DRIVE_TYPE_NONE) {
 319            break;
 320        }
 321        size = (parse->max_head + 1) * parse->max_track * parse->last_sect;
 322        if (nb_sectors == size) {
 323            if (magic || parse->drive == drv->drive) {
 324                /* (1) perfect match -- nb_sectors and drive type */
 325                goto out;
 326            } else if (drive_size(parse->drive) == drive_size(drv->drive)) {
 327                /* (2) size match -- nb_sectors and physical medium size */
 328                match = (match == -1) ? i : match;
 329            } else {
 330                /* This is suspicious -- Did the user misconfigure? */
 331                size_match = (size_match == -1) ? i : size_match;
 332            }
 333        } else if (type_match == -1) {
 334            if ((parse->drive == drv->drive) ||
 335                (magic && (parse->drive == get_fallback_drive_type(drv)))) {
 336                /* (3) type match -- nb_sectors mismatch, but matches the type
 337                 *     specified explicitly by the user, or matches the fallback
 338                 *     default type when using the drive autodetect mechanism */
 339                type_match = i;
 340            }
 341        }
 342    }
 343
 344    /* No exact match found */
 345    if (match == -1) {
 346        if (size_match != -1) {
 347            parse = &fd_formats[size_match];
 348            FLOPPY_DPRINTF("User requested floppy drive type '%s', "
 349                           "but inserted medium appears to be a "
 350                           "%"PRId64" sector '%s' type\n",
 351                           FloppyDriveType_str(drv->drive),
 352                           nb_sectors,
 353                           FloppyDriveType_str(parse->drive));
 354        }
 355        assert(type_match != -1 && "misconfigured fd_format");
 356        match = type_match;
 357    }
 358    parse = &(fd_formats[match]);
 359
 360 out:
 361    if (parse->max_head == 0) {
 362        drv->flags &= ~FDISK_DBL_SIDES;
 363    } else {
 364        drv->flags |= FDISK_DBL_SIDES;
 365    }
 366    drv->max_track = parse->max_track;
 367    drv->last_sect = parse->last_sect;
 368    drv->disk = parse->drive;
 369    drv->media_rate = parse->rate;
 370    return 0;
 371}
 372
 373static void pick_drive_type(FDrive *drv)
 374{
 375    if (drv->drive != FLOPPY_DRIVE_TYPE_AUTO) {
 376        return;
 377    }
 378
 379    if (pick_geometry(drv) == 0) {
 380        drv->drive = drv->disk;
 381    } else {
 382        drv->drive = get_fallback_drive_type(drv);
 383    }
 384
 385    g_assert(drv->drive != FLOPPY_DRIVE_TYPE_AUTO);
 386}
 387
 388/* Revalidate a disk drive after a disk change */
 389static void fd_revalidate(FDrive *drv)
 390{
 391    int rc;
 392
 393    FLOPPY_DPRINTF("revalidate\n");
 394    if (drv->blk != NULL) {
 395        drv->ro = !blk_is_writable(drv->blk);
 396        if (!blk_is_inserted(drv->blk)) {
 397            FLOPPY_DPRINTF("No disk in drive\n");
 398            drv->disk = FLOPPY_DRIVE_TYPE_NONE;
 399            fd_empty_seek_hack(drv);
 400        } else if (!drv->media_validated) {
 401            rc = pick_geometry(drv);
 402            if (rc) {
 403                FLOPPY_DPRINTF("Could not validate floppy drive media");
 404            } else {
 405                drv->media_validated = true;
 406                FLOPPY_DPRINTF("Floppy disk (%d h %d t %d s) %s\n",
 407                               (drv->flags & FDISK_DBL_SIDES) ? 2 : 1,
 408                               drv->max_track, drv->last_sect,
 409                               drv->ro ? "ro" : "rw");
 410            }
 411        }
 412    } else {
 413        FLOPPY_DPRINTF("No drive connected\n");
 414        drv->last_sect = 0;
 415        drv->max_track = 0;
 416        drv->flags &= ~FDISK_DBL_SIDES;
 417        drv->drive = FLOPPY_DRIVE_TYPE_NONE;
 418        drv->disk = FLOPPY_DRIVE_TYPE_NONE;
 419    }
 420}
 421
 422static void fd_change_cb(void *opaque, bool load, Error **errp)
 423{
 424    FDrive *drive = opaque;
 425
 426    if (!load) {
 427        blk_set_perm(drive->blk, 0, BLK_PERM_ALL, &error_abort);
 428    } else {
 429        if (!blkconf_apply_backend_options(drive->conf,
 430                                           !blk_supports_write_perm(drive->blk),
 431                                           false, errp)) {
 432            return;
 433        }
 434    }
 435
 436    drive->media_changed = 1;
 437    drive->media_validated = false;
 438    fd_revalidate(drive);
 439}
 440
 441static const BlockDevOps fd_block_ops = {
 442    .change_media_cb = fd_change_cb,
 443};
 444
 445
 446#define TYPE_FLOPPY_DRIVE "floppy"
 447OBJECT_DECLARE_SIMPLE_TYPE(FloppyDrive, FLOPPY_DRIVE)
 448
 449struct FloppyDrive {
 450    DeviceState     qdev;
 451    uint32_t        unit;
 452    BlockConf       conf;
 453    FloppyDriveType type;
 454};
 455
 456static Property floppy_drive_properties[] = {
 457    DEFINE_PROP_UINT32("unit", FloppyDrive, unit, -1),
 458    DEFINE_BLOCK_PROPERTIES(FloppyDrive, conf),
 459    DEFINE_PROP_SIGNED("drive-type", FloppyDrive, type,
 460                        FLOPPY_DRIVE_TYPE_AUTO, qdev_prop_fdc_drive_type,
 461                        FloppyDriveType),
 462    DEFINE_PROP_END_OF_LIST(),
 463};
 464
 465static void floppy_drive_realize(DeviceState *qdev, Error **errp)
 466{
 467    FloppyDrive *dev = FLOPPY_DRIVE(qdev);
 468    FloppyBus *bus = FLOPPY_BUS(qdev->parent_bus);
 469    FDrive *drive;
 470    bool read_only;
 471    int ret;
 472
 473    if (dev->unit == -1) {
 474        for (dev->unit = 0; dev->unit < MAX_FD; dev->unit++) {
 475            drive = get_drv(bus->fdc, dev->unit);
 476            if (!drive->blk) {
 477                break;
 478            }
 479        }
 480    }
 481
 482    if (dev->unit >= MAX_FD) {
 483        error_setg(errp, "Can't create floppy unit %d, bus supports "
 484                   "only %d units", dev->unit, MAX_FD);
 485        return;
 486    }
 487
 488    drive = get_drv(bus->fdc, dev->unit);
 489    if (drive->blk) {
 490        error_setg(errp, "Floppy unit %d is in use", dev->unit);
 491        return;
 492    }
 493
 494    if (!dev->conf.blk) {
 495        dev->conf.blk = blk_create_empty_drive();
 496        ret = blk_attach_dev(dev->conf.blk, qdev);
 497        assert(ret == 0);
 498
 499        /* Don't take write permissions on an empty drive to allow attaching a
 500         * read-only node later */
 501        read_only = true;
 502    } else {
 503        read_only = !blk_bs(dev->conf.blk) ||
 504                    !blk_supports_write_perm(dev->conf.blk);
 505    }
 506
 507    if (!blkconf_blocksizes(&dev->conf, errp)) {
 508        return;
 509    }
 510
 511    if (dev->conf.logical_block_size != 512 ||
 512        dev->conf.physical_block_size != 512)
 513    {
 514        error_setg(errp, "Physical and logical block size must "
 515                   "be 512 for floppy");
 516        return;
 517    }
 518
 519    /* rerror/werror aren't supported by fdc and therefore not even registered
 520     * with qdev. So set the defaults manually before they are used in
 521     * blkconf_apply_backend_options(). */
 522    dev->conf.rerror = BLOCKDEV_ON_ERROR_AUTO;
 523    dev->conf.werror = BLOCKDEV_ON_ERROR_AUTO;
 524
 525    if (!blkconf_apply_backend_options(&dev->conf, read_only, false, errp)) {
 526        return;
 527    }
 528
 529    /* 'enospc' is the default for -drive, 'report' is what blk_new() gives us
 530     * for empty drives. */
 531    if (blk_get_on_error(dev->conf.blk, 0) != BLOCKDEV_ON_ERROR_ENOSPC &&
 532        blk_get_on_error(dev->conf.blk, 0) != BLOCKDEV_ON_ERROR_REPORT) {
 533        error_setg(errp, "fdc doesn't support drive option werror");
 534        return;
 535    }
 536    if (blk_get_on_error(dev->conf.blk, 1) != BLOCKDEV_ON_ERROR_REPORT) {
 537        error_setg(errp, "fdc doesn't support drive option rerror");
 538        return;
 539    }
 540
 541    drive->conf = &dev->conf;
 542    drive->blk = dev->conf.blk;
 543    drive->fdctrl = bus->fdc;
 544
 545    fd_init(drive);
 546    blk_set_dev_ops(drive->blk, &fd_block_ops, drive);
 547
 548    /* Keep 'type' qdev property and FDrive->drive in sync */
 549    drive->drive = dev->type;
 550    pick_drive_type(drive);
 551    dev->type = drive->drive;
 552
 553    fd_revalidate(drive);
 554}
 555
 556static void floppy_drive_class_init(ObjectClass *klass, void *data)
 557{
 558    DeviceClass *k = DEVICE_CLASS(klass);
 559    k->realize = floppy_drive_realize;
 560    set_bit(DEVICE_CATEGORY_STORAGE, k->categories);
 561    k->bus_type = TYPE_FLOPPY_BUS;
 562    device_class_set_props(k, floppy_drive_properties);
 563    k->desc = "virtual floppy drive";
 564}
 565
 566static const TypeInfo floppy_drive_info = {
 567    .name = TYPE_FLOPPY_DRIVE,
 568    .parent = TYPE_DEVICE,
 569    .instance_size = sizeof(FloppyDrive),
 570    .class_init = floppy_drive_class_init,
 571};
 572
 573/********************************************************/
 574/* Intel 82078 floppy disk controller emulation          */
 575
 576static void fdctrl_to_command_phase(FDCtrl *fdctrl);
 577static void fdctrl_raise_irq(FDCtrl *fdctrl);
 578static FDrive *get_cur_drv(FDCtrl *fdctrl);
 579
 580static uint32_t fdctrl_read_statusA(FDCtrl *fdctrl);
 581static uint32_t fdctrl_read_statusB(FDCtrl *fdctrl);
 582static uint32_t fdctrl_read_dor(FDCtrl *fdctrl);
 583static void fdctrl_write_dor(FDCtrl *fdctrl, uint32_t value);
 584static uint32_t fdctrl_read_tape(FDCtrl *fdctrl);
 585static void fdctrl_write_tape(FDCtrl *fdctrl, uint32_t value);
 586static uint32_t fdctrl_read_main_status(FDCtrl *fdctrl);
 587static void fdctrl_write_rate(FDCtrl *fdctrl, uint32_t value);
 588static uint32_t fdctrl_read_data(FDCtrl *fdctrl);
 589static void fdctrl_write_data(FDCtrl *fdctrl, uint32_t value);
 590static uint32_t fdctrl_read_dir(FDCtrl *fdctrl);
 591static void fdctrl_write_ccr(FDCtrl *fdctrl, uint32_t value);
 592
 593enum {
 594    FD_DIR_WRITE   = 0,
 595    FD_DIR_READ    = 1,
 596    FD_DIR_SCANE   = 2,
 597    FD_DIR_SCANL   = 3,
 598    FD_DIR_SCANH   = 4,
 599    FD_DIR_VERIFY  = 5,
 600};
 601
 602enum {
 603    FD_STATE_MULTI  = 0x01,     /* multi track flag */
 604    FD_STATE_FORMAT = 0x02,     /* format flag */
 605};
 606
 607enum {
 608    FD_REG_SRA = 0x00,
 609    FD_REG_SRB = 0x01,
 610    FD_REG_DOR = 0x02,
 611    FD_REG_TDR = 0x03,
 612    FD_REG_MSR = 0x04,
 613    FD_REG_DSR = 0x04,
 614    FD_REG_FIFO = 0x05,
 615    FD_REG_DIR = 0x07,
 616    FD_REG_CCR = 0x07,
 617};
 618
 619enum {
 620    FD_CMD_READ_TRACK = 0x02,
 621    FD_CMD_SPECIFY = 0x03,
 622    FD_CMD_SENSE_DRIVE_STATUS = 0x04,
 623    FD_CMD_WRITE = 0x05,
 624    FD_CMD_READ = 0x06,
 625    FD_CMD_RECALIBRATE = 0x07,
 626    FD_CMD_SENSE_INTERRUPT_STATUS = 0x08,
 627    FD_CMD_WRITE_DELETED = 0x09,
 628    FD_CMD_READ_ID = 0x0a,
 629    FD_CMD_READ_DELETED = 0x0c,
 630    FD_CMD_FORMAT_TRACK = 0x0d,
 631    FD_CMD_DUMPREG = 0x0e,
 632    FD_CMD_SEEK = 0x0f,
 633    FD_CMD_VERSION = 0x10,
 634    FD_CMD_SCAN_EQUAL = 0x11,
 635    FD_CMD_PERPENDICULAR_MODE = 0x12,
 636    FD_CMD_CONFIGURE = 0x13,
 637    FD_CMD_LOCK = 0x14,
 638    FD_CMD_VERIFY = 0x16,
 639    FD_CMD_POWERDOWN_MODE = 0x17,
 640    FD_CMD_PART_ID = 0x18,
 641    FD_CMD_SCAN_LOW_OR_EQUAL = 0x19,
 642    FD_CMD_SCAN_HIGH_OR_EQUAL = 0x1d,
 643    FD_CMD_SAVE = 0x2e,
 644    FD_CMD_OPTION = 0x33,
 645    FD_CMD_RESTORE = 0x4e,
 646    FD_CMD_DRIVE_SPECIFICATION_COMMAND = 0x8e,
 647    FD_CMD_RELATIVE_SEEK_OUT = 0x8f,
 648    FD_CMD_FORMAT_AND_WRITE = 0xcd,
 649    FD_CMD_RELATIVE_SEEK_IN = 0xcf,
 650};
 651
 652enum {
 653    FD_CONFIG_PRETRK = 0xff, /* Pre-compensation set to track 0 */
 654    FD_CONFIG_FIFOTHR = 0x0f, /* FIFO threshold set to 1 byte */
 655    FD_CONFIG_POLL  = 0x10, /* Poll enabled */
 656    FD_CONFIG_EFIFO = 0x20, /* FIFO disabled */
 657    FD_CONFIG_EIS   = 0x40, /* No implied seeks */
 658};
 659
 660enum {
 661    FD_SR0_DS0      = 0x01,
 662    FD_SR0_DS1      = 0x02,
 663    FD_SR0_HEAD     = 0x04,
 664    FD_SR0_EQPMT    = 0x10,
 665    FD_SR0_SEEK     = 0x20,
 666    FD_SR0_ABNTERM  = 0x40,
 667    FD_SR0_INVCMD   = 0x80,
 668    FD_SR0_RDYCHG   = 0xc0,
 669};
 670
 671enum {
 672    FD_SR1_MA       = 0x01, /* Missing address mark */
 673    FD_SR1_NW       = 0x02, /* Not writable */
 674    FD_SR1_EC       = 0x80, /* End of cylinder */
 675};
 676
 677enum {
 678    FD_SR2_SNS      = 0x04, /* Scan not satisfied */
 679    FD_SR2_SEH      = 0x08, /* Scan equal hit */
 680};
 681
 682enum {
 683    FD_SRA_DIR      = 0x01,
 684    FD_SRA_nWP      = 0x02,
 685    FD_SRA_nINDX    = 0x04,
 686    FD_SRA_HDSEL    = 0x08,
 687    FD_SRA_nTRK0    = 0x10,
 688    FD_SRA_STEP     = 0x20,
 689    FD_SRA_nDRV2    = 0x40,
 690    FD_SRA_INTPEND  = 0x80,
 691};
 692
 693enum {
 694    FD_SRB_MTR0     = 0x01,
 695    FD_SRB_MTR1     = 0x02,
 696    FD_SRB_WGATE    = 0x04,
 697    FD_SRB_RDATA    = 0x08,
 698    FD_SRB_WDATA    = 0x10,
 699    FD_SRB_DR0      = 0x20,
 700};
 701
 702enum {
 703#if MAX_FD == 4
 704    FD_DOR_SELMASK  = 0x03,
 705#else
 706    FD_DOR_SELMASK  = 0x01,
 707#endif
 708    FD_DOR_nRESET   = 0x04,
 709    FD_DOR_DMAEN    = 0x08,
 710    FD_DOR_MOTEN0   = 0x10,
 711    FD_DOR_MOTEN1   = 0x20,
 712    FD_DOR_MOTEN2   = 0x40,
 713    FD_DOR_MOTEN3   = 0x80,
 714};
 715
 716enum {
 717#if MAX_FD == 4
 718    FD_TDR_BOOTSEL  = 0x0c,
 719#else
 720    FD_TDR_BOOTSEL  = 0x04,
 721#endif
 722};
 723
 724enum {
 725    FD_DSR_DRATEMASK= 0x03,
 726    FD_DSR_PWRDOWN  = 0x40,
 727    FD_DSR_SWRESET  = 0x80,
 728};
 729
 730enum {
 731    FD_MSR_DRV0BUSY = 0x01,
 732    FD_MSR_DRV1BUSY = 0x02,
 733    FD_MSR_DRV2BUSY = 0x04,
 734    FD_MSR_DRV3BUSY = 0x08,
 735    FD_MSR_CMDBUSY  = 0x10,
 736    FD_MSR_NONDMA   = 0x20,
 737    FD_MSR_DIO      = 0x40,
 738    FD_MSR_RQM      = 0x80,
 739};
 740
 741enum {
 742    FD_DIR_DSKCHG   = 0x80,
 743};
 744
 745/*
 746 * See chapter 5.0 "Controller phases" of the spec:
 747 *
 748 * Command phase:
 749 * The host writes a command and its parameters into the FIFO. The command
 750 * phase is completed when all parameters for the command have been supplied,
 751 * and execution phase is entered.
 752 *
 753 * Execution phase:
 754 * Data transfers, either DMA or non-DMA. For non-DMA transfers, the FIFO
 755 * contains the payload now, otherwise it's unused. When all bytes of the
 756 * required data have been transferred, the state is switched to either result
 757 * phase (if the command produces status bytes) or directly back into the
 758 * command phase for the next command.
 759 *
 760 * Result phase:
 761 * The host reads out the FIFO, which contains one or more result bytes now.
 762 */
 763enum {
 764    /* Only for migration: reconstruct phase from registers like qemu 2.3 */
 765    FD_PHASE_RECONSTRUCT    = 0,
 766
 767    FD_PHASE_COMMAND        = 1,
 768    FD_PHASE_EXECUTION      = 2,
 769    FD_PHASE_RESULT         = 3,
 770};
 771
 772#define FD_MULTI_TRACK(state) ((state) & FD_STATE_MULTI)
 773#define FD_FORMAT_CMD(state) ((state) & FD_STATE_FORMAT)
 774
 775static FloppyDriveType get_fallback_drive_type(FDrive *drv)
 776{
 777    return drv->fdctrl->fallback;
 778}
 779
 780uint32_t fdctrl_read(void *opaque, uint32_t reg)
 781{
 782    FDCtrl *fdctrl = opaque;
 783    uint32_t retval;
 784
 785    reg &= 7;
 786    switch (reg) {
 787    case FD_REG_SRA:
 788        retval = fdctrl_read_statusA(fdctrl);
 789        break;
 790    case FD_REG_SRB:
 791        retval = fdctrl_read_statusB(fdctrl);
 792        break;
 793    case FD_REG_DOR:
 794        retval = fdctrl_read_dor(fdctrl);
 795        break;
 796    case FD_REG_TDR:
 797        retval = fdctrl_read_tape(fdctrl);
 798        break;
 799    case FD_REG_MSR:
 800        retval = fdctrl_read_main_status(fdctrl);
 801        break;
 802    case FD_REG_FIFO:
 803        retval = fdctrl_read_data(fdctrl);
 804        break;
 805    case FD_REG_DIR:
 806        retval = fdctrl_read_dir(fdctrl);
 807        break;
 808    default:
 809        retval = (uint32_t)(-1);
 810        break;
 811    }
 812    trace_fdc_ioport_read(reg, retval);
 813
 814    return retval;
 815}
 816
 817void fdctrl_write(void *opaque, uint32_t reg, uint32_t value)
 818{
 819    FDCtrl *fdctrl = opaque;
 820
 821    reg &= 7;
 822    trace_fdc_ioport_write(reg, value);
 823    switch (reg) {
 824    case FD_REG_DOR:
 825        fdctrl_write_dor(fdctrl, value);
 826        break;
 827    case FD_REG_TDR:
 828        fdctrl_write_tape(fdctrl, value);
 829        break;
 830    case FD_REG_DSR:
 831        fdctrl_write_rate(fdctrl, value);
 832        break;
 833    case FD_REG_FIFO:
 834        fdctrl_write_data(fdctrl, value);
 835        break;
 836    case FD_REG_CCR:
 837        fdctrl_write_ccr(fdctrl, value);
 838        break;
 839    default:
 840        break;
 841    }
 842}
 843
 844static bool fdrive_media_changed_needed(void *opaque)
 845{
 846    FDrive *drive = opaque;
 847
 848    return (drive->blk != NULL && drive->media_changed != 1);
 849}
 850
 851static const VMStateDescription vmstate_fdrive_media_changed = {
 852    .name = "fdrive/media_changed",
 853    .version_id = 1,
 854    .minimum_version_id = 1,
 855    .needed = fdrive_media_changed_needed,
 856    .fields = (VMStateField[]) {
 857        VMSTATE_UINT8(media_changed, FDrive),
 858        VMSTATE_END_OF_LIST()
 859    }
 860};
 861
 862static const VMStateDescription vmstate_fdrive_media_rate = {
 863    .name = "fdrive/media_rate",
 864    .version_id = 1,
 865    .minimum_version_id = 1,
 866    .fields = (VMStateField[]) {
 867        VMSTATE_UINT8(media_rate, FDrive),
 868        VMSTATE_END_OF_LIST()
 869    }
 870};
 871
 872static bool fdrive_perpendicular_needed(void *opaque)
 873{
 874    FDrive *drive = opaque;
 875
 876    return drive->perpendicular != 0;
 877}
 878
 879static const VMStateDescription vmstate_fdrive_perpendicular = {
 880    .name = "fdrive/perpendicular",
 881    .version_id = 1,
 882    .minimum_version_id = 1,
 883    .needed = fdrive_perpendicular_needed,
 884    .fields = (VMStateField[]) {
 885        VMSTATE_UINT8(perpendicular, FDrive),
 886        VMSTATE_END_OF_LIST()
 887    }
 888};
 889
 890static int fdrive_post_load(void *opaque, int version_id)
 891{
 892    fd_revalidate(opaque);
 893    return 0;
 894}
 895
 896static const VMStateDescription vmstate_fdrive = {
 897    .name = "fdrive",
 898    .version_id = 1,
 899    .minimum_version_id = 1,
 900    .post_load = fdrive_post_load,
 901    .fields = (VMStateField[]) {
 902        VMSTATE_UINT8(head, FDrive),
 903        VMSTATE_UINT8(track, FDrive),
 904        VMSTATE_UINT8(sect, FDrive),
 905        VMSTATE_END_OF_LIST()
 906    },
 907    .subsections = (const VMStateDescription*[]) {
 908        &vmstate_fdrive_media_changed,
 909        &vmstate_fdrive_media_rate,
 910        &vmstate_fdrive_perpendicular,
 911        NULL
 912    }
 913};
 914
 915/*
 916 * Reconstructs the phase from register values according to the logic that was
 917 * implemented in qemu 2.3. This is the default value that is used if the phase
 918 * subsection is not present on migration.
 919 *
 920 * Don't change this function to reflect newer qemu versions, it is part of
 921 * the migration ABI.
 922 */
 923static int reconstruct_phase(FDCtrl *fdctrl)
 924{
 925    if (fdctrl->msr & FD_MSR_NONDMA) {
 926        return FD_PHASE_EXECUTION;
 927    } else if ((fdctrl->msr & FD_MSR_RQM) == 0) {
 928        /* qemu 2.3 disabled RQM only during DMA transfers */
 929        return FD_PHASE_EXECUTION;
 930    } else if (fdctrl->msr & FD_MSR_DIO) {
 931        return FD_PHASE_RESULT;
 932    } else {
 933        return FD_PHASE_COMMAND;
 934    }
 935}
 936
 937static int fdc_pre_save(void *opaque)
 938{
 939    FDCtrl *s = opaque;
 940
 941    s->dor_vmstate = s->dor | GET_CUR_DRV(s);
 942
 943    return 0;
 944}
 945
 946static int fdc_pre_load(void *opaque)
 947{
 948    FDCtrl *s = opaque;
 949    s->phase = FD_PHASE_RECONSTRUCT;
 950    return 0;
 951}
 952
 953static int fdc_post_load(void *opaque, int version_id)
 954{
 955    FDCtrl *s = opaque;
 956
 957    SET_CUR_DRV(s, s->dor_vmstate & FD_DOR_SELMASK);
 958    s->dor = s->dor_vmstate & ~FD_DOR_SELMASK;
 959
 960    if (s->phase == FD_PHASE_RECONSTRUCT) {
 961        s->phase = reconstruct_phase(s);
 962    }
 963
 964    return 0;
 965}
 966
 967static bool fdc_reset_sensei_needed(void *opaque)
 968{
 969    FDCtrl *s = opaque;
 970
 971    return s->reset_sensei != 0;
 972}
 973
 974static const VMStateDescription vmstate_fdc_reset_sensei = {
 975    .name = "fdc/reset_sensei",
 976    .version_id = 1,
 977    .minimum_version_id = 1,
 978    .needed = fdc_reset_sensei_needed,
 979    .fields = (VMStateField[]) {
 980        VMSTATE_INT32(reset_sensei, FDCtrl),
 981        VMSTATE_END_OF_LIST()
 982    }
 983};
 984
 985static bool fdc_result_timer_needed(void *opaque)
 986{
 987    FDCtrl *s = opaque;
 988
 989    return timer_pending(s->result_timer);
 990}
 991
 992static const VMStateDescription vmstate_fdc_result_timer = {
 993    .name = "fdc/result_timer",
 994    .version_id = 1,
 995    .minimum_version_id = 1,
 996    .needed = fdc_result_timer_needed,
 997    .fields = (VMStateField[]) {
 998        VMSTATE_TIMER_PTR(result_timer, FDCtrl),
 999        VMSTATE_END_OF_LIST()
1000    }
1001};
1002
1003static bool fdc_phase_needed(void *opaque)
1004{
1005    FDCtrl *fdctrl = opaque;
1006
1007    return reconstruct_phase(fdctrl) != fdctrl->phase;
1008}
1009
1010static const VMStateDescription vmstate_fdc_phase = {
1011    .name = "fdc/phase",
1012    .version_id = 1,
1013    .minimum_version_id = 1,
1014    .needed = fdc_phase_needed,
1015    .fields = (VMStateField[]) {
1016        VMSTATE_UINT8(phase, FDCtrl),
1017        VMSTATE_END_OF_LIST()
1018    }
1019};
1020
1021const VMStateDescription vmstate_fdc = {
1022    .name = "fdc",
1023    .version_id = 2,
1024    .minimum_version_id = 2,
1025    .pre_save = fdc_pre_save,
1026    .pre_load = fdc_pre_load,
1027    .post_load = fdc_post_load,
1028    .fields = (VMStateField[]) {
1029        /* Controller State */
1030        VMSTATE_UINT8(sra, FDCtrl),
1031        VMSTATE_UINT8(srb, FDCtrl),
1032        VMSTATE_UINT8(dor_vmstate, FDCtrl),
1033        VMSTATE_UINT8(tdr, FDCtrl),
1034        VMSTATE_UINT8(dsr, FDCtrl),
1035        VMSTATE_UINT8(msr, FDCtrl),
1036        VMSTATE_UINT8(status0, FDCtrl),
1037        VMSTATE_UINT8(status1, FDCtrl),
1038        VMSTATE_UINT8(status2, FDCtrl),
1039        /* Command FIFO */
1040        VMSTATE_VARRAY_INT32(fifo, FDCtrl, fifo_size, 0, vmstate_info_uint8,
1041                             uint8_t),
1042        VMSTATE_UINT32(data_pos, FDCtrl),
1043        VMSTATE_UINT32(data_len, FDCtrl),
1044        VMSTATE_UINT8(data_state, FDCtrl),
1045        VMSTATE_UINT8(data_dir, FDCtrl),
1046        VMSTATE_UINT8(eot, FDCtrl),
1047        /* States kept only to be returned back */
1048        VMSTATE_UINT8(timer0, FDCtrl),
1049        VMSTATE_UINT8(timer1, FDCtrl),
1050        VMSTATE_UINT8(precomp_trk, FDCtrl),
1051        VMSTATE_UINT8(config, FDCtrl),
1052        VMSTATE_UINT8(lock, FDCtrl),
1053        VMSTATE_UINT8(pwrd, FDCtrl),
1054        VMSTATE_UINT8_EQUAL(num_floppies, FDCtrl, NULL),
1055        VMSTATE_STRUCT_ARRAY(drives, FDCtrl, MAX_FD, 1,
1056                             vmstate_fdrive, FDrive),
1057        VMSTATE_END_OF_LIST()
1058    },
1059    .subsections = (const VMStateDescription*[]) {
1060        &vmstate_fdc_reset_sensei,
1061        &vmstate_fdc_result_timer,
1062        &vmstate_fdc_phase,
1063        NULL
1064    }
1065};
1066
1067/* Change IRQ state */
1068static void fdctrl_reset_irq(FDCtrl *fdctrl)
1069{
1070    fdctrl->status0 = 0;
1071    if (!(fdctrl->sra & FD_SRA_INTPEND))
1072        return;
1073    FLOPPY_DPRINTF("Reset interrupt\n");
1074    qemu_set_irq(fdctrl->irq, 0);
1075    fdctrl->sra &= ~FD_SRA_INTPEND;
1076}
1077
1078static void fdctrl_raise_irq(FDCtrl *fdctrl)
1079{
1080    if (!(fdctrl->sra & FD_SRA_INTPEND)) {
1081        qemu_set_irq(fdctrl->irq, 1);
1082        fdctrl->sra |= FD_SRA_INTPEND;
1083    }
1084
1085    fdctrl->reset_sensei = 0;
1086    FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", fdctrl->status0);
1087}
1088
1089/* Reset controller */
1090void fdctrl_reset(FDCtrl *fdctrl, int do_irq)
1091{
1092    int i;
1093
1094    FLOPPY_DPRINTF("reset controller\n");
1095    fdctrl_reset_irq(fdctrl);
1096    /* Initialise controller */
1097    fdctrl->sra = 0;
1098    fdctrl->srb = 0xc0;
1099    if (!fdctrl->drives[1].blk) {
1100        fdctrl->sra |= FD_SRA_nDRV2;
1101    }
1102    fdctrl->cur_drv = 0;
1103    fdctrl->dor = FD_DOR_nRESET;
1104    fdctrl->dor |= (fdctrl->dma_chann != -1) ? FD_DOR_DMAEN : 0;
1105    fdctrl->msr = FD_MSR_RQM;
1106    fdctrl->reset_sensei = 0;
1107    timer_del(fdctrl->result_timer);
1108    /* FIFO state */
1109    fdctrl->data_pos = 0;
1110    fdctrl->data_len = 0;
1111    fdctrl->data_state = 0;
1112    fdctrl->data_dir = FD_DIR_WRITE;
1113    for (i = 0; i < MAX_FD; i++)
1114        fd_recalibrate(&fdctrl->drives[i]);
1115    fdctrl_to_command_phase(fdctrl);
1116    if (do_irq) {
1117        fdctrl->status0 |= FD_SR0_RDYCHG;
1118        fdctrl_raise_irq(fdctrl);
1119        fdctrl->reset_sensei = FD_RESET_SENSEI_COUNT;
1120    }
1121}
1122
1123static inline FDrive *drv0(FDCtrl *fdctrl)
1124{
1125    return &fdctrl->drives[(fdctrl->tdr & FD_TDR_BOOTSEL) >> 2];
1126}
1127
1128static inline FDrive *drv1(FDCtrl *fdctrl)
1129{
1130    if ((fdctrl->tdr & FD_TDR_BOOTSEL) < (1 << 2))
1131        return &fdctrl->drives[1];
1132    else
1133        return &fdctrl->drives[0];
1134}
1135
1136#if MAX_FD == 4
1137static inline FDrive *drv2(FDCtrl *fdctrl)
1138{
1139    if ((fdctrl->tdr & FD_TDR_BOOTSEL) < (2 << 2))
1140        return &fdctrl->drives[2];
1141    else
1142        return &fdctrl->drives[1];
1143}
1144
1145static inline FDrive *drv3(FDCtrl *fdctrl)
1146{
1147    if ((fdctrl->tdr & FD_TDR_BOOTSEL) < (3 << 2))
1148        return &fdctrl->drives[3];
1149    else
1150        return &fdctrl->drives[2];
1151}
1152#endif
1153
1154static FDrive *get_drv(FDCtrl *fdctrl, int unit)
1155{
1156    switch (unit) {
1157        case 0: return drv0(fdctrl);
1158        case 1: return drv1(fdctrl);
1159#if MAX_FD == 4
1160        case 2: return drv2(fdctrl);
1161        case 3: return drv3(fdctrl);
1162#endif
1163        default: return NULL;
1164    }
1165}
1166
1167static FDrive *get_cur_drv(FDCtrl *fdctrl)
1168{
1169    FDrive *cur_drv = get_drv(fdctrl, fdctrl->cur_drv);
1170
1171    if (!cur_drv->blk) {
1172        /*
1173         * Kludge: empty drive line selected. Create an anonymous
1174         * BlockBackend to avoid NULL deref with various BlockBackend
1175         * API calls within this model (CVE-2021-20196).
1176         * Due to the controller QOM model limitations, we don't
1177         * attach the created to the controller device.
1178         */
1179        cur_drv->blk = blk_create_empty_drive();
1180    }
1181    return cur_drv;
1182}
1183
1184/* Status A register : 0x00 (read-only) */
1185static uint32_t fdctrl_read_statusA(FDCtrl *fdctrl)
1186{
1187    uint32_t retval = fdctrl->sra;
1188
1189    FLOPPY_DPRINTF("status register A: 0x%02x\n", retval);
1190
1191    return retval;
1192}
1193
1194/* Status B register : 0x01 (read-only) */
1195static uint32_t fdctrl_read_statusB(FDCtrl *fdctrl)
1196{
1197    uint32_t retval = fdctrl->srb;
1198
1199    FLOPPY_DPRINTF("status register B: 0x%02x\n", retval);
1200
1201    return retval;
1202}
1203
1204/* Digital output register : 0x02 */
1205static uint32_t fdctrl_read_dor(FDCtrl *fdctrl)
1206{
1207    uint32_t retval = fdctrl->dor;
1208
1209    /* Selected drive */
1210    retval |= fdctrl->cur_drv;
1211    FLOPPY_DPRINTF("digital output register: 0x%02x\n", retval);
1212
1213    return retval;
1214}
1215
1216static void fdctrl_write_dor(FDCtrl *fdctrl, uint32_t value)
1217{
1218    FLOPPY_DPRINTF("digital output register set to 0x%02x\n", value);
1219
1220    /* Motors */
1221    if (value & FD_DOR_MOTEN0)
1222        fdctrl->srb |= FD_SRB_MTR0;
1223    else
1224        fdctrl->srb &= ~FD_SRB_MTR0;
1225    if (value & FD_DOR_MOTEN1)
1226        fdctrl->srb |= FD_SRB_MTR1;
1227    else
1228        fdctrl->srb &= ~FD_SRB_MTR1;
1229
1230    /* Drive */
1231    if (value & 1)
1232        fdctrl->srb |= FD_SRB_DR0;
1233    else
1234        fdctrl->srb &= ~FD_SRB_DR0;
1235
1236    /* Reset */
1237    if (!(value & FD_DOR_nRESET)) {
1238        if (fdctrl->dor & FD_DOR_nRESET) {
1239            FLOPPY_DPRINTF("controller enter RESET state\n");
1240        }
1241    } else {
1242        if (!(fdctrl->dor & FD_DOR_nRESET)) {
1243            FLOPPY_DPRINTF("controller out of RESET state\n");
1244            fdctrl_reset(fdctrl, 1);
1245            fdctrl->dsr &= ~FD_DSR_PWRDOWN;
1246        }
1247    }
1248    /* Selected drive */
1249    fdctrl->cur_drv = value & FD_DOR_SELMASK;
1250
1251    fdctrl->dor = value;
1252}
1253
1254/* Tape drive register : 0x03 */
1255static uint32_t fdctrl_read_tape(FDCtrl *fdctrl)
1256{
1257    uint32_t retval = fdctrl->tdr;
1258
1259    FLOPPY_DPRINTF("tape drive register: 0x%02x\n", retval);
1260
1261    return retval;
1262}
1263
1264static void fdctrl_write_tape(FDCtrl *fdctrl, uint32_t value)
1265{
1266    /* Reset mode */
1267    if (!(fdctrl->dor & FD_DOR_nRESET)) {
1268        FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
1269        return;
1270    }
1271    FLOPPY_DPRINTF("tape drive register set to 0x%02x\n", value);
1272    /* Disk boot selection indicator */
1273    fdctrl->tdr = value & FD_TDR_BOOTSEL;
1274    /* Tape indicators: never allow */
1275}
1276
1277/* Main status register : 0x04 (read) */
1278static uint32_t fdctrl_read_main_status(FDCtrl *fdctrl)
1279{
1280    uint32_t retval = fdctrl->msr;
1281
1282    fdctrl->dsr &= ~FD_DSR_PWRDOWN;
1283    fdctrl->dor |= FD_DOR_nRESET;
1284
1285    FLOPPY_DPRINTF("main status register: 0x%02x\n", retval);
1286
1287    return retval;
1288}
1289
1290/* Data select rate register : 0x04 (write) */
1291static void fdctrl_write_rate(FDCtrl *fdctrl, uint32_t value)
1292{
1293    /* Reset mode */
1294    if (!(fdctrl->dor & FD_DOR_nRESET)) {
1295        FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
1296        return;
1297    }
1298    FLOPPY_DPRINTF("select rate register set to 0x%02x\n", value);
1299    /* Reset: autoclear */
1300    if (value & FD_DSR_SWRESET) {
1301        fdctrl->dor &= ~FD_DOR_nRESET;
1302        fdctrl_reset(fdctrl, 1);
1303        fdctrl->dor |= FD_DOR_nRESET;
1304    }
1305    if (value & FD_DSR_PWRDOWN) {
1306        fdctrl_reset(fdctrl, 1);
1307    }
1308    fdctrl->dsr = value;
1309}
1310
1311/* Configuration control register: 0x07 (write) */
1312static void fdctrl_write_ccr(FDCtrl *fdctrl, uint32_t value)
1313{
1314    /* Reset mode */
1315    if (!(fdctrl->dor & FD_DOR_nRESET)) {
1316        FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
1317        return;
1318    }
1319    FLOPPY_DPRINTF("configuration control register set to 0x%02x\n", value);
1320
1321    /* Only the rate selection bits used in AT mode, and we
1322     * store those in the DSR.
1323     */
1324    fdctrl->dsr = (fdctrl->dsr & ~FD_DSR_DRATEMASK) |
1325                  (value & FD_DSR_DRATEMASK);
1326}
1327
1328static int fdctrl_media_changed(FDrive *drv)
1329{
1330    return drv->media_changed;
1331}
1332
1333/* Digital input register : 0x07 (read-only) */
1334static uint32_t fdctrl_read_dir(FDCtrl *fdctrl)
1335{
1336    uint32_t retval = 0;
1337
1338    if (fdctrl_media_changed(get_cur_drv(fdctrl))) {
1339        retval |= FD_DIR_DSKCHG;
1340    }
1341    if (retval != 0) {
1342        FLOPPY_DPRINTF("Floppy digital input register: 0x%02x\n", retval);
1343    }
1344
1345    return retval;
1346}
1347
1348/* Clear the FIFO and update the state for receiving the next command */
1349static void fdctrl_to_command_phase(FDCtrl *fdctrl)
1350{
1351    fdctrl->phase = FD_PHASE_COMMAND;
1352    fdctrl->data_dir = FD_DIR_WRITE;
1353    fdctrl->data_pos = 0;
1354    fdctrl->data_len = 1; /* Accept command byte, adjust for params later */
1355    fdctrl->msr &= ~(FD_MSR_CMDBUSY | FD_MSR_DIO);
1356    fdctrl->msr |= FD_MSR_RQM;
1357}
1358
1359/* Update the state to allow the guest to read out the command status.
1360 * @fifo_len is the number of result bytes to be read out. */
1361static void fdctrl_to_result_phase(FDCtrl *fdctrl, int fifo_len)
1362{
1363    fdctrl->phase = FD_PHASE_RESULT;
1364    fdctrl->data_dir = FD_DIR_READ;
1365    fdctrl->data_len = fifo_len;
1366    fdctrl->data_pos = 0;
1367    fdctrl->msr |= FD_MSR_CMDBUSY | FD_MSR_RQM | FD_MSR_DIO;
1368}
1369
1370/* Set an error: unimplemented/unknown command */
1371static void fdctrl_unimplemented(FDCtrl *fdctrl, int direction)
1372{
1373    qemu_log_mask(LOG_UNIMP, "fdc: unimplemented command 0x%02x\n",
1374                  fdctrl->fifo[0]);
1375    fdctrl->fifo[0] = FD_SR0_INVCMD;
1376    fdctrl_to_result_phase(fdctrl, 1);
1377}
1378
1379/* Seek to next sector
1380 * returns 0 when end of track reached (for DBL_SIDES on head 1)
1381 * otherwise returns 1
1382 */
1383static int fdctrl_seek_to_next_sect(FDCtrl *fdctrl, FDrive *cur_drv)
1384{
1385    FLOPPY_DPRINTF("seek to next sector (%d %02x %02x => %d)\n",
1386                   cur_drv->head, cur_drv->track, cur_drv->sect,
1387                   fd_sector(cur_drv));
1388    /* XXX: cur_drv->sect >= cur_drv->last_sect should be an
1389       error in fact */
1390    uint8_t new_head = cur_drv->head;
1391    uint8_t new_track = cur_drv->track;
1392    uint8_t new_sect = cur_drv->sect;
1393
1394    int ret = 1;
1395
1396    if (new_sect >= cur_drv->last_sect ||
1397        new_sect == fdctrl->eot) {
1398        new_sect = 1;
1399        if (FD_MULTI_TRACK(fdctrl->data_state)) {
1400            if (new_head == 0 &&
1401                (cur_drv->flags & FDISK_DBL_SIDES) != 0) {
1402                new_head = 1;
1403            } else {
1404                new_head = 0;
1405                new_track++;
1406                fdctrl->status0 |= FD_SR0_SEEK;
1407                if ((cur_drv->flags & FDISK_DBL_SIDES) == 0) {
1408                    ret = 0;
1409                }
1410            }
1411        } else {
1412            fdctrl->status0 |= FD_SR0_SEEK;
1413            new_track++;
1414            ret = 0;
1415        }
1416        if (ret == 1) {
1417            FLOPPY_DPRINTF("seek to next track (%d %02x %02x => %d)\n",
1418                    new_head, new_track, new_sect, fd_sector(cur_drv));
1419        }
1420    } else {
1421        new_sect++;
1422    }
1423    fd_seek(cur_drv, new_head, new_track, new_sect, 1);
1424    return ret;
1425}
1426
1427/* Callback for transfer end (stop or abort) */
1428static void fdctrl_stop_transfer(FDCtrl *fdctrl, uint8_t status0,
1429                                 uint8_t status1, uint8_t status2)
1430{
1431    FDrive *cur_drv;
1432    cur_drv = get_cur_drv(fdctrl);
1433
1434    fdctrl->status0 &= ~(FD_SR0_DS0 | FD_SR0_DS1 | FD_SR0_HEAD);
1435    fdctrl->status0 |= GET_CUR_DRV(fdctrl);
1436    if (cur_drv->head) {
1437        fdctrl->status0 |= FD_SR0_HEAD;
1438    }
1439    fdctrl->status0 |= status0;
1440
1441    FLOPPY_DPRINTF("transfer status: %02x %02x %02x (%02x)\n",
1442                   status0, status1, status2, fdctrl->status0);
1443    fdctrl->fifo[0] = fdctrl->status0;
1444    fdctrl->fifo[1] = status1;
1445    fdctrl->fifo[2] = status2;
1446    fdctrl->fifo[3] = cur_drv->track;
1447    fdctrl->fifo[4] = cur_drv->head;
1448    fdctrl->fifo[5] = cur_drv->sect;
1449    fdctrl->fifo[6] = FD_SECTOR_SC;
1450    fdctrl->data_dir = FD_DIR_READ;
1451    if (fdctrl->dma_chann != -1 && !(fdctrl->msr & FD_MSR_NONDMA)) {
1452        IsaDmaClass *k = ISADMA_GET_CLASS(fdctrl->dma);
1453        k->release_DREQ(fdctrl->dma, fdctrl->dma_chann);
1454    }
1455    fdctrl->msr |= FD_MSR_RQM | FD_MSR_DIO;
1456    fdctrl->msr &= ~FD_MSR_NONDMA;
1457
1458    fdctrl_to_result_phase(fdctrl, 7);
1459    fdctrl_raise_irq(fdctrl);
1460}
1461
1462/* Prepare a data transfer (either DMA or FIFO) */
1463static void fdctrl_start_transfer(FDCtrl *fdctrl, int direction)
1464{
1465    FDrive *cur_drv;
1466    uint8_t kh, kt, ks;
1467
1468    SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
1469    cur_drv = get_cur_drv(fdctrl);
1470    kt = fdctrl->fifo[2];
1471    kh = fdctrl->fifo[3];
1472    ks = fdctrl->fifo[4];
1473    FLOPPY_DPRINTF("Start transfer at %d %d %02x %02x (%d)\n",
1474                   GET_CUR_DRV(fdctrl), kh, kt, ks,
1475                   fd_sector_calc(kh, kt, ks, cur_drv->last_sect,
1476                                  NUM_SIDES(cur_drv)));
1477    switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & FD_CONFIG_EIS)) {
1478    case 2:
1479        /* sect too big */
1480        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
1481        fdctrl->fifo[3] = kt;
1482        fdctrl->fifo[4] = kh;
1483        fdctrl->fifo[5] = ks;
1484        return;
1485    case 3:
1486        /* track too big */
1487        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, FD_SR1_EC, 0x00);
1488        fdctrl->fifo[3] = kt;
1489        fdctrl->fifo[4] = kh;
1490        fdctrl->fifo[5] = ks;
1491        return;
1492    case 4:
1493        /* No seek enabled */
1494        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
1495        fdctrl->fifo[3] = kt;
1496        fdctrl->fifo[4] = kh;
1497        fdctrl->fifo[5] = ks;
1498        return;
1499    case 1:
1500        fdctrl->status0 |= FD_SR0_SEEK;
1501        break;
1502    default:
1503        break;
1504    }
1505
1506    /* Check the data rate. If the programmed data rate does not match
1507     * the currently inserted medium, the operation has to fail. */
1508    if ((fdctrl->dsr & FD_DSR_DRATEMASK) != cur_drv->media_rate) {
1509        FLOPPY_DPRINTF("data rate mismatch (fdc=%d, media=%d)\n",
1510                       fdctrl->dsr & FD_DSR_DRATEMASK, cur_drv->media_rate);
1511        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, FD_SR1_MA, 0x00);
1512        fdctrl->fifo[3] = kt;
1513        fdctrl->fifo[4] = kh;
1514        fdctrl->fifo[5] = ks;
1515        return;
1516    }
1517
1518    /* Set the FIFO state */
1519    fdctrl->data_dir = direction;
1520    fdctrl->data_pos = 0;
1521    assert(fdctrl->msr & FD_MSR_CMDBUSY);
1522    if (fdctrl->fifo[0] & 0x80)
1523        fdctrl->data_state |= FD_STATE_MULTI;
1524    else
1525        fdctrl->data_state &= ~FD_STATE_MULTI;
1526    if (fdctrl->fifo[5] == 0) {
1527        fdctrl->data_len = fdctrl->fifo[8];
1528    } else {
1529        int tmp;
1530        fdctrl->data_len = 128 << (fdctrl->fifo[5] > 7 ? 7 : fdctrl->fifo[5]);
1531        tmp = (fdctrl->fifo[6] - ks + 1);
1532        if (fdctrl->fifo[0] & 0x80)
1533            tmp += fdctrl->fifo[6];
1534        fdctrl->data_len *= tmp;
1535    }
1536    fdctrl->eot = fdctrl->fifo[6];
1537    if (fdctrl->dor & FD_DOR_DMAEN) {
1538        /* DMA transfer is enabled. */
1539        IsaDmaClass *k = ISADMA_GET_CLASS(fdctrl->dma);
1540
1541        FLOPPY_DPRINTF("direction=%d (%d - %d)\n",
1542                       direction, (128 << fdctrl->fifo[5]) *
1543                       (cur_drv->last_sect - ks + 1), fdctrl->data_len);
1544
1545        /* No access is allowed until DMA transfer has completed */
1546        fdctrl->msr &= ~FD_MSR_RQM;
1547        if (direction != FD_DIR_VERIFY) {
1548            /*
1549             * Now, we just have to wait for the DMA controller to
1550             * recall us...
1551             */
1552            k->hold_DREQ(fdctrl->dma, fdctrl->dma_chann);
1553            k->schedule(fdctrl->dma);
1554        } else {
1555            /* Start transfer */
1556            fdctrl_transfer_handler(fdctrl, fdctrl->dma_chann, 0,
1557                    fdctrl->data_len);
1558        }
1559        return;
1560    }
1561    FLOPPY_DPRINTF("start non-DMA transfer\n");
1562    fdctrl->msr |= FD_MSR_NONDMA | FD_MSR_RQM;
1563    if (direction != FD_DIR_WRITE)
1564        fdctrl->msr |= FD_MSR_DIO;
1565    /* IO based transfer: calculate len */
1566    fdctrl_raise_irq(fdctrl);
1567}
1568
1569/* Prepare a transfer of deleted data */
1570static void fdctrl_start_transfer_del(FDCtrl *fdctrl, int direction)
1571{
1572    qemu_log_mask(LOG_UNIMP, "fdctrl_start_transfer_del() unimplemented\n");
1573
1574    /* We don't handle deleted data,
1575     * so we don't return *ANYTHING*
1576     */
1577    fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00);
1578}
1579
1580/* handlers for DMA transfers */
1581int fdctrl_transfer_handler(void *opaque, int nchan, int dma_pos, int dma_len)
1582{
1583    FDCtrl *fdctrl;
1584    FDrive *cur_drv;
1585    int len, start_pos, rel_pos;
1586    uint8_t status0 = 0x00, status1 = 0x00, status2 = 0x00;
1587    IsaDmaClass *k;
1588
1589    fdctrl = opaque;
1590    if (fdctrl->msr & FD_MSR_RQM) {
1591        FLOPPY_DPRINTF("Not in DMA transfer mode !\n");
1592        return 0;
1593    }
1594    k = ISADMA_GET_CLASS(fdctrl->dma);
1595    cur_drv = get_cur_drv(fdctrl);
1596    if (fdctrl->data_dir == FD_DIR_SCANE || fdctrl->data_dir == FD_DIR_SCANL ||
1597        fdctrl->data_dir == FD_DIR_SCANH)
1598        status2 = FD_SR2_SNS;
1599    if (dma_len > fdctrl->data_len)
1600        dma_len = fdctrl->data_len;
1601    if (cur_drv->blk == NULL) {
1602        if (fdctrl->data_dir == FD_DIR_WRITE)
1603            fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00);
1604        else
1605            fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
1606        len = 0;
1607        goto transfer_error;
1608    }
1609    rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;
1610    for (start_pos = fdctrl->data_pos; fdctrl->data_pos < dma_len;) {
1611        len = dma_len - fdctrl->data_pos;
1612        if (len + rel_pos > FD_SECTOR_LEN)
1613            len = FD_SECTOR_LEN - rel_pos;
1614        FLOPPY_DPRINTF("copy %d bytes (%d %d %d) %d pos %d %02x "
1615                       "(%d-0x%08x 0x%08x)\n", len, dma_len, fdctrl->data_pos,
1616                       fdctrl->data_len, GET_CUR_DRV(fdctrl), cur_drv->head,
1617                       cur_drv->track, cur_drv->sect, fd_sector(cur_drv),
1618                       fd_sector(cur_drv) * FD_SECTOR_LEN);
1619        if (fdctrl->data_dir != FD_DIR_WRITE ||
1620            len < FD_SECTOR_LEN || rel_pos != 0) {
1621            /* READ & SCAN commands and realign to a sector for WRITE */
1622            if (blk_pread(cur_drv->blk, fd_offset(cur_drv),
1623                          fdctrl->fifo, BDRV_SECTOR_SIZE) < 0) {
1624                FLOPPY_DPRINTF("Floppy: error getting sector %d\n",
1625                               fd_sector(cur_drv));
1626                /* Sure, image size is too small... */
1627                memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
1628            }
1629        }
1630        switch (fdctrl->data_dir) {
1631        case FD_DIR_READ:
1632            /* READ commands */
1633            k->write_memory(fdctrl->dma, nchan, fdctrl->fifo + rel_pos,
1634                            fdctrl->data_pos, len);
1635            break;
1636        case FD_DIR_WRITE:
1637            /* WRITE commands */
1638            if (cur_drv->ro) {
1639                /* Handle readonly medium early, no need to do DMA, touch the
1640                 * LED or attempt any writes. A real floppy doesn't attempt
1641                 * to write to readonly media either. */
1642                fdctrl_stop_transfer(fdctrl,
1643                                     FD_SR0_ABNTERM | FD_SR0_SEEK, FD_SR1_NW,
1644                                     0x00);
1645                goto transfer_error;
1646            }
1647
1648            k->read_memory(fdctrl->dma, nchan, fdctrl->fifo + rel_pos,
1649                           fdctrl->data_pos, len);
1650            if (blk_pwrite(cur_drv->blk, fd_offset(cur_drv),
1651                           fdctrl->fifo, BDRV_SECTOR_SIZE, 0) < 0) {
1652                FLOPPY_DPRINTF("error writing sector %d\n",
1653                               fd_sector(cur_drv));
1654                fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00);
1655                goto transfer_error;
1656            }
1657            break;
1658        case FD_DIR_VERIFY:
1659            /* VERIFY commands */
1660            break;
1661        default:
1662            /* SCAN commands */
1663            {
1664                uint8_t tmpbuf[FD_SECTOR_LEN];
1665                int ret;
1666                k->read_memory(fdctrl->dma, nchan, tmpbuf, fdctrl->data_pos,
1667                               len);
1668                ret = memcmp(tmpbuf, fdctrl->fifo + rel_pos, len);
1669                if (ret == 0) {
1670                    status2 = FD_SR2_SEH;
1671                    goto end_transfer;
1672                }
1673                if ((ret < 0 && fdctrl->data_dir == FD_DIR_SCANL) ||
1674                    (ret > 0 && fdctrl->data_dir == FD_DIR_SCANH)) {
1675                    status2 = 0x00;
1676                    goto end_transfer;
1677                }
1678            }
1679            break;
1680        }
1681        fdctrl->data_pos += len;
1682        rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;
1683        if (rel_pos == 0) {
1684            /* Seek to next sector */
1685            if (!fdctrl_seek_to_next_sect(fdctrl, cur_drv))
1686                break;
1687        }
1688    }
1689 end_transfer:
1690    len = fdctrl->data_pos - start_pos;
1691    FLOPPY_DPRINTF("end transfer %d %d %d\n",
1692                   fdctrl->data_pos, len, fdctrl->data_len);
1693    if (fdctrl->data_dir == FD_DIR_SCANE ||
1694        fdctrl->data_dir == FD_DIR_SCANL ||
1695        fdctrl->data_dir == FD_DIR_SCANH)
1696        status2 = FD_SR2_SEH;
1697    fdctrl->data_len -= len;
1698    fdctrl_stop_transfer(fdctrl, status0, status1, status2);
1699 transfer_error:
1700
1701    return len;
1702}
1703
1704/* Data register : 0x05 */
1705static uint32_t fdctrl_read_data(FDCtrl *fdctrl)
1706{
1707    FDrive *cur_drv;
1708    uint32_t retval = 0;
1709    uint32_t pos;
1710
1711    cur_drv = get_cur_drv(fdctrl);
1712    fdctrl->dsr &= ~FD_DSR_PWRDOWN;
1713    if (!(fdctrl->msr & FD_MSR_RQM) || !(fdctrl->msr & FD_MSR_DIO)) {
1714        FLOPPY_DPRINTF("error: controller not ready for reading\n");
1715        return 0;
1716    }
1717
1718    /* If data_len spans multiple sectors, the current position in the FIFO
1719     * wraps around while fdctrl->data_pos is the real position in the whole
1720     * request. */
1721    pos = fdctrl->data_pos;
1722    pos %= FD_SECTOR_LEN;
1723
1724    switch (fdctrl->phase) {
1725    case FD_PHASE_EXECUTION:
1726        assert(fdctrl->msr & FD_MSR_NONDMA);
1727        if (pos == 0) {
1728            if (fdctrl->data_pos != 0)
1729                if (!fdctrl_seek_to_next_sect(fdctrl, cur_drv)) {
1730                    FLOPPY_DPRINTF("error seeking to next sector %d\n",
1731                                   fd_sector(cur_drv));
1732                    return 0;
1733                }
1734            if (blk_pread(cur_drv->blk, fd_offset(cur_drv), fdctrl->fifo,
1735                          BDRV_SECTOR_SIZE)
1736                < 0) {
1737                FLOPPY_DPRINTF("error getting sector %d\n",
1738                               fd_sector(cur_drv));
1739                /* Sure, image size is too small... */
1740                memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
1741            }
1742        }
1743
1744        if (++fdctrl->data_pos == fdctrl->data_len) {
1745            fdctrl->msr &= ~FD_MSR_RQM;
1746            fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
1747        }
1748        break;
1749
1750    case FD_PHASE_RESULT:
1751        assert(!(fdctrl->msr & FD_MSR_NONDMA));
1752        if (++fdctrl->data_pos == fdctrl->data_len) {
1753            fdctrl->msr &= ~FD_MSR_RQM;
1754            fdctrl_to_command_phase(fdctrl);
1755            fdctrl_reset_irq(fdctrl);
1756        }
1757        break;
1758
1759    case FD_PHASE_COMMAND:
1760    default:
1761        abort();
1762    }
1763
1764    retval = fdctrl->fifo[pos];
1765    FLOPPY_DPRINTF("data register: 0x%02x\n", retval);
1766
1767    return retval;
1768}
1769
1770static void fdctrl_format_sector(FDCtrl *fdctrl)
1771{
1772    FDrive *cur_drv;
1773    uint8_t kh, kt, ks;
1774
1775    SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
1776    cur_drv = get_cur_drv(fdctrl);
1777    kt = fdctrl->fifo[6];
1778    kh = fdctrl->fifo[7];
1779    ks = fdctrl->fifo[8];
1780    FLOPPY_DPRINTF("format sector at %d %d %02x %02x (%d)\n",
1781                   GET_CUR_DRV(fdctrl), kh, kt, ks,
1782                   fd_sector_calc(kh, kt, ks, cur_drv->last_sect,
1783                                  NUM_SIDES(cur_drv)));
1784    switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & FD_CONFIG_EIS)) {
1785    case 2:
1786        /* sect too big */
1787        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
1788        fdctrl->fifo[3] = kt;
1789        fdctrl->fifo[4] = kh;
1790        fdctrl->fifo[5] = ks;
1791        return;
1792    case 3:
1793        /* track too big */
1794        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, FD_SR1_EC, 0x00);
1795        fdctrl->fifo[3] = kt;
1796        fdctrl->fifo[4] = kh;
1797        fdctrl->fifo[5] = ks;
1798        return;
1799    case 4:
1800        /* No seek enabled */
1801        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, 0x00, 0x00);
1802        fdctrl->fifo[3] = kt;
1803        fdctrl->fifo[4] = kh;
1804        fdctrl->fifo[5] = ks;
1805        return;
1806    case 1:
1807        fdctrl->status0 |= FD_SR0_SEEK;
1808        break;
1809    default:
1810        break;
1811    }
1812    memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
1813    if (cur_drv->blk == NULL ||
1814        blk_pwrite(cur_drv->blk, fd_offset(cur_drv), fdctrl->fifo,
1815                   BDRV_SECTOR_SIZE, 0) < 0) {
1816        FLOPPY_DPRINTF("error formatting sector %d\n", fd_sector(cur_drv));
1817        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM | FD_SR0_SEEK, 0x00, 0x00);
1818    } else {
1819        if (cur_drv->sect == cur_drv->last_sect) {
1820            fdctrl->data_state &= ~FD_STATE_FORMAT;
1821            /* Last sector done */
1822            fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
1823        } else {
1824            /* More to do */
1825            fdctrl->data_pos = 0;
1826            fdctrl->data_len = 4;
1827        }
1828    }
1829}
1830
1831static void fdctrl_handle_lock(FDCtrl *fdctrl, int direction)
1832{
1833    fdctrl->lock = (fdctrl->fifo[0] & 0x80) ? 1 : 0;
1834    fdctrl->fifo[0] = fdctrl->lock << 4;
1835    fdctrl_to_result_phase(fdctrl, 1);
1836}
1837
1838static void fdctrl_handle_dumpreg(FDCtrl *fdctrl, int direction)
1839{
1840    FDrive *cur_drv = get_cur_drv(fdctrl);
1841
1842    /* Drives position */
1843    fdctrl->fifo[0] = drv0(fdctrl)->track;
1844    fdctrl->fifo[1] = drv1(fdctrl)->track;
1845#if MAX_FD == 4
1846    fdctrl->fifo[2] = drv2(fdctrl)->track;
1847    fdctrl->fifo[3] = drv3(fdctrl)->track;
1848#else
1849    fdctrl->fifo[2] = 0;
1850    fdctrl->fifo[3] = 0;
1851#endif
1852    /* timers */
1853    fdctrl->fifo[4] = fdctrl->timer0;
1854    fdctrl->fifo[5] = (fdctrl->timer1 << 1) | (fdctrl->dor & FD_DOR_DMAEN ? 1 : 0);
1855    fdctrl->fifo[6] = cur_drv->last_sect;
1856    fdctrl->fifo[7] = (fdctrl->lock << 7) |
1857        (cur_drv->perpendicular << 2);
1858    fdctrl->fifo[8] = fdctrl->config;
1859    fdctrl->fifo[9] = fdctrl->precomp_trk;
1860    fdctrl_to_result_phase(fdctrl, 10);
1861}
1862
1863static void fdctrl_handle_version(FDCtrl *fdctrl, int direction)
1864{
1865    /* Controller's version */
1866    fdctrl->fifo[0] = fdctrl->version;
1867    fdctrl_to_result_phase(fdctrl, 1);
1868}
1869
1870static void fdctrl_handle_partid(FDCtrl *fdctrl, int direction)
1871{
1872    fdctrl->fifo[0] = 0x41; /* Stepping 1 */
1873    fdctrl_to_result_phase(fdctrl, 1);
1874}
1875
1876static void fdctrl_handle_restore(FDCtrl *fdctrl, int direction)
1877{
1878    FDrive *cur_drv = get_cur_drv(fdctrl);
1879
1880    /* Drives position */
1881    drv0(fdctrl)->track = fdctrl->fifo[3];
1882    drv1(fdctrl)->track = fdctrl->fifo[4];
1883#if MAX_FD == 4
1884    drv2(fdctrl)->track = fdctrl->fifo[5];
1885    drv3(fdctrl)->track = fdctrl->fifo[6];
1886#endif
1887    /* timers */
1888    fdctrl->timer0 = fdctrl->fifo[7];
1889    fdctrl->timer1 = fdctrl->fifo[8];
1890    cur_drv->last_sect = fdctrl->fifo[9];
1891    fdctrl->lock = fdctrl->fifo[10] >> 7;
1892    cur_drv->perpendicular = (fdctrl->fifo[10] >> 2) & 0xF;
1893    fdctrl->config = fdctrl->fifo[11];
1894    fdctrl->precomp_trk = fdctrl->fifo[12];
1895    fdctrl->pwrd = fdctrl->fifo[13];
1896    fdctrl_to_command_phase(fdctrl);
1897}
1898
1899static void fdctrl_handle_save(FDCtrl *fdctrl, int direction)
1900{
1901    FDrive *cur_drv = get_cur_drv(fdctrl);
1902
1903    fdctrl->fifo[0] = 0;
1904    fdctrl->fifo[1] = 0;
1905    /* Drives position */
1906    fdctrl->fifo[2] = drv0(fdctrl)->track;
1907    fdctrl->fifo[3] = drv1(fdctrl)->track;
1908#if MAX_FD == 4
1909    fdctrl->fifo[4] = drv2(fdctrl)->track;
1910    fdctrl->fifo[5] = drv3(fdctrl)->track;
1911#else
1912    fdctrl->fifo[4] = 0;
1913    fdctrl->fifo[5] = 0;
1914#endif
1915    /* timers */
1916    fdctrl->fifo[6] = fdctrl->timer0;
1917    fdctrl->fifo[7] = fdctrl->timer1;
1918    fdctrl->fifo[8] = cur_drv->last_sect;
1919    fdctrl->fifo[9] = (fdctrl->lock << 7) |
1920        (cur_drv->perpendicular << 2);
1921    fdctrl->fifo[10] = fdctrl->config;
1922    fdctrl->fifo[11] = fdctrl->precomp_trk;
1923    fdctrl->fifo[12] = fdctrl->pwrd;
1924    fdctrl->fifo[13] = 0;
1925    fdctrl->fifo[14] = 0;
1926    fdctrl_to_result_phase(fdctrl, 15);
1927}
1928
1929static void fdctrl_handle_readid(FDCtrl *fdctrl, int direction)
1930{
1931    FDrive *cur_drv = get_cur_drv(fdctrl);
1932
1933    cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
1934    timer_mod(fdctrl->result_timer, qemu_clock_get_ns(QEMU_CLOCK_VIRTUAL) +
1935             (NANOSECONDS_PER_SECOND / 50));
1936}
1937
1938static void fdctrl_handle_format_track(FDCtrl *fdctrl, int direction)
1939{
1940    FDrive *cur_drv;
1941
1942    SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
1943    cur_drv = get_cur_drv(fdctrl);
1944    fdctrl->data_state |= FD_STATE_FORMAT;
1945    if (fdctrl->fifo[0] & 0x80)
1946        fdctrl->data_state |= FD_STATE_MULTI;
1947    else
1948        fdctrl->data_state &= ~FD_STATE_MULTI;
1949    cur_drv->bps =
1950        fdctrl->fifo[2] > 7 ? 16384 : 128 << fdctrl->fifo[2];
1951#if 0
1952    cur_drv->last_sect =
1953        cur_drv->flags & FDISK_DBL_SIDES ? fdctrl->fifo[3] :
1954        fdctrl->fifo[3] / 2;
1955#else
1956    cur_drv->last_sect = fdctrl->fifo[3];
1957#endif
1958    /* TODO: implement format using DMA expected by the Bochs BIOS
1959     * and Linux fdformat (read 3 bytes per sector via DMA and fill
1960     * the sector with the specified fill byte
1961     */
1962    fdctrl->data_state &= ~FD_STATE_FORMAT;
1963    fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
1964}
1965
1966static void fdctrl_handle_specify(FDCtrl *fdctrl, int direction)
1967{
1968    fdctrl->timer0 = (fdctrl->fifo[1] >> 4) & 0xF;
1969    fdctrl->timer1 = fdctrl->fifo[2] >> 1;
1970    if (fdctrl->fifo[2] & 1)
1971        fdctrl->dor &= ~FD_DOR_DMAEN;
1972    else
1973        fdctrl->dor |= FD_DOR_DMAEN;
1974    /* No result back */
1975    fdctrl_to_command_phase(fdctrl);
1976}
1977
1978static void fdctrl_handle_sense_drive_status(FDCtrl *fdctrl, int direction)
1979{
1980    FDrive *cur_drv;
1981
1982    SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
1983    cur_drv = get_cur_drv(fdctrl);
1984    cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
1985    /* 1 Byte status back */
1986    fdctrl->fifo[0] = (cur_drv->ro << 6) |
1987        (cur_drv->track == 0 ? 0x10 : 0x00) |
1988        (cur_drv->head << 2) |
1989        GET_CUR_DRV(fdctrl) |
1990        0x28;
1991    fdctrl_to_result_phase(fdctrl, 1);
1992}
1993
1994static void fdctrl_handle_recalibrate(FDCtrl *fdctrl, int direction)
1995{
1996    FDrive *cur_drv;
1997
1998    SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
1999    cur_drv = get_cur_drv(fdctrl);
2000    fd_recalibrate(cur_drv);
2001    fdctrl_to_command_phase(fdctrl);
2002    /* Raise Interrupt */
2003    fdctrl->status0 |= FD_SR0_SEEK;
2004    fdctrl_raise_irq(fdctrl);
2005}
2006
2007static void fdctrl_handle_sense_interrupt_status(FDCtrl *fdctrl, int direction)
2008{
2009    FDrive *cur_drv = get_cur_drv(fdctrl);
2010
2011    if (fdctrl->reset_sensei > 0) {
2012        fdctrl->fifo[0] =
2013            FD_SR0_RDYCHG + FD_RESET_SENSEI_COUNT - fdctrl->reset_sensei;
2014        fdctrl->reset_sensei--;
2015    } else if (!(fdctrl->sra & FD_SRA_INTPEND)) {
2016        fdctrl->fifo[0] = FD_SR0_INVCMD;
2017        fdctrl_to_result_phase(fdctrl, 1);
2018        return;
2019    } else {
2020        fdctrl->fifo[0] =
2021                (fdctrl->status0 & ~(FD_SR0_HEAD | FD_SR0_DS1 | FD_SR0_DS0))
2022                | GET_CUR_DRV(fdctrl);
2023    }
2024
2025    fdctrl->fifo[1] = cur_drv->track;
2026    fdctrl_to_result_phase(fdctrl, 2);
2027    fdctrl_reset_irq(fdctrl);
2028    fdctrl->status0 = FD_SR0_RDYCHG;
2029}
2030
2031static void fdctrl_handle_seek(FDCtrl *fdctrl, int direction)
2032{
2033    FDrive *cur_drv;
2034
2035    SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
2036    cur_drv = get_cur_drv(fdctrl);
2037    fdctrl_to_command_phase(fdctrl);
2038    /* The seek command just sends step pulses to the drive and doesn't care if
2039     * there is a medium inserted of if it's banging the head against the drive.
2040     */
2041    fd_seek(cur_drv, cur_drv->head, fdctrl->fifo[2], cur_drv->sect, 1);
2042    /* Raise Interrupt */
2043    fdctrl->status0 |= FD_SR0_SEEK;
2044    fdctrl_raise_irq(fdctrl);
2045}
2046
2047static void fdctrl_handle_perpendicular_mode(FDCtrl *fdctrl, int direction)
2048{
2049    FDrive *cur_drv = get_cur_drv(fdctrl);
2050
2051    if (fdctrl->fifo[1] & 0x80)
2052        cur_drv->perpendicular = fdctrl->fifo[1] & 0x7;
2053    /* No result back */
2054    fdctrl_to_command_phase(fdctrl);
2055}
2056
2057static void fdctrl_handle_configure(FDCtrl *fdctrl, int direction)
2058{
2059    fdctrl->config = fdctrl->fifo[2];
2060    fdctrl->precomp_trk =  fdctrl->fifo[3];
2061    /* No result back */
2062    fdctrl_to_command_phase(fdctrl);
2063}
2064
2065static void fdctrl_handle_powerdown_mode(FDCtrl *fdctrl, int direction)
2066{
2067    fdctrl->pwrd = fdctrl->fifo[1];
2068    fdctrl->fifo[0] = fdctrl->fifo[1];
2069    fdctrl_to_result_phase(fdctrl, 1);
2070}
2071
2072static void fdctrl_handle_option(FDCtrl *fdctrl, int direction)
2073{
2074    /* No result back */
2075    fdctrl_to_command_phase(fdctrl);
2076}
2077
2078static void fdctrl_handle_drive_specification_command(FDCtrl *fdctrl, int direction)
2079{
2080    FDrive *cur_drv = get_cur_drv(fdctrl);
2081    uint32_t pos;
2082
2083    pos = fdctrl->data_pos - 1;
2084    pos %= FD_SECTOR_LEN;
2085    if (fdctrl->fifo[pos] & 0x80) {
2086        /* Command parameters done */
2087        if (fdctrl->fifo[pos] & 0x40) {
2088            fdctrl->fifo[0] = fdctrl->fifo[1];
2089            fdctrl->fifo[2] = 0;
2090            fdctrl->fifo[3] = 0;
2091            fdctrl_to_result_phase(fdctrl, 4);
2092        } else {
2093            fdctrl_to_command_phase(fdctrl);
2094        }
2095    } else if (fdctrl->data_len > 7) {
2096        /* ERROR */
2097        fdctrl->fifo[0] = 0x80 |
2098            (cur_drv->head << 2) | GET_CUR_DRV(fdctrl);
2099        fdctrl_to_result_phase(fdctrl, 1);
2100    }
2101}
2102
2103static void fdctrl_handle_relative_seek_in(FDCtrl *fdctrl, int direction)
2104{
2105    FDrive *cur_drv;
2106
2107    SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
2108    cur_drv = get_cur_drv(fdctrl);
2109    if (fdctrl->fifo[2] + cur_drv->track >= cur_drv->max_track) {
2110        fd_seek(cur_drv, cur_drv->head, cur_drv->max_track - 1,
2111                cur_drv->sect, 1);
2112    } else {
2113        fd_seek(cur_drv, cur_drv->head,
2114                cur_drv->track + fdctrl->fifo[2], cur_drv->sect, 1);
2115    }
2116    fdctrl_to_command_phase(fdctrl);
2117    /* Raise Interrupt */
2118    fdctrl->status0 |= FD_SR0_SEEK;
2119    fdctrl_raise_irq(fdctrl);
2120}
2121
2122static void fdctrl_handle_relative_seek_out(FDCtrl *fdctrl, int direction)
2123{
2124    FDrive *cur_drv;
2125
2126    SET_CUR_DRV(fdctrl, fdctrl->fifo[1] & FD_DOR_SELMASK);
2127    cur_drv = get_cur_drv(fdctrl);
2128    if (fdctrl->fifo[2] > cur_drv->track) {
2129        fd_seek(cur_drv, cur_drv->head, 0, cur_drv->sect, 1);
2130    } else {
2131        fd_seek(cur_drv, cur_drv->head,
2132                cur_drv->track - fdctrl->fifo[2], cur_drv->sect, 1);
2133    }
2134    fdctrl_to_command_phase(fdctrl);
2135    /* Raise Interrupt */
2136    fdctrl->status0 |= FD_SR0_SEEK;
2137    fdctrl_raise_irq(fdctrl);
2138}
2139
2140/*
2141 * Handlers for the execution phase of each command
2142 */
2143typedef struct FDCtrlCommand {
2144    uint8_t value;
2145    uint8_t mask;
2146    const char* name;
2147    int parameters;
2148    void (*handler)(FDCtrl *fdctrl, int direction);
2149    int direction;
2150} FDCtrlCommand;
2151
2152static const FDCtrlCommand handlers[] = {
2153    { FD_CMD_READ, 0x1f, "READ", 8, fdctrl_start_transfer, FD_DIR_READ },
2154    { FD_CMD_WRITE, 0x3f, "WRITE", 8, fdctrl_start_transfer, FD_DIR_WRITE },
2155    { FD_CMD_SEEK, 0xff, "SEEK", 2, fdctrl_handle_seek },
2156    { FD_CMD_SENSE_INTERRUPT_STATUS, 0xff, "SENSE INTERRUPT STATUS", 0, fdctrl_handle_sense_interrupt_status },
2157    { FD_CMD_RECALIBRATE, 0xff, "RECALIBRATE", 1, fdctrl_handle_recalibrate },
2158    { FD_CMD_FORMAT_TRACK, 0xbf, "FORMAT TRACK", 5, fdctrl_handle_format_track },
2159    { FD_CMD_READ_TRACK, 0xbf, "READ TRACK", 8, fdctrl_start_transfer, FD_DIR_READ },
2160    { FD_CMD_RESTORE, 0xff, "RESTORE", 17, fdctrl_handle_restore }, /* part of READ DELETED DATA */
2161    { FD_CMD_SAVE, 0xff, "SAVE", 0, fdctrl_handle_save }, /* part of READ DELETED DATA */
2162    { FD_CMD_READ_DELETED, 0x1f, "READ DELETED DATA", 8, fdctrl_start_transfer_del, FD_DIR_READ },
2163    { FD_CMD_SCAN_EQUAL, 0x1f, "SCAN EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANE },
2164    { FD_CMD_VERIFY, 0x1f, "VERIFY", 8, fdctrl_start_transfer, FD_DIR_VERIFY },
2165    { FD_CMD_SCAN_LOW_OR_EQUAL, 0x1f, "SCAN LOW OR EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANL },
2166    { FD_CMD_SCAN_HIGH_OR_EQUAL, 0x1f, "SCAN HIGH OR EQUAL", 8, fdctrl_start_transfer, FD_DIR_SCANH },
2167    { FD_CMD_WRITE_DELETED, 0x3f, "WRITE DELETED DATA", 8, fdctrl_start_transfer_del, FD_DIR_WRITE },
2168    { FD_CMD_READ_ID, 0xbf, "READ ID", 1, fdctrl_handle_readid },
2169    { FD_CMD_SPECIFY, 0xff, "SPECIFY", 2, fdctrl_handle_specify },
2170    { FD_CMD_SENSE_DRIVE_STATUS, 0xff, "SENSE DRIVE STATUS", 1, fdctrl_handle_sense_drive_status },
2171    { FD_CMD_PERPENDICULAR_MODE, 0xff, "PERPENDICULAR MODE", 1, fdctrl_handle_perpendicular_mode },
2172    { FD_CMD_CONFIGURE, 0xff, "CONFIGURE", 3, fdctrl_handle_configure },
2173    { FD_CMD_POWERDOWN_MODE, 0xff, "POWERDOWN MODE", 2, fdctrl_handle_powerdown_mode },
2174    { FD_CMD_OPTION, 0xff, "OPTION", 1, fdctrl_handle_option },
2175    { FD_CMD_DRIVE_SPECIFICATION_COMMAND, 0xff, "DRIVE SPECIFICATION COMMAND", 5, fdctrl_handle_drive_specification_command },
2176    { FD_CMD_RELATIVE_SEEK_OUT, 0xff, "RELATIVE SEEK OUT", 2, fdctrl_handle_relative_seek_out },
2177    { FD_CMD_FORMAT_AND_WRITE, 0xff, "FORMAT AND WRITE", 10, fdctrl_unimplemented },
2178    { FD_CMD_RELATIVE_SEEK_IN, 0xff, "RELATIVE SEEK IN", 2, fdctrl_handle_relative_seek_in },
2179    { FD_CMD_LOCK, 0x7f, "LOCK", 0, fdctrl_handle_lock },
2180    { FD_CMD_DUMPREG, 0xff, "DUMPREG", 0, fdctrl_handle_dumpreg },
2181    { FD_CMD_VERSION, 0xff, "VERSION", 0, fdctrl_handle_version },
2182    { FD_CMD_PART_ID, 0xff, "PART ID", 0, fdctrl_handle_partid },
2183    { FD_CMD_WRITE, 0x1f, "WRITE (BeOS)", 8, fdctrl_start_transfer, FD_DIR_WRITE }, /* not in specification ; BeOS 4.5 bug */
2184    { 0, 0, "unknown", 0, fdctrl_unimplemented }, /* default handler */
2185};
2186/* Associate command to an index in the 'handlers' array */
2187static uint8_t command_to_handler[256];
2188
2189static const FDCtrlCommand *get_command(uint8_t cmd)
2190{
2191    int idx;
2192
2193    idx = command_to_handler[cmd];
2194    FLOPPY_DPRINTF("%s command\n", handlers[idx].name);
2195    return &handlers[idx];
2196}
2197
2198static void fdctrl_write_data(FDCtrl *fdctrl, uint32_t value)
2199{
2200    FDrive *cur_drv;
2201    const FDCtrlCommand *cmd;
2202    uint32_t pos;
2203
2204    /* Reset mode */
2205    if (!(fdctrl->dor & FD_DOR_nRESET)) {
2206        FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
2207        return;
2208    }
2209    if (!(fdctrl->msr & FD_MSR_RQM) || (fdctrl->msr & FD_MSR_DIO)) {
2210        FLOPPY_DPRINTF("error: controller not ready for writing\n");
2211        return;
2212    }
2213    fdctrl->dsr &= ~FD_DSR_PWRDOWN;
2214
2215    FLOPPY_DPRINTF("%s: %02x\n", __func__, value);
2216
2217    /* If data_len spans multiple sectors, the current position in the FIFO
2218     * wraps around while fdctrl->data_pos is the real position in the whole
2219     * request. */
2220    pos = fdctrl->data_pos++;
2221    pos %= FD_SECTOR_LEN;
2222    fdctrl->fifo[pos] = value;
2223
2224    if (fdctrl->data_pos == fdctrl->data_len) {
2225        fdctrl->msr &= ~FD_MSR_RQM;
2226    }
2227
2228    switch (fdctrl->phase) {
2229    case FD_PHASE_EXECUTION:
2230        /* For DMA requests, RQM should be cleared during execution phase, so
2231         * we would have errored out above. */
2232        assert(fdctrl->msr & FD_MSR_NONDMA);
2233
2234        /* FIFO data write */
2235        if (pos == FD_SECTOR_LEN - 1 ||
2236            fdctrl->data_pos == fdctrl->data_len) {
2237            cur_drv = get_cur_drv(fdctrl);
2238            if (blk_pwrite(cur_drv->blk, fd_offset(cur_drv), fdctrl->fifo,
2239                           BDRV_SECTOR_SIZE, 0) < 0) {
2240                FLOPPY_DPRINTF("error writing sector %d\n",
2241                               fd_sector(cur_drv));
2242                break;
2243            }
2244            if (!fdctrl_seek_to_next_sect(fdctrl, cur_drv)) {
2245                FLOPPY_DPRINTF("error seeking to next sector %d\n",
2246                               fd_sector(cur_drv));
2247                break;
2248            }
2249        }
2250
2251        /* Switch to result phase when done with the transfer */
2252        if (fdctrl->data_pos == fdctrl->data_len) {
2253            fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
2254        }
2255        break;
2256
2257    case FD_PHASE_COMMAND:
2258        assert(!(fdctrl->msr & FD_MSR_NONDMA));
2259        assert(fdctrl->data_pos < FD_SECTOR_LEN);
2260
2261        if (pos == 0) {
2262            /* The first byte specifies the command. Now we start reading
2263             * as many parameters as this command requires. */
2264            cmd = get_command(value);
2265            fdctrl->data_len = cmd->parameters + 1;
2266            if (cmd->parameters) {
2267                fdctrl->msr |= FD_MSR_RQM;
2268            }
2269            fdctrl->msr |= FD_MSR_CMDBUSY;
2270        }
2271
2272        if (fdctrl->data_pos == fdctrl->data_len) {
2273            /* We have all parameters now, execute the command */
2274            fdctrl->phase = FD_PHASE_EXECUTION;
2275
2276            if (fdctrl->data_state & FD_STATE_FORMAT) {
2277                fdctrl_format_sector(fdctrl);
2278                break;
2279            }
2280
2281            cmd = get_command(fdctrl->fifo[0]);
2282            FLOPPY_DPRINTF("Calling handler for '%s'\n", cmd->name);
2283            cmd->handler(fdctrl, cmd->direction);
2284        }
2285        break;
2286
2287    case FD_PHASE_RESULT:
2288    default:
2289        abort();
2290    }
2291}
2292
2293static void fdctrl_result_timer(void *opaque)
2294{
2295    FDCtrl *fdctrl = opaque;
2296    FDrive *cur_drv = get_cur_drv(fdctrl);
2297
2298    /* Pretend we are spinning.
2299     * This is needed for Coherent, which uses READ ID to check for
2300     * sector interleaving.
2301     */
2302    if (cur_drv->last_sect != 0) {
2303        cur_drv->sect = (cur_drv->sect % cur_drv->last_sect) + 1;
2304    }
2305    /* READ_ID can't automatically succeed! */
2306    if ((fdctrl->dsr & FD_DSR_DRATEMASK) != cur_drv->media_rate) {
2307        FLOPPY_DPRINTF("read id rate mismatch (fdc=%d, media=%d)\n",
2308                       fdctrl->dsr & FD_DSR_DRATEMASK, cur_drv->media_rate);
2309        fdctrl_stop_transfer(fdctrl, FD_SR0_ABNTERM, FD_SR1_MA, 0x00);
2310    } else {
2311        fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
2312    }
2313}
2314
2315/* Init functions */
2316
2317void fdctrl_init_drives(FloppyBus *bus, DriveInfo **fds)
2318{
2319    DeviceState *dev;
2320    int i;
2321
2322    for (i = 0; i < MAX_FD; i++) {
2323        if (fds[i]) {
2324            dev = qdev_new("floppy");
2325            qdev_prop_set_uint32(dev, "unit", i);
2326            qdev_prop_set_enum(dev, "drive-type", FLOPPY_DRIVE_TYPE_AUTO);
2327            qdev_prop_set_drive_err(dev, "drive", blk_by_legacy_dinfo(fds[i]),
2328                                    &error_fatal);
2329            qdev_realize_and_unref(dev, &bus->bus, &error_fatal);
2330        }
2331    }
2332}
2333
2334void fdctrl_realize_common(DeviceState *dev, FDCtrl *fdctrl, Error **errp)
2335{
2336    int i, j;
2337    FDrive *drive;
2338    static int command_tables_inited = 0;
2339
2340    if (fdctrl->fallback == FLOPPY_DRIVE_TYPE_AUTO) {
2341        error_setg(errp, "Cannot choose a fallback FDrive type of 'auto'");
2342        return;
2343    }
2344
2345    /* Fill 'command_to_handler' lookup table */
2346    if (!command_tables_inited) {
2347        command_tables_inited = 1;
2348        for (i = ARRAY_SIZE(handlers) - 1; i >= 0; i--) {
2349            for (j = 0; j < sizeof(command_to_handler); j++) {
2350                if ((j & handlers[i].mask) == handlers[i].value) {
2351                    command_to_handler[j] = i;
2352                }
2353            }
2354        }
2355    }
2356
2357    FLOPPY_DPRINTF("init controller\n");
2358    fdctrl->fifo = qemu_memalign(512, FD_SECTOR_LEN);
2359    memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
2360    fdctrl->fifo_size = 512;
2361    fdctrl->result_timer = timer_new_ns(QEMU_CLOCK_VIRTUAL,
2362                                             fdctrl_result_timer, fdctrl);
2363
2364    fdctrl->version = 0x90; /* Intel 82078 controller */
2365    fdctrl->config = FD_CONFIG_EIS | FD_CONFIG_EFIFO; /* Implicit seek, polling & FIFO enabled */
2366    fdctrl->num_floppies = MAX_FD;
2367
2368    floppy_bus_create(fdctrl, &fdctrl->bus, dev);
2369
2370    for (i = 0; i < MAX_FD; i++) {
2371        drive = &fdctrl->drives[i];
2372        drive->fdctrl = fdctrl;
2373        fd_init(drive);
2374        fd_revalidate(drive);
2375    }
2376}
2377
2378static void fdc_register_types(void)
2379{
2380    type_register_static(&floppy_bus_info);
2381    type_register_static(&floppy_drive_info);
2382}
2383
2384type_init(fdc_register_types)
2385