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