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