vcs.maemo.org Git - qemu/blob - hw/fdc.c

   1 /*
   2  * QEMU Floppy disk emulator (Intel 82078)
   3  *
   4  * Copyright (c) 2003 Jocelyn Mayer
   5  *
   6  * Permission is hereby granted, free of charge, to any person obtaining a copy
   7  * of this software and associated documentation files (the "Software"), to deal
   8  * in the Software without restriction, including without limitation the rights
   9  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  10  * copies of the Software, and to permit persons to whom the Software is
  11  * furnished to do so, subject to the following conditions:
  12  *
  13  * The above copyright notice and this permission notice shall be included in
  14  * all copies or substantial portions of the Software.
  15  *
  16  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  17  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  18  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
  19  * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  20  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  21  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  22  * THE SOFTWARE.
  23  */
  24 #include "vl.h"
  25
  26 /********************************************************/
  27 /* debug Floppy devices */
  28 //#define DEBUG_FLOPPY
  29
  30 #ifdef DEBUG_FLOPPY
  31 #define FLOPPY_DPRINTF(fmt, args...) \
  32 do { printf("FLOPPY: " fmt , ##args); } while (0)
  33 #else
  34 #define FLOPPY_DPRINTF(fmt, args...)
  35 #endif
  36
  37 #define FLOPPY_ERROR(fmt, args...) \
  38 do { printf("FLOPPY ERROR: %s: " fmt, __func__ , ##args); } while (0)
  39
  40 /********************************************************/
  41 /* Floppy drive emulation                               */
  42
  43 /* Will always be a fixed parameter for us */
  44 #define FD_SECTOR_LEN 512
  45 #define FD_SECTOR_SC  2   /* Sector size code */
  46
  47 /* Floppy disk drive emulation */
  48 typedef enum fdisk_type_t {
  49     FDRIVE_DISK_288   = 0x01, /* 2.88 MB disk           */
  50     FDRIVE_DISK_144   = 0x02, /* 1.44 MB disk           */
  51     FDRIVE_DISK_720   = 0x03, /* 720 kB disk            */
  52     FDRIVE_DISK_USER  = 0x04, /* User defined geometry  */
  53     FDRIVE_DISK_NONE  = 0x05, /* No disk                */
  54 } fdisk_type_t;
  55
  56 typedef enum fdrive_type_t {
  57     FDRIVE_DRV_144  = 0x00,   /* 1.44 MB 3"5 drive      */
  58     FDRIVE_DRV_288  = 0x01,   /* 2.88 MB 3"5 drive      */
  59     FDRIVE_DRV_120  = 0x02,   /* 1.2  MB 5"25 drive     */
  60     FDRIVE_DRV_NONE = 0x03,   /* No drive connected     */
  61 } fdrive_type_t;
  62
  63 typedef enum fdrive_flags_t {
  64     FDRIVE_MOTOR_ON   = 0x01, /* motor on/off           */
  65     FDRIVE_REVALIDATE = 0x02, /* Revalidated            */
  66 } fdrive_flags_t;
  67
  68 typedef enum fdisk_flags_t {
  69     FDISK_DBL_SIDES  = 0x01,
  70 } fdisk_flags_t;
  71
  72 typedef struct fdrive_t {
  73     BlockDriverState *bs;
  74     /* Drive status */
  75     fdrive_type_t drive;
  76     fdrive_flags_t drflags;
  77     uint8_t perpendicular;    /* 2.88 MB access mode    */
  78     /* Position */
  79     uint8_t head;
  80     uint8_t track;
  81     uint8_t sect;
  82     /* Last operation status */
  83     uint8_t dir;              /* Direction              */
  84     uint8_t rw;               /* Read/write             */
  85     /* Media */
  86     fdisk_flags_t flags;
  87     uint8_t last_sect;        /* Nb sector per track    */
  88     uint8_t max_track;        /* Nb of tracks           */
  89     uint16_t bps;             /* Bytes per sector       */
  90     uint8_t ro;               /* Is read-only           */
  91 } fdrive_t;
  92
  93 static void fd_init (fdrive_t *drv, BlockDriverState *bs)
  94 {
  95     /* Drive */
  96     drv->bs = bs;
  97     drv->drive = FDRIVE_DRV_NONE;
  98     drv->drflags = 0;
  99     drv->perpendicular = 0;
 100     /* Disk */
 101     drv->last_sect = 0;
 102     drv->max_track = 0;
 103 }
 104
 105 static int _fd_sector (uint8_t head, uint8_t track,
 106                         uint8_t sect, uint8_t last_sect)
 107 {
 108     return (((track * 2) + head) * last_sect) + sect - 1;
 109 }
 110
 111 /* Returns current position, in sectors, for given drive */
 112 static int fd_sector (fdrive_t *drv)
 113 {
 114     return _fd_sector(drv->head, drv->track, drv->sect, drv->last_sect);
 115 }
 116
 117 static int fd_seek (fdrive_t *drv, uint8_t head, uint8_t track, uint8_t sect,
 118                     int enable_seek)
 119 {
 120     uint32_t sector;
 121     int ret;
 122
 123     if (track > drv->max_track ||
 124         (head != 0 && (drv->flags & FDISK_DBL_SIDES) == 0)) {
 125         FLOPPY_DPRINTF("try to read %d %02x %02x (max=%d %d %02x %02x)\n",
 126                        head, track, sect, 1,
 127                        (drv->flags & FDISK_DBL_SIDES) == 0 ? 0 : 1,
 128                        drv->max_track, drv->last_sect);
 129         return 2;
 130     }
 131     if (sect > drv->last_sect) {
 132         FLOPPY_DPRINTF("try to read %d %02x %02x (max=%d %d %02x %02x)\n",
 133                        head, track, sect, 1,
 134                        (drv->flags & FDISK_DBL_SIDES) == 0 ? 0 : 1,
 135                        drv->max_track, drv->last_sect);
 136         return 3;
 137     }
 138     sector = _fd_sector(head, track, sect, drv->last_sect);
 139     ret = 0;
 140     if (sector != fd_sector(drv)) {
 141 #if 0
 142         if (!enable_seek) {
 143             FLOPPY_ERROR("no implicit seek %d %02x %02x (max=%d %02x %02x)\n",
 144                          head, track, sect, 1, drv->max_track, drv->last_sect);
 145             return 4;
 146         }
 147 #endif
 148         drv->head = head;
 149         if (drv->track != track)
 150             ret = 1;
 151         drv->track = track;
 152         drv->sect = sect;
 153     }
 154
 155     return ret;
 156 }
 157
 158 /* Set drive back to track 0 */
 159 static void fd_recalibrate (fdrive_t *drv)
 160 {
 161     FLOPPY_DPRINTF("recalibrate\n");
 162     drv->head = 0;
 163     drv->track = 0;
 164     drv->sect = 1;
 165     drv->dir = 1;
 166     drv->rw = 0;
 167 }
 168
 169 /* Recognize floppy formats */
 170 typedef struct fd_format_t {
 171     fdrive_type_t drive;
 172     fdisk_type_t  disk;
 173     uint8_t last_sect;
 174     uint8_t max_track;
 175     uint8_t max_head;
 176     const unsigned char *str;
 177 } fd_format_t;
 178
 179 static fd_format_t fd_formats[] = {
 180     /* First entry is default format */
 181     /* 1.44 MB 3"1/2 floppy disks */
 182     { FDRIVE_DRV_144, FDRIVE_DISK_144, 18, 80, 1, "1.44 MB 3\"1/2", },
 183     { FDRIVE_DRV_144, FDRIVE_DISK_144, 20, 80, 1,  "1.6 MB 3\"1/2", },
 184     { FDRIVE_DRV_144, FDRIVE_DISK_144, 21, 80, 1, "1.68 MB 3\"1/2", },
 185     { FDRIVE_DRV_144, FDRIVE_DISK_144, 21, 82, 1, "1.72 MB 3\"1/2", },
 186     { FDRIVE_DRV_144, FDRIVE_DISK_144, 21, 83, 1, "1.74 MB 3\"1/2", },
 187     { FDRIVE_DRV_144, FDRIVE_DISK_144, 22, 80, 1, "1.76 MB 3\"1/2", },
 188     { FDRIVE_DRV_144, FDRIVE_DISK_144, 23, 80, 1, "1.84 MB 3\"1/2", },
 189     { FDRIVE_DRV_144, FDRIVE_DISK_144, 24, 80, 1, "1.92 MB 3\"1/2", },
 190     /* 2.88 MB 3"1/2 floppy disks */
 191     { FDRIVE_DRV_288, FDRIVE_DISK_288, 36, 80, 1, "2.88 MB 3\"1/2", },
 192     { FDRIVE_DRV_288, FDRIVE_DISK_288, 39, 80, 1, "3.12 MB 3\"1/2", },
 193     { FDRIVE_DRV_288, FDRIVE_DISK_288, 40, 80, 1,  "3.2 MB 3\"1/2", },
 194     { FDRIVE_DRV_288, FDRIVE_DISK_288, 44, 80, 1, "3.52 MB 3\"1/2", },
 195     { FDRIVE_DRV_288, FDRIVE_DISK_288, 48, 80, 1, "3.84 MB 3\"1/2", },
 196     /* 720 kB 3"1/2 floppy disks */
 197     { FDRIVE_DRV_144, FDRIVE_DISK_720,  9, 80, 1,  "720 kB 3\"1/2", },
 198     { FDRIVE_DRV_144, FDRIVE_DISK_720, 10, 80, 1,  "800 kB 3\"1/2", },
 199     { FDRIVE_DRV_144, FDRIVE_DISK_720, 10, 82, 1,  "820 kB 3\"1/2", },
 200     { FDRIVE_DRV_144, FDRIVE_DISK_720, 10, 83, 1,  "830 kB 3\"1/2", },
 201     { FDRIVE_DRV_144, FDRIVE_DISK_720, 13, 80, 1, "1.04 MB 3\"1/2", },
 202     { FDRIVE_DRV_144, FDRIVE_DISK_720, 14, 80, 1, "1.12 MB 3\"1/2", },
 203     /* 1.2 MB 5"1/4 floppy disks */
 204     { FDRIVE_DRV_120, FDRIVE_DISK_288, 15, 80, 1,  "1.2 kB 5\"1/4", },
 205     { FDRIVE_DRV_120, FDRIVE_DISK_288, 18, 80, 1, "1.44 MB 5\"1/4", },
 206     { FDRIVE_DRV_120, FDRIVE_DISK_288, 18, 82, 1, "1.48 MB 5\"1/4", },
 207     { FDRIVE_DRV_120, FDRIVE_DISK_288, 18, 83, 1, "1.49 MB 5\"1/4", },
 208     { FDRIVE_DRV_120, FDRIVE_DISK_288, 20, 80, 1,  "1.6 MB 5\"1/4", },
 209     /* 720 kB 5"1/4 floppy disks */
 210     { FDRIVE_DRV_120, FDRIVE_DISK_288,  9, 80, 1,  "720 kB 5\"1/4", },
 211     { FDRIVE_DRV_120, FDRIVE_DISK_288, 11, 80, 1,  "880 kB 5\"1/4", },
 212     /* 360 kB 5"1/4 floppy disks */
 213     { FDRIVE_DRV_120, FDRIVE_DISK_288,  9, 40, 1,  "360 kB 5\"1/4", },
 214     { FDRIVE_DRV_120, FDRIVE_DISK_288,  9, 40, 0,  "180 kB 5\"1/4", },
 215     { FDRIVE_DRV_120, FDRIVE_DISK_288, 10, 41, 1,  "410 kB 5\"1/4", },
 216     { FDRIVE_DRV_120, FDRIVE_DISK_288, 10, 42, 1,  "420 kB 5\"1/4", },
 217     /* 320 kB 5"1/4 floppy disks */
 218     { FDRIVE_DRV_120, FDRIVE_DISK_288,  8, 40, 1,  "320 kB 5\"1/4", },
 219     { FDRIVE_DRV_120, FDRIVE_DISK_288,  8, 40, 0,  "160 kB 5\"1/4", },
 220     /* 360 kB must match 5"1/4 better than 3"1/2... */
 221     { FDRIVE_DRV_144, FDRIVE_DISK_720,  9, 80, 0,  "360 kB 3\"1/2", },
 222     /* end */
 223     { FDRIVE_DRV_NONE, FDRIVE_DISK_NONE, -1, -1, 0, NULL, },
 224 };
 225
 226 /* Revalidate a disk drive after a disk change */
 227 static void fd_revalidate (fdrive_t *drv)
 228 {
 229     fd_format_t *parse;
 230     int64_t nb_sectors, size;
 231     int i, first_match, match;
 232     int nb_heads, max_track, last_sect, ro;
 233
 234     FLOPPY_DPRINTF("revalidate\n");
 235     drv->drflags &= ~FDRIVE_REVALIDATE;
 236     if (drv->bs != NULL && bdrv_is_inserted(drv->bs)) {
 237         ro = bdrv_is_read_only(drv->bs);
 238         bdrv_get_geometry_hint(drv->bs, &nb_heads, &max_track, &last_sect);
 239         if (nb_heads != 0 && max_track != 0 && last_sect != 0) {
 240             FLOPPY_DPRINTF("User defined disk (%d %d %d)",
 241                            nb_heads - 1, max_track, last_sect);
 242         } else {
 243             bdrv_get_geometry(drv->bs, &nb_sectors);
 244             match = -1;
 245             first_match = -1;
 246             for (i = 0;; i++) {
 247                 parse = &fd_formats[i];
 248                 if (parse->drive == FDRIVE_DRV_NONE)
 249                     break;
 250                 if (drv->drive == parse->drive ||
 251                     drv->drive == FDRIVE_DRV_NONE) {
 252                     size = (parse->max_head + 1) * parse->max_track *
 253                         parse->last_sect;
 254                     if (nb_sectors == size) {
 255                         match = i;
 256                         break;
 257                     }
 258                     if (first_match == -1)
 259                         first_match = i;
 260                 }
 261             }
 262             if (match == -1) {
 263                 if (first_match == -1)
 264                     match = 1;
 265                 else
 266                     match = first_match;
 267                 parse = &fd_formats[match];
 268             }
 269             nb_heads = parse->max_head + 1;
 270             max_track = parse->max_track;
 271             last_sect = parse->last_sect;
 272             drv->drive = parse->drive;
 273             FLOPPY_DPRINTF("%s floppy disk (%d h %d t %d s) %s\n", parse->str,
 274                            nb_heads, max_track, last_sect, ro ? "ro" : "rw");
 275         }
 276             if (nb_heads == 1) {
 277                 drv->flags &= ~FDISK_DBL_SIDES;
 278             } else {
 279                 drv->flags |= FDISK_DBL_SIDES;
 280             }
 281             drv->max_track = max_track;
 282             drv->last_sect = last_sect;
 283         drv->ro = ro;
 284     } else {
 285         FLOPPY_DPRINTF("No disk in drive\n");
 286         drv->last_sect = 0;
 287         drv->max_track = 0;
 288         drv->flags &= ~FDISK_DBL_SIDES;
 289     }
 290     drv->drflags |= FDRIVE_REVALIDATE;
 291 }
 292
 293 /* Motor control */
 294 static void fd_start (fdrive_t *drv)
 295 {
 296     drv->drflags |= FDRIVE_MOTOR_ON;
 297 }
 298
 299 static void fd_stop (fdrive_t *drv)
 300 {
 301     drv->drflags &= ~FDRIVE_MOTOR_ON;
 302 }
 303
 304 /* Re-initialise a drives (motor off, repositioned) */
 305 static void fd_reset (fdrive_t *drv)
 306 {
 307     fd_stop(drv);
 308     fd_recalibrate(drv);
 309 }
 310
 311 /********************************************************/
 312 /* Intel 82078 floppy disk controller emulation          */
 313
 314 static void fdctrl_reset (fdctrl_t *fdctrl, int do_irq);
 315 static void fdctrl_reset_fifo (fdctrl_t *fdctrl);
 316 static int fdctrl_transfer_handler (void *opaque, target_ulong addr, int size);
 317 static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status);
 318 static void fdctrl_result_timer(void *opaque);
 319
 320 static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl);
 321 static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl);
 322 static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value);
 323 static uint32_t fdctrl_read_tape (fdctrl_t *fdctrl);
 324 static void fdctrl_write_tape (fdctrl_t *fdctrl, uint32_t value);
 325 static uint32_t fdctrl_read_main_status (fdctrl_t *fdctrl);
 326 static void fdctrl_write_rate (fdctrl_t *fdctrl, uint32_t value);
 327 static uint32_t fdctrl_read_data (fdctrl_t *fdctrl);
 328 static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value);
 329 static uint32_t fdctrl_read_dir (fdctrl_t *fdctrl);
 330
 331 enum {
 332     FD_CTRL_ACTIVE = 0x01, /* XXX: suppress that */
 333     FD_CTRL_RESET  = 0x02,
 334     FD_CTRL_SLEEP  = 0x04, /* XXX: suppress that */
 335     FD_CTRL_BUSY   = 0x08, /* dma transfer in progress */
 336     FD_CTRL_INTR   = 0x10,
 337 };
 338
 339 enum {
 340     FD_DIR_WRITE   = 0,
 341     FD_DIR_READ    = 1,
 342     FD_DIR_SCANE   = 2,
 343     FD_DIR_SCANL   = 3,
 344     FD_DIR_SCANH   = 4,
 345 };
 346
 347 enum {
 348     FD_STATE_CMD    = 0x00,
 349     FD_STATE_STATUS = 0x01,
 350     FD_STATE_DATA   = 0x02,
 351     FD_STATE_STATE  = 0x03,
 352     FD_STATE_MULTI  = 0x10,
 353     FD_STATE_SEEK   = 0x20,
 354     FD_STATE_FORMAT = 0x40,
 355 };
 356
 357 #define FD_STATE(state) ((state) & FD_STATE_STATE)
 358 #define FD_SET_STATE(state, new_state) \
 359 do { (state) = ((state) & ~FD_STATE_STATE) | (new_state); } while (0)
 360 #define FD_MULTI_TRACK(state) ((state) & FD_STATE_MULTI)
 361 #define FD_DID_SEEK(state) ((state) & FD_STATE_SEEK)
 362 #define FD_FORMAT_CMD(state) ((state) & FD_STATE_FORMAT)
 363
 364 struct fdctrl_t {
 365     fdctrl_t *fdctrl;
 366     /* Controller's identification */
 367     uint8_t version;
 368     /* HW */
 369     int irq_lvl;
 370     int dma_chann;
 371     uint32_t io_base;
 372     /* Controller state */
 373     QEMUTimer *result_timer;
 374     uint8_t state;
 375     uint8_t dma_en;
 376     uint8_t cur_drv;
 377     uint8_t bootsel;
 378     /* Command FIFO */
 379     uint8_t fifo[FD_SECTOR_LEN];
 380     uint32_t data_pos;
 381     uint32_t data_len;
 382     uint8_t data_state;
 383     uint8_t data_dir;
 384     uint8_t int_status;
 385     uint8_t eot; /* last wanted sector */
 386     /* States kept only to be returned back */
 387     /* Timers state */
 388     uint8_t timer0;
 389     uint8_t timer1;
 390     /* precompensation */
 391     uint8_t precomp_trk;
 392     uint8_t config;
 393     uint8_t lock;
 394     /* Power down config (also with status regB access mode */
 395     uint8_t pwrd;
 396     /* Floppy drives */
 397     fdrive_t drives[2];
 398 };
 399
 400 static uint32_t fdctrl_read (void *opaque, uint32_t reg)
 401 {
 402     fdctrl_t *fdctrl = opaque;
 403     uint32_t retval;
 404
 405     switch (reg & 0x07) {
 406     case 0x01:
 407         retval = fdctrl_read_statusB(fdctrl);
 408         break;
 409     case 0x02:
 410         retval = fdctrl_read_dor(fdctrl);
 411         break;
 412     case 0x03:
 413         retval = fdctrl_read_tape(fdctrl);
 414         break;
 415     case 0x04:
 416         retval = fdctrl_read_main_status(fdctrl);
 417         break;
 418     case 0x05:
 419         retval = fdctrl_read_data(fdctrl);
 420         break;
 421     case 0x07:
 422         retval = fdctrl_read_dir(fdctrl);
 423         break;
 424     default:
 425         retval = (uint32_t)(-1);
 426         break;
 427     }
 428     FLOPPY_DPRINTF("read reg%d: 0x%02x\n", reg & 7, retval);
 429
 430     return retval;
 431 }
 432
 433 static void fdctrl_write (void *opaque, uint32_t reg, uint32_t value)
 434 {
 435     fdctrl_t *fdctrl = opaque;
 436
 437     FLOPPY_DPRINTF("write reg%d: 0x%02x\n", reg & 7, value);
 438
 439     switch (reg & 0x07) {
 440     case 0x02:
 441         fdctrl_write_dor(fdctrl, value);
 442         break;
 443     case 0x03:
 444         fdctrl_write_tape(fdctrl, value);
 445         break;
 446     case 0x04:
 447         fdctrl_write_rate(fdctrl, value);
 448         break;
 449     case 0x05:
 450         fdctrl_write_data(fdctrl, value);
 451         break;
 452     default:
 453         break;
 454     }
 455 }
 456
 457 static void fd_change_cb (void *opaque)
 458 {
 459     fdrive_t *drv = opaque;
 460
 461     FLOPPY_DPRINTF("disk change\n");
 462     fd_revalidate(drv);
 463 #if 0
 464     fd_recalibrate(drv);
 465     fdctrl_reset_fifo(drv->fdctrl);
 466     fdctrl_raise_irq(drv->fdctrl, 0x20);
 467 #endif
 468 }
 469
 470 fdctrl_t *fdctrl_init (int irq_lvl, int dma_chann, int mem_mapped,
 471                        uint32_t io_base,
 472                        BlockDriverState **fds)
 473 {
 474     fdctrl_t *fdctrl;
 475 //    int io_mem;
 476     int i;
 477
 478     FLOPPY_DPRINTF("init controller\n");
 479     fdctrl = qemu_mallocz(sizeof(fdctrl_t));
 480     if (!fdctrl)
 481         return NULL;
 482     fdctrl->result_timer = qemu_new_timer(vm_clock,
 483                                           fdctrl_result_timer, fdctrl);
 484
 485     fdctrl->version = 0x90; /* Intel 82078 controller */
 486     fdctrl->irq_lvl = irq_lvl;
 487     fdctrl->dma_chann = dma_chann;
 488     fdctrl->io_base = io_base;
 489     fdctrl->config = 0x60; /* Implicit seek, polling & FIFO enabled */
 490     if (fdctrl->dma_chann != -1) {
 491         fdctrl->dma_en = 1;
 492         DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl);
 493     } else {
 494         fdctrl->dma_en = 0;
 495     }
 496     for (i = 0; i < 2; i++) {
 497         fd_init(&fdctrl->drives[i], fds[i]);
 498         if (fds[i]) {
 499             bdrv_set_change_cb(fds[i],
 500                                &fd_change_cb, &fdctrl->drives[i]);
 501         }
 502     }
 503     fdctrl_reset(fdctrl, 0);
 504     fdctrl->state = FD_CTRL_ACTIVE;
 505     if (mem_mapped) {
 506         FLOPPY_ERROR("memory mapped floppy not supported by now !\n");
 507 #if 0
 508         io_mem = cpu_register_io_memory(0, fdctrl_mem_read, fdctrl_mem_write);
 509         cpu_register_physical_memory(base, 0x08, io_mem);
 510 #endif
 511     } else {
 512         register_ioport_read(io_base + 0x01, 5, 1, &fdctrl_read, fdctrl);
 513         register_ioport_read(io_base + 0x07, 1, 1, &fdctrl_read, fdctrl);
 514         register_ioport_write(io_base + 0x01, 5, 1, &fdctrl_write, fdctrl);
 515         register_ioport_write(io_base + 0x07, 1, 1, &fdctrl_write, fdctrl);
 516     }
 517     for (i = 0; i < 2; i++) {
 518         fd_revalidate(&fdctrl->drives[i]);
 519     }
 520
 521     return fdctrl;
 522 }
 523
 524 /* XXX: may change if moved to bdrv */
 525 int fdctrl_get_drive_type(fdctrl_t *fdctrl, int drive_num)
 526 {
 527     return fdctrl->drives[drive_num].drive;
 528 }
 529
 530 /* Change IRQ state */
 531 static void fdctrl_reset_irq (fdctrl_t *fdctrl)
 532 {
 533     FLOPPY_DPRINTF("Reset interrupt\n");
 534     pic_set_irq(fdctrl->irq_lvl, 0);
 535     fdctrl->state &= ~FD_CTRL_INTR;
 536 }
 537
 538 static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status)
 539 {
 540     if (~(fdctrl->state & FD_CTRL_INTR)) {
 541         pic_set_irq(fdctrl->irq_lvl, 1);
 542         fdctrl->state |= FD_CTRL_INTR;
 543     }
 544     FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", status);
 545     fdctrl->int_status = status;
 546 }
 547
 548 /* Reset controller */
 549 static void fdctrl_reset (fdctrl_t *fdctrl, int do_irq)
 550 {
 551     int i;
 552
 553     FLOPPY_DPRINTF("reset controller\n");
 554     fdctrl_reset_irq(fdctrl);
 555     /* Initialise controller */
 556     fdctrl->cur_drv = 0;
 557     /* FIFO state */
 558     fdctrl->data_pos = 0;
 559     fdctrl->data_len = 0;
 560     fdctrl->data_state = FD_STATE_CMD;
 561     fdctrl->data_dir = FD_DIR_WRITE;
 562     for (i = 0; i < MAX_FD; i++)
 563         fd_reset(&fdctrl->drives[i]);
 564     fdctrl_reset_fifo(fdctrl);
 565     if (do_irq)
 566         fdctrl_raise_irq(fdctrl, 0xc0);
 567 }
 568
 569 static inline fdrive_t *drv0 (fdctrl_t *fdctrl)
 570 {
 571     return &fdctrl->drives[fdctrl->bootsel];
 572 }
 573
 574 static inline fdrive_t *drv1 (fdctrl_t *fdctrl)
 575 {
 576     return &fdctrl->drives[1 - fdctrl->bootsel];
 577 }
 578
 579 static fdrive_t *get_cur_drv (fdctrl_t *fdctrl)
 580 {
 581     return fdctrl->cur_drv == 0 ? drv0(fdctrl) : drv1(fdctrl);
 582 }
 583
 584 /* Status B register : 0x01 (read-only) */
 585 static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl)
 586 {
 587     FLOPPY_DPRINTF("status register: 0x00\n");
 588     return 0;
 589 }
 590
 591 /* Digital output register : 0x02 */
 592 static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl)
 593 {
 594     uint32_t retval = 0;
 595
 596     /* Drive motors state indicators */
 597     if (drv0(fdctrl)->drflags & FDRIVE_MOTOR_ON)
 598         retval |= 1 << 5;
 599     if (drv1(fdctrl)->drflags & FDRIVE_MOTOR_ON)
 600         retval |= 1 << 4;
 601     /* DMA enable */
 602     retval |= fdctrl->dma_en << 3;
 603     /* Reset indicator */
 604     retval |= (fdctrl->state & FD_CTRL_RESET) == 0 ? 0x04 : 0;
 605     /* Selected drive */
 606     retval |= fdctrl->cur_drv;
 607     FLOPPY_DPRINTF("digital output register: 0x%02x\n", retval);
 608
 609     return retval;
 610 }
 611
 612 static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value)
 613 {
 614     /* Reset mode */
 615     if (fdctrl->state & FD_CTRL_RESET) {
 616         if (!(value & 0x04)) {
 617             FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
 618             return;
 619         }
 620     }
 621     FLOPPY_DPRINTF("digital output register set to 0x%02x\n", value);
 622     /* Drive motors state indicators */
 623     if (value & 0x20)
 624         fd_start(drv1(fdctrl));
 625     else
 626         fd_stop(drv1(fdctrl));
 627     if (value & 0x10)
 628         fd_start(drv0(fdctrl));
 629     else
 630         fd_stop(drv0(fdctrl));
 631     /* DMA enable */
 632 #if 0
 633     if (fdctrl->dma_chann != -1)
 634         fdctrl->dma_en = 1 - ((value >> 3) & 1);
 635 #endif
 636     /* Reset */
 637     if (!(value & 0x04)) {
 638         if (!(fdctrl->state & FD_CTRL_RESET)) {
 639             FLOPPY_DPRINTF("controller enter RESET state\n");
 640             fdctrl->state |= FD_CTRL_RESET;
 641         }
 642     } else {
 643         if (fdctrl->state & FD_CTRL_RESET) {
 644             FLOPPY_DPRINTF("controller out of RESET state\n");
 645             fdctrl_reset(fdctrl, 1);
 646             fdctrl->state &= ~(FD_CTRL_RESET | FD_CTRL_SLEEP);
 647         }
 648     }
 649     /* Selected drive */
 650     fdctrl->cur_drv = value & 1;
 651 }
 652
 653 /* Tape drive register : 0x03 */
 654 static uint32_t fdctrl_read_tape (fdctrl_t *fdctrl)
 655 {
 656     uint32_t retval = 0;
 657
 658     /* Disk boot selection indicator */
 659     retval |= fdctrl->bootsel << 2;
 660     /* Tape indicators: never allowed */
 661     FLOPPY_DPRINTF("tape drive register: 0x%02x\n", retval);
 662
 663     return retval;
 664 }
 665
 666 static void fdctrl_write_tape (fdctrl_t *fdctrl, uint32_t value)
 667 {
 668     /* Reset mode */
 669     if (fdctrl->state & FD_CTRL_RESET) {
 670         FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
 671         return;
 672     }
 673     FLOPPY_DPRINTF("tape drive register set to 0x%02x\n", value);
 674     /* Disk boot selection indicator */
 675     fdctrl->bootsel = (value >> 2) & 1;
 676     /* Tape indicators: never allow */
 677 }
 678
 679 /* Main status register : 0x04 (read) */
 680 static uint32_t fdctrl_read_main_status (fdctrl_t *fdctrl)
 681 {
 682     uint32_t retval = 0;
 683
 684     fdctrl->state &= ~(FD_CTRL_SLEEP | FD_CTRL_RESET);
 685     if (!(fdctrl->state & FD_CTRL_BUSY)) {
 686         /* Data transfer allowed */
 687         retval |= 0x80;
 688         /* Data transfer direction indicator */
 689         if (fdctrl->data_dir == FD_DIR_READ)
 690             retval |= 0x40;
 691     }
 692     /* Should handle 0x20 for SPECIFY command */
 693     /* Command busy indicator */
 694     if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA ||
 695         FD_STATE(fdctrl->data_state) == FD_STATE_STATUS)
 696         retval |= 0x10;
 697     FLOPPY_DPRINTF("main status register: 0x%02x\n", retval);
 698
 699     return retval;
 700 }
 701
 702 /* Data select rate register : 0x04 (write) */
 703 static void fdctrl_write_rate (fdctrl_t *fdctrl, uint32_t value)
 704 {
 705     /* Reset mode */
 706     if (fdctrl->state & FD_CTRL_RESET) {
 707             FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
 708             return;
 709         }
 710     FLOPPY_DPRINTF("select rate register set to 0x%02x\n", value);
 711     /* Reset: autoclear */
 712     if (value & 0x80) {
 713         fdctrl->state |= FD_CTRL_RESET;
 714         fdctrl_reset(fdctrl, 1);
 715         fdctrl->state &= ~FD_CTRL_RESET;
 716     }
 717     if (value & 0x40) {
 718         fdctrl->state |= FD_CTRL_SLEEP;
 719         fdctrl_reset(fdctrl, 1);
 720     }
 721 //        fdctrl.precomp = (value >> 2) & 0x07;
 722 }
 723
 724 /* Digital input register : 0x07 (read-only) */
 725 static uint32_t fdctrl_read_dir (fdctrl_t *fdctrl)
 726 {
 727     uint32_t retval = 0;
 728
 729     if (drv0(fdctrl)->drflags & FDRIVE_REVALIDATE ||
 730         drv1(fdctrl)->drflags & FDRIVE_REVALIDATE)
 731         retval |= 0x80;
 732     if (retval != 0)
 733         FLOPPY_DPRINTF("Floppy digital input register: 0x%02x\n", retval);
 734     drv0(fdctrl)->drflags &= ~FDRIVE_REVALIDATE;
 735     drv1(fdctrl)->drflags &= ~FDRIVE_REVALIDATE;
 736
 737     return retval;
 738 }
 739
 740 /* FIFO state control */
 741 static void fdctrl_reset_fifo (fdctrl_t *fdctrl)
 742 {
 743     fdctrl->data_dir = FD_DIR_WRITE;
 744     fdctrl->data_pos = 0;
 745     FD_SET_STATE(fdctrl->data_state, FD_STATE_CMD);
 746 }
 747
 748 /* Set FIFO status for the host to read */
 749 static void fdctrl_set_fifo (fdctrl_t *fdctrl, int fifo_len, int do_irq)
 750 {
 751     fdctrl->data_dir = FD_DIR_READ;
 752     fdctrl->data_len = fifo_len;
 753     fdctrl->data_pos = 0;
 754     FD_SET_STATE(fdctrl->data_state, FD_STATE_STATUS);
 755     if (do_irq)
 756         fdctrl_raise_irq(fdctrl, 0x00);
 757 }
 758
 759 /* Set an error: unimplemented/unknown command */
 760 static void fdctrl_unimplemented (fdctrl_t *fdctrl)
 761 {
 762 #if 0
 763     fdrive_t *cur_drv;
 764
 765     cur_drv = get_cur_drv(fdctrl);
 766     fdctrl->fifo[0] = 0x60 | (cur_drv->head << 2) | fdctrl->cur_drv;
 767     fdctrl->fifo[1] = 0x00;
 768     fdctrl->fifo[2] = 0x00;
 769     fdctrl_set_fifo(fdctrl, 3, 1);
 770 #else
 771     //    fdctrl_reset_fifo(fdctrl);
 772     fdctrl->fifo[0] = 0x80;
 773     fdctrl_set_fifo(fdctrl, 1, 0);
 774 #endif
 775 }
 776
 777 /* Callback for transfer end (stop or abort) */
 778 static void fdctrl_stop_transfer (fdctrl_t *fdctrl, uint8_t status0,
 779                                   uint8_t status1, uint8_t status2)
 780 {
 781     fdrive_t *cur_drv;
 782
 783     cur_drv = get_cur_drv(fdctrl);
 784     FLOPPY_DPRINTF("transfer status: %02x %02x %02x (%02x)\n",
 785                    status0, status1, status2,
 786                    status0 | (cur_drv->head << 2) | fdctrl->cur_drv);
 787     fdctrl->fifo[0] = status0 | (cur_drv->head << 2) | fdctrl->cur_drv;
 788     fdctrl->fifo[1] = status1;
 789     fdctrl->fifo[2] = status2;
 790     fdctrl->fifo[3] = cur_drv->track;
 791     fdctrl->fifo[4] = cur_drv->head;
 792     fdctrl->fifo[5] = cur_drv->sect;
 793     fdctrl->fifo[6] = FD_SECTOR_SC;
 794     fdctrl->data_dir = FD_DIR_READ;
 795     if (fdctrl->state & FD_CTRL_BUSY) {
 796         DMA_release_DREQ(fdctrl->dma_chann);
 797         fdctrl->state &= ~FD_CTRL_BUSY;
 798     }
 799     fdctrl_set_fifo(fdctrl, 7, 1);
 800 }
 801
 802 /* Prepare a data transfer (either DMA or FIFO) */
 803 static void fdctrl_start_transfer (fdctrl_t *fdctrl, int direction)
 804 {
 805     fdrive_t *cur_drv;
 806     uint8_t kh, kt, ks;
 807     int did_seek;
 808
 809     fdctrl->cur_drv = fdctrl->fifo[1] & 1;
 810     cur_drv = get_cur_drv(fdctrl);
 811     kt = fdctrl->fifo[2];
 812     kh = fdctrl->fifo[3];
 813     ks = fdctrl->fifo[4];
 814     FLOPPY_DPRINTF("Start transfer at %d %d %02x %02x (%d)\n",
 815                    fdctrl->cur_drv, kh, kt, ks,
 816                    _fd_sector(kh, kt, ks, cur_drv->last_sect));
 817     did_seek = 0;
 818     switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & 0x40)) {
 819     case 2:
 820         /* sect too big */
 821         fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
 822         fdctrl->fifo[3] = kt;
 823         fdctrl->fifo[4] = kh;
 824         fdctrl->fifo[5] = ks;
 825         return;
 826     case 3:
 827         /* track too big */
 828         fdctrl_stop_transfer(fdctrl, 0x40, 0x80, 0x00);
 829         fdctrl->fifo[3] = kt;
 830         fdctrl->fifo[4] = kh;
 831         fdctrl->fifo[5] = ks;
 832         return;
 833     case 4:
 834         /* No seek enabled */
 835         fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
 836         fdctrl->fifo[3] = kt;
 837         fdctrl->fifo[4] = kh;
 838         fdctrl->fifo[5] = ks;
 839         return;
 840     case 1:
 841         did_seek = 1;
 842         break;
 843     default:
 844         break;
 845     }
 846     /* Set the FIFO state */
 847     fdctrl->data_dir = direction;
 848     fdctrl->data_pos = 0;
 849     FD_SET_STATE(fdctrl->data_state, FD_STATE_DATA); /* FIFO ready for data */
 850     if (fdctrl->fifo[0] & 0x80)
 851         fdctrl->data_state |= FD_STATE_MULTI;
 852     else
 853         fdctrl->data_state &= ~FD_STATE_MULTI;
 854     if (did_seek)
 855         fdctrl->data_state |= FD_STATE_SEEK;
 856     else
 857         fdctrl->data_state &= ~FD_STATE_SEEK;
 858     if (fdctrl->fifo[5] == 00) {
 859         fdctrl->data_len = fdctrl->fifo[8];
 860     } else {
 861         int tmp;
 862         fdctrl->data_len = 128 << fdctrl->fifo[5];
 863         tmp = (cur_drv->last_sect - ks + 1);
 864         if (fdctrl->fifo[0] & 0x80)
 865             tmp += cur_drv->last_sect;
 866         fdctrl->data_len *= tmp;
 867     }
 868     fdctrl->eot = fdctrl->fifo[6];
 869     if (fdctrl->dma_en) {
 870         int dma_mode;
 871         /* DMA transfer are enabled. Check if DMA channel is well programmed */
 872         dma_mode = DMA_get_channel_mode(fdctrl->dma_chann);
 873         dma_mode = (dma_mode >> 2) & 3;
 874         FLOPPY_DPRINTF("dma_mode=%d direction=%d (%d - %d)\n",
 875                        dma_mode, direction,
 876                        (128 << fdctrl->fifo[5]) *
 877                        (cur_drv->last_sect - ks + 1), fdctrl->data_len);
 878         if (((direction == FD_DIR_SCANE || direction == FD_DIR_SCANL ||
 879               direction == FD_DIR_SCANH) && dma_mode == 0) ||
 880             (direction == FD_DIR_WRITE && dma_mode == 2) ||
 881             (direction == FD_DIR_READ && dma_mode == 1)) {
 882             /* No access is allowed until DMA transfer has completed */
 883             fdctrl->state |= FD_CTRL_BUSY;
 884             /* Now, we just have to wait for the DMA controller to
 885              * recall us...
 886              */
 887             DMA_hold_DREQ(fdctrl->dma_chann);
 888             DMA_schedule(fdctrl->dma_chann);
 889             return;
 890         } else {
 891             FLOPPY_ERROR("dma_mode=%d direction=%d\n", dma_mode, direction);
 892         }
 893     }
 894     FLOPPY_DPRINTF("start non-DMA transfer\n");
 895     /* IO based transfer: calculate len */
 896     fdctrl_raise_irq(fdctrl, 0x00);
 897
 898     return;
 899 }
 900
 901 /* Prepare a transfer of deleted data */
 902 static void fdctrl_start_transfer_del (fdctrl_t *fdctrl, int direction)
 903 {
 904     /* We don't handle deleted data,
 905      * so we don't return *ANYTHING*
 906      */
 907     fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
 908 }
 909
 910 /* handlers for DMA transfers */
 911 static int fdctrl_transfer_handler (void *opaque, target_ulong addr, int size)
 912 {
 913     fdctrl_t *fdctrl;
 914     fdrive_t *cur_drv;
 915     int len, start_pos, rel_pos;
 916     uint8_t status0 = 0x00, status1 = 0x00, status2 = 0x00;
 917
 918     fdctrl = opaque;
 919     if (!(fdctrl->state & FD_CTRL_BUSY)) {
 920         FLOPPY_DPRINTF("Not in DMA transfer mode !\n");
 921         return 0;
 922     }
 923     cur_drv = get_cur_drv(fdctrl);
 924     if (fdctrl->data_dir == FD_DIR_SCANE || fdctrl->data_dir == FD_DIR_SCANL ||
 925         fdctrl->data_dir == FD_DIR_SCANH)
 926         status2 = 0x04;
 927     if (size > fdctrl->data_len)
 928         size = fdctrl->data_len;
 929     if (cur_drv->bs == NULL) {
 930         if (fdctrl->data_dir == FD_DIR_WRITE)
 931             fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
 932         else
 933             fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
 934         len = 0;
 935         goto transfer_error;
 936     }
 937     rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;
 938     for (start_pos = fdctrl->data_pos; fdctrl->data_pos < size;) {
 939         len = size - fdctrl->data_pos;
 940         if (len + rel_pos > FD_SECTOR_LEN)
 941             len = FD_SECTOR_LEN - rel_pos;
 942         FLOPPY_DPRINTF("copy %d bytes (%d %d %d) %d pos %d %02x %02x "
 943                        "(%d-0x%08x 0x%08x)\n", len, size, fdctrl->data_pos,
 944                        fdctrl->data_len, fdctrl->cur_drv, cur_drv->head,
 945                        cur_drv->track, cur_drv->sect, fd_sector(cur_drv),
 946                        fd_sector(cur_drv) * 512, addr);
 947         if (fdctrl->data_dir != FD_DIR_WRITE ||
 948             len < FD_SECTOR_LEN || rel_pos != 0) {
 949             /* READ & SCAN commands and realign to a sector for WRITE */
 950             if (bdrv_read(cur_drv->bs, fd_sector(cur_drv),
 951                           fdctrl->fifo, 1) < 0) {
 952                 FLOPPY_DPRINTF("Floppy: error getting sector %d\n",
 953                                fd_sector(cur_drv));
 954                 /* Sure, image size is too small... */
 955                 memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
 956             }
 957         }
 958         switch (fdctrl->data_dir) {
 959         case FD_DIR_READ:
 960             /* READ commands */
 961             cpu_physical_memory_write(addr + fdctrl->data_pos,
 962                                       fdctrl->fifo + rel_pos, len);
 963             break;
 964         case FD_DIR_WRITE:
 965             /* WRITE commands */
 966             cpu_physical_memory_read(addr + fdctrl->data_pos,
 967                                      fdctrl->fifo + rel_pos, len);
 968             if (bdrv_write(cur_drv->bs, fd_sector(cur_drv),
 969                            fdctrl->fifo, 1) < 0) {
 970                 FLOPPY_ERROR("writting sector %d\n", fd_sector(cur_drv));
 971                 fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
 972                 goto transfer_error;
 973             }
 974             break;
 975         default:
 976             /* SCAN commands */
 977             {
 978                 uint8_t tmpbuf[FD_SECTOR_LEN];
 979                 int ret;
 980                 cpu_physical_memory_read(addr + fdctrl->data_pos,
 981                                          tmpbuf, len);
 982                 ret = memcmp(tmpbuf, fdctrl->fifo + rel_pos, len);
 983                 if (ret == 0) {
 984                     status2 = 0x08;
 985                     goto end_transfer;
 986                 }
 987                 if ((ret < 0 && fdctrl->data_dir == FD_DIR_SCANL) ||
 988                     (ret > 0 && fdctrl->data_dir == FD_DIR_SCANH)) {
 989                     status2 = 0x00;
 990                     goto end_transfer;
 991                 }
 992             }
 993             break;
 994         }
 995         fdctrl->data_pos += len;
 996         rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;
 997         if (rel_pos == 0) {
 998             /* Seek to next sector */
 999             FLOPPY_DPRINTF("seek to next sector (%d %02x %02x => %d) (%d)\n",
1000                            cur_drv->head, cur_drv->track, cur_drv->sect,
1001                            fd_sector(cur_drv),
1002                            fdctrl->data_pos - size);
1003             /* XXX: cur_drv->sect >= cur_drv->last_sect should be an
1004                error in fact */
1005             if (cur_drv->sect >= cur_drv->last_sect ||
1006                 cur_drv->sect == fdctrl->eot) {
1007                 cur_drv->sect = 1;
1008                 if (FD_MULTI_TRACK(fdctrl->data_state)) {
1009                     if (cur_drv->head == 0 &&
1010                         (cur_drv->flags & FDISK_DBL_SIDES) != 0) {
1011                         cur_drv->head = 1;
1012                     } else {
1013                         cur_drv->head = 0;
1014                         cur_drv->track++;
1015                         if ((cur_drv->flags & FDISK_DBL_SIDES) == 0)
1016                             break;
1017                     }
1018                 } else {
1019                     cur_drv->track++;
1020                     break;
1021                 }
1022                 FLOPPY_DPRINTF("seek to next track (%d %02x %02x => %d)\n",
1023                                cur_drv->head, cur_drv->track,
1024                                cur_drv->sect, fd_sector(cur_drv));
1025             } else {
1026                 cur_drv->sect++;
1027             }
1028         }
1029     }
1030 end_transfer:
1031     len = fdctrl->data_pos - start_pos;
1032     FLOPPY_DPRINTF("end transfer %d %d %d\n",
1033                    fdctrl->data_pos, len, fdctrl->data_len);
1034     if (fdctrl->data_dir == FD_DIR_SCANE ||
1035         fdctrl->data_dir == FD_DIR_SCANL ||
1036         fdctrl->data_dir == FD_DIR_SCANH)
1037         status2 = 0x08;
1038     if (FD_DID_SEEK(fdctrl->data_state))
1039         status0 |= 0x20;
1040     fdctrl->data_len -= len;
1041     //    if (fdctrl->data_len == 0)
1042     fdctrl_stop_transfer(fdctrl, status0, status1, status2);
1043 transfer_error:
1044
1045     return len;
1046 }
1047
1048 /* Data register : 0x05 */
1049 static uint32_t fdctrl_read_data (fdctrl_t *fdctrl)
1050 {
1051     fdrive_t *cur_drv;
1052     uint32_t retval = 0;
1053     int pos, len;
1054
1055     cur_drv = get_cur_drv(fdctrl);
1056     fdctrl->state &= ~FD_CTRL_SLEEP;
1057     if (FD_STATE(fdctrl->data_state) == FD_STATE_CMD) {
1058         FLOPPY_ERROR("can't read data in CMD state\n");
1059         return 0;
1060     }
1061     pos = fdctrl->data_pos;
1062     if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
1063         pos %= FD_SECTOR_LEN;
1064         if (pos == 0) {
1065             len = fdctrl->data_len - fdctrl->data_pos;
1066             if (len > FD_SECTOR_LEN)
1067                 len = FD_SECTOR_LEN;
1068             bdrv_read(cur_drv->bs, fd_sector(cur_drv),
1069                       fdctrl->fifo, len);
1070         }
1071     }
1072     retval = fdctrl->fifo[pos];
1073     if (++fdctrl->data_pos == fdctrl->data_len) {
1074         fdctrl->data_pos = 0;
1075         /* Switch from transfer mode to status mode
1076          * then from status mode to command mode
1077          */
1078         if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
1079             fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
1080         } else {
1081             fdctrl_reset_fifo(fdctrl);
1082             fdctrl_reset_irq(fdctrl);
1083         }
1084     }
1085     FLOPPY_DPRINTF("data register: 0x%02x\n", retval);
1086
1087     return retval;
1088 }
1089
1090 static void fdctrl_format_sector (fdctrl_t *fdctrl)
1091 {
1092     fdrive_t *cur_drv;
1093     uint8_t kh, kt, ks;
1094     int did_seek;
1095
1096     fdctrl->cur_drv = fdctrl->fifo[1] & 1;
1097     cur_drv = get_cur_drv(fdctrl);
1098     kt = fdctrl->fifo[6];
1099     kh = fdctrl->fifo[7];
1100     ks = fdctrl->fifo[8];
1101     FLOPPY_DPRINTF("format sector at %d %d %02x %02x (%d)\n",
1102                    fdctrl->cur_drv, kh, kt, ks,
1103                    _fd_sector(kh, kt, ks, cur_drv->last_sect));
1104     did_seek = 0;
1105     switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & 0x40)) {
1106     case 2:
1107         /* sect too big */
1108         fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
1109         fdctrl->fifo[3] = kt;
1110         fdctrl->fifo[4] = kh;
1111         fdctrl->fifo[5] = ks;
1112         return;
1113     case 3:
1114         /* track too big */
1115         fdctrl_stop_transfer(fdctrl, 0x40, 0x80, 0x00);
1116         fdctrl->fifo[3] = kt;
1117         fdctrl->fifo[4] = kh;
1118         fdctrl->fifo[5] = ks;
1119         return;
1120     case 4:
1121         /* No seek enabled */
1122         fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
1123         fdctrl->fifo[3] = kt;
1124         fdctrl->fifo[4] = kh;
1125         fdctrl->fifo[5] = ks;
1126         return;
1127     case 1:
1128         did_seek = 1;
1129         fdctrl->data_state |= FD_STATE_SEEK;
1130         break;
1131     default:
1132         break;
1133     }
1134     memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
1135     if (cur_drv->bs == NULL ||
1136         bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1) < 0) {
1137         FLOPPY_ERROR("formating sector %d\n", fd_sector(cur_drv));
1138         fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
1139     } else {
1140         if (cur_drv->sect == cur_drv->last_sect) {
1141             fdctrl->data_state &= ~FD_STATE_FORMAT;
1142             /* Last sector done */
1143             if (FD_DID_SEEK(fdctrl->data_state))
1144                 fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
1145             else
1146                 fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
1147         } else {
1148             /* More to do */
1149             fdctrl->data_pos = 0;
1150             fdctrl->data_len = 4;
1151         }
1152     }
1153 }
1154
1155 static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
1156 {
1157     fdrive_t *cur_drv;
1158
1159     cur_drv = get_cur_drv(fdctrl);
1160     /* Reset mode */
1161     if (fdctrl->state & FD_CTRL_RESET) {
1162         FLOPPY_DPRINTF("Floppy controller in RESET state !\n");
1163         return;
1164     }
1165     fdctrl->state &= ~FD_CTRL_SLEEP;
1166     if (FD_STATE(fdctrl->data_state) == FD_STATE_STATUS) {
1167         FLOPPY_ERROR("can't write data in status mode\n");
1168         return;
1169     }
1170     /* Is it write command time ? */
1171     if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
1172         /* FIFO data write */
1173         fdctrl->fifo[fdctrl->data_pos++] = value;
1174         if (fdctrl->data_pos % FD_SECTOR_LEN == (FD_SECTOR_LEN - 1) ||
1175             fdctrl->data_pos == fdctrl->data_len) {
1176             bdrv_write(cur_drv->bs, fd_sector(cur_drv),
1177                        fdctrl->fifo, FD_SECTOR_LEN);
1178         }
1179         /* Switch from transfer mode to status mode
1180          * then from status mode to command mode
1181          */
1182         if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA)
1183             fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
1184         return;
1185     }
1186     if (fdctrl->data_pos == 0) {
1187         /* Command */
1188         switch (value & 0x5F) {
1189         case 0x46:
1190             /* READ variants */
1191             FLOPPY_DPRINTF("READ command\n");
1192             /* 8 parameters cmd */
1193             fdctrl->data_len = 9;
1194             goto enqueue;
1195         case 0x4C:
1196             /* READ_DELETED variants */
1197             FLOPPY_DPRINTF("READ_DELETED command\n");
1198             /* 8 parameters cmd */
1199             fdctrl->data_len = 9;
1200             goto enqueue;
1201         case 0x50:
1202             /* SCAN_EQUAL variants */
1203             FLOPPY_DPRINTF("SCAN_EQUAL command\n");
1204             /* 8 parameters cmd */
1205             fdctrl->data_len = 9;
1206             goto enqueue;
1207         case 0x56:
1208             /* VERIFY variants */
1209             FLOPPY_DPRINTF("VERIFY command\n");
1210             /* 8 parameters cmd */
1211             fdctrl->data_len = 9;
1212             goto enqueue;
1213         case 0x59:
1214             /* SCAN_LOW_OR_EQUAL variants */
1215             FLOPPY_DPRINTF("SCAN_LOW_OR_EQUAL command\n");
1216             /* 8 parameters cmd */
1217             fdctrl->data_len = 9;
1218             goto enqueue;
1219         case 0x5D:
1220             /* SCAN_HIGH_OR_EQUAL variants */
1221             FLOPPY_DPRINTF("SCAN_HIGH_OR_EQUAL command\n");
1222             /* 8 parameters cmd */
1223             fdctrl->data_len = 9;
1224             goto enqueue;
1225         default:
1226             break;
1227         }
1228         switch (value & 0x7F) {
1229         case 0x45:
1230             /* WRITE variants */
1231             FLOPPY_DPRINTF("WRITE command\n");
1232             /* 8 parameters cmd */
1233             fdctrl->data_len = 9;
1234             goto enqueue;
1235         case 0x49:
1236             /* WRITE_DELETED variants */
1237             FLOPPY_DPRINTF("WRITE_DELETED command\n");
1238             /* 8 parameters cmd */
1239             fdctrl->data_len = 9;
1240             goto enqueue;
1241         default:
1242             break;
1243         }
1244         switch (value) {
1245         case 0x03:
1246             /* SPECIFY */
1247             FLOPPY_DPRINTF("SPECIFY command\n");
1248             /* 1 parameter cmd */
1249             fdctrl->data_len = 3;
1250             goto enqueue;
1251         case 0x04:
1252             /* SENSE_DRIVE_STATUS */
1253             FLOPPY_DPRINTF("SENSE_DRIVE_STATUS command\n");
1254             /* 1 parameter cmd */
1255             fdctrl->data_len = 2;
1256             goto enqueue;
1257         case 0x07:
1258             /* RECALIBRATE */
1259             FLOPPY_DPRINTF("RECALIBRATE command\n");
1260             /* 1 parameter cmd */
1261             fdctrl->data_len = 2;
1262             goto enqueue;
1263         case 0x08:
1264             /* SENSE_INTERRUPT_STATUS */
1265             FLOPPY_DPRINTF("SENSE_INTERRUPT_STATUS command (%02x)\n",
1266                            fdctrl->int_status);
1267             /* No parameters cmd: returns status if no interrupt */
1268             fdctrl->fifo[0] =
1269                 fdctrl->int_status | (cur_drv->head << 2) | fdctrl->cur_drv;
1270             fdctrl->fifo[1] = cur_drv->track;
1271             fdctrl_set_fifo(fdctrl, 2, 0);
1272             fdctrl_reset_irq(fdctrl);
1273             fdctrl->int_status = 0xC0;
1274             return;
1275         case 0x0E:
1276             /* DUMPREG */
1277             FLOPPY_DPRINTF("DUMPREG command\n");
1278             /* Drives position */
1279             fdctrl->fifo[0] = drv0(fdctrl)->track;
1280             fdctrl->fifo[1] = drv1(fdctrl)->track;
1281             fdctrl->fifo[2] = 0;
1282             fdctrl->fifo[3] = 0;
1283             /* timers */
1284             fdctrl->fifo[4] = fdctrl->timer0;
1285             fdctrl->fifo[5] = (fdctrl->timer1 << 1) | fdctrl->dma_en;
1286             fdctrl->fifo[6] = cur_drv->last_sect;
1287             fdctrl->fifo[7] = (fdctrl->lock << 7) |
1288                     (cur_drv->perpendicular << 2);
1289             fdctrl->fifo[8] = fdctrl->config;
1290             fdctrl->fifo[9] = fdctrl->precomp_trk;
1291             fdctrl_set_fifo(fdctrl, 10, 0);
1292             return;
1293         case 0x0F:
1294             /* SEEK */
1295             FLOPPY_DPRINTF("SEEK command\n");
1296             /* 2 parameters cmd */
1297             fdctrl->data_len = 3;
1298             goto enqueue;
1299         case 0x10:
1300             /* VERSION */
1301             FLOPPY_DPRINTF("VERSION command\n");
1302             /* No parameters cmd */
1303             /* Controller's version */
1304             fdctrl->fifo[0] = fdctrl->version;
1305             fdctrl_set_fifo(fdctrl, 1, 1);
1306             return;
1307         case 0x12:
1308             /* PERPENDICULAR_MODE */
1309             FLOPPY_DPRINTF("PERPENDICULAR_MODE command\n");
1310             /* 1 parameter cmd */
1311             fdctrl->data_len = 2;
1312             goto enqueue;
1313         case 0x13:
1314             /* CONFIGURE */
1315             FLOPPY_DPRINTF("CONFIGURE command\n");
1316             /* 3 parameters cmd */
1317             fdctrl->data_len = 4;
1318             goto enqueue;
1319         case 0x14:
1320             /* UNLOCK */
1321             FLOPPY_DPRINTF("UNLOCK command\n");
1322             /* No parameters cmd */
1323             fdctrl->lock = 0;
1324             fdctrl->fifo[0] = 0;
1325             fdctrl_set_fifo(fdctrl, 1, 0);
1326             return;
1327         case 0x17:
1328             /* POWERDOWN_MODE */
1329             FLOPPY_DPRINTF("POWERDOWN_MODE command\n");
1330             /* 2 parameters cmd */
1331             fdctrl->data_len = 3;
1332             goto enqueue;
1333         case 0x18:
1334             /* PART_ID */
1335             FLOPPY_DPRINTF("PART_ID command\n");
1336             /* No parameters cmd */
1337             fdctrl->fifo[0] = 0x41; /* Stepping 1 */
1338             fdctrl_set_fifo(fdctrl, 1, 0);
1339             return;
1340         case 0x2C:
1341             /* SAVE */
1342             FLOPPY_DPRINTF("SAVE command\n");
1343             /* No parameters cmd */
1344             fdctrl->fifo[0] = 0;
1345             fdctrl->fifo[1] = 0;
1346             /* Drives position */
1347             fdctrl->fifo[2] = drv0(fdctrl)->track;
1348             fdctrl->fifo[3] = drv1(fdctrl)->track;
1349             fdctrl->fifo[4] = 0;
1350             fdctrl->fifo[5] = 0;
1351             /* timers */
1352             fdctrl->fifo[6] = fdctrl->timer0;
1353             fdctrl->fifo[7] = fdctrl->timer1;
1354             fdctrl->fifo[8] = cur_drv->last_sect;
1355             fdctrl->fifo[9] = (fdctrl->lock << 7) |
1356                     (cur_drv->perpendicular << 2);
1357             fdctrl->fifo[10] = fdctrl->config;
1358             fdctrl->fifo[11] = fdctrl->precomp_trk;
1359             fdctrl->fifo[12] = fdctrl->pwrd;
1360             fdctrl->fifo[13] = 0;
1361             fdctrl->fifo[14] = 0;
1362             fdctrl_set_fifo(fdctrl, 15, 1);
1363             return;
1364         case 0x33:
1365             /* OPTION */
1366             FLOPPY_DPRINTF("OPTION command\n");
1367             /* 1 parameter cmd */
1368             fdctrl->data_len = 2;
1369             goto enqueue;
1370         case 0x42:
1371             /* READ_TRACK */
1372             FLOPPY_DPRINTF("READ_TRACK command\n");
1373             /* 8 parameters cmd */
1374             fdctrl->data_len = 9;
1375             goto enqueue;
1376         case 0x4A:
1377             /* READ_ID */
1378             FLOPPY_DPRINTF("READ_ID command\n");
1379             /* 1 parameter cmd */
1380             fdctrl->data_len = 2;
1381             goto enqueue;
1382         case 0x4C:
1383             /* RESTORE */
1384             FLOPPY_DPRINTF("RESTORE command\n");
1385             /* 17 parameters cmd */
1386             fdctrl->data_len = 18;
1387             goto enqueue;
1388         case 0x4D:
1389             /* FORMAT_TRACK */
1390             FLOPPY_DPRINTF("FORMAT_TRACK command\n");
1391             /* 5 parameters cmd */
1392             fdctrl->data_len = 6;
1393             goto enqueue;
1394         case 0x8E:
1395             /* DRIVE_SPECIFICATION_COMMAND */
1396             FLOPPY_DPRINTF("DRIVE_SPECIFICATION_COMMAND command\n");
1397             /* 5 parameters cmd */
1398             fdctrl->data_len = 6;
1399             goto enqueue;
1400         case 0x8F:
1401             /* RELATIVE_SEEK_OUT */
1402             FLOPPY_DPRINTF("RELATIVE_SEEK_OUT command\n");
1403             /* 2 parameters cmd */
1404             fdctrl->data_len = 3;
1405             goto enqueue;
1406         case 0x94:
1407             /* LOCK */
1408             FLOPPY_DPRINTF("LOCK command\n");
1409             /* No parameters cmd */
1410             fdctrl->lock = 1;
1411             fdctrl->fifo[0] = 0x10;
1412             fdctrl_set_fifo(fdctrl, 1, 1);
1413             return;
1414         case 0xCD:
1415             /* FORMAT_AND_WRITE */
1416             FLOPPY_DPRINTF("FORMAT_AND_WRITE command\n");
1417             /* 10 parameters cmd */
1418             fdctrl->data_len = 11;
1419             goto enqueue;
1420         case 0xCF:
1421             /* RELATIVE_SEEK_IN */
1422             FLOPPY_DPRINTF("RELATIVE_SEEK_IN command\n");
1423             /* 2 parameters cmd */
1424             fdctrl->data_len = 3;
1425             goto enqueue;
1426         default:
1427             /* Unknown command */
1428             FLOPPY_ERROR("unknown command: 0x%02x\n", value);
1429             fdctrl_unimplemented(fdctrl);
1430             return;
1431         }
1432     }
1433 enqueue:
1434     FLOPPY_DPRINTF("%s: %02x\n", __func__, value);
1435     fdctrl->fifo[fdctrl->data_pos] = value;
1436     if (++fdctrl->data_pos == fdctrl->data_len) {
1437         /* We now have all parameters
1438          * and will be able to treat the command
1439          */
1440         if (fdctrl->data_state & FD_STATE_FORMAT) {
1441             fdctrl_format_sector(fdctrl);
1442             return;
1443         }
1444         switch (fdctrl->fifo[0] & 0x1F) {
1445         case 0x06:
1446         {
1447             /* READ variants */
1448             FLOPPY_DPRINTF("treat READ command\n");
1449             fdctrl_start_transfer(fdctrl, FD_DIR_READ);
1450             return;
1451         }
1452         case 0x0C:
1453             /* READ_DELETED variants */
1454 //            FLOPPY_DPRINTF("treat READ_DELETED command\n");
1455             FLOPPY_ERROR("treat READ_DELETED command\n");
1456             fdctrl_start_transfer_del(fdctrl, FD_DIR_READ);
1457             return;
1458         case 0x16:
1459             /* VERIFY variants */
1460 //            FLOPPY_DPRINTF("treat VERIFY command\n");
1461             FLOPPY_ERROR("treat VERIFY command\n");
1462             fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
1463             return;
1464         case 0x10:
1465             /* SCAN_EQUAL variants */
1466 //            FLOPPY_DPRINTF("treat SCAN_EQUAL command\n");
1467             FLOPPY_ERROR("treat SCAN_EQUAL command\n");
1468             fdctrl_start_transfer(fdctrl, FD_DIR_SCANE);
1469             return;
1470         case 0x19:
1471             /* SCAN_LOW_OR_EQUAL variants */
1472 //            FLOPPY_DPRINTF("treat SCAN_LOW_OR_EQUAL command\n");
1473             FLOPPY_ERROR("treat SCAN_LOW_OR_EQUAL command\n");
1474             fdctrl_start_transfer(fdctrl, FD_DIR_SCANL);
1475             return;
1476         case 0x1D:
1477             /* SCAN_HIGH_OR_EQUAL variants */
1478 //            FLOPPY_DPRINTF("treat SCAN_HIGH_OR_EQUAL command\n");
1479             FLOPPY_ERROR("treat SCAN_HIGH_OR_EQUAL command\n");
1480             fdctrl_start_transfer(fdctrl, FD_DIR_SCANH);
1481             return;
1482         default:
1483             break;
1484         }
1485         switch (fdctrl->fifo[0] & 0x3F) {
1486         case 0x05:
1487             /* WRITE variants */
1488             FLOPPY_DPRINTF("treat WRITE command (%02x)\n", fdctrl->fifo[0]);
1489             fdctrl_start_transfer(fdctrl, FD_DIR_WRITE);
1490             return;
1491         case 0x09:
1492             /* WRITE_DELETED variants */
1493 //            FLOPPY_DPRINTF("treat WRITE_DELETED command\n");
1494             FLOPPY_ERROR("treat WRITE_DELETED command\n");
1495             fdctrl_start_transfer_del(fdctrl, FD_DIR_WRITE);
1496             return;
1497         default:
1498             break;
1499         }
1500         switch (fdctrl->fifo[0]) {
1501         case 0x03:
1502             /* SPECIFY */
1503             FLOPPY_DPRINTF("treat SPECIFY command\n");
1504             fdctrl->timer0 = (fdctrl->fifo[1] >> 4) & 0xF;
1505             fdctrl->timer1 = fdctrl->fifo[2] >> 1;
1506             fdctrl->dma_en = 1 - (fdctrl->fifo[2] & 1) ;
1507             /* No result back */
1508             fdctrl_reset_fifo(fdctrl);
1509             break;
1510         case 0x04:
1511             /* SENSE_DRIVE_STATUS */
1512             FLOPPY_DPRINTF("treat SENSE_DRIVE_STATUS command\n");
1513             fdctrl->cur_drv = fdctrl->fifo[1] & 1;
1514             cur_drv = get_cur_drv(fdctrl);
1515             cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
1516             /* 1 Byte status back */
1517             fdctrl->fifo[0] = (cur_drv->ro << 6) |
1518                 (cur_drv->track == 0 ? 0x10 : 0x00) |
1519                 (cur_drv->head << 2) |
1520                 fdctrl->cur_drv |
1521                 0x28;
1522             fdctrl_set_fifo(fdctrl, 1, 0);
1523             break;
1524         case 0x07:
1525             /* RECALIBRATE */
1526             FLOPPY_DPRINTF("treat RECALIBRATE command\n");
1527             fdctrl->cur_drv = fdctrl->fifo[1] & 1;
1528             cur_drv = get_cur_drv(fdctrl);
1529             fd_recalibrate(cur_drv);
1530             fdctrl_reset_fifo(fdctrl);
1531             /* Raise Interrupt */
1532             fdctrl_raise_irq(fdctrl, 0x20);
1533             break;
1534         case 0x0F:
1535             /* SEEK */
1536             FLOPPY_DPRINTF("treat SEEK command\n");
1537             fdctrl->cur_drv = fdctrl->fifo[1] & 1;
1538             cur_drv = get_cur_drv(fdctrl);
1539             fd_start(cur_drv);
1540             if (fdctrl->fifo[2] <= cur_drv->track)
1541                 cur_drv->dir = 1;
1542             else
1543                 cur_drv->dir = 0;
1544             fdctrl_reset_fifo(fdctrl);
1545             if (fdctrl->fifo[2] > cur_drv->max_track) {
1546                 fdctrl_raise_irq(fdctrl, 0x60);
1547             } else {
1548                 cur_drv->track = fdctrl->fifo[2];
1549                 /* Raise Interrupt */
1550                 fdctrl_raise_irq(fdctrl, 0x20);
1551             }
1552             break;
1553         case 0x12:
1554             /* PERPENDICULAR_MODE */
1555             FLOPPY_DPRINTF("treat PERPENDICULAR_MODE command\n");
1556             if (fdctrl->fifo[1] & 0x80)
1557                 cur_drv->perpendicular = fdctrl->fifo[1] & 0x7;
1558             /* No result back */
1559             fdctrl_reset_fifo(fdctrl);
1560             break;
1561         case 0x13:
1562             /* CONFIGURE */
1563             FLOPPY_DPRINTF("treat CONFIGURE command\n");
1564             fdctrl->config = fdctrl->fifo[2];
1565             fdctrl->precomp_trk =  fdctrl->fifo[3];
1566             /* No result back */
1567             fdctrl_reset_fifo(fdctrl);
1568             break;
1569         case 0x17:
1570             /* POWERDOWN_MODE */
1571             FLOPPY_DPRINTF("treat POWERDOWN_MODE command\n");
1572             fdctrl->pwrd = fdctrl->fifo[1];
1573             fdctrl->fifo[0] = fdctrl->fifo[1];
1574             fdctrl_set_fifo(fdctrl, 1, 1);
1575             break;
1576         case 0x33:
1577             /* OPTION */
1578             FLOPPY_DPRINTF("treat OPTION command\n");
1579             /* No result back */
1580             fdctrl_reset_fifo(fdctrl);
1581             break;
1582         case 0x42:
1583             /* READ_TRACK */
1584 //            FLOPPY_DPRINTF("treat READ_TRACK command\n");
1585             FLOPPY_ERROR("treat READ_TRACK command\n");
1586             fdctrl_start_transfer(fdctrl, FD_DIR_READ);
1587             break;
1588         case 0x4A:
1589                 /* READ_ID */
1590             FLOPPY_DPRINTF("treat READ_ID command\n");
1591             /* XXX: should set main status register to busy */
1592             cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
1593             qemu_mod_timer(fdctrl->result_timer,
1594                            qemu_get_clock(vm_clock) + (ticks_per_sec / 50));
1595             break;
1596         case 0x4C:
1597             /* RESTORE */
1598             FLOPPY_DPRINTF("treat RESTORE command\n");
1599             /* Drives position */
1600             drv0(fdctrl)->track = fdctrl->fifo[3];
1601             drv1(fdctrl)->track = fdctrl->fifo[4];
1602             /* timers */
1603             fdctrl->timer0 = fdctrl->fifo[7];
1604             fdctrl->timer1 = fdctrl->fifo[8];
1605             cur_drv->last_sect = fdctrl->fifo[9];
1606             fdctrl->lock = fdctrl->fifo[10] >> 7;
1607             cur_drv->perpendicular = (fdctrl->fifo[10] >> 2) & 0xF;
1608             fdctrl->config = fdctrl->fifo[11];
1609             fdctrl->precomp_trk = fdctrl->fifo[12];
1610             fdctrl->pwrd = fdctrl->fifo[13];
1611             fdctrl_reset_fifo(fdctrl);
1612             break;
1613         case 0x4D:
1614             /* FORMAT_TRACK */
1615             FLOPPY_DPRINTF("treat FORMAT_TRACK command\n");
1616             fdctrl->cur_drv = fdctrl->fifo[1] & 1;
1617             cur_drv = get_cur_drv(fdctrl);
1618             fdctrl->data_state |= FD_STATE_FORMAT;
1619             if (fdctrl->fifo[0] & 0x80)
1620                 fdctrl->data_state |= FD_STATE_MULTI;
1621             else
1622                 fdctrl->data_state &= ~FD_STATE_MULTI;
1623             fdctrl->data_state &= ~FD_STATE_SEEK;
1624             cur_drv->bps =
1625                 fdctrl->fifo[2] > 7 ? 16384 : 128 << fdctrl->fifo[2];
1626 #if 0
1627             cur_drv->last_sect =
1628                 cur_drv->flags & FDISK_DBL_SIDES ? fdctrl->fifo[3] :
1629                 fdctrl->fifo[3] / 2;
1630 #else
1631             cur_drv->last_sect = fdctrl->fifo[3];
1632 #endif
1633             /* Bochs BIOS is buggy and don't send format informations
1634              * for each sector. So, pretend all's done right now...
1635              */
1636             fdctrl->data_state &= ~FD_STATE_FORMAT;
1637             fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
1638             break;
1639         case 0x8E:
1640             /* DRIVE_SPECIFICATION_COMMAND */
1641             FLOPPY_DPRINTF("treat DRIVE_SPECIFICATION_COMMAND command\n");
1642             if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x80) {
1643                 /* Command parameters done */
1644                 if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x40) {
1645                     fdctrl->fifo[0] = fdctrl->fifo[1];
1646                     fdctrl->fifo[2] = 0;
1647                     fdctrl->fifo[3] = 0;
1648                     fdctrl_set_fifo(fdctrl, 4, 1);
1649                 } else {
1650                     fdctrl_reset_fifo(fdctrl);
1651                 }
1652             } else if (fdctrl->data_len > 7) {
1653                 /* ERROR */
1654                 fdctrl->fifo[0] = 0x80 |
1655                     (cur_drv->head << 2) | fdctrl->cur_drv;
1656                 fdctrl_set_fifo(fdctrl, 1, 1);
1657             }
1658             break;
1659         case 0x8F:
1660             /* RELATIVE_SEEK_OUT */
1661             FLOPPY_DPRINTF("treat RELATIVE_SEEK_OUT command\n");
1662             fdctrl->cur_drv = fdctrl->fifo[1] & 1;
1663             cur_drv = get_cur_drv(fdctrl);
1664             fd_start(cur_drv);
1665                 cur_drv->dir = 0;
1666             if (fdctrl->fifo[2] + cur_drv->track >= cur_drv->max_track) {
1667                 cur_drv->track = cur_drv->max_track - 1;
1668             } else {
1669                 cur_drv->track += fdctrl->fifo[2];
1670             }
1671             fdctrl_reset_fifo(fdctrl);
1672             fdctrl_raise_irq(fdctrl, 0x20);
1673             break;
1674         case 0xCD:
1675             /* FORMAT_AND_WRITE */
1676 //                FLOPPY_DPRINTF("treat FORMAT_AND_WRITE command\n");
1677             FLOPPY_ERROR("treat FORMAT_AND_WRITE command\n");
1678             fdctrl_unimplemented(fdctrl);
1679             break;
1680         case 0xCF:
1681                 /* RELATIVE_SEEK_IN */
1682             FLOPPY_DPRINTF("treat RELATIVE_SEEK_IN command\n");
1683             fdctrl->cur_drv = fdctrl->fifo[1] & 1;
1684             cur_drv = get_cur_drv(fdctrl);
1685             fd_start(cur_drv);
1686                 cur_drv->dir = 1;
1687             if (fdctrl->fifo[2] > cur_drv->track) {
1688                 cur_drv->track = 0;
1689             } else {
1690                 cur_drv->track -= fdctrl->fifo[2];
1691             }
1692             fdctrl_reset_fifo(fdctrl);
1693             /* Raise Interrupt */
1694             fdctrl_raise_irq(fdctrl, 0x20);
1695             break;
1696         }
1697     }
1698 }
1699
1700 static void fdctrl_result_timer(void *opaque)
1701 {
1702     fdctrl_t *fdctrl = opaque;
1703     fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
1704 }