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