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