hw/block/fdc.c

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