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

   1 /*
   2  * QEMU Floppy disk emulator
   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 controler 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     /* Controler's identification */
 367     uint8_t version;
 368     /* HW */
 369     int irq_lvl;
 370     int dma_chann;
 371     uint32_t io_base;
 372     /* Controler 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     /* States kept only to be returned back */
 386     /* Timers state */
 387     uint8_t timer0;
 388     uint8_t timer1;
 389     /* precompensation */
 390     uint8_t precomp_trk;
 391     uint8_t config;
 392     uint8_t lock;
 393     /* Power down config (also with status regB access mode */
 394     uint8_t pwrd;
 395     /* Floppy drives */
 396     fdrive_t drives[2];
 397 };
 398
 399 static uint32_t fdctrl_read (void *opaque, uint32_t reg)
 400 {
 401     fdctrl_t *fdctrl = opaque;
 402     uint32_t retval;
 403
 404     switch (reg & 0x07) {
 405     case 0x01:
 406         retval = fdctrl_read_statusB(fdctrl);
 407         break;
 408     case 0x02:
 409         retval = fdctrl_read_dor(fdctrl);
 410         break;
 411     case 0x03:
 412         retval = fdctrl_read_tape(fdctrl);
 413         break;
 414     case 0x04:
 415         retval = fdctrl_read_main_status(fdctrl);
 416         break;
 417     case 0x05:
 418         retval = fdctrl_read_data(fdctrl);
 419         break;
 420     case 0x07:
 421         retval = fdctrl_read_dir(fdctrl);
 422         break;
 423     default:
 424         retval = (uint32_t)(-1);
 425         break;
 426     }
 427     FLOPPY_DPRINTF("read reg%d: 0x%02x\n", reg & 7, retval);
 428
 429     return retval;
 430 }
 431
 432 static void fdctrl_write (void *opaque, uint32_t reg, uint32_t value)
 433 {
 434     fdctrl_t *fdctrl = opaque;
 435
 436     FLOPPY_DPRINTF("write reg%d: 0x%02x\n", reg & 7, value);
 437
 438     switch (reg & 0x07) {
 439     case 0x02:
 440         fdctrl_write_dor(fdctrl, value);
 441         break;
 442     case 0x03:
 443         fdctrl_write_tape(fdctrl, value);
 444         break;
 445     case 0x04:
 446         fdctrl_write_rate(fdctrl, value);
 447         break;
 448     case 0x05:
 449         fdctrl_write_data(fdctrl, value);
 450         break;
 451     default:
 452         break;
 453     }
 454 }
 455
 456 static void fd_change_cb (void *opaque)
 457 {
 458     fdrive_t *drv = opaque;
 459
 460     FLOPPY_DPRINTF("disk change\n");
 461     fd_revalidate(drv);
 462 #if 0
 463     fd_recalibrate(drv);
 464     fdctrl_reset_fifo(drv->fdctrl);
 465     fdctrl_raise_irq(drv->fdctrl, 0x20);
 466 #endif
 467 }
 468
 469 fdctrl_t *fdctrl_init (int irq_lvl, int dma_chann, int mem_mapped,
 470                        uint32_t io_base,
 471                        BlockDriverState **fds)
 472 {
 473     fdctrl_t *fdctrl;
 474 //    int io_mem;
 475     int i;
 476
 477     FLOPPY_DPRINTF("init controler\n");
 478     fdctrl = qemu_mallocz(sizeof(fdctrl_t));
 479     if (!fdctrl)
 480         return NULL;
 481     fdctrl->result_timer = qemu_new_timer(vm_clock,
 482                                           fdctrl_result_timer, fdctrl);
 483
 484     fdctrl->version = 0x90; /* Intel 82078 controler */
 485     fdctrl->irq_lvl = irq_lvl;
 486     fdctrl->dma_chann = dma_chann;
 487     fdctrl->io_base = io_base;
 488     fdctrl->config = 0x60; /* Implicit seek, polling & FIFO enabled */
 489     if (fdctrl->dma_chann != -1) {
 490         fdctrl->dma_en = 1;
 491         DMA_register_channel(dma_chann, &fdctrl_transfer_handler, fdctrl);
 492     } else {
 493         fdctrl->dma_en = 0;
 494     }
 495     for (i = 0; i < 2; i++) {
 496         fd_init(&fdctrl->drives[i], fds[i]);
 497         if (fds[i]) {
 498             bdrv_set_change_cb(fds[i],
 499                                &fd_change_cb, &fdctrl->drives[i]);
 500         }
 501     }
 502     fdctrl_reset(fdctrl, 0);
 503     fdctrl->state = FD_CTRL_ACTIVE;
 504     if (mem_mapped) {
 505         FLOPPY_ERROR("memory mapped floppy not supported by now !\n");
 506 #if 0
 507         io_mem = cpu_register_io_memory(0, fdctrl_mem_read, fdctrl_mem_write);
 508         cpu_register_physical_memory(base, 0x08, io_mem);
 509 #endif
 510     } else {
 511         register_ioport_read(io_base + 0x01, 5, 1, &fdctrl_read, fdctrl);
 512         register_ioport_read(io_base + 0x07, 1, 1, &fdctrl_read, fdctrl);
 513         register_ioport_write(io_base + 0x01, 5, 1, &fdctrl_write, fdctrl);
 514         register_ioport_write(io_base + 0x07, 1, 1, &fdctrl_write, fdctrl);
 515     }
 516     for (i = 0; i < 2; i++) {
 517         fd_revalidate(&fdctrl->drives[i]);
 518     }
 519
 520     return fdctrl;
 521 }
 522
 523 /* XXX: may change if moved to bdrv */
 524 int fdctrl_get_drive_type(fdctrl_t *fdctrl, int drive_num)
 525 {
 526     return fdctrl->drives[drive_num].drive;
 527 }
 528
 529 /* Change IRQ state */
 530 static void fdctrl_reset_irq (fdctrl_t *fdctrl)
 531 {
 532     FLOPPY_DPRINTF("Reset interrupt\n");
 533     pic_set_irq(fdctrl->irq_lvl, 0);
 534     fdctrl->state &= ~FD_CTRL_INTR;
 535 }
 536
 537 static void fdctrl_raise_irq (fdctrl_t *fdctrl, uint8_t status)
 538 {
 539     if (~(fdctrl->state & FD_CTRL_INTR)) {
 540         pic_set_irq(fdctrl->irq_lvl, 1);
 541         fdctrl->state |= FD_CTRL_INTR;
 542     }
 543     FLOPPY_DPRINTF("Set interrupt status to 0x%02x\n", status);
 544     fdctrl->int_status = status;
 545 }
 546
 547 /* Reset controler */
 548 static void fdctrl_reset (fdctrl_t *fdctrl, int do_irq)
 549 {
 550     int i;
 551
 552     FLOPPY_DPRINTF("reset controler\n");
 553     fdctrl_reset_irq(fdctrl);
 554     /* Initialise controler */
 555     fdctrl->cur_drv = 0;
 556     /* FIFO state */
 557     fdctrl->data_pos = 0;
 558     fdctrl->data_len = 0;
 559     fdctrl->data_state = FD_STATE_CMD;
 560     fdctrl->data_dir = FD_DIR_WRITE;
 561     for (i = 0; i < MAX_FD; i++)
 562         fd_reset(&fdctrl->drives[i]);
 563     fdctrl_reset_fifo(fdctrl);
 564     if (do_irq)
 565         fdctrl_raise_irq(fdctrl, 0xc0);
 566 }
 567
 568 static inline fdrive_t *drv0 (fdctrl_t *fdctrl)
 569 {
 570     return &fdctrl->drives[fdctrl->bootsel];
 571 }
 572
 573 static inline fdrive_t *drv1 (fdctrl_t *fdctrl)
 574 {
 575     return &fdctrl->drives[1 - fdctrl->bootsel];
 576 }
 577
 578 static fdrive_t *get_cur_drv (fdctrl_t *fdctrl)
 579 {
 580     return fdctrl->cur_drv == 0 ? drv0(fdctrl) : drv1(fdctrl);
 581 }
 582
 583 /* Status B register : 0x01 (read-only) */
 584 static uint32_t fdctrl_read_statusB (fdctrl_t *fdctrl)
 585 {
 586     FLOPPY_DPRINTF("status register: 0x00\n");
 587     return 0;
 588 }
 589
 590 /* Digital output register : 0x02 */
 591 static uint32_t fdctrl_read_dor (fdctrl_t *fdctrl)
 592 {
 593     uint32_t retval = 0;
 594
 595     /* Drive motors state indicators */
 596     if (drv0(fdctrl)->drflags & FDRIVE_MOTOR_ON)
 597         retval |= 1 << 5;
 598     if (drv1(fdctrl)->drflags & FDRIVE_MOTOR_ON)
 599         retval |= 1 << 4;
 600     /* DMA enable */
 601     retval |= fdctrl->dma_en << 3;
 602     /* Reset indicator */
 603     retval |= (fdctrl->state & FD_CTRL_RESET) == 0 ? 0x04 : 0;
 604     /* Selected drive */
 605     retval |= fdctrl->cur_drv;
 606     FLOPPY_DPRINTF("digital output register: 0x%02x\n", retval);
 607
 608     return retval;
 609 }
 610
 611 static void fdctrl_write_dor (fdctrl_t *fdctrl, uint32_t value)
 612 {
 613     /* Reset mode */
 614     if (fdctrl->state & FD_CTRL_RESET) {
 615         if (!(value & 0x04)) {
 616             FLOPPY_DPRINTF("Floppy controler in RESET state !\n");
 617             return;
 618         }
 619     }
 620     FLOPPY_DPRINTF("digital output register set to 0x%02x\n", value);
 621     /* Drive motors state indicators */
 622     if (value & 0x20)
 623         fd_start(drv1(fdctrl));
 624     else
 625         fd_stop(drv1(fdctrl));
 626     if (value & 0x10)
 627         fd_start(drv0(fdctrl));
 628     else
 629         fd_stop(drv0(fdctrl));
 630     /* DMA enable */
 631 #if 0
 632     if (fdctrl->dma_chann != -1)
 633         fdctrl->dma_en = 1 - ((value >> 3) & 1);
 634 #endif
 635     /* Reset */
 636     if (!(value & 0x04)) {
 637         if (!(fdctrl->state & FD_CTRL_RESET)) {
 638             FLOPPY_DPRINTF("controler enter RESET state\n");
 639             fdctrl->state |= FD_CTRL_RESET;
 640         }
 641     } else {
 642         if (fdctrl->state & FD_CTRL_RESET) {
 643             FLOPPY_DPRINTF("controler out of RESET state\n");
 644             fdctrl_reset(fdctrl, 1);
 645             fdctrl->state &= ~(FD_CTRL_RESET | FD_CTRL_SLEEP);
 646         }
 647     }
 648     /* Selected drive */
 649     fdctrl->cur_drv = value & 1;
 650 }
 651
 652 /* Tape drive register : 0x03 */
 653 static uint32_t fdctrl_read_tape (fdctrl_t *fdctrl)
 654 {
 655     uint32_t retval = 0;
 656
 657     /* Disk boot selection indicator */
 658     retval |= fdctrl->bootsel << 2;
 659     /* Tape indicators: never allowed */
 660     FLOPPY_DPRINTF("tape drive register: 0x%02x\n", retval);
 661
 662     return retval;
 663 }
 664
 665 static void fdctrl_write_tape (fdctrl_t *fdctrl, uint32_t value)
 666 {
 667     /* Reset mode */
 668     if (fdctrl->state & FD_CTRL_RESET) {
 669         FLOPPY_DPRINTF("Floppy controler in RESET state !\n");
 670         return;
 671     }
 672     FLOPPY_DPRINTF("tape drive register set to 0x%02x\n", value);
 673     /* Disk boot selection indicator */
 674     fdctrl->bootsel = (value >> 2) & 1;
 675     /* Tape indicators: never allow */
 676 }
 677
 678 /* Main status register : 0x04 (read) */
 679 static uint32_t fdctrl_read_main_status (fdctrl_t *fdctrl)
 680 {
 681     uint32_t retval = 0;
 682
 683     fdctrl->state &= ~(FD_CTRL_SLEEP | FD_CTRL_RESET);
 684     if (!(fdctrl->state & FD_CTRL_BUSY)) {
 685         /* Data transfer allowed */
 686         retval |= 0x80;
 687         /* Data transfer direction indicator */
 688         if (fdctrl->data_dir == FD_DIR_READ)
 689             retval |= 0x40;
 690     }
 691     /* Should handle 0x20 for SPECIFY command */
 692     /* Command busy indicator */
 693     if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA ||
 694         FD_STATE(fdctrl->data_state) == FD_STATE_STATUS)
 695         retval |= 0x10;
 696     FLOPPY_DPRINTF("main status register: 0x%02x\n", retval);
 697
 698     return retval;
 699 }
 700
 701 /* Data select rate register : 0x04 (write) */
 702 static void fdctrl_write_rate (fdctrl_t *fdctrl, uint32_t value)
 703 {
 704     /* Reset mode */
 705     if (fdctrl->state & FD_CTRL_RESET) {
 706             FLOPPY_DPRINTF("Floppy controler in RESET state !\n");
 707             return;
 708         }
 709     FLOPPY_DPRINTF("select rate register set to 0x%02x\n", value);
 710     /* Reset: autoclear */
 711     if (value & 0x80) {
 712         fdctrl->state |= FD_CTRL_RESET;
 713         fdctrl_reset(fdctrl, 1);
 714         fdctrl->state &= ~FD_CTRL_RESET;
 715     }
 716     if (value & 0x40) {
 717         fdctrl->state |= FD_CTRL_SLEEP;
 718         fdctrl_reset(fdctrl, 1);
 719     }
 720 //        fdctrl.precomp = (value >> 2) & 0x07;
 721 }
 722
 723 /* Digital input register : 0x07 (read-only) */
 724 static uint32_t fdctrl_read_dir (fdctrl_t *fdctrl)
 725 {
 726     uint32_t retval = 0;
 727
 728     if (drv0(fdctrl)->drflags & FDRIVE_REVALIDATE ||
 729         drv1(fdctrl)->drflags & FDRIVE_REVALIDATE)
 730         retval |= 0x80;
 731     if (retval != 0)
 732         FLOPPY_DPRINTF("Floppy digital input register: 0x%02x\n", retval);
 733     drv0(fdctrl)->drflags &= ~FDRIVE_REVALIDATE;
 734     drv1(fdctrl)->drflags &= ~FDRIVE_REVALIDATE;
 735
 736     return retval;
 737 }
 738
 739 /* FIFO state control */
 740 static void fdctrl_reset_fifo (fdctrl_t *fdctrl)
 741 {
 742     fdctrl->data_dir = FD_DIR_WRITE;
 743     fdctrl->data_pos = 0;
 744     FD_SET_STATE(fdctrl->data_state, FD_STATE_CMD);
 745 }
 746
 747 /* Set FIFO status for the host to read */
 748 static void fdctrl_set_fifo (fdctrl_t *fdctrl, int fifo_len, int do_irq)
 749 {
 750     fdctrl->data_dir = FD_DIR_READ;
 751     fdctrl->data_len = fifo_len;
 752     fdctrl->data_pos = 0;
 753     FD_SET_STATE(fdctrl->data_state, FD_STATE_STATUS);
 754     if (do_irq)
 755         fdctrl_raise_irq(fdctrl, 0x00);
 756 }
 757
 758 /* Set an error: unimplemented/unknown command */
 759 static void fdctrl_unimplemented (fdctrl_t *fdctrl)
 760 {
 761 #if 0
 762     fdrive_t *cur_drv;
 763
 764     cur_drv = get_cur_drv(fdctrl);
 765     fdctrl->fifo[0] = 0x60 | (cur_drv->head << 1) | fdctrl->cur_drv;
 766     fdctrl->fifo[1] = 0x00;
 767     fdctrl->fifo[2] = 0x00;
 768     fdctrl_set_fifo(fdctrl, 3, 1);
 769 #else
 770     //    fdctrl_reset_fifo(fdctrl);
 771     fdctrl->fifo[0] = 0x80;
 772     fdctrl_set_fifo(fdctrl, 1, 0);
 773 #endif
 774 }
 775
 776 /* Callback for transfer end (stop or abort) */
 777 static void fdctrl_stop_transfer (fdctrl_t *fdctrl, uint8_t status0,
 778                                   uint8_t status1, uint8_t status2)
 779 {
 780     fdrive_t *cur_drv;
 781
 782     cur_drv = get_cur_drv(fdctrl);
 783     FLOPPY_DPRINTF("transfer status: %02x %02x %02x (%02x)\n",
 784                    status0, status1, status2,
 785                    status0 | (cur_drv->head << 1) | fdctrl->cur_drv);
 786     fdctrl->fifo[0] = status0 | (cur_drv->head << 1) | fdctrl->cur_drv;
 787     fdctrl->fifo[1] = status1;
 788     fdctrl->fifo[2] = status2;
 789     fdctrl->fifo[3] = cur_drv->track;
 790     fdctrl->fifo[4] = cur_drv->head;
 791     fdctrl->fifo[5] = cur_drv->sect;
 792     fdctrl->fifo[6] = FD_SECTOR_SC;
 793     fdctrl->data_dir = FD_DIR_READ;
 794     if (fdctrl->state & FD_CTRL_BUSY) {
 795         DMA_release_DREQ(fdctrl->dma_chann);
 796         fdctrl->state &= ~FD_CTRL_BUSY;
 797     }
 798     fdctrl_set_fifo(fdctrl, 7, 1);
 799 }
 800
 801 /* Prepare a data transfer (either DMA or FIFO) */
 802 static void fdctrl_start_transfer (fdctrl_t *fdctrl, int direction)
 803 {
 804     fdrive_t *cur_drv;
 805     uint8_t kh, kt, ks;
 806     int did_seek;
 807
 808     fdctrl->cur_drv = fdctrl->fifo[1] & 1;
 809     cur_drv = get_cur_drv(fdctrl);
 810     kt = fdctrl->fifo[2];
 811     kh = fdctrl->fifo[3];
 812     ks = fdctrl->fifo[4];
 813     FLOPPY_DPRINTF("Start tranfert at %d %d %02x %02x (%d)\n",
 814                    fdctrl->cur_drv, kh, kt, ks,
 815                    _fd_sector(kh, kt, ks, cur_drv->last_sect));
 816     did_seek = 0;
 817     switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & 0x40)) {
 818     case 2:
 819         /* sect too big */
 820         fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
 821         fdctrl->fifo[3] = kt;
 822         fdctrl->fifo[4] = kh;
 823         fdctrl->fifo[5] = ks;
 824         return;
 825     case 3:
 826         /* track too big */
 827         fdctrl_stop_transfer(fdctrl, 0x40, 0x80, 0x00);
 828         fdctrl->fifo[3] = kt;
 829         fdctrl->fifo[4] = kh;
 830         fdctrl->fifo[5] = ks;
 831         return;
 832     case 4:
 833         /* No seek enabled */
 834         fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
 835         fdctrl->fifo[3] = kt;
 836         fdctrl->fifo[4] = kh;
 837         fdctrl->fifo[5] = ks;
 838         return;
 839     case 1:
 840         did_seek = 1;
 841         break;
 842     default:
 843         break;
 844     }
 845     /* Set the FIFO state */
 846     fdctrl->data_dir = direction;
 847     fdctrl->data_pos = 0;
 848     FD_SET_STATE(fdctrl->data_state, FD_STATE_DATA); /* FIFO ready for data */
 849     if (fdctrl->fifo[0] & 0x80)
 850         fdctrl->data_state |= FD_STATE_MULTI;
 851     else
 852         fdctrl->data_state &= ~FD_STATE_MULTI;
 853     if (did_seek)
 854         fdctrl->data_state |= FD_STATE_SEEK;
 855     else
 856         fdctrl->data_state &= ~FD_STATE_SEEK;
 857     if (fdctrl->fifo[5] == 00) {
 858         fdctrl->data_len = fdctrl->fifo[8];
 859     } else {
 860         int tmp;
 861         fdctrl->data_len = 128 << fdctrl->fifo[5];
 862         tmp = (cur_drv->last_sect - ks + 1);
 863         if (fdctrl->fifo[0] & 0x80)
 864             tmp += cur_drv->last_sect;
 865         fdctrl->data_len *= tmp;
 866     }
 867     if (fdctrl->dma_en) {
 868         int dma_mode;
 869         /* DMA transfer are enabled. Check if DMA channel is well programmed */
 870         dma_mode = DMA_get_channel_mode(fdctrl->dma_chann);
 871         dma_mode = (dma_mode >> 2) & 3;
 872         FLOPPY_DPRINTF("dma_mode=%d direction=%d (%d - %d)\n",
 873                        dma_mode, direction,
 874                        (128 << fdctrl->fifo[5]) *
 875                        (cur_drv->last_sect - ks + 1), fdctrl->data_len);
 876         if (((direction == FD_DIR_SCANE || direction == FD_DIR_SCANL ||
 877               direction == FD_DIR_SCANH) && dma_mode == 0) ||
 878             (direction == FD_DIR_WRITE && dma_mode == 2) ||
 879             (direction == FD_DIR_READ && dma_mode == 1)) {
 880             /* No access is allowed until DMA transfer has completed */
 881             fdctrl->state |= FD_CTRL_BUSY;
 882             /* Now, we just have to wait for the DMA controler to
 883              * recall us...
 884              */
 885             DMA_hold_DREQ(fdctrl->dma_chann);
 886             DMA_schedule(fdctrl->dma_chann);
 887             return;
 888         } else {
 889             FLOPPY_ERROR("dma_mode=%d direction=%d\n", dma_mode, direction);
 890         }
 891     }
 892     FLOPPY_DPRINTF("start non-DMA transfer\n");
 893     /* IO based transfer: calculate len */
 894     fdctrl_raise_irq(fdctrl, 0x00);
 895
 896     return;
 897 }
 898
 899 /* Prepare a transfer of deleted data */
 900 static void fdctrl_start_transfer_del (fdctrl_t *fdctrl, int direction)
 901 {
 902     /* We don't handle deleted data,
 903      * so we don't return *ANYTHING*
 904      */
 905     fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
 906 }
 907
 908 /* handlers for DMA transfers */
 909 static int fdctrl_transfer_handler (void *opaque, target_ulong addr, int size)
 910 {
 911     fdctrl_t *fdctrl;
 912     fdrive_t *cur_drv;
 913     int len, start_pos, rel_pos;
 914     uint8_t status0 = 0x00, status1 = 0x00, status2 = 0x00;
 915
 916     fdctrl = opaque;
 917     if (!(fdctrl->state & FD_CTRL_BUSY)) {
 918         FLOPPY_DPRINTF("Not in DMA transfer mode !\n");
 919         return 0;
 920     }
 921     cur_drv = get_cur_drv(fdctrl);
 922     if (fdctrl->data_dir == FD_DIR_SCANE || fdctrl->data_dir == FD_DIR_SCANL ||
 923         fdctrl->data_dir == FD_DIR_SCANH)
 924         status2 = 0x04;
 925     if (size > fdctrl->data_len)
 926         size = fdctrl->data_len;
 927             if (cur_drv->bs == NULL) {
 928         if (fdctrl->data_dir == FD_DIR_WRITE)
 929             fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
 930         else
 931             fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
 932         len = 0;
 933                 goto transfer_error;
 934             }
 935     rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;
 936     for (start_pos = fdctrl->data_pos; fdctrl->data_pos < size;) {
 937         len = size - fdctrl->data_pos;
 938         if (len + rel_pos > FD_SECTOR_LEN)
 939             len = FD_SECTOR_LEN - rel_pos;
 940         FLOPPY_DPRINTF("copy %d bytes (%d %d %d) %d pos %d %02x %02x "
 941                        "(%d-0x%08x 0x%08x)\n", len, size, fdctrl->data_pos,
 942                        fdctrl->data_len, fdctrl->cur_drv, cur_drv->head,
 943                        cur_drv->track, cur_drv->sect, fd_sector(cur_drv),
 944                        fd_sector(cur_drv) * 512, addr);
 945         if (fdctrl->data_dir != FD_DIR_WRITE ||
 946             len < FD_SECTOR_LEN || rel_pos != 0) {
 947             /* READ & SCAN commands and realign to a sector for WRITE */
 948             if (bdrv_read(cur_drv->bs, fd_sector(cur_drv),
 949                           fdctrl->fifo, 1) < 0) {
 950                 FLOPPY_DPRINTF("Floppy: error getting sector %d\n",
 951                                fd_sector(cur_drv));
 952                 /* Sure, image size is too small... */
 953                 memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
 954             }
 955                 }
 956         switch (fdctrl->data_dir) {
 957         case FD_DIR_READ:
 958             /* READ commands */
 959             cpu_physical_memory_write(addr + fdctrl->data_pos,
 960                                       fdctrl->fifo + rel_pos, len);
 961             break;
 962         case FD_DIR_WRITE:
 963             /* WRITE commands */
 964             cpu_physical_memory_read(addr + fdctrl->data_pos,
 965                                      fdctrl->fifo + rel_pos, len);
 966             if (bdrv_write(cur_drv->bs, fd_sector(cur_drv),
 967                            fdctrl->fifo, 1) < 0) {
 968                 FLOPPY_ERROR("writting sector %d\n", fd_sector(cur_drv));
 969                 fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
 970                 goto transfer_error;
 971                 }
 972             break;
 973         default:
 974             /* SCAN commands */
 975             {
 976                 uint8_t tmpbuf[FD_SECTOR_LEN];
 977                 int ret;
 978                 cpu_physical_memory_read(addr + fdctrl->data_pos,
 979                                          tmpbuf, len);
 980                 ret = memcmp(tmpbuf, fdctrl->fifo + rel_pos, len);
 981                 if (ret == 0) {
 982                     status2 = 0x08;
 983                     goto end_transfer;
 984                 }
 985                 if ((ret < 0 && fdctrl->data_dir == FD_DIR_SCANL) ||
 986                     (ret > 0 && fdctrl->data_dir == FD_DIR_SCANH)) {
 987                     status2 = 0x00;
 988                     goto end_transfer;
 989                 }
 990             }
 991             break;
 992         }
 993         fdctrl->data_pos += len;
 994         rel_pos = fdctrl->data_pos % FD_SECTOR_LEN;
 995         if (rel_pos == 0) {
 996             /* Seek to next sector */
 997             cur_drv->sect++;
 998             FLOPPY_DPRINTF("seek to next sector (%d %02x %02x => %d) (%d)\n",
 999                            cur_drv->head, cur_drv->track, cur_drv->sect,
1000                            fd_sector(cur_drv),
1001                            fdctrl->data_pos - size);
1002             if (cur_drv->sect > cur_drv->last_sect) {
1003                 cur_drv->sect = 1;
1004                 if (FD_MULTI_TRACK(fdctrl->data_state)) {
1005                     if (cur_drv->head == 0 &&
1006                         (cur_drv->flags & FDISK_DBL_SIDES) != 0) {
1007                     cur_drv->head = 1;
1008                 } else {
1009                     cur_drv->head = 0;
1010                         cur_drv->track++;
1011                         if ((cur_drv->flags & FDISK_DBL_SIDES) == 0)
1012                             break;
1013                 }
1014             } else {
1015                     cur_drv->track++;
1016                     break;
1017                 }
1018                 FLOPPY_DPRINTF("seek to next track (%d %02x %02x => %d)\n",
1019                                cur_drv->head, cur_drv->track,
1020                                cur_drv->sect, fd_sector(cur_drv));
1021             }
1022         }
1023     }
1024 end_transfer:
1025     len = fdctrl->data_pos - start_pos;
1026     FLOPPY_DPRINTF("end transfer %d %d %d\n",
1027                    fdctrl->data_pos, len, fdctrl->data_len);
1028     if (fdctrl->data_dir == FD_DIR_SCANE ||
1029         fdctrl->data_dir == FD_DIR_SCANL ||
1030         fdctrl->data_dir == FD_DIR_SCANH)
1031         status2 = 0x08;
1032     if (FD_DID_SEEK(fdctrl->data_state))
1033         status0 |= 0x20;
1034     fdctrl->data_len -= len;
1035     //    if (fdctrl->data_len == 0)
1036         fdctrl_stop_transfer(fdctrl, status0, status1, status2);
1037 transfer_error:
1038
1039     return len;
1040 }
1041
1042 /* Data register : 0x05 */
1043 static uint32_t fdctrl_read_data (fdctrl_t *fdctrl)
1044 {
1045     fdrive_t *cur_drv;
1046     uint32_t retval = 0;
1047     int pos, len;
1048
1049     cur_drv = get_cur_drv(fdctrl);
1050     fdctrl->state &= ~FD_CTRL_SLEEP;
1051     if (FD_STATE(fdctrl->data_state) == FD_STATE_CMD) {
1052         FLOPPY_ERROR("can't read data in CMD state\n");
1053         return 0;
1054     }
1055     pos = fdctrl->data_pos;
1056     if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
1057         pos %= FD_SECTOR_LEN;
1058         if (pos == 0) {
1059             len = fdctrl->data_len - fdctrl->data_pos;
1060             if (len > FD_SECTOR_LEN)
1061                 len = FD_SECTOR_LEN;
1062             bdrv_read(cur_drv->bs, fd_sector(cur_drv),
1063                       fdctrl->fifo, len);
1064         }
1065     }
1066     retval = fdctrl->fifo[pos];
1067     if (++fdctrl->data_pos == fdctrl->data_len) {
1068         fdctrl->data_pos = 0;
1069         /* Switch from transfert mode to status mode
1070          * then from status mode to command mode
1071          */
1072         if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
1073             fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
1074         } else {
1075             fdctrl_reset_fifo(fdctrl);
1076             fdctrl_reset_irq(fdctrl);
1077         }
1078     }
1079     FLOPPY_DPRINTF("data register: 0x%02x\n", retval);
1080
1081     return retval;
1082 }
1083
1084 static void fdctrl_format_sector (fdctrl_t *fdctrl)
1085 {
1086     fdrive_t *cur_drv;
1087     uint8_t kh, kt, ks;
1088     int did_seek;
1089
1090     fdctrl->cur_drv = fdctrl->fifo[1] & 1;
1091     cur_drv = get_cur_drv(fdctrl);
1092     kt = fdctrl->fifo[6];
1093     kh = fdctrl->fifo[7];
1094     ks = fdctrl->fifo[8];
1095     FLOPPY_DPRINTF("format sector at %d %d %02x %02x (%d)\n",
1096                    fdctrl->cur_drv, kh, kt, ks,
1097                    _fd_sector(kh, kt, ks, cur_drv->last_sect));
1098     did_seek = 0;
1099     switch (fd_seek(cur_drv, kh, kt, ks, fdctrl->config & 0x40)) {
1100     case 2:
1101         /* sect too big */
1102         fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
1103         fdctrl->fifo[3] = kt;
1104         fdctrl->fifo[4] = kh;
1105         fdctrl->fifo[5] = ks;
1106         return;
1107     case 3:
1108         /* track too big */
1109         fdctrl_stop_transfer(fdctrl, 0x40, 0x80, 0x00);
1110         fdctrl->fifo[3] = kt;
1111         fdctrl->fifo[4] = kh;
1112         fdctrl->fifo[5] = ks;
1113         return;
1114     case 4:
1115         /* No seek enabled */
1116         fdctrl_stop_transfer(fdctrl, 0x40, 0x00, 0x00);
1117         fdctrl->fifo[3] = kt;
1118         fdctrl->fifo[4] = kh;
1119         fdctrl->fifo[5] = ks;
1120         return;
1121     case 1:
1122         did_seek = 1;
1123         fdctrl->data_state |= FD_STATE_SEEK;
1124         break;
1125     default:
1126         break;
1127     }
1128     memset(fdctrl->fifo, 0, FD_SECTOR_LEN);
1129     if (cur_drv->bs == NULL ||
1130         bdrv_write(cur_drv->bs, fd_sector(cur_drv), fdctrl->fifo, 1) < 0) {
1131         FLOPPY_ERROR("formating sector %d\n", fd_sector(cur_drv));
1132         fdctrl_stop_transfer(fdctrl, 0x60, 0x00, 0x00);
1133     } else {
1134         if (cur_drv->sect == cur_drv->last_sect) {
1135             fdctrl->data_state &= ~FD_STATE_FORMAT;
1136             /* Last sector done */
1137             if (FD_DID_SEEK(fdctrl->data_state))
1138                 fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
1139             else
1140                 fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
1141         } else {
1142             /* More to do */
1143             fdctrl->data_pos = 0;
1144             fdctrl->data_len = 4;
1145         }
1146     }
1147 }
1148
1149 static void fdctrl_write_data (fdctrl_t *fdctrl, uint32_t value)
1150 {
1151     fdrive_t *cur_drv;
1152
1153     cur_drv = get_cur_drv(fdctrl);
1154     /* Reset mode */
1155     if (fdctrl->state & FD_CTRL_RESET) {
1156         FLOPPY_DPRINTF("Floppy controler in RESET state !\n");
1157         return;
1158     }
1159     fdctrl->state &= ~FD_CTRL_SLEEP;
1160     if (FD_STATE(fdctrl->data_state) == FD_STATE_STATUS) {
1161         FLOPPY_ERROR("can't write data in status mode\n");
1162         return;
1163     }
1164     /* Is it write command time ? */
1165     if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA) {
1166         /* FIFO data write */
1167         fdctrl->fifo[fdctrl->data_pos++] = value;
1168         if (fdctrl->data_pos % FD_SECTOR_LEN == (FD_SECTOR_LEN - 1) ||
1169             fdctrl->data_pos == fdctrl->data_len) {
1170             bdrv_write(cur_drv->bs, fd_sector(cur_drv),
1171                        fdctrl->fifo, FD_SECTOR_LEN);
1172         }
1173         /* Switch from transfert mode to status mode
1174          * then from status mode to command mode
1175          */
1176         if (FD_STATE(fdctrl->data_state) == FD_STATE_DATA)
1177             fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
1178         return;
1179     }
1180     if (fdctrl->data_pos == 0) {
1181         /* Command */
1182         switch (value & 0x5F) {
1183         case 0x46:
1184             /* READ variants */
1185             FLOPPY_DPRINTF("READ command\n");
1186             /* 8 parameters cmd */
1187             fdctrl->data_len = 9;
1188             goto enqueue;
1189         case 0x4C:
1190             /* READ_DELETED variants */
1191             FLOPPY_DPRINTF("READ_DELETED command\n");
1192             /* 8 parameters cmd */
1193             fdctrl->data_len = 9;
1194             goto enqueue;
1195         case 0x50:
1196             /* SCAN_EQUAL variants */
1197             FLOPPY_DPRINTF("SCAN_EQUAL command\n");
1198             /* 8 parameters cmd */
1199             fdctrl->data_len = 9;
1200             goto enqueue;
1201         case 0x56:
1202             /* VERIFY variants */
1203             FLOPPY_DPRINTF("VERIFY command\n");
1204             /* 8 parameters cmd */
1205             fdctrl->data_len = 9;
1206             goto enqueue;
1207         case 0x59:
1208             /* SCAN_LOW_OR_EQUAL variants */
1209             FLOPPY_DPRINTF("SCAN_LOW_OR_EQUAL command\n");
1210             /* 8 parameters cmd */
1211             fdctrl->data_len = 9;
1212             goto enqueue;
1213         case 0x5D:
1214             /* SCAN_HIGH_OR_EQUAL variants */
1215             FLOPPY_DPRINTF("SCAN_HIGH_OR_EQUAL command\n");
1216             /* 8 parameters cmd */
1217             fdctrl->data_len = 9;
1218             goto enqueue;
1219         default:
1220             break;
1221         }
1222         switch (value & 0x7F) {
1223         case 0x45:
1224             /* WRITE variants */
1225             FLOPPY_DPRINTF("WRITE command\n");
1226             /* 8 parameters cmd */
1227             fdctrl->data_len = 9;
1228             goto enqueue;
1229         case 0x49:
1230             /* WRITE_DELETED variants */
1231             FLOPPY_DPRINTF("WRITE_DELETED command\n");
1232             /* 8 parameters cmd */
1233             fdctrl->data_len = 9;
1234             goto enqueue;
1235         default:
1236             break;
1237         }
1238         switch (value) {
1239         case 0x03:
1240             /* SPECIFY */
1241             FLOPPY_DPRINTF("SPECIFY command\n");
1242             /* 1 parameter cmd */
1243             fdctrl->data_len = 3;
1244             goto enqueue;
1245         case 0x04:
1246             /* SENSE_DRIVE_STATUS */
1247             FLOPPY_DPRINTF("SENSE_DRIVE_STATUS command\n");
1248             /* 1 parameter cmd */
1249             fdctrl->data_len = 2;
1250             goto enqueue;
1251         case 0x07:
1252             /* RECALIBRATE */
1253             FLOPPY_DPRINTF("RECALIBRATE command\n");
1254             /* 1 parameter cmd */
1255             fdctrl->data_len = 2;
1256             goto enqueue;
1257         case 0x08:
1258             /* SENSE_INTERRUPT_STATUS */
1259             FLOPPY_DPRINTF("SENSE_INTERRUPT_STATUS command (%02x)\n",
1260                            fdctrl->int_status);
1261             /* No parameters cmd: returns status if no interrupt */
1262             fdctrl->fifo[0] =
1263                 fdctrl->int_status | (cur_drv->head << 2) | fdctrl->cur_drv;
1264             fdctrl->fifo[1] = cur_drv->track;
1265             fdctrl_set_fifo(fdctrl, 2, 0);
1266             fdctrl_reset_irq(fdctrl);
1267             fdctrl->int_status = 0xC0;
1268             return;
1269         case 0x0E:
1270             /* DUMPREG */
1271             FLOPPY_DPRINTF("DUMPREG command\n");
1272             /* Drives position */
1273             fdctrl->fifo[0] = drv0(fdctrl)->track;
1274             fdctrl->fifo[1] = drv1(fdctrl)->track;
1275             fdctrl->fifo[2] = 0;
1276             fdctrl->fifo[3] = 0;
1277             /* timers */
1278             fdctrl->fifo[4] = fdctrl->timer0;
1279             fdctrl->fifo[5] = (fdctrl->timer1 << 1) | fdctrl->dma_en;
1280             fdctrl->fifo[6] = cur_drv->last_sect;
1281             fdctrl->fifo[7] = (fdctrl->lock << 7) |
1282                     (cur_drv->perpendicular << 2);
1283             fdctrl->fifo[8] = fdctrl->config;
1284             fdctrl->fifo[9] = fdctrl->precomp_trk;
1285             fdctrl_set_fifo(fdctrl, 10, 0);
1286             return;
1287         case 0x0F:
1288             /* SEEK */
1289             FLOPPY_DPRINTF("SEEK command\n");
1290             /* 2 parameters cmd */
1291             fdctrl->data_len = 3;
1292             goto enqueue;
1293         case 0x10:
1294             /* VERSION */
1295             FLOPPY_DPRINTF("VERSION command\n");
1296             /* No parameters cmd */
1297             /* Controler's version */
1298             fdctrl->fifo[0] = fdctrl->version;
1299             fdctrl_set_fifo(fdctrl, 1, 1);
1300             return;
1301         case 0x12:
1302             /* PERPENDICULAR_MODE */
1303             FLOPPY_DPRINTF("PERPENDICULAR_MODE command\n");
1304             /* 1 parameter cmd */
1305             fdctrl->data_len = 2;
1306             goto enqueue;
1307         case 0x13:
1308             /* CONFIGURE */
1309             FLOPPY_DPRINTF("CONFIGURE command\n");
1310             /* 3 parameters cmd */
1311             fdctrl->data_len = 4;
1312             goto enqueue;
1313         case 0x14:
1314             /* UNLOCK */
1315             FLOPPY_DPRINTF("UNLOCK command\n");
1316             /* No parameters cmd */
1317             fdctrl->lock = 0;
1318             fdctrl->fifo[0] = 0;
1319             fdctrl_set_fifo(fdctrl, 1, 0);
1320             return;
1321         case 0x17:
1322             /* POWERDOWN_MODE */
1323             FLOPPY_DPRINTF("POWERDOWN_MODE command\n");
1324             /* 2 parameters cmd */
1325             fdctrl->data_len = 3;
1326             goto enqueue;
1327         case 0x18:
1328             /* PART_ID */
1329             FLOPPY_DPRINTF("PART_ID command\n");
1330             /* No parameters cmd */
1331             fdctrl->fifo[0] = 0x41; /* Stepping 1 */
1332             fdctrl_set_fifo(fdctrl, 1, 0);
1333             return;
1334         case 0x2C:
1335             /* SAVE */
1336             FLOPPY_DPRINTF("SAVE command\n");
1337             /* No parameters cmd */
1338             fdctrl->fifo[0] = 0;
1339             fdctrl->fifo[1] = 0;
1340             /* Drives position */
1341             fdctrl->fifo[2] = drv0(fdctrl)->track;
1342             fdctrl->fifo[3] = drv1(fdctrl)->track;
1343             fdctrl->fifo[4] = 0;
1344             fdctrl->fifo[5] = 0;
1345             /* timers */
1346             fdctrl->fifo[6] = fdctrl->timer0;
1347             fdctrl->fifo[7] = fdctrl->timer1;
1348             fdctrl->fifo[8] = cur_drv->last_sect;
1349             fdctrl->fifo[9] = (fdctrl->lock << 7) |
1350                     (cur_drv->perpendicular << 2);
1351             fdctrl->fifo[10] = fdctrl->config;
1352             fdctrl->fifo[11] = fdctrl->precomp_trk;
1353             fdctrl->fifo[12] = fdctrl->pwrd;
1354             fdctrl->fifo[13] = 0;
1355             fdctrl->fifo[14] = 0;
1356             fdctrl_set_fifo(fdctrl, 15, 1);
1357             return;
1358         case 0x33:
1359             /* OPTION */
1360             FLOPPY_DPRINTF("OPTION command\n");
1361             /* 1 parameter cmd */
1362             fdctrl->data_len = 2;
1363             goto enqueue;
1364         case 0x42:
1365             /* READ_TRACK */
1366             FLOPPY_DPRINTF("READ_TRACK command\n");
1367             /* 8 parameters cmd */
1368             fdctrl->data_len = 9;
1369             goto enqueue;
1370         case 0x4A:
1371             /* READ_ID */
1372             FLOPPY_DPRINTF("READ_ID command\n");
1373             /* 1 parameter cmd */
1374             fdctrl->data_len = 2;
1375             goto enqueue;
1376         case 0x4C:
1377             /* RESTORE */
1378             FLOPPY_DPRINTF("RESTORE command\n");
1379             /* 17 parameters cmd */
1380             fdctrl->data_len = 18;
1381             goto enqueue;
1382         case 0x4D:
1383             /* FORMAT_TRACK */
1384             FLOPPY_DPRINTF("FORMAT_TRACK command\n");
1385             /* 5 parameters cmd */
1386             fdctrl->data_len = 6;
1387             goto enqueue;
1388         case 0x8E:
1389             /* DRIVE_SPECIFICATION_COMMAND */
1390             FLOPPY_DPRINTF("DRIVE_SPECIFICATION_COMMAND command\n");
1391             /* 5 parameters cmd */
1392             fdctrl->data_len = 6;
1393             goto enqueue;
1394         case 0x8F:
1395             /* RELATIVE_SEEK_OUT */
1396             FLOPPY_DPRINTF("RELATIVE_SEEK_OUT command\n");
1397             /* 2 parameters cmd */
1398             fdctrl->data_len = 3;
1399             goto enqueue;
1400         case 0x94:
1401             /* LOCK */
1402             FLOPPY_DPRINTF("LOCK command\n");
1403             /* No parameters cmd */
1404             fdctrl->lock = 1;
1405             fdctrl->fifo[0] = 0x10;
1406             fdctrl_set_fifo(fdctrl, 1, 1);
1407             return;
1408         case 0xCD:
1409             /* FORMAT_AND_WRITE */
1410             FLOPPY_DPRINTF("FORMAT_AND_WRITE command\n");
1411             /* 10 parameters cmd */
1412             fdctrl->data_len = 11;
1413             goto enqueue;
1414         case 0xCF:
1415             /* RELATIVE_SEEK_IN */
1416             FLOPPY_DPRINTF("RELATIVE_SEEK_IN command\n");
1417             /* 2 parameters cmd */
1418             fdctrl->data_len = 3;
1419             goto enqueue;
1420         default:
1421             /* Unknown command */
1422             FLOPPY_ERROR("unknown command: 0x%02x\n", value);
1423             fdctrl_unimplemented(fdctrl);
1424             return;
1425         }
1426     }
1427 enqueue:
1428     FLOPPY_DPRINTF("%s: %02x\n", __func__, value);
1429     fdctrl->fifo[fdctrl->data_pos] = value;
1430     if (++fdctrl->data_pos == fdctrl->data_len) {
1431         /* We now have all parameters
1432          * and will be able to treat the command
1433          */
1434         if (fdctrl->data_state & FD_STATE_FORMAT) {
1435             fdctrl_format_sector(fdctrl);
1436             return;
1437         }
1438         switch (fdctrl->fifo[0] & 0x1F) {
1439         case 0x06:
1440         {
1441             /* READ variants */
1442             FLOPPY_DPRINTF("treat READ command\n");
1443             fdctrl_start_transfer(fdctrl, FD_DIR_READ);
1444             return;
1445         }
1446         case 0x0C:
1447             /* READ_DELETED variants */
1448 //            FLOPPY_DPRINTF("treat READ_DELETED command\n");
1449             FLOPPY_ERROR("treat READ_DELETED command\n");
1450             fdctrl_start_transfer_del(fdctrl, FD_DIR_READ);
1451             return;
1452         case 0x16:
1453             /* VERIFY variants */
1454 //            FLOPPY_DPRINTF("treat VERIFY command\n");
1455             FLOPPY_ERROR("treat VERIFY command\n");
1456             fdctrl_stop_transfer(fdctrl, 0x20, 0x00, 0x00);
1457             return;
1458         case 0x10:
1459             /* SCAN_EQUAL variants */
1460 //            FLOPPY_DPRINTF("treat SCAN_EQUAL command\n");
1461             FLOPPY_ERROR("treat SCAN_EQUAL command\n");
1462             fdctrl_start_transfer(fdctrl, FD_DIR_SCANE);
1463             return;
1464         case 0x19:
1465             /* SCAN_LOW_OR_EQUAL variants */
1466 //            FLOPPY_DPRINTF("treat SCAN_LOW_OR_EQUAL command\n");
1467             FLOPPY_ERROR("treat SCAN_LOW_OR_EQUAL command\n");
1468             fdctrl_start_transfer(fdctrl, FD_DIR_SCANL);
1469             return;
1470         case 0x1D:
1471             /* SCAN_HIGH_OR_EQUAL variants */
1472 //            FLOPPY_DPRINTF("treat SCAN_HIGH_OR_EQUAL command\n");
1473             FLOPPY_ERROR("treat SCAN_HIGH_OR_EQUAL command\n");
1474             fdctrl_start_transfer(fdctrl, FD_DIR_SCANH);
1475             return;
1476         default:
1477             break;
1478         }
1479         switch (fdctrl->fifo[0] & 0x3F) {
1480         case 0x05:
1481             /* WRITE variants */
1482             FLOPPY_DPRINTF("treat WRITE command (%02x)\n", fdctrl->fifo[0]);
1483             fdctrl_start_transfer(fdctrl, FD_DIR_WRITE);
1484             return;
1485         case 0x09:
1486             /* WRITE_DELETED variants */
1487 //            FLOPPY_DPRINTF("treat WRITE_DELETED command\n");
1488             FLOPPY_ERROR("treat WRITE_DELETED command\n");
1489             fdctrl_start_transfer_del(fdctrl, FD_DIR_WRITE);
1490             return;
1491         default:
1492             break;
1493         }
1494         switch (fdctrl->fifo[0]) {
1495         case 0x03:
1496             /* SPECIFY */
1497             FLOPPY_DPRINTF("treat SPECIFY command\n");
1498             fdctrl->timer0 = (fdctrl->fifo[1] >> 4) & 0xF;
1499             fdctrl->timer1 = fdctrl->fifo[2] >> 1;
1500             fdctrl->dma_en = 1 - (fdctrl->fifo[2] & 1) ;
1501             /* No result back */
1502             fdctrl_reset_fifo(fdctrl);
1503             break;
1504         case 0x04:
1505             /* SENSE_DRIVE_STATUS */
1506             FLOPPY_DPRINTF("treat SENSE_DRIVE_STATUS command\n");
1507             fdctrl->cur_drv = fdctrl->fifo[1] & 1;
1508             cur_drv = get_cur_drv(fdctrl);
1509             cur_drv->head = (fdctrl->fifo[1] >> 2) & 1;
1510             /* 1 Byte status back */
1511             fdctrl->fifo[0] = (cur_drv->ro << 6) |
1512                 (cur_drv->track == 0 ? 0x10 : 0x00) |
1513                 fdctrl->cur_drv;
1514             fdctrl_set_fifo(fdctrl, 1, 0);
1515             break;
1516         case 0x07:
1517             /* RECALIBRATE */
1518             FLOPPY_DPRINTF("treat RECALIBRATE command\n");
1519             fdctrl->cur_drv = fdctrl->fifo[1] & 1;
1520             cur_drv = get_cur_drv(fdctrl);
1521             fd_recalibrate(cur_drv);
1522             fdctrl_reset_fifo(fdctrl);
1523             /* Raise Interrupt */
1524             fdctrl_raise_irq(fdctrl, 0x20);
1525             break;
1526         case 0x0F:
1527             /* SEEK */
1528             FLOPPY_DPRINTF("treat SEEK command\n");
1529             fdctrl->cur_drv = fdctrl->fifo[1] & 1;
1530             cur_drv = get_cur_drv(fdctrl);
1531             fd_start(cur_drv);
1532             if (fdctrl->fifo[2] <= cur_drv->track)
1533                 cur_drv->dir = 1;
1534             else
1535                 cur_drv->dir = 0;
1536             fdctrl_reset_fifo(fdctrl);
1537             if (fdctrl->fifo[2] > cur_drv->max_track) {
1538                 fdctrl_raise_irq(fdctrl, 0x60);
1539             } else {
1540                 cur_drv->track = fdctrl->fifo[2];
1541                 /* Raise Interrupt */
1542                 fdctrl_raise_irq(fdctrl, 0x20);
1543             }
1544             break;
1545         case 0x12:
1546             /* PERPENDICULAR_MODE */
1547             FLOPPY_DPRINTF("treat PERPENDICULAR_MODE command\n");
1548             if (fdctrl->fifo[1] & 0x80)
1549                 cur_drv->perpendicular = fdctrl->fifo[1] & 0x7;
1550             /* No result back */
1551             fdctrl_reset_fifo(fdctrl);
1552             break;
1553         case 0x13:
1554             /* CONFIGURE */
1555             FLOPPY_DPRINTF("treat CONFIGURE command\n");
1556             fdctrl->config = fdctrl->fifo[2];
1557             fdctrl->precomp_trk =  fdctrl->fifo[3];
1558             /* No result back */
1559             fdctrl_reset_fifo(fdctrl);
1560             break;
1561         case 0x17:
1562             /* POWERDOWN_MODE */
1563             FLOPPY_DPRINTF("treat POWERDOWN_MODE command\n");
1564             fdctrl->pwrd = fdctrl->fifo[1];
1565             fdctrl->fifo[0] = fdctrl->fifo[1];
1566             fdctrl_set_fifo(fdctrl, 1, 1);
1567             break;
1568         case 0x33:
1569             /* OPTION */
1570             FLOPPY_DPRINTF("treat OPTION command\n");
1571             /* No result back */
1572             fdctrl_reset_fifo(fdctrl);
1573             break;
1574         case 0x42:
1575             /* READ_TRACK */
1576 //            FLOPPY_DPRINTF("treat READ_TRACK command\n");
1577             FLOPPY_ERROR("treat READ_TRACK command\n");
1578             fdctrl_start_transfer(fdctrl, FD_DIR_READ);
1579             break;
1580         case 0x4A:
1581                 /* READ_ID */
1582             FLOPPY_DPRINTF("treat READ_ID command\n");
1583             /* XXX: should set main status register to busy */
1584             qemu_mod_timer(fdctrl->result_timer,
1585                            qemu_get_clock(vm_clock) + (ticks_per_sec / 50));
1586             break;
1587         case 0x4C:
1588             /* RESTORE */
1589             FLOPPY_DPRINTF("treat RESTORE command\n");
1590             /* Drives position */
1591             drv0(fdctrl)->track = fdctrl->fifo[3];
1592             drv1(fdctrl)->track = fdctrl->fifo[4];
1593             /* timers */
1594             fdctrl->timer0 = fdctrl->fifo[7];
1595             fdctrl->timer1 = fdctrl->fifo[8];
1596             cur_drv->last_sect = fdctrl->fifo[9];
1597             fdctrl->lock = fdctrl->fifo[10] >> 7;
1598             cur_drv->perpendicular = (fdctrl->fifo[10] >> 2) & 0xF;
1599             fdctrl->config = fdctrl->fifo[11];
1600             fdctrl->precomp_trk = fdctrl->fifo[12];
1601             fdctrl->pwrd = fdctrl->fifo[13];
1602             fdctrl_reset_fifo(fdctrl);
1603             break;
1604         case 0x4D:
1605             /* FORMAT_TRACK */
1606             FLOPPY_DPRINTF("treat FORMAT_TRACK command\n");
1607             fdctrl->cur_drv = fdctrl->fifo[1] & 1;
1608             cur_drv = get_cur_drv(fdctrl);
1609             fdctrl->data_state |= FD_STATE_FORMAT;
1610             if (fdctrl->fifo[0] & 0x80)
1611                 fdctrl->data_state |= FD_STATE_MULTI;
1612             else
1613                 fdctrl->data_state &= ~FD_STATE_MULTI;
1614             fdctrl->data_state &= ~FD_STATE_SEEK;
1615             cur_drv->bps =
1616                 fdctrl->fifo[2] > 7 ? 16384 : 128 << fdctrl->fifo[2];
1617 #if 0
1618             cur_drv->last_sect =
1619                 cur_drv->flags & FDISK_DBL_SIDES ? fdctrl->fifo[3] :
1620                 fdctrl->fifo[3] / 2;
1621 #else
1622             cur_drv->last_sect = fdctrl->fifo[3];
1623 #endif
1624             /* Bochs BIOS is buggy and don't send format informations
1625              * for each sector. So, pretend all's done right now...
1626              */
1627             fdctrl->data_state &= ~FD_STATE_FORMAT;
1628             fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
1629             break;
1630         case 0x8E:
1631             /* DRIVE_SPECIFICATION_COMMAND */
1632             FLOPPY_DPRINTF("treat DRIVE_SPECIFICATION_COMMAND command\n");
1633             if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x80) {
1634                 /* Command parameters done */
1635                 if (fdctrl->fifo[fdctrl->data_pos - 1] & 0x40) {
1636                     fdctrl->fifo[0] = fdctrl->fifo[1];
1637                     fdctrl->fifo[2] = 0;
1638                     fdctrl->fifo[3] = 0;
1639                     fdctrl_set_fifo(fdctrl, 4, 1);
1640                 } else {
1641                     fdctrl_reset_fifo(fdctrl);
1642                 }
1643             } else if (fdctrl->data_len > 7) {
1644                 /* ERROR */
1645                 fdctrl->fifo[0] = 0x80 |
1646                     (cur_drv->head << 2) | fdctrl->cur_drv;
1647                 fdctrl_set_fifo(fdctrl, 1, 1);
1648             }
1649             break;
1650         case 0x8F:
1651             /* RELATIVE_SEEK_OUT */
1652             FLOPPY_DPRINTF("treat RELATIVE_SEEK_OUT command\n");
1653             fdctrl->cur_drv = fdctrl->fifo[1] & 1;
1654             cur_drv = get_cur_drv(fdctrl);
1655             fd_start(cur_drv);
1656                 cur_drv->dir = 0;
1657             if (fdctrl->fifo[2] + cur_drv->track >= cur_drv->max_track) {
1658                 cur_drv->track = cur_drv->max_track - 1;
1659             } else {
1660                 cur_drv->track += fdctrl->fifo[2];
1661             }
1662             fdctrl_reset_fifo(fdctrl);
1663             fdctrl_raise_irq(fdctrl, 0x20);
1664             break;
1665         case 0xCD:
1666             /* FORMAT_AND_WRITE */
1667 //                FLOPPY_DPRINTF("treat FORMAT_AND_WRITE command\n");
1668             FLOPPY_ERROR("treat FORMAT_AND_WRITE command\n");
1669             fdctrl_unimplemented(fdctrl);
1670             break;
1671         case 0xCF:
1672                 /* RELATIVE_SEEK_IN */
1673             FLOPPY_DPRINTF("treat RELATIVE_SEEK_IN command\n");
1674             fdctrl->cur_drv = fdctrl->fifo[1] & 1;
1675             cur_drv = get_cur_drv(fdctrl);
1676             fd_start(cur_drv);
1677                 cur_drv->dir = 1;
1678             if (fdctrl->fifo[2] > cur_drv->track) {
1679                 cur_drv->track = 0;
1680             } else {
1681                 cur_drv->track -= fdctrl->fifo[2];
1682             }
1683             fdctrl_reset_fifo(fdctrl);
1684             /* Raise Interrupt */
1685             fdctrl_raise_irq(fdctrl, 0x20);
1686             break;
1687         }
1688     }
1689 }
1690
1691 static void fdctrl_result_timer(void *opaque)
1692 {
1693     fdctrl_t *fdctrl = opaque;
1694     fdctrl_stop_transfer(fdctrl, 0x00, 0x00, 0x00);
1695 }